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-06-20 15:40:34 +08:00 · 2022-01-21 12:53:26 -05:00 · 2022-01-21 09:32:30 -05:00 · 2022-01-21 09:18:49 -05:00 · 2022-01-21 09:13:03 +01:00 · 2022-01-20 12:58:20 -05:00
454 changed files with 8476 additions and 2774 deletions
@@ -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
@@ -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,7 +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_MODULE_PATHS
                 BRIEF_DOCS "PX4 module paths"
                 FULL_DOCS "List of paths to all PX4 modules"
@@ -133,29 +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)

 # 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)
@@ -191,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")
@@ -230,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)
@@ -246,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)
@@ -315,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)
 #
@@ -329,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
 #
@@ -410,8 +422,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})
@@ -433,6 +450,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'
 #
@@ -460,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"
@@ -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:
@@ -0,0 +1,23 @@
+#!/bin/sh
+#
+# @name Holybro X500 V2
+#
+# @type Quadrotor x
+# @class Copter
+#
+# @maintainer Farhang Naderi <farhang.nba@gmail.com>
+#
+# @board px4_fmu-v2 exclude
+# @board bitcraze_crazyflie exclude
+#
+
+. ${R}etc/init.d/rc.mc_defaults
+
+param set-default IMU_GYRO_CUTOFF 30
+
+param set-default MC_ROLLRATE_P 0.14
+param set-default MC_PITCHRATE_P 0.14
+param set-default MC_ROLLRATE_I 0.3
+param set-default MC_PITCHRATE_I 0.3
+param set-default MC_ROLLRATE_D 0.004
+param set-default MC_PITCHRATE_D 0.004
@@ -73,6 +73,7 @@ px4_add_romfs_files(
 	4016_holybro_px4vision
 	4017_nxp_hovergames
 	4018_s500_ctrlalloc
+	4019_x500_v2
 	4030_3dr_solo
 	4031_3dr_quad
 	4040_reaper
@@ -44,4 +44,6 @@ px4_add_module(
 		syslink.c
 	DEPENDS
 		battery
+	SUBSCRIPTIONS
+		"px4::msg::BatteryStatus /battery_status"
 	)
@@ -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,7 @@ if("${PX4_BOARD_LABEL}" STREQUAL  "canbootloader")
 			nuttx_arch
 			nuttx_drivers
 			canbootloader
+			drivers__device
 	)
 	target_include_directories(drivers_board PRIVATE ${PX4_SOURCE_DIR}/platforms/nuttx/src/canbootloader)

@@ -60,6 +60,7 @@
 #define STM32_HSI_FREQUENCY     16000000ul
 #define STM32_LSI_FREQUENCY     32000
 #define STM32_HSE_FREQUENCY     STM32_BOARD_XTAL
+#define STM32_LSE_FREQUENCY     0

 /* Main PLL Configuration.
 *
@@ -81,55 +82,56 @@
 *   PLLP2,3 = {2, 3, 4, ..., 128}
 *   CPUCLK <= 480 MHz
 */
+#define STM32_PLLCFG_PLLSRC      RCC_PLLCKSELR_PLLSRC_HSE

 /* PLL1, wide 4 - 8 MHz input, enable DIVP, DIVQ, DIVR
 *
- *   PLL1_VCO = (24,000,000 / 3) * 100 = 800 MHz
+ *   PLL1_VCO = (24,000,000 / 2) * 80 = 960 MHz
 *
- *   PLL1P = PLL1_VCO/2  = 800 MHz / 2   = 400 MHz
- *   PLL1Q = PLL1_VCO/8  = 800 MHz / 8   = 100 MHz
- *   PLL1R = PLL1_VCO/2  = 800 MHz / 2   = 400 MHz
+ *   PLL1P = PLL1_VCO/2  = 960 MHz / 2   = 480 MHz
+ *   PLL1Q = PLL1_VCO/4  = 960 MHz / 4   = 240 MHz
+ *   PLL1R = PLL1_VCO/8  = 960 MHz / 8   = 120 MHz
 */
 #define STM32_PLLCFG_PLL1CFG    (RCC_PLLCFGR_PLL1VCOSEL_WIDE|RCC_PLLCFGR_PLL1RGE_4_8_MHZ|RCC_PLLCFGR_DIVP1EN|RCC_PLLCFGR_DIVQ1EN|RCC_PLLCFGR_DIVR1EN)
-#define STM32_PLLCFG_PLL1M       RCC_PLLCKSELR_DIVM1(3)
-#define STM32_PLLCFG_PLL1N       RCC_PLL1DIVR_N1(100)
+#define STM32_PLLCFG_PLL1M       RCC_PLLCKSELR_DIVM1(2)
+#define STM32_PLLCFG_PLL1N       RCC_PLL1DIVR_N1(80)
 #define STM32_PLLCFG_PLL1P       RCC_PLL1DIVR_P1(2)
-#define STM32_PLLCFG_PLL1Q       RCC_PLL1DIVR_Q1(8)
-#define STM32_PLLCFG_PLL1R       RCC_PLL1DIVR_R1(2)
+#define STM32_PLLCFG_PLL1Q       RCC_PLL1DIVR_Q1(4)
+#define STM32_PLLCFG_PLL1R       RCC_PLL1DIVR_R1(8)

-#define STM32_VCO1_FREQUENCY     ((STM32_HSE_FREQUENCY / 3) * 100)
+#define STM32_VCO1_FREQUENCY     ((STM32_HSE_FREQUENCY / 2) * 80)
 #define STM32_PLL1P_FREQUENCY    (STM32_VCO1_FREQUENCY / 2)
-#define STM32_PLL1Q_FREQUENCY    (STM32_VCO1_FREQUENCY / 8)
-#define STM32_PLL1R_FREQUENCY    (STM32_VCO1_FREQUENCY / 2)
+#define STM32_PLL1Q_FREQUENCY    (STM32_VCO1_FREQUENCY / 4)
+#define STM32_PLL1R_FREQUENCY    (STM32_VCO1_FREQUENCY / 8)

 /* PLL2 */
 #define STM32_PLLCFG_PLL2CFG     (RCC_PLLCFGR_PLL2VCOSEL_WIDE|RCC_PLLCFGR_PLL2RGE_4_8_MHZ|RCC_PLLCFGR_DIVP2EN|RCC_PLLCFGR_DIVQ2EN|RCC_PLLCFGR_DIVR2EN)
-#define STM32_PLLCFG_PLL2M       RCC_PLLCKSELR_DIVM2(2)
-#define STM32_PLLCFG_PLL2N       RCC_PLL2DIVR_N2(30)
-#define STM32_PLLCFG_PLL2P       RCC_PLL2DIVR_P2(4)
-#define STM32_PLLCFG_PLL2Q       RCC_PLL2DIVR_Q2(5)
-#define STM32_PLLCFG_PLL2R       RCC_PLL2DIVR_R2(1)
+#define STM32_PLLCFG_PLL2M       RCC_PLLCKSELR_DIVM2(4)
+#define STM32_PLLCFG_PLL2N       RCC_PLL2DIVR_N2(32)
+#define STM32_PLLCFG_PLL2P       RCC_PLL2DIVR_P2(2)
+#define STM32_PLLCFG_PLL2Q       RCC_PLL2DIVR_Q2(2)
+#define STM32_PLLCFG_PLL2R       RCC_PLL2DIVR_R2(2)

-#define STM32_VCO2_FREQUENCY     ((STM32_HSE_FREQUENCY / 2) * 30)
-#define STM32_PLL2P_FREQUENCY    (STM32_VCO2_FREQUENCY / 4)
-#define STM32_PLL2Q_FREQUENCY    (STM32_VCO2_FREQUENCY / 5)
-#define STM32_PLL2R_FREQUENCY    (STM32_VCO2_FREQUENCY / 1)
+#define STM32_VCO2_FREQUENCY     ((STM32_HSE_FREQUENCY / 4) * 32)
+#define STM32_PLL2P_FREQUENCY    (STM32_VCO2_FREQUENCY / 2)
+#define STM32_PLL2Q_FREQUENCY    (STM32_VCO2_FREQUENCY / 2)
+#define STM32_PLL2R_FREQUENCY    (STM32_VCO2_FREQUENCY / 2)

 /* PLL3 */
 #define STM32_PLLCFG_PLL3CFG    (RCC_PLLCFGR_PLL3VCOSEL_WIDE|RCC_PLLCFGR_PLL3RGE_4_8_MHZ|RCC_PLLCFGR_DIVQ3EN)
-#define STM32_PLLCFG_PLL3M      RCC_PLLCKSELR_DIVM3(6)
-#define STM32_PLLCFG_PLL3N      RCC_PLL3DIVR_N3(72)
-#define STM32_PLLCFG_PLL3P      RCC_PLL3DIVR_P3(3)
-#define STM32_PLLCFG_PLL3Q      RCC_PLL3DIVR_Q3(6)
-#define STM32_PLLCFG_PLL3R      RCC_PLL3DIVR_R3(9)
+#define STM32_PLLCFG_PLL3M      RCC_PLLCKSELR_DIVM3(4)
+#define STM32_PLLCFG_PLL3N      RCC_PLL3DIVR_N3(32)
+#define STM32_PLLCFG_PLL3P      RCC_PLL3DIVR_P3(2)
+#define STM32_PLLCFG_PLL3Q      RCC_PLL3DIVR_Q3(4)
+#define STM32_PLLCFG_PLL3R      RCC_PLL3DIVR_R3(2)

-#define STM32_VCO3_FREQUENCY    ((STM32_HSE_FREQUENCY / 6) * 72)
-#define STM32_PLL3P_FREQUENCY   (STM32_VCO3_FREQUENCY / 3)
-#define STM32_PLL3Q_FREQUENCY   (STM32_VCO3_FREQUENCY / 6)
-#define STM32_PLL3R_FREQUENCY   (STM32_VCO3_FREQUENCY / 9)
+#define STM32_VCO3_FREQUENCY    ((STM32_HSE_FREQUENCY / 4) * 32)
+#define STM32_PLL3P_FREQUENCY   (STM32_VCO3_FREQUENCY / 2)
+#define STM32_PLL3Q_FREQUENCY   (STM32_VCO3_FREQUENCY / 4)
+#define STM32_PLL3R_FREQUENCY   (STM32_VCO3_FREQUENCY / 2)

-/* SYSCLK = PLL1P = 400MHz
- * CPUCLK = SYSCLK / 1 = 400 MHz
+/* SYSCLK = PLL1P = 480MHz
+ * CPUCLK = SYSCLK / 1 = 480 MHz
 */
 #define STM32_RCC_D1CFGR_D1CPRE  (RCC_D1CFGR_D1CPRE_SYSCLK)
 #define STM32_SYSCLK_FREQUENCY   (STM32_PLL1P_FREQUENCY)
@@ -138,14 +140,14 @@
 /* Configure Clock Assignments */

 /* AHB clock (HCLK) is SYSCLK/2 (240 MHz max)
- * HCLK1 = HCLK2 = HCLK3 = HCLK4 = 200
+ * HCLK1 = HCLK2 = HCLK3 = HCLK4 = 240
 */
 #define STM32_RCC_D1CFGR_HPRE   RCC_D1CFGR_HPRE_SYSCLKd2        /* HCLK  = SYSCLK / 2 */
 #define STM32_ACLK_FREQUENCY    (STM32_CPUCLK_FREQUENCY / 2)    /* ACLK in D1, HCLK3 in D1 */
 #define STM32_HCLK_FREQUENCY    (STM32_CPUCLK_FREQUENCY / 2)    /* HCLK in D2, HCLK4 in D3 */
 #define STM32_BOARD_HCLK        STM32_HCLK_FREQUENCY            /* same as above, to satisfy compiler */

-/* APB1 clock (PCLK1) is HCLK/4 (120 MHz) */
+/* APB1 clock (PCLK1) is HCLK/2 (120 MHz) */
 #define STM32_RCC_D2CFGR_D2PPRE1  RCC_D2CFGR_D2PPRE1_HCLKd2       /* PCLK1 = HCLK / 2 */
 #define STM32_PCLK1_FREQUENCY     (STM32_HCLK_FREQUENCY/2)

@@ -184,29 +186,37 @@
 /* Kernel Clock Configuration
 *  Note: look at Table 54 in ST Manual
 */
-#define STM32_RCC_D1CCIPR_SDMMCSEL   RCC_D1CCIPR_SDMMC_PLL1
-
 #define STM32_RCC_D2CCIP2R_I2C123SRC RCC_D2CCIP2R_I2C123SEL_HSI  /* I2C123 clock source */
+#define STM32_RCC_D3CCIPR_I2C4SRC	   RCC_D3CCIPR_I2C4SEL_HSI	 /* I2C4 clock source */
 #define STM32_RCC_D2CCIP1R_SPI123SRC RCC_D2CCIP1R_SPI123SEL_PLL2 /* SPI123 clock source */
 #define STM32_RCC_D2CCIP1R_SPI45SRC  RCC_D2CCIP1R_SPI45SEL_PLL2  /* SPI45 clock source */
 #define STM32_RCC_D3CCIPR_SPI6SRC    RCC_D3CCIPR_SPI6SEL_PLL2    /* SPI6 clock source */
 #define STM32_RCC_D2CCIP2R_USBSRC    RCC_D2CCIP2R_USBSEL_PLL3    /* USB 1 and 2 clock source */
+#define STM32_RCC_D3CCIPR_ADCSEL     RCC_D3CCIPR_ADCSEL_PLL2     /* ADC 1 2 3 clock source */
 #define STM32_RCC_D2CCIP1R_FDCANSEL  RCC_D2CCIP1R_FDCANSEL_HSE   /* FDCAN 1 2 clock source */

-#define STM32_RCC_D3CCIPR_ADCSEL     RCC_D3CCIPR_ADCSEL_PLL2     /* ADC 1 2 3 clock source */
+#define STM32_FDCANCLK               STM32_HSE_FREQUENCY

 /* FLASH wait states */
 #define BOARD_FLASH_WAITSTATES 2

 /* SDMMC definitions ********************************************************/
 /* Init 400kHz, freq = PLL1Q/(2*div)  div =  PLL1Q/(2*freq) */
-#define STM32_SDMMC_INIT_CLKDIV     (125 << STM32_SDMMC_CLKCR_CLKDIV_SHIFT)
+#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 = 100 / (2*25)
+/* 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
 */
-#define STM32_SDMMC_MMCXFR_CLKDIV   (2 << STM32_SDMMC_CLKCR_CLKDIV_SHIFT)
-#define STM32_SDMMC_SDXFR_CLKDIV    (2 << STM32_SDMMC_CLKCR_CLKDIV_SHIFT)
+#if defined(CONFIG_STM32H7_SDMMC_XDMA) || defined(CONFIG_STM32H7_SDMMC_IDMA)
+#  define STM32_SDMMC_MMCXFR_CLKDIV   (5 << 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)
+#else
+#  define STM32_SDMMC_SDXFR_CLKDIV    (100 << STM32_SDMMC_CLKCR_CLKDIV_SHIFT)
+#endif

 #define STM32_SDMMC_CLKCR_EDGE      STM32_SDMMC_CLKCR_NEGEDGE

@@ -60,6 +60,7 @@
 #define STM32_HSI_FREQUENCY     16000000ul
 #define STM32_LSI_FREQUENCY     32000
 #define STM32_HSE_FREQUENCY     STM32_BOARD_XTAL
+#define STM32_LSE_FREQUENCY     0

 /* Main PLL Configuration.
 *
@@ -81,55 +82,56 @@
 *   PLLP2,3 = {2, 3, 4, ..., 128}
 *   CPUCLK <= 480 MHz
 */
+#define STM32_PLLCFG_PLLSRC      RCC_PLLCKSELR_PLLSRC_HSE

 /* PLL1, wide 4 - 8 MHz input, enable DIVP, DIVQ, DIVR
 *
- *   PLL1_VCO = (24,000,000 / 3) * 100 = 800 MHz
+ *   PLL1_VCO = (24,000,000 / 2) * 80 = 960 MHz
 *
- *   PLL1P = PLL1_VCO/2  = 800 MHz / 2   = 400 MHz
- *   PLL1Q = PLL1_VCO/8  = 800 MHz / 8   = 100 MHz
- *   PLL1R = PLL1_VCO/2  = 800 MHz / 2   = 400 MHz
+ *   PLL1P = PLL1_VCO/2  = 960 MHz / 2   = 480 MHz
+ *   PLL1Q = PLL1_VCO/4  = 960 MHz / 4   = 240 MHz
+ *   PLL1R = PLL1_VCO/8  = 960 MHz / 8   = 120 MHz
 */
 #define STM32_PLLCFG_PLL1CFG    (RCC_PLLCFGR_PLL1VCOSEL_WIDE|RCC_PLLCFGR_PLL1RGE_4_8_MHZ|RCC_PLLCFGR_DIVP1EN|RCC_PLLCFGR_DIVQ1EN|RCC_PLLCFGR_DIVR1EN)
-#define STM32_PLLCFG_PLL1M       RCC_PLLCKSELR_DIVM1(3)
-#define STM32_PLLCFG_PLL1N       RCC_PLL1DIVR_N1(100)
+#define STM32_PLLCFG_PLL1M       RCC_PLLCKSELR_DIVM1(2)
+#define STM32_PLLCFG_PLL1N       RCC_PLL1DIVR_N1(80)
 #define STM32_PLLCFG_PLL1P       RCC_PLL1DIVR_P1(2)
-#define STM32_PLLCFG_PLL1Q       RCC_PLL1DIVR_Q1(8)
-#define STM32_PLLCFG_PLL1R       RCC_PLL1DIVR_R1(2)
+#define STM32_PLLCFG_PLL1Q       RCC_PLL1DIVR_Q1(4)
+#define STM32_PLLCFG_PLL1R       RCC_PLL1DIVR_R1(8)

-#define STM32_VCO1_FREQUENCY     ((STM32_HSE_FREQUENCY / 3) * 100)
+#define STM32_VCO1_FREQUENCY     ((STM32_HSE_FREQUENCY / 2) * 80)
 #define STM32_PLL1P_FREQUENCY    (STM32_VCO1_FREQUENCY / 2)
-#define STM32_PLL1Q_FREQUENCY    (STM32_VCO1_FREQUENCY / 8)
-#define STM32_PLL1R_FREQUENCY    (STM32_VCO1_FREQUENCY / 2)
+#define STM32_PLL1Q_FREQUENCY    (STM32_VCO1_FREQUENCY / 4)
+#define STM32_PLL1R_FREQUENCY    (STM32_VCO1_FREQUENCY / 8)

 /* PLL2 */
 #define STM32_PLLCFG_PLL2CFG     (RCC_PLLCFGR_PLL2VCOSEL_WIDE|RCC_PLLCFGR_PLL2RGE_4_8_MHZ|RCC_PLLCFGR_DIVP2EN|RCC_PLLCFGR_DIVQ2EN|RCC_PLLCFGR_DIVR2EN)
-#define STM32_PLLCFG_PLL2M       RCC_PLLCKSELR_DIVM2(2)
-#define STM32_PLLCFG_PLL2N       RCC_PLL2DIVR_N2(30)
-#define STM32_PLLCFG_PLL2P       RCC_PLL2DIVR_P2(4)
-#define STM32_PLLCFG_PLL2Q       RCC_PLL2DIVR_Q2(5)
-#define STM32_PLLCFG_PLL2R       RCC_PLL2DIVR_R2(1)
+#define STM32_PLLCFG_PLL2M       RCC_PLLCKSELR_DIVM2(4)
+#define STM32_PLLCFG_PLL2N       RCC_PLL2DIVR_N2(32)
+#define STM32_PLLCFG_PLL2P       RCC_PLL2DIVR_P2(2)
+#define STM32_PLLCFG_PLL2Q       RCC_PLL2DIVR_Q2(2)
+#define STM32_PLLCFG_PLL2R       RCC_PLL2DIVR_R2(2)

-#define STM32_VCO2_FREQUENCY     ((STM32_HSE_FREQUENCY / 2) * 30)
-#define STM32_PLL2P_FREQUENCY    (STM32_VCO2_FREQUENCY / 4)
-#define STM32_PLL2Q_FREQUENCY    (STM32_VCO2_FREQUENCY / 5)
-#define STM32_PLL2R_FREQUENCY    (STM32_VCO2_FREQUENCY / 1)
+#define STM32_VCO2_FREQUENCY     ((STM32_HSE_FREQUENCY / 4) * 32)
+#define STM32_PLL2P_FREQUENCY    (STM32_VCO2_FREQUENCY / 2)
+#define STM32_PLL2Q_FREQUENCY    (STM32_VCO2_FREQUENCY / 2)
+#define STM32_PLL2R_FREQUENCY    (STM32_VCO2_FREQUENCY / 2)

 /* PLL3 */
 #define STM32_PLLCFG_PLL3CFG    (RCC_PLLCFGR_PLL3VCOSEL_WIDE|RCC_PLLCFGR_PLL3RGE_4_8_MHZ|RCC_PLLCFGR_DIVQ3EN)
-#define STM32_PLLCFG_PLL3M      RCC_PLLCKSELR_DIVM3(6)
-#define STM32_PLLCFG_PLL3N      RCC_PLL3DIVR_N3(72)
-#define STM32_PLLCFG_PLL3P      RCC_PLL3DIVR_P3(3)
-#define STM32_PLLCFG_PLL3Q      RCC_PLL3DIVR_Q3(6)
-#define STM32_PLLCFG_PLL3R      RCC_PLL3DIVR_R3(9)
+#define STM32_PLLCFG_PLL3M      RCC_PLLCKSELR_DIVM3(4)
+#define STM32_PLLCFG_PLL3N      RCC_PLL3DIVR_N3(32)
+#define STM32_PLLCFG_PLL3P      RCC_PLL3DIVR_P3(2)
+#define STM32_PLLCFG_PLL3Q      RCC_PLL3DIVR_Q3(4)
+#define STM32_PLLCFG_PLL3R      RCC_PLL3DIVR_R3(2)

-#define STM32_VCO3_FREQUENCY    ((STM32_HSE_FREQUENCY / 6) * 72)
-#define STM32_PLL3P_FREQUENCY   (STM32_VCO3_FREQUENCY / 3)
-#define STM32_PLL3Q_FREQUENCY   (STM32_VCO3_FREQUENCY / 6)
-#define STM32_PLL3R_FREQUENCY   (STM32_VCO3_FREQUENCY / 9)
+#define STM32_VCO3_FREQUENCY    ((STM32_HSE_FREQUENCY / 4) * 32)
+#define STM32_PLL3P_FREQUENCY   (STM32_VCO3_FREQUENCY / 2)
+#define STM32_PLL3Q_FREQUENCY   (STM32_VCO3_FREQUENCY / 4)
+#define STM32_PLL3R_FREQUENCY   (STM32_VCO3_FREQUENCY / 2)

-/* SYSCLK = PLL1P = 400MHz
- * CPUCLK = SYSCLK / 1 = 400 MHz
+/* SYSCLK = PLL1P = 480MHz
+ * CPUCLK = SYSCLK / 1 = 480 MHz
 */
 #define STM32_RCC_D1CFGR_D1CPRE  (RCC_D1CFGR_D1CPRE_SYSCLK)
 #define STM32_SYSCLK_FREQUENCY   (STM32_PLL1P_FREQUENCY)
@@ -138,14 +140,14 @@
 /* Configure Clock Assignments */

 /* AHB clock (HCLK) is SYSCLK/2 (240 MHz max)
- * HCLK1 = HCLK2 = HCLK3 = HCLK4 = 200
+ * HCLK1 = HCLK2 = HCLK3 = HCLK4 = 240
 */
 #define STM32_RCC_D1CFGR_HPRE   RCC_D1CFGR_HPRE_SYSCLKd2        /* HCLK  = SYSCLK / 2 */
 #define STM32_ACLK_FREQUENCY    (STM32_CPUCLK_FREQUENCY / 2)    /* ACLK in D1, HCLK3 in D1 */
 #define STM32_HCLK_FREQUENCY    (STM32_CPUCLK_FREQUENCY / 2)    /* HCLK in D2, HCLK4 in D3 */
 #define STM32_BOARD_HCLK        STM32_HCLK_FREQUENCY            /* same as above, to satisfy compiler */

-/* APB1 clock (PCLK1) is HCLK/4 (120 MHz) */
+/* APB1 clock (PCLK1) is HCLK/2 (120 MHz) */
 #define STM32_RCC_D2CFGR_D2PPRE1  RCC_D2CFGR_D2PPRE1_HCLKd2       /* PCLK1 = HCLK / 2 */
 #define STM32_PCLK1_FREQUENCY     (STM32_HCLK_FREQUENCY/2)

@@ -184,29 +186,36 @@
 /* Kernel Clock Configuration
 *  Note: look at Table 54 in ST Manual
 */
-#define STM32_RCC_D1CCIPR_SDMMCSEL   RCC_D1CCIPR_SDMMC_PLL1
-
 #define STM32_RCC_D2CCIP2R_I2C123SRC RCC_D2CCIP2R_I2C123SEL_HSI  /* I2C123 clock source */
 #define STM32_RCC_D2CCIP1R_SPI123SRC RCC_D2CCIP1R_SPI123SEL_PLL2 /* SPI123 clock source */
 #define STM32_RCC_D2CCIP1R_SPI45SRC  RCC_D2CCIP1R_SPI45SEL_PLL2  /* SPI45 clock source */
 #define STM32_RCC_D3CCIPR_SPI6SRC    RCC_D3CCIPR_SPI6SEL_PLL2    /* SPI6 clock source */
 #define STM32_RCC_D2CCIP2R_USBSRC    RCC_D2CCIP2R_USBSEL_PLL3    /* USB 1 and 2 clock source */
+#define STM32_RCC_D3CCIPR_ADCSEL     RCC_D3CCIPR_ADCSEL_PLL2     /* ADC 1 2 3 clock source */
 #define STM32_RCC_D2CCIP1R_FDCANSEL  RCC_D2CCIP1R_FDCANSEL_HSE   /* FDCAN 1 2 clock source */

-#define STM32_RCC_D3CCIPR_ADCSEL     RCC_D3CCIPR_ADCSEL_PLL2     /* ADC 1 2 3 clock source */
+#define STM32_FDCANCLK               STM32_HSE_FREQUENCY

 /* FLASH wait states */
 #define BOARD_FLASH_WAITSTATES 2

 /* SDMMC definitions ********************************************************/
 /* Init 400kHz, freq = PLL1Q/(2*div)  div =  PLL1Q/(2*freq) */
-#define STM32_SDMMC_INIT_CLKDIV     (125 << STM32_SDMMC_CLKCR_CLKDIV_SHIFT)
+#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 = 100 / (2*25)
+/* 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
 */
-#define STM32_SDMMC_MMCXFR_CLKDIV   (2 << STM32_SDMMC_CLKCR_CLKDIV_SHIFT)
-#define STM32_SDMMC_SDXFR_CLKDIV    (2 << STM32_SDMMC_CLKCR_CLKDIV_SHIFT)
+#if defined(CONFIG_STM32H7_SDMMC_XDMA) || defined(CONFIG_STM32H7_SDMMC_IDMA)
+#  define STM32_SDMMC_MMCXFR_CLKDIV   (5 << 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)
+#else
+#  define STM32_SDMMC_SDXFR_CLKDIV    (100 << STM32_SDMMC_CLKCR_CLKDIV_SHIFT)
+#endif

 #define STM32_SDMMC_CLKCR_EDGE      STM32_SDMMC_CLKCR_NEGEDGE

@@ -60,6 +60,7 @@
 #define STM32_HSI_FREQUENCY     16000000ul
 #define STM32_LSI_FREQUENCY     32000
 #define STM32_HSE_FREQUENCY     STM32_BOARD_XTAL
+#define STM32_LSE_FREQUENCY     0

 /* Main PLL Configuration.
 *
@@ -81,55 +82,56 @@
 *   PLLP2,3 = {2, 3, 4, ..., 128}
 *   CPUCLK <= 480 MHz
 */
+#define STM32_PLLCFG_PLLSRC      RCC_PLLCKSELR_PLLSRC_HSE

 /* PLL1, wide 4 - 8 MHz input, enable DIVP, DIVQ, DIVR
 *
- *   PLL1_VCO = (24,000,000 / 3) * 100 = 800 MHz
+ *   PLL1_VCO = (24,000,000 / 2) * 80 = 960 MHz
 *
- *   PLL1P = PLL1_VCO/2  = 800 MHz / 2   = 400 MHz
- *   PLL1Q = PLL1_VCO/8  = 800 MHz / 8   = 100 MHz
- *   PLL1R = PLL1_VCO/2  = 800 MHz / 2   = 400 MHz
+ *   PLL1P = PLL1_VCO/2  = 960 MHz / 2   = 480 MHz
+ *   PLL1Q = PLL1_VCO/4  = 960 MHz / 4   = 240 MHz
+ *   PLL1R = PLL1_VCO/8  = 960 MHz / 8   = 120 MHz
 */
 #define STM32_PLLCFG_PLL1CFG    (RCC_PLLCFGR_PLL1VCOSEL_WIDE|RCC_PLLCFGR_PLL1RGE_4_8_MHZ|RCC_PLLCFGR_DIVP1EN|RCC_PLLCFGR_DIVQ1EN|RCC_PLLCFGR_DIVR1EN)
-#define STM32_PLLCFG_PLL1M       RCC_PLLCKSELR_DIVM1(3)
-#define STM32_PLLCFG_PLL1N       RCC_PLL1DIVR_N1(100)
+#define STM32_PLLCFG_PLL1M       RCC_PLLCKSELR_DIVM1(2)
+#define STM32_PLLCFG_PLL1N       RCC_PLL1DIVR_N1(80)
 #define STM32_PLLCFG_PLL1P       RCC_PLL1DIVR_P1(2)
-#define STM32_PLLCFG_PLL1Q       RCC_PLL1DIVR_Q1(8)
-#define STM32_PLLCFG_PLL1R       RCC_PLL1DIVR_R1(2)
+#define STM32_PLLCFG_PLL1Q       RCC_PLL1DIVR_Q1(4)
+#define STM32_PLLCFG_PLL1R       RCC_PLL1DIVR_R1(8)

-#define STM32_VCO1_FREQUENCY     ((STM32_HSE_FREQUENCY / 3) * 100)
+#define STM32_VCO1_FREQUENCY     ((STM32_HSE_FREQUENCY / 2) * 80)
 #define STM32_PLL1P_FREQUENCY    (STM32_VCO1_FREQUENCY / 2)
-#define STM32_PLL1Q_FREQUENCY    (STM32_VCO1_FREQUENCY / 8)
-#define STM32_PLL1R_FREQUENCY    (STM32_VCO1_FREQUENCY / 2)
+#define STM32_PLL1Q_FREQUENCY    (STM32_VCO1_FREQUENCY / 4)
+#define STM32_PLL1R_FREQUENCY    (STM32_VCO1_FREQUENCY / 8)

 /* PLL2 */
 #define STM32_PLLCFG_PLL2CFG     (RCC_PLLCFGR_PLL2VCOSEL_WIDE|RCC_PLLCFGR_PLL2RGE_4_8_MHZ|RCC_PLLCFGR_DIVP2EN|RCC_PLLCFGR_DIVQ2EN|RCC_PLLCFGR_DIVR2EN)
-#define STM32_PLLCFG_PLL2M       RCC_PLLCKSELR_DIVM2(2)
-#define STM32_PLLCFG_PLL2N       RCC_PLL2DIVR_N2(30)
-#define STM32_PLLCFG_PLL2P       RCC_PLL2DIVR_P2(4)
-#define STM32_PLLCFG_PLL2Q       RCC_PLL2DIVR_Q2(5)
-#define STM32_PLLCFG_PLL2R       RCC_PLL2DIVR_R2(1)
+#define STM32_PLLCFG_PLL2M       RCC_PLLCKSELR_DIVM2(4)
+#define STM32_PLLCFG_PLL2N       RCC_PLL2DIVR_N2(32)
+#define STM32_PLLCFG_PLL2P       RCC_PLL2DIVR_P2(2)
+#define STM32_PLLCFG_PLL2Q       RCC_PLL2DIVR_Q2(2)
+#define STM32_PLLCFG_PLL2R       RCC_PLL2DIVR_R2(2)

-#define STM32_VCO2_FREQUENCY     ((STM32_HSE_FREQUENCY / 2) * 30)
-#define STM32_PLL2P_FREQUENCY    (STM32_VCO2_FREQUENCY / 4)
-#define STM32_PLL2Q_FREQUENCY    (STM32_VCO2_FREQUENCY / 5)
-#define STM32_PLL2R_FREQUENCY    (STM32_VCO2_FREQUENCY / 1)
+#define STM32_VCO2_FREQUENCY     ((STM32_HSE_FREQUENCY / 4) * 32)
+#define STM32_PLL2P_FREQUENCY    (STM32_VCO2_FREQUENCY / 2)
+#define STM32_PLL2Q_FREQUENCY    (STM32_VCO2_FREQUENCY / 2)
+#define STM32_PLL2R_FREQUENCY    (STM32_VCO2_FREQUENCY / 2)

 /* PLL3 */
 #define STM32_PLLCFG_PLL3CFG    (RCC_PLLCFGR_PLL3VCOSEL_WIDE|RCC_PLLCFGR_PLL3RGE_4_8_MHZ|RCC_PLLCFGR_DIVQ3EN)
-#define STM32_PLLCFG_PLL3M      RCC_PLLCKSELR_DIVM3(6)
-#define STM32_PLLCFG_PLL3N      RCC_PLL3DIVR_N3(72)
-#define STM32_PLLCFG_PLL3P      RCC_PLL3DIVR_P3(3)
-#define STM32_PLLCFG_PLL3Q      RCC_PLL3DIVR_Q3(6)
-#define STM32_PLLCFG_PLL3R      RCC_PLL3DIVR_R3(9)
+#define STM32_PLLCFG_PLL3M      RCC_PLLCKSELR_DIVM3(4)
+#define STM32_PLLCFG_PLL3N      RCC_PLL3DIVR_N3(32)
+#define STM32_PLLCFG_PLL3P      RCC_PLL3DIVR_P3(2)
+#define STM32_PLLCFG_PLL3Q      RCC_PLL3DIVR_Q3(4)
+#define STM32_PLLCFG_PLL3R      RCC_PLL3DIVR_R3(2)

-#define STM32_VCO3_FREQUENCY    ((STM32_HSE_FREQUENCY / 6) * 72)
-#define STM32_PLL3P_FREQUENCY   (STM32_VCO3_FREQUENCY / 3)
-#define STM32_PLL3Q_FREQUENCY   (STM32_VCO3_FREQUENCY / 6)
-#define STM32_PLL3R_FREQUENCY   (STM32_VCO3_FREQUENCY / 9)
+#define STM32_VCO3_FREQUENCY    ((STM32_HSE_FREQUENCY / 4) * 32)
+#define STM32_PLL3P_FREQUENCY   (STM32_VCO3_FREQUENCY / 2)
+#define STM32_PLL3Q_FREQUENCY   (STM32_VCO3_FREQUENCY / 4)
+#define STM32_PLL3R_FREQUENCY   (STM32_VCO3_FREQUENCY / 2)

-/* SYSCLK = PLL1P = 400MHz
- * CPUCLK = SYSCLK / 1 = 400 MHz
+/* SYSCLK = PLL1P = 480MHz
+ * CPUCLK = SYSCLK / 1 = 480 MHz
 */
 #define STM32_RCC_D1CFGR_D1CPRE  (RCC_D1CFGR_D1CPRE_SYSCLK)
 #define STM32_SYSCLK_FREQUENCY   (STM32_PLL1P_FREQUENCY)
@@ -138,14 +140,14 @@
 /* Configure Clock Assignments */

 /* AHB clock (HCLK) is SYSCLK/2 (240 MHz max)
- * HCLK1 = HCLK2 = HCLK3 = HCLK4 = 200
+ * HCLK1 = HCLK2 = HCLK3 = HCLK4 = 240
 */
 #define STM32_RCC_D1CFGR_HPRE   RCC_D1CFGR_HPRE_SYSCLKd2        /* HCLK  = SYSCLK / 2 */
 #define STM32_ACLK_FREQUENCY    (STM32_CPUCLK_FREQUENCY / 2)    /* ACLK in D1, HCLK3 in D1 */
 #define STM32_HCLK_FREQUENCY    (STM32_CPUCLK_FREQUENCY / 2)    /* HCLK in D2, HCLK4 in D3 */
 #define STM32_BOARD_HCLK        STM32_HCLK_FREQUENCY            /* same as above, to satisfy compiler */

-/* APB1 clock (PCLK1) is HCLK/4 (120 MHz) */
+/* APB1 clock (PCLK1) is HCLK/2 (120 MHz) */
 #define STM32_RCC_D2CFGR_D2PPRE1  RCC_D2CFGR_D2PPRE1_HCLKd2       /* PCLK1 = HCLK / 2 */
 #define STM32_PCLK1_FREQUENCY     (STM32_HCLK_FREQUENCY/2)

@@ -184,29 +186,36 @@
 /* Kernel Clock Configuration
 *  Note: look at Table 54 in ST Manual
 */
-#define STM32_RCC_D1CCIPR_SDMMCSEL   RCC_D1CCIPR_SDMMC_PLL1
-
 #define STM32_RCC_D2CCIP2R_I2C123SRC RCC_D2CCIP2R_I2C123SEL_HSI  /* I2C123 clock source */
 #define STM32_RCC_D2CCIP1R_SPI123SRC RCC_D2CCIP1R_SPI123SEL_PLL2 /* SPI123 clock source */
 #define STM32_RCC_D2CCIP1R_SPI45SRC  RCC_D2CCIP1R_SPI45SEL_PLL2  /* SPI45 clock source */
 #define STM32_RCC_D3CCIPR_SPI6SRC    RCC_D3CCIPR_SPI6SEL_PLL2    /* SPI6 clock source */
 #define STM32_RCC_D2CCIP2R_USBSRC    RCC_D2CCIP2R_USBSEL_PLL3    /* USB 1 and 2 clock source */
+#define STM32_RCC_D3CCIPR_ADCSEL     RCC_D3CCIPR_ADCSEL_PLL2     /* ADC 1 2 3 clock source */
 #define STM32_RCC_D2CCIP1R_FDCANSEL  RCC_D2CCIP1R_FDCANSEL_HSE   /* FDCAN 1 2 clock source */

-#define STM32_RCC_D3CCIPR_ADCSEL     RCC_D3CCIPR_ADCSEL_PLL2     /* ADC 1 2 3 clock source */
+#define STM32_FDCANCLK               STM32_HSE_FREQUENCY

 /* FLASH wait states */
 #define BOARD_FLASH_WAITSTATES 2

 /* SDMMC definitions ********************************************************/
 /* Init 400kHz, freq = PLL1Q/(2*div)  div =  PLL1Q/(2*freq) */
-#define STM32_SDMMC_INIT_CLKDIV     (125 << STM32_SDMMC_CLKCR_CLKDIV_SHIFT)
+#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 = 100 / (2*25)
+/* 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
 */
-#define STM32_SDMMC_MMCXFR_CLKDIV   (2 << STM32_SDMMC_CLKCR_CLKDIV_SHIFT)
-#define STM32_SDMMC_SDXFR_CLKDIV    (2 << STM32_SDMMC_CLKCR_CLKDIV_SHIFT)
+#if defined(CONFIG_STM32H7_SDMMC_XDMA) || defined(CONFIG_STM32H7_SDMMC_IDMA)
+#  define STM32_SDMMC_MMCXFR_CLKDIV   (5 << 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)
+#else
+#  define STM32_SDMMC_SDXFR_CLKDIV    (100 << STM32_SDMMC_CLKCR_CLKDIV_SHIFT)
+#endif

 #define STM32_SDMMC_CLKCR_EDGE      STM32_SDMMC_CLKCR_NEGEDGE

@@ -0,0 +1 @@
+CONFIG_PLATFORM_ROS2=y
@@ -0,0 +1,36 @@
+############################################################################
+#
+#   Copyright (c) 2021 PX4 Development Team. All rights reserved.
+#
+# Redistribution and use in source and binary forms, with or without
+# modification, are permitted provided that the following conditions
+# are met:
+#
+# 1. Redistributions of source code must retain the above copyright
+#    notice, this list of conditions and the following disclaimer.
+# 2. Redistributions in binary form must reproduce the above copyright
+#    notice, this list of conditions and the following disclaimer in
+#    the documentation and/or other materials provided with the
+#    distribution.
+# 3. Neither the name PX4 nor the names of its contributors may be
+#    used to endorse or promote products derived from this software
+#    without specific prior written permission.
+#
+# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+# "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+# LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+# FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+# COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+# INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+# BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
+# OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
+# AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+# LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+# ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+# POSSIBILITY OF SUCH DAMAGE.
+#
+############################################################################
+
+add_library(drivers_board
+	init.cpp
+)
@@ -1,7 +1,6 @@
 /****************************************************************************
 *
- *   Copyright (C) 2012 PX4 Development Team. All rights reserved.
- *   Author: @author Lorenz Meier <lm@inf.ethz.ch>
+ *   Copyright (c) 2021 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
@@ -33,25 +32,19 @@
 ****************************************************************************/

 /**
- * @file conversions.c
- * Implementation of commonly used conversions.
+ * @file board_config.h
+ *
+ * SITL internal definitions
 */

-#include <px4_platform_common/px4_config.h>
-#include <float.h>
+#pragma once

-#include "conversions.h"
+#define BOARD_OVERRIDE_UUID "SIMULATIONID0000" // must be of length 16
+#define PX4_SOC_ARCH_ID     PX4_SOC_ARCH_ID_SITL

-int16_t
-int16_t_from_bytes(uint8_t bytes[])
-{
-	union {
-		uint8_t    b[2];
-		int16_t    w;
-	} u;
+#define BOARD_HAS_POWER_CONTROL 1

-	u.b[1] = bytes[0];
-	u.b[0] = bytes[1];
+#define BOARD_NUMBER_BRICKS     0

-	return u.w;
-}
+#include <system_config.h>
+#include <px4_platform_common/board_common.h>
@@ -57,4 +57,7 @@ else()
 endif()

 include(CPack)
-include(bloaty)
+
+if(${PX4_PLATFORM} MATCHES "nuttx")
+	include(bloaty)
+endif()
@@ -71,11 +71,11 @@ function(px4_add_common_flags)
 		-Werror

 		-Warray-bounds
-		-Wcast-align
+		#-Wcast-align # TODO
 		-Wdisabled-optimization
 		-Wdouble-promotion
 		-Wfatal-errors
-		-Wfloat-equal
+		#-Wfloat-equal
 		-Wformat-security
 		-Winit-self
 		-Wlogical-op
@@ -156,9 +156,6 @@ function(px4_add_common_flags)
 	# CXX only flags
 	set(cxx_flags)
 	list(APPEND cxx_flags
-		-fno-exceptions
-		-fno-threadsafe-statics
-
 		-Wreorder

 		# disabled warnings
@@ -175,16 +172,19 @@ function(px4_add_common_flags)
 		add_compile_options($<$<COMPILE_LANGUAGE:CXX>:${flag}>)
 	endforeach()

+	if(NOT (${PX4_PLATFORM} MATCHES "ros2")) # TODO: fix
+		include_directories(
+			${PX4_SOURCE_DIR}/platforms/${PX4_PLATFORM}/src/px4/${PX4_CHIP_MANUFACTURER}/${PX4_CHIP}/include
+			${PX4_SOURCE_DIR}/platforms/${PX4_PLATFORM}/src/px4/common/include
+			${PX4_SOURCE_DIR}/platforms/common
+			${PX4_SOURCE_DIR}/platforms/common/include
+		)
+	endif()

 	include_directories(
 		${PX4_BINARY_DIR}
 		${PX4_BINARY_DIR}/src/lib

-		${PX4_SOURCE_DIR}/platforms/${PX4_PLATFORM}/src/px4/${PX4_CHIP_MANUFACTURER}/${PX4_CHIP}/include
-		${PX4_SOURCE_DIR}/platforms/${PX4_PLATFORM}/src/px4/common/include
-		${PX4_SOURCE_DIR}/platforms/common
-		${PX4_SOURCE_DIR}/platforms/common/include
-
 		${PX4_SOURCE_DIR}/src
 		${PX4_SOURCE_DIR}/src/include
 		${PX4_SOURCE_DIR}/src/lib
@@ -39,7 +39,10 @@ include(px4_list_make_absolute)
 #	Like add_library but with PX4 platform dependencies
 #
 function(px4_add_library target)
-	add_library(${target} EXCLUDE_FROM_ALL ${ARGN})
+	add_library(${target} STATIC
+		EXCLUDE_FROM_ALL
+		${ARGN}
+	)

 	target_compile_definitions(${target} PRIVATE MODULE_NAME="${target}")

@@ -48,6 +48,7 @@ include(px4_list_make_absolute)
 #			[ DEPENDS <string> ]
 #			[ SRCS <list> ]
 #			[ MODULE_CONFIG <list> ]
+#			[ PUBLICATIONS <list> ]
 #			[ EXTERNAL ]
 #			[ DYNAMIC ]
 #			)
@@ -64,6 +65,8 @@ include(px4_list_make_absolute)
 #		MODULE_CONFIG		: yaml config file(s)
 #		INCLUDES		: include directories
 #		DEPENDS			: targets which this module depends on
+#		PUBLICATIONS		: List of publications from this module
+#               SUBSCRIPTIONS		: List of subsriptions used by this module
 #		EXTERNAL		: flag to indicate that this module is out-of-tree
 #		DYNAMIC			: don't compile into the px4 binary, but build a separate dynamically loadable module (posix)
 #		UNITY_BUILD		: merge all source files and build this module as a single compilation unit
@@ -86,7 +89,7 @@ function(px4_add_module)
 	px4_parse_function_args(
 		NAME px4_add_module
 		ONE_VALUE MODULE MAIN STACK_MAIN STACK_MAX PRIORITY
-		MULTI_VALUE COMPILE_FLAGS LINK_FLAGS SRCS INCLUDES DEPENDS MODULE_CONFIG
+		MULTI_VALUE COMPILE_FLAGS LINK_FLAGS SRCS INCLUDES DEPENDS MODULE_CONFIG PUBLICATIONS SUBSCRIPTIONS
 		OPTIONS EXTERNAL DYNAMIC UNITY_BUILD
 		REQUIRED MODULE MAIN
 		ARGN ${ARGN})
@@ -165,7 +168,7 @@ function(px4_add_module)

 	# set defaults if not set
 	set(MAIN_DEFAULT MAIN-NOTFOUND)
-	set(STACK_MAIN_DEFAULT 2048)
+	set(STACK_MAIN_DEFAULT 4096)
 	set(PRIORITY_DEFAULT SCHED_PRIORITY_DEFAULT)

 	foreach(property MAIN STACK_MAIN PRIORITY)
@@ -214,7 +217,21 @@ function(px4_add_module)
 		endforeach()
 	endif()

-	foreach (prop LINK_FLAGS STACK_MAIN MAIN PRIORITY)
+	if(PUBLICATIONS)
+		foreach(publication ${PUBLICATIONS})
+			#message(STATUS "${MODULE} PUBLICATION: ${publication}")
+			set_property(GLOBAL APPEND PROPERTY PX4_PUBLICATIONS ${publication})
+		endforeach()
+	endif()
+
+	if(SUBSCRIPTIONS)
+		foreach(subscription ${SUBSCRIPTIONS})
+			#message(STATUS "${MODULE} SUBSCRIPTION: ${subscription}")
+			set_property(GLOBAL APPEND PROPERTY PX4_SUBSCRIPTIONS ${subscription})
+		endforeach()
+	endif()
+
+	foreach(prop LINK_FLAGS STACK_MAIN MAIN PRIORITY)
 		if (${prop})
 			set_target_properties(${MODULE} PROPERTIES ${prop} ${${prop}})
 		endif()
@@ -63,9 +63,9 @@ if(NOT PX4_CONFIG_FILE)
 			)
 				set(PX4_CONFIG_FILE "${PX4_SOURCE_DIR}/boards/${filename}" CACHE FILEPATH "path to PX4 CONFIG file" FORCE)
 				set(PX4_BOARD_DIR "${PX4_SOURCE_DIR}/boards/${vendor}/${model}" CACHE STRING "PX4 board directory" FORCE)
-                set(MODEL "${model}" CACHE STRING "PX4 board model" FORCE)
-                set(VENDOR "${vendor}" CACHE STRING "PX4 board vendor" FORCE)
-                set(LABEL "${label}" CACHE STRING "PX4 board vendor" FORCE)
+				set(MODEL "${model}" CACHE STRING "PX4 board model" FORCE)
+				set(VENDOR "${vendor}" CACHE STRING "PX4 board vendor" FORCE)
+				set(LABEL "${label}" CACHE STRING "PX4 board vendor" FORCE)
 				break()
 			endif()

@@ -76,9 +76,9 @@ if(NOT PX4_CONFIG_FILE)
 			)
 				set(PX4_CONFIG_FILE "${PX4_SOURCE_DIR}/boards/${filename}" CACHE FILEPATH "path to PX4 CONFIG file" FORCE)
 				set(PX4_BOARD_DIR "${PX4_SOURCE_DIR}/boards/${vendor}/${model}" CACHE STRING "PX4 board directory" FORCE)
-                set(MODEL "${model}" CACHE STRING "PX4 board model" FORCE)
-                set(VENDOR "${vendor}" CACHE STRING "PX4 board vendor" FORCE)
-                set(LABEL "${label}" CACHE STRING "PX4 board vendor" FORCE)
+				set(MODEL "${model}" CACHE STRING "PX4 board model" FORCE)
+				set(VENDOR "${vendor}" CACHE STRING "PX4 board vendor" FORCE)
+				set(LABEL "${label}" CACHE STRING "PX4 board vendor" FORCE)
 				break()
 			endif()
 		endif()
@@ -1,114 +0,0 @@
-<?xml version="1.0"?>
-<launch>
-    <!-- MAVROS posix SITL environment launch script -->
-    <!-- launches Gazebo environment and 2x: MAVROS, PX4 SITL, and spawns vehicle -->
-    <!-- vehicle model and world -->
-    <arg name="est" default="ekf2"/>
-    <arg name="vehicle" default="iris"/>
-    <arg name="world" default="$(find mavlink_sitl_gazebo)/worlds/empty.world"/>
-    <!-- gazebo configs -->
-    <arg name="gui" default="true"/>
-    <arg name="debug" default="false"/>
-    <arg name="verbose" default="false"/>
-    <arg name="paused" default="false"/>
-    <!-- Gazebo sim -->
-    <include file="$(find gazebo_ros)/launch/empty_world.launch">
-        <arg name="gui" value="$(arg gui)"/>
-        <arg name="world_name" value="$(arg world)"/>
-        <arg name="debug" value="$(arg debug)"/>
-        <arg name="verbose" value="$(arg verbose)"/>
-        <arg name="paused" value="$(arg paused)"/>
-    </include>
-    <!-- UAV0 -->
-    <group ns="uav0">
-        <!-- MAVROS and vehicle configs -->
-        <arg name="ID" value="0"/>
-        <arg name="fcu_url" default="udp://:14540@localhost:14580"/>
-        <!-- PX4 SITL and vehicle spawn -->
-        <include file="$(find px4)/launch/single_vehicle_spawn.launch">
-            <arg name="x" value="0"/>
-            <arg name="y" value="0"/>
-            <arg name="z" value="0"/>
-            <arg name="R" value="0"/>
-            <arg name="P" value="0"/>
-            <arg name="Y" value="0"/>
-            <arg name="vehicle" value="$(arg vehicle)"/>
-            <arg name="mavlink_udp_port" value="14560"/>
-            <arg name="mavlink_tcp_port" value="4560"/>
-            <arg name="ID" value="$(arg ID)"/>
-            <arg name="gst_udp_port" value="$(eval 5600 + arg('ID'))"/>
-            <arg name="video_uri" value="$(eval 5600 + arg('ID'))"/>
-            <arg name="mavlink_cam_udp_port" value="$(eval 14530 + arg('ID'))"/>
-        </include>
-        <!-- MAVROS -->
-        <include file="$(find mavros)/launch/px4.launch">
-            <arg name="fcu_url" value="$(arg fcu_url)"/>
-            <arg name="gcs_url" value=""/>
-            <arg name="tgt_system" value="$(eval 1 + arg('ID'))"/>
-            <arg name="tgt_component" value="1"/>
-        </include>
-    </group>
-    <!-- UAV1 -->
-    <group ns="uav1">
-        <!-- MAVROS and vehicle configs -->
-        <arg name="ID" value="1"/>
-        <arg name="fcu_url" default="udp://:14541@localhost:14581"/>
-        <!-- PX4 SITL and vehicle spawn -->
-        <include file="$(find px4)/launch/single_vehicle_spawn.launch">
-            <arg name="x" value="1"/>
-            <arg name="y" value="0"/>
-            <arg name="z" value="0"/>
-            <arg name="R" value="0"/>
-            <arg name="P" value="0"/>
-            <arg name="Y" value="0"/>
-            <arg name="vehicle" value="$(arg vehicle)"/>
-            <arg name="mavlink_udp_port" value="14561"/>
-            <arg name="mavlink_tcp_port" value="4561"/>
-            <arg name="ID" value="$(arg ID)"/>
-            <arg name="gst_udp_port" value="$(eval 5600 + arg('ID'))"/>
-            <arg name="video_uri" value="$(eval 5600 + arg('ID'))"/>
-            <arg name="mavlink_cam_udp_port" value="$(eval 14530 + arg('ID'))"/>
-        </include>
-        <!-- MAVROS -->
-        <include file="$(find mavros)/launch/px4.launch">
-            <arg name="fcu_url" value="$(arg fcu_url)"/>
-            <arg name="gcs_url" value=""/>
-            <arg name="tgt_system" value="$(eval 1 + arg('ID'))"/>
-            <arg name="tgt_component" value="1"/>
-        </include>
-    </group>
-    <!-- UAV2 -->
-    <group ns="uav2">
-        <!-- MAVROS and vehicle configs -->
-        <arg name="ID" value="2"/>
-        <arg name="fcu_url" default="udp://:14542@localhost:14582"/>
-        <!-- PX4 SITL and vehicle spawn -->
-        <include file="$(find px4)/launch/single_vehicle_spawn.launch">
-            <arg name="x" value="0"/>
-            <arg name="y" value="1"/>
-            <arg name="z" value="0"/>
-            <arg name="R" value="0"/>
-            <arg name="P" value="0"/>
-            <arg name="Y" value="0"/>
-            <arg name="vehicle" value="$(arg vehicle)"/>
-            <arg name="mavlink_udp_port" value="14562"/>
-            <arg name="mavlink_tcp_port" value="4562"/>
-            <arg name="ID" value="$(arg ID)"/>
-            <arg name="gst_udp_port" value="$(eval 5600 + arg('ID'))"/>
-            <arg name="video_uri" value="$(eval 5600 + arg('ID'))"/>
-            <arg name="mavlink_cam_udp_port" value="$(eval 14530 + arg('ID'))"/>
-        </include>
-        <!-- MAVROS -->
-        <include file="$(find mavros)/launch/px4.launch">
-            <arg name="fcu_url" value="$(arg fcu_url)"/>
-            <arg name="gcs_url" value=""/>
-            <arg name="tgt_system" value="$(eval 1 + arg('ID'))"/>
-            <arg name="tgt_component" value="1"/>
-        </include>
-    </group>
-</launch>
-<!-- to add more UAVs (up to 10):
-Increase the id
-Change the name space
-Set the FCU to default="udp://:14540+id@localhost:14550+id"
-Set the malink_udp_port to 14560+id) -->
@@ -1,105 +0,0 @@
-<?xml version="1.0"?>
-<launch>
-    <!-- MAVROS posix SITL environment launch script -->
-    <!-- launches Gazebo environment and 2x: MAVROS, PX4 SITL, and spawns vehicle -->
-    <!-- vehicle model and world -->
-    <arg name="est" default="ekf2"/>
-    <arg name="vehicle" default="plane"/>
-    <arg name="world" default="$(find mavlink_sitl_gazebo)/worlds/empty.world"/>
-    <!-- gazebo configs -->
-    <arg name="gui" default="true"/>
-    <arg name="debug" default="false"/>
-    <arg name="verbose" default="false"/>
-    <arg name="paused" default="false"/>
-    <!-- Gazebo sim -->
-    <include file="$(find gazebo_ros)/launch/empty_world.launch">
-        <arg name="gui" value="$(arg gui)"/>
-        <arg name="world_name" value="$(arg world)"/>
-        <arg name="debug" value="$(arg debug)"/>
-        <arg name="verbose" value="$(arg verbose)"/>
-        <arg name="paused" value="$(arg paused)"/>
-    </include>
-    <!-- UAV0 -->
-    <group ns="uav0">
-        <!-- MAVROS and vehicle configs -->
-        <arg name="ID" value="0"/>
-        <arg name="fcu_url" default="udp://:14540@localhost:14580"/>
-        <!-- PX4 SITL and vehicle spawn -->
-        <include file="$(find px4)/launch/single_vehicle_spawn_sdf.launch">
-            <arg name="x" value="0"/>
-            <arg name="y" value="0"/>
-            <arg name="z" value="0"/>
-            <arg name="R" value="0"/>
-            <arg name="P" value="0"/>
-            <arg name="Y" value="0"/>
-            <arg name="vehicle" value="$(arg vehicle)"/>
-            <arg name="mavlink_udp_port" value="14560"/>
-            <arg name="mavlink_tcp_port" value="4560"/>
-            <arg name="ID" value="$(arg ID)"/>
-        </include>
-        <!-- MAVROS -->
-        <include file="$(find mavros)/launch/px4.launch">
-            <arg name="fcu_url" value="$(arg fcu_url)"/>
-            <arg name="gcs_url" value=""/>
-            <arg name="tgt_system" value="$(eval 1 + arg('ID'))"/>
-            <arg name="tgt_component" value="1"/>
-        </include>
-    </group>
-    <!-- UAV1 -->
-    <group ns="uav1">
-        <!-- MAVROS and vehicle configs -->
-        <arg name="ID" value="1"/>
-        <arg name="fcu_url" default="udp://:14541@localhost:14581"/>
-        <!-- PX4 SITL and vehicle spawn -->
-        <include file="$(find px4)/launch/single_vehicle_spawn_sdf.launch">
-            <arg name="x" value="1"/>
-            <arg name="y" value="0"/>
-            <arg name="z" value="0"/>
-            <arg name="R" value="0"/>
-            <arg name="P" value="0"/>
-            <arg name="Y" value="0"/>
-            <arg name="vehicle" value="$(arg vehicle)"/>
-            <arg name="mavlink_udp_port" value="14561"/>
-            <arg name="mavlink_tcp_port" value="4561"/>
-            <arg name="ID" value="$(arg ID)"/>
-        </include>
-        <!-- MAVROS -->
-        <include file="$(find mavros)/launch/px4.launch">
-            <arg name="fcu_url" value="$(arg fcu_url)"/>
-            <arg name="gcs_url" value=""/>
-            <arg name="tgt_system" value="$(eval 1 + arg('ID'))"/>
-            <arg name="tgt_component" value="1"/>
-        </include>
-    </group>
-    <!-- UAV2 -->
-    <group ns="uav2">
-        <!-- MAVROS and vehicle configs -->
-        <arg name="ID" value="2"/>
-        <arg name="fcu_url" default="udp://:14542@localhost:14582"/>
-        <!-- PX4 SITL and vehicle spawn -->
-        <include file="$(find px4)/launch/single_vehicle_spawn_sdf.launch">
-            <arg name="x" value="0"/>
-            <arg name="y" value="1"/>
-            <arg name="z" value="0"/>
-            <arg name="R" value="0"/>
-            <arg name="P" value="0"/>
-            <arg name="Y" value="0"/>
-            <arg name="vehicle" value="$(arg vehicle)"/>
-            <arg name="mavlink_udp_port" value="14562"/>
-            <arg name="mavlink_tcp_port" value="4562"/>
-            <arg name="ID" value="$(arg ID)"/>
-        </include>
-        <!-- MAVROS -->
-        <include file="$(find mavros)/launch/px4.launch">
-            <arg name="fcu_url" value="$(arg fcu_url)"/>
-            <arg name="gcs_url" value=""/>
-            <arg name="tgt_system" value="$(eval 1 + arg('ID'))"/>
-            <arg name="tgt_component" value="1"/>
-        </include>
-    </group>
-</launch>
-<!-- to add more UAVs (up to 10):
-Increase the id
-Change the name space
-Set the FCU to default="udp://:14540+id@localhost:14550+id"
-Set the malink_udp_port to 14560+id) -->
@@ -1,13 +0,0 @@
-<launch>
-    <!-- Test mavros posix sitl publishes pose at 30 Hz -->
-    <include file="$(find px4)/launch/mavros_posix_sitl.launch">
-        <arg name="gui" value="false"/>
-        <arg name="interactive" value="false"/>
-        <arg name="verbose" value="true"/>
-    </include>
-    <param name="pub_test/topic" value="/mavros/local_position/pose"/>
-    <param name="pub_test/hz" value="30.0"/>
-    <param name="pub_test/hzerror" value="1.0"/>
-    <param name="pub_test/test_duration" value="10.0"/>
-    <test test-name="pub_test" pkg="rostest" type="hztest" name="pub_test"/>
-</launch>
@@ -1,18 +0,0 @@
-<?xml version="1.0"?>
-<launch>
-    <!-- Posix SITL PX4 launch script -->
-    <!-- Launches Only PX4 SITL. This can be used by external projects -->
-
-    <!-- PX4 config arguments -->
-    <arg name="est" default="ekf2"/>
-    <arg name="vehicle" default="iris"/>
-    <arg name="ID" default="0"/>
-    <arg name="interactive" default="true"/>
-
-    <env name="PX4_SIM_MODEL" value="$(arg vehicle)" />
-    <env name="PX4_ESTIMATOR" value="$(arg est)" />
-    <arg unless="$(arg interactive)" name="px4_command_arg1" value="-d"/>
-    <arg     if="$(arg interactive)" name="px4_command_arg1" value=""/>
-    <node name="sitl_$(arg ID)" pkg="px4" type="px4" output="screen" args="$(find px4)/build/px4_sitl_default/etc -s etc/init.d-posix/rcS -i $(arg ID) $(arg px4_command_arg1)">
-    </node>
-</launch>
@@ -1,36 +0,0 @@
-<?xml version="1.0"?>
-<launch>
-    <!-- Posix SITL environment launch script -->
-    <!-- launchs PX4 SITL and spawns vehicle -->
-    <!-- vehicle pose -->
-    <arg name="x" default="0"/>
-    <arg name="y" default="0"/>
-    <arg name="z" default="0"/>
-    <arg name="R" default="0"/>
-    <arg name="P" default="0"/>
-    <arg name="Y" default="0"/>
-    <!-- vehicle model and config -->
-    <arg name="est" default="ekf2"/>
-    <arg name="vehicle" default="iris"/>
-    <arg name="ID" default="1"/>
-    <env name="PX4_SIM_MODEL" value="$(arg vehicle)" />
-    <env name="PX4_ESTIMATOR" value="$(arg est)" />
-    <arg name="mavlink_udp_port" default="14560"/>
-    <arg name="mavlink_tcp_port" default="4560"/>
-    <arg name="gst_udp_port" default="5600"/>
-    <arg name="video_uri" default="5600"/>
-    <arg name="mavlink_cam_udp_port" default="14530"/>
-    <arg name="mavlink_id" value="$(eval 1 + arg('ID'))" />
-    <!-- PX4 configs -->
-    <arg name="interactive" default="true"/>
-    <!-- generate sdf vehicle model -->
-    <arg name="cmd" default="$(find mavlink_sitl_gazebo)/scripts/jinja_gen.py --stdout --mavlink_id=$(arg mavlink_id) --mavlink_udp_port=$(arg mavlink_udp_port) --mavlink_tcp_port=$(arg mavlink_tcp_port) --gst_udp_port=$(arg gst_udp_port) --video_uri=$(arg video_uri) --mavlink_cam_udp_port=$(arg mavlink_cam_udp_port) $(find mavlink_sitl_gazebo)/models/$(arg vehicle)/$(arg vehicle).sdf.jinja $(find mavlink_sitl_gazebo)"/>
-    <param command="$(arg cmd)" name="sdf_$(arg vehicle)$(arg ID)"/>
-    <!-- PX4 SITL -->
-    <arg unless="$(arg interactive)" name="px4_command_arg1" value=""/>
-    <arg     if="$(arg interactive)" name="px4_command_arg1" value="-d"/>
-    <node name="sitl_$(arg ID)" pkg="px4" type="px4" output="screen" args="$(find px4)/build/px4_sitl_default/etc -s etc/init.d-posix/rcS -i $(arg ID) -w sitl_$(arg vehicle)_$(arg ID) $(arg px4_command_arg1)">
-    </node>
-    <!-- spawn vehicle -->
-    <node name="$(anon vehicle_spawn)" pkg="gazebo_ros" type="spawn_model" output="screen" args="-sdf -param sdf_$(arg vehicle)$(arg ID) -model $(arg vehicle)$(arg ID) -x $(arg x) -y $(arg y) -z $(arg z) -R $(arg R) -P $(arg P) -Y $(arg Y)"/>
-</launch>
@@ -1,32 +0,0 @@
-<?xml version="1.0"?>
-<launch>
-    <!-- Posix SITL environment launch script -->
-    <!-- launchs PX4 SITL and spawns vehicle -->
-    <!-- vehicle pose -->
-    <arg name="x" default="0"/>
-    <arg name="y" default="0"/>
-    <arg name="z" default="0"/>
-    <arg name="R" default="0"/>
-    <arg name="P" default="0"/>
-    <arg name="Y" default="0"/>
-    <!-- vehcile model and config -->
-    <arg name="est" default="ekf2"/>
-    <arg name="vehicle" default="plane"/>
-    <arg name="ID" default="1"/>
-    <env name="PX4_SIM_MODEL" value="$(arg vehicle)" />
-    <env name="PX4_ESTIMATOR" value="$(arg est)" />
-    <arg name="mavlink_udp_port" default="14560"/>
-    <arg name="mavlink_tcp_port" default="4560"/>
-    <!-- PX4 configs -->
-    <arg name="interactive" default="true"/>
-    <!-- generate sdf vehicle model -->
-    <arg name="cmd" default="xmlstarlet ed -d '//plugin[@name=&quot;mavlink_interface&quot;]/mavlink_tcp_port' -s '//plugin[@name=&quot;mavlink_interface&quot;]' -t elem -n mavlink_tcp_port -v $(arg mavlink_tcp_port) $(find px4)/Tools/sitl_gazebo/models/$(arg vehicle)/$(arg vehicle).sdf"/>
-    <param command="$(arg cmd)" name="model_description"/>
-    <!-- PX4 SITL -->
-    <arg unless="$(arg interactive)" name="px4_command_arg1" value=""/>
-    <arg     if="$(arg interactive)" name="px4_command_arg1" value="-d"/>
-    <node name="sitl_$(arg ID)" pkg="px4" type="px4" output="screen" args="$(find px4)/build/px4_sitl_default/etc -s etc/init.d-posix/rcS -i $(arg ID) -w sitl_$(arg vehicle)_$(arg ID) $(arg px4_command_arg1)">
-    </node>
-    <!-- spawn vehicle -->
-    <node name="$(arg vehicle)_$(arg ID)_spawn" output="screen" pkg="gazebo_ros" type="spawn_model" args="-sdf -param model_description -model $(arg vehicle)_$(arg ID) -x $(arg x) -y $(arg y) -z $(arg z) -R $(arg R) -P $(arg P) -Y $(arg Y)"/>
-</launch>
@@ -20,6 +20,3 @@ uint8 GROUP_INDEX_PAYLOAD = 6

 uint64 timestamp_sample	    # the timestamp the data this control response is based on was sampled
 float32[8] control
-
-# TOPICS actuator_controls actuator_controls_0 actuator_controls_1 actuator_controls_2 actuator_controls_3
-# TOPICS actuator_controls_virtual_fw actuator_controls_virtual_mc
@@ -6,5 +6,3 @@ uint8 INDEX_YAW = 2
 uint8 INDEX_THROTTLE = 3

 float32[4] control_power
-
-# TOPICS actuator_controls_status actuator_controls_status_0 actuator_controls_status_1
@@ -1,6 +1,6 @@
 ############################################################################
 #
-#   Copyright (c) 2016 PX4 Development Team. All rights reserved.
+#   Copyright (c) 2016-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
@@ -37,212 +37,178 @@ cmake_policy(SET CMP0057 NEW)
 include(px4_list_make_absolute)

 set(msg_files
-	action_request.msg
-	actuator_armed.msg
-	actuator_controls.msg
-	actuator_controls_status.msg
-	actuator_motors.msg
-	actuator_outputs.msg
-	actuator_servos.msg
-	actuator_servos_trim.msg
-	actuator_test.msg
-	adc_report.msg
-	airspeed.msg
-	airspeed_validated.msg
-	airspeed_wind.msg
-	autotune_attitude_control_status.msg
-	battery_status.msg
-	camera_capture.msg
-	camera_status.msg
-	camera_trigger.msg
-	cellular_status.msg
-	collision_constraints.msg
-	collision_report.msg
-	commander_state.msg
-	control_allocator_status.msg
-	cpuload.msg
-	differential_pressure.msg
-	distance_sensor.msg
-	ekf2_timestamps.msg
-	ekf_gps_drift.msg
-	esc_report.msg
-	esc_status.msg
-	estimator_baro_bias.msg
-	estimator_event_flags.msg
-	estimator_innovations.msg
-	estimator_optical_flow_vel.msg
-	estimator_selector_status.msg
-	estimator_sensor_bias.msg
-	estimator_states.msg
-	estimator_status.msg
-	estimator_status_flags.msg
-	event.msg
-	follow_target.msg
-	failure_detector_status.msg
-	generator_status.msg
-	geofence_result.msg
-	gimbal_device_attitude_status.msg
-	gimbal_device_information.msg
-	gimbal_device_set_attitude.msg
-	gimbal_manager_information.msg
-	gimbal_manager_set_attitude.msg
-	gimbal_manager_set_manual_control.msg
-	gimbal_manager_status.msg
-	gps_dump.msg
-	gps_inject_data.msg
-	heater_status.msg
-	home_position.msg
-	hover_thrust_estimate.msg
-	internal_combustion_engine_status.msg
-	input_rc.msg
-	iridiumsbd_status.msg
-	irlock_report.msg
-	landing_gear.msg
-	landing_target_innovations.msg
-	landing_target_pose.msg
-	led_control.msg
-	log_message.msg
-	logger_status.msg
-	mag_worker_data.msg
-	magnetometer_bias_estimate.msg
-	manual_control_setpoint.msg
-	manual_control_switches.msg
-	mavlink_log.msg
-	mission.msg
-	mission_result.msg
-	mount_orientation.msg
-	navigator_mission_item.msg
-	npfg_status.msg
-	obstacle_distance.msg
-	offboard_control_mode.msg
-	onboard_computer_status.msg
-	optical_flow.msg
-	orbit_status.msg
-	parameter_update.msg
-	ping.msg
-	pps_capture.msg
-	position_controller_landing_status.msg
-	position_controller_status.msg
-	position_setpoint.msg
-	position_setpoint_triplet.msg
-	power_button_state.msg
-	power_monitor.msg
-	pwm_input.msg
-	px4io_status.msg
-	radio_status.msg
-	rate_ctrl_status.msg
-	rc_channels.msg
-	rc_parameter_map.msg
-	rpm.msg
-	rtl_time_estimate.msg
-	safety.msg
-	satellite_info.msg
-	sensor_accel.msg
-	sensor_accel_fifo.msg
-	sensor_baro.msg
-	sensor_combined.msg
-	sensor_correction.msg
-	sensor_gps.msg
-	sensor_gyro.msg
-	sensor_gyro_fft.msg
-	sensor_gyro_fifo.msg
-	sensor_hygrometer.msg
-	sensor_mag.msg
-	sensor_preflight_mag.msg
-	sensor_selection.msg
-	sensors_status_imu.msg
-	system_power.msg
-	takeoff_status.msg
-	task_stack_info.msg
-	tecs_status.msg
-	telemetry_status.msg
-	test_motor.msg
-	timesync.msg
-	timesync_status.msg
-	trajectory_bezier.msg
-	trajectory_waypoint.msg
-	transponder_report.msg
-	tune_control.msg
-	uavcan_parameter_request.msg
-	uavcan_parameter_value.msg
-	ulog_stream.msg
-	ulog_stream_ack.msg
-	vehicle_acceleration.msg
-	vehicle_air_data.msg
-	vehicle_angular_acceleration.msg
-	vehicle_angular_acceleration_setpoint.msg
-	vehicle_angular_velocity.msg
-	vehicle_attitude.msg
-	vehicle_attitude_setpoint.msg
-	vehicle_command.msg
-	vehicle_command_ack.msg
-	vehicle_constraints.msg
-	vehicle_control_mode.msg
-	vehicle_global_position.msg
-	vehicle_gps_position.msg
-	vehicle_imu.msg
-	vehicle_imu_status.msg
-	vehicle_land_detected.msg
-	vehicle_local_position.msg
-	vehicle_local_position_setpoint.msg
-	vehicle_magnetometer.msg
-	vehicle_odometry.msg
-	vehicle_rates_setpoint.msg
-	vehicle_roi.msg
-	vehicle_status.msg
-	vehicle_status_flags.msg
-	vehicle_thrust_setpoint.msg
-	vehicle_torque_setpoint.msg
-	vehicle_trajectory_bezier.msg
-	vehicle_trajectory_waypoint.msg
-	vtol_vehicle_status.msg
-	wheel_encoders.msg
-	wind.msg
-	yaw_estimator_status.msg
+	ActionRequest.msg
+	ActuatorArmed.msg
+	ActuatorControls.msg
+	ActuatorControlsStatus.msg
+	ActuatorMotors.msg
+	ActuatorOutputs.msg
+	ActuatorServos.msg
+	ActuatorServosTrim.msg
+	ActuatorTest.msg
+	AdcReport.msg
+	Airspeed.msg
+	AirspeedValidated.msg
+	AirspeedWind.msg
+	AutotuneAttitudeControlStatus.msg
+	BatteryStatus.msg
+	CameraCapture.msg
+	CameraStatus.msg
+	CameraTrigger.msg
+	CellularStatus.msg
+	CollisionConstraints.msg
+	CollisionReport.msg
+	CommanderState.msg
+	ControlAllocatorStatus.msg
+	Cpuload.msg
+	DebugArray.msg
+	DebugKeyValue.msg
+	DebugValue.msg
+	DebugVect.msg
+	DifferentialPressure.msg
+	DistanceSensor.msg
+	Ekf2Timestamps.msg
+	EkfGpsDrift.msg
+	EscReport.msg
+	EscStatus.msg
+	EstimatorBaroBias.msg
+	EstimatorEventFlags.msg
+	EstimatorInnovations.msg
+	EstimatorOpticalFlowVel.msg
+	EstimatorSelectorStatus.msg
+	EstimatorSensorBias.msg
+	EstimatorStates.msg
+	EstimatorStatus.msg
+	EstimatorStatusFlags.msg
+	Event.msg
+	FailureDetectorStatus.msg
+	FollowTarget.msg
+	GeneratorStatus.msg
+	GeofenceResult.msg
+	GimbalDeviceAttitudeStatus.msg
+	GimbalDeviceInformation.msg
+	GimbalDeviceSetAttitude.msg
+	GimbalManagerInformation.msg
+	GimbalManagerSetAttitude.msg
+	GimbalManagerSetManualControl.msg
+	GimbalManagerStatus.msg
+	GpsDump.msg
+	GpsInjectData.msg
+	HeaterStatus.msg
+	HomePosition.msg
+	HoverThrustEstimate.msg
+	InputRc.msg
+	InternalCombustionEngineStatus.msg
+	IridiumsbdStatus.msg
+	IrlockReport.msg
+	LandingGear.msg
+	LandingTargetInnovations.msg
+	LandingTargetPose.msg
+	LedControl.msg
+	LoggerStatus.msg
+	LogMessage.msg
+	MagnetometerBiasEstimate.msg
+	MagWorkerData.msg
+	ManualControlSetpoint.msg
+	ManualControlSwitches.msg
+	MavlinkLog.msg
+	Mission.msg
+	MissionResult.msg
+	MountOrientation.msg
+	NavigatorMissionItem.msg
+	NpfgStatus.msg
+	ObstacleDistance.msg
+	OffboardControlMode.msg
+	OnboardComputerStatus.msg
+	OpticalFlow.msg
+	OrbitStatus.msg
+	OrbTest.msg
+	OrbTestLarge.msg
+	OrbTestMedium.msg
+	ParameterUpdate.msg
+	Ping.msg
+	PpsCapture.msg
+	PositionControllerLandingStatus.msg
+	PositionControllerStatus.msg
+	PositionSetpoint.msg
+	PositionSetpointTriplet.msg
+	PowerButtonState.msg
+	PowerMonitor.msg
+	PwmInput.msg
+	Px4ioStatus.msg
+	RadioStatus.msg
+	RateCtrlStatus.msg
+	RcChannels.msg
+	RcParameterMap.msg
+	Rpm.msg
+	RtlTimeEstimate.msg
+	Safety.msg
+	SatelliteInfo.msg
+	SensorAccel.msg
+	SensorAccelFifo.msg
+	SensorBaro.msg
+	SensorCombined.msg
+	SensorCorrection.msg
+	SensorGps.msg
+	SensorGyro.msg
+	SensorGyroFft.msg
+	SensorGyroFifo.msg
+	SensorHygrometer.msg
+	SensorMag.msg
+	SensorPreflightMag.msg
+	SensorSelection.msg
+	SensorsStatusImu.msg
+	SystemPower.msg
+	TakeoffStatus.msg
+	TaskStackInfo.msg
+	TecsStatus.msg
+	TelemetryStatus.msg
+	TestMotor.msg
+	Timesync.msg
+	TimesyncStatus.msg
+	TrajectoryBezier.msg
+	TrajectoryWaypoint.msg
+	TransponderReport.msg
+	TuneControl.msg
+	UavcanParameterRequest.msg
+	UavcanParameterValue.msg
+	UlogStream.msg
+	UlogStreamAck.msg
+	VehicleAcceleration.msg
+	VehicleAirData.msg
+	VehicleAngularAcceleration.msg
+	VehicleAngularAccelerationSetpoint.msg
+	VehicleAngularVelocity.msg
+	VehicleAttitude.msg
+	VehicleAttitudeSetpoint.msg
+	VehicleCommand.msg
+	VehicleCommandAck.msg
+	VehicleConstraints.msg
+	VehicleControlMode.msg
+	VehicleGlobalPosition.msg
+	VehicleGpsPosition.msg
+	VehicleImu.msg
+	VehicleImuStatus.msg
+	VehicleLandDetected.msg
+	VehicleLocalPosition.msg
+	VehicleLocalPositionSetpoint.msg
+	VehicleMagnetometer.msg
+	VehicleOdometry.msg
+	VehicleRatesSetpoint.msg
+	VehicleRoi.msg
+	VehicleStatus.msg
+	VehicleStatusFlags.msg
+	VehicleThrustSetpoint.msg
+	VehicleTorqueSetpoint.msg
+	VehicleTrajectoryBezier.msg
+	VehicleTrajectoryWaypoint.msg
+	VtolVehicleStatus.msg
+	WheelEncoders.msg
+	Wind.msg
+	YawEstimatorStatus.msg
 )
-
-if(NOT px4_constrained_flash_build)
-	list(APPEND msg_files
-		debug_array.msg
-		debug_key_value.msg
-		debug_value.msg
-		debug_vect.msg
-	)
-endif()
-
-if(PX4_TESTING)
-	list(APPEND msg_files
-		orb_test.msg
-		orb_test_large.msg
-		orb_test_medium.msg
-	)
-endif()
-
 list(SORT msg_files)

-set(deprecated_msgs
-	ekf2_innovations.msg	# 2019-11-22, Updated estimator interface and logging; replaced by 'estimator_innovations'.
-	)
-
-foreach(msg IN LISTS deprecated_msgs)
-	if(msg IN_LIST msg_files)
-		get_filename_component(msg_we ${msg} NAME_WE)
-		list(APPEND invalid_msgs ${msg_we})
-	endif()
-endforeach()
-if(invalid_msgs)
-	list(LENGTH invalid_msgs invalid_msgs_size)
-	if(${invalid_msgs_size} GREATER 1)
-		foreach(msg IN LISTS invalid_msgs)
-			string(CONCAT invalid_msgs_cs ${invalid_msgs_cs} "'${msg}', ")
-		endforeach()
-		STRING(REGEX REPLACE ", +$" "" invalid_msgs_cs ${invalid_msgs_cs})
-		message(FATAL_ERROR "${invalid_msgs_cs} are listed as deprecated. Please use different names for the messages.")
-	else()
-		message(FATAL_ERROR "'${invalid_msgs}' is listed as deprecated. Please use a different name for the message.")
-	endif()
-endif()
-
 px4_list_make_absolute(msg_files ${CMAKE_CURRENT_SOURCE_DIR} ${msg_files})

 if(NOT EXTERNAL_MODULES_LOCATION STREQUAL "")
@@ -257,40 +223,40 @@ if(NOT EXTERNAL_MODULES_LOCATION STREQUAL "")
 	endif()
 endif()

+# set parent scope msg_files for other modules to consume (eg topic_listener)
+set(msg_files ${msg_files} PARENT_SCOPE)
+
 # headers
 set(msg_out_path ${PX4_BINARY_DIR}/uORB/topics)
 set(msg_source_out_path	${CMAKE_CURRENT_BINARY_DIR}/topics_sources)

-set(uorb_headers ${msg_out_path}/uORBTopics.hpp)
-set(uorb_sources ${msg_source_out_path}/uORBTopics.cpp)
+set(uorb_headers)
 foreach(msg_file ${msg_files})
 	get_filename_component(msg ${msg_file} NAME_WE)
+
+	# Pascal case to snake case (MsgFile -> msg_file)
+	string(REGEX REPLACE "(.)([A-Z][a-z]+)" "\\1_\\2" msg "${msg}")
+	string(REGEX REPLACE "([a-z0-9])([A-Z])" "\\1_\\2" msg "${msg}")
+	string(TOLOWER "${msg}" msg)
+
 	list(APPEND uorb_headers ${msg_out_path}/${msg}.h)
-	list(APPEND uorb_sources ${msg_source_out_path}/${msg}.cpp)
 endforeach()

-if (px4_constrained_flash_build)
+if(px4_constrained_flash_build)
 	set(added_arguments --constrained-flash)
 endif()

-# set parent scope msg_files for other modules to consume (eg topic_listener)
-set(msg_files ${msg_files} PARENT_SCOPE)
-
 # Generate uORB headers
 add_custom_command(OUTPUT ${uorb_headers}
 	COMMAND ${PYTHON_EXECUTABLE} tools/px_generate_uorb_topic_files.py
-		--headers
 		-f ${msg_files}
 		-i ${CMAKE_CURRENT_SOURCE_DIR}
 		-o ${msg_out_path}
 		-e templates/uorb
-		-t ${CMAKE_CURRENT_BINARY_DIR}/tmp/headers
-		-q
 		${added_arguments}
 	DEPENDS
 		${msg_files}
 		templates/uorb/msg.h.em
-		templates/uorb/uORBTopics.hpp.em
 		tools/px_generate_uorb_topic_files.py
 		tools/px_generate_uorb_topic_helper.py
 	COMMENT "Generating uORB topic headers"
@@ -298,28 +264,3 @@ add_custom_command(OUTPUT ${uorb_headers}
 	VERBATIM
 	)
 add_custom_target(uorb_headers DEPENDS ${uorb_headers})
-
-# Generate uORB sources
-add_custom_command(OUTPUT ${uorb_sources}
-	COMMAND ${PYTHON_EXECUTABLE} tools/px_generate_uorb_topic_files.py
-		--sources
-		-f ${msg_files}
-		-i ${CMAKE_CURRENT_SOURCE_DIR}
-		-o ${msg_source_out_path}
-		-e templates/uorb
-		-t ${CMAKE_CURRENT_BINARY_DIR}/tmp/sources
-		-q
-		${added_arguments}
-	DEPENDS
-		${msg_files}
-		templates/uorb/msg.cpp.em
-		templates/uorb/uORBTopics.cpp.em
-		tools/px_generate_uorb_topic_files.py
-		tools/px_generate_uorb_topic_helper.py
-	COMMENT "Generating uORB topic sources"
-	WORKING_DIRECTORY ${CMAKE_CURRENT_SOURCE_DIR}
-	VERBATIM
-	)
-
-add_library(uorb_msgs ${uorb_sources})
-add_dependencies(uorb_msgs prebuild_targets uorb_headers)
@@ -5,5 +5,3 @@ uint32 seq		# Image sequence number
 bool feedback	# Trigger feedback from camera

 uint32 ORB_QUEUE_LENGTH = 2
-
-# TOPICS camera_trigger
@@ -25,4 +25,4 @@ uint8 esc_online_flags					# Bitmask indicating which ESC is online/offline

 uint8 esc_armed_flags					# Bitmask indicating which ESC is armed. For ESC's where the arming state is not known (returned by the ESC), the arming bits should always be set.

-esc_report[8] esc
+px4/EscReport[8] esc
@@ -34,5 +34,3 @@ float32 hagl		# height of ground innovation (m) and innovation variance (m**2)

 # The innovation test ratios are scalar values. In case the field is a vector,
 # the test ratio will be put in the first component of the vector.
-
-# TOPICS estimator_innovations estimator_innovation_variances estimator_innovation_test_ratios
@@ -1,8 +1,8 @@
 uint64 timestamp		# time since system start (microseconds)
 float64 lat			# target position (deg * 1e7)
-float64 lon			# target position (deg * 1e7) 
+float64 lon			# target position (deg * 1e7)
 float32 alt			# target position
 float32 vy			# target vel in y
 float32 vx			# target vel in x
-float32 vz			# target vel in z 
+float32 vz			# target vel in z
 uint8 est_cap # target reporting capabilities
--- 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
Daniel Agar	7cb8ed3a1f	sensors/vehicle_gps_position: don't work with 64 time with 32 bit floats	2022-01-21 09:18:49 -05:00
JaeyoungLim	0607982b23	Publish orbit status also when the waypoint is a loiter waypoint (#19048 ) * Publish orbit status also when the waypoint is a loiter waypoint * Remove redundant orbit status publishing	2022-01-21 09:13:03 +01:00
Beat Küng	bcd057ac3e	uORB: fix copy-paste mistake in orb_print_message_internal Could have led to invalid memory access.	2022-01-20 12:58:20 -05:00
bresch	a2260e53da	ekf2: replace yaw failure detection criteria The existing logic using the angle between velocity vectors failed to determine a yaw failure in practice because the state velocity is often too small compared to its uncertainty to be used. In all the failures reported, the yaw emergency estimator converged properly and yaw reset would have fixed the issue. A much simpler check using the yaw difference between the main EKF and the emergency estimator is now used to tell if the vel/pos update failure is most likely caused by a wrong heading.	2022-01-20 11:54:24 +01:00
Daniel Agar	446729566d	platforms/nuttx: px4io_serial always cleanup DMA before next bus exchange - this really shouldn't be necessary, but worst case it's harmless and much better than potentially falling out of the sky	2022-01-19 18:00:18 -05:00
Jaeyoung Lim	0dea56f88b	Use current_sp for publishing orbit status	2022-01-19 12:00:03 -05:00
Jaeyoung-Lim	dec5e7e9c8	Publish orbit status whenever the vehicle is in loiter This commit moves the publish orbit status to be published whenever the vehicle is considering a loiter setpoint	2022-01-19 12:00:03 -05:00
Thomas Stastny	19c98b8841	npfg: fix typo in Vector2f indexing	2022-01-19 11:17:44 -05:00
bresch	e89e3c1b0c	ekf2: extract logic to test yaw emergency estimate quality	2022-01-19 09:05:41 -05:00
bresch	ea80c5027e	ekf2: split yaw estimator state getter into several functions	2022-01-19 09:05:41 -05:00
dagar	476c1e5c09	[AUTO COMMIT] update change indication	2022-01-19 09:04:53 -05:00
bresch	ffebd8e771	ekf2: initialize cos tilt variable assuming no tilt	2022-01-19 09:04:53 -05:00
Daniel Agar	92a5bbe97f	ekf2: improve ring buffer sizing and dynamically allocate	2022-01-19 09:04:53 -05:00
Daniel Agar	36605bfff6	sensors: lower SENS_MAG_RATE default - reduce estimator 3D mag fusion cpu load (-3% on Pixhawk4)	2022-01-19 08:51:57 -05:00
Matthias Grob	c9f7c20d46	MulticoperPositionController: time failsafe messages together The following output instead of printing the action "stop and wait" just once: WARN [mc_pos_control] invalid setpoints WARN [mc_pos_control] invalid setpoints WARN [mc_pos_control] Failsafe: stop and wait WARN [mc_pos_control] invalid setpoints WARN [mc_pos_control] Failsafe: stop and wait WARN [mc_pos_control] invalid setpoints WARN [mc_pos_control] Failsafe: stop and wait	2022-01-19 14:31:19 +01:00
Matthias Grob	d4e356a1ac	PosititionControl: fix integrator windup with invalid setpoint	2022-01-19 14:31:19 +01:00
Matthias Grob	8811482f1d	PositionControlTest: add integrator windup with invalid setpoint combination unit test This bug was by chance found during simulation testing and debugging. The unit test is to easily reproduce and cover this case.	2022-01-19 14:31:19 +01:00
Silvan Fuhrer	632dfa55e6	FW Pos C: add option to disable airspeed setpoint via stick input -rename FW_POSCTL_INV_ST to FW_POS_STK_CONF and make bitmask out of it: - bit 0: alternative mapping (height rate on throttle stick, airspeed on pitch) - bit 1: enable/disable airspeed setpoints via stick Signed-off-by: Silvan Fuhrer <silvan@auterion.com>	2022-01-19 10:13:07 +01:00
Silvan Fuhrer	cbb8c90245	ControlAllocation: fix calculation of roll/pitch normalization scale Take into account the actual number of roll and pitch acutators, instead of assuming that all actuators have a roll/pitch component. Signed-off-by: Silvan Fuhrer <silvan@auterion.com>	2022-01-19 08:22:12 +01:00
Silvan Fuhrer	0950bb81ab	ControlAllocation: update normalization scale only if matrix updated is forced The forced flag is used to distinguish between updates due to a configuration (parameter) change (only enabled when disarmed), and matrix updates due to motor tilt change. Only update the normalization scale if the forced flag is true, and use a tilt angle of vertical position for it to have the scales tilt-invariant. Signed-off-by: Silvan Fuhrer <silvan@auterion.com>	2022-01-19 08:22:12 +01:00
CUAVmengxiao	258a563dd5	barometer: Add ICP10100 and ICP1011	2022-01-18 19:14:40 -05:00
FARHANG	fe44e281e5	ROMFS: Holybro X500 v2 airframe addition	2022-01-18 15:11:41 -05:00