updated warning (#22367)

Signed-off-by: frederik <frederik@auterion.com>
Co-authored-by: frederik <frederik@auterion.com>

.gitmodules

@@ -62,3 +62,16 @@
 	path = src/modules/uxrce_dds_client/Micro-XRCE-DDS-Client
 	url = https://github.com/PX4/Micro-XRCE-DDS-Client.git
 	branch = px4
+[submodule "src/lib/cdrstream/cyclonedds"]
+	path = src/lib/cdrstream/cyclonedds
+	url = https://github.com/px4/cyclonedds
+[submodule "src/lib/cdrstream/rosidl"]
+	path = src/lib/cdrstream/rosidl
+	url = https://github.com/px4/rosidl
+[submodule "src/modules/zenoh/zenoh-pico"]
+	path = src/modules/zenoh/zenoh-pico
+	url = https://github.com/px4/zenoh-pico
+	branch = pr-zubf-werror-fix
+[submodule "src/lib/heatshrink/heatshrink"]
+	path = src/lib/heatshrink/heatshrink
+	url = https://github.com/PX4/heatshrink.git

Kconfig

@@ -205,3 +205,5 @@ menu "platforms"
 depends on PLATFORM_QURT || PLATFORM_POSIX
 source "platforms/common/Kconfig"
 endmenu
+
+source "src/lib/*/Kconfig"

Makefile

@@ -484,7 +484,9 @@ validate_module_configs:
 	@find "$(SRC_DIR)"/src/modules "$(SRC_DIR)"/src/drivers "$(SRC_DIR)"/src/lib -name *.yaml -type f \
 	-not -path "$(SRC_DIR)/src/lib/mixer_module/*" \
 	-not -path "$(SRC_DIR)/src/modules/uxrce_dds_client/dds_topics.yaml" \
+	-not -path "$(SRC_DIR)/src/modules/zenoh/zenoh-pico/*" \
 	-not -path "$(SRC_DIR)/src/lib/events/libevents/*" \
+	-not -path "$(SRC_DIR)/src/lib/cdrstream/*" \
 	-not -path "$(SRC_DIR)/src/lib/crypto/libtommath/*" -print0 | \
 	xargs -0 "$(SRC_DIR)"/Tools/validate_yaml.py --schema-file "$(SRC_DIR)"/validation/module_schema.yaml
 

README.md

@@ -114,7 +114,7 @@ These boards are maintained to be compatible with PX4-Autopilot by the Manufactu
 
 ### Community supported
 
-These boards don't fully comply industry standards, and thus is solely maintained by the PX4 publc community members.
+These boards don't fully comply industry standards, and thus is solely maintained by the PX4 public community members.
 
 ### Experimental
 

ROMFS/px4fmu_common/init.d-posix/airframes/4008_gz_advanced_plane

@@ -0,0 +1,82 @@
+#!/bin/sh
+#
+# @name Advanced Plane SITL
+#
+
+. ${R}etc/init.d/rc.fw_defaults
+
+PX4_SIMULATOR=${PX4_SIMULATOR:=gz}
+PX4_GZ_WORLD=${PX4_GZ_WORLD:=default}
+PX4_SIM_MODEL=${PX4_SIM_MODEL:=advanced_plane}
+
+param set-default SIM_GZ_EN 1
+
+param set-default SENS_EN_GPSSIM 1
+param set-default SENS_EN_MAGSIM 1
+param set-default SENS_EN_ARSPDSIM 1
+
+param set-default FW_LND_ANG 8
+param set-default NPFG_PERIOD 12
+
+param set-default FW_MAN_P_MAX 30
+param set-default FW_PR_P 0.9
+param set-default FW_PR_FF 0.5
+param set-default FW_PR_I 0.5
+
+param set-default FW_PSP_OFF 2
+param set-default FW_P_LIM_MAX 32
+param set-default FW_P_LIM_MIN -15
+
+param set-default FW_RR_FF 0.5
+param set-default FW_RR_P 0.3
+param set-default FW_RR_I 0.5
+
+param set-default FW_YR_FF 0.5
+param set-default FW_YR_P 0.6
+param set-default FW_YR_I 0.5
+
+param set-default FW_SPOILERS_LND 0.4
+
+param set-default FW_THR_MIN 0.05
+param set-default FW_THR_TRIM 0.25
+
+param set-default FW_T_CLMB_MAX 8
+param set-default FW_T_SINK_MAX 2.7
+param set-default FW_T_SINK_MIN 2.2
+
+param set-default FW_W_EN 1
+
+param set-default MIS_TAKEOFF_ALT 30
+
+param set-default NAV_ACC_RAD 15
+param set-default NAV_DLL_ACT 2
+
+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 6
+param set-default CA_SV_CS0_TRQ_R -0.5
+param set-default CA_SV_CS0_TYPE 1
+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_CS4_TYPE 9
+param set-default CA_SV_CS5_TYPE 10
+
+param set-default SIM_GZ_EC_FUNC1 101
+param set-default SIM_GZ_EC_MIN1 10
+param set-default SIM_GZ_EC_MAX1 1600
+
+param set-default SIM_GZ_SV_FUNC1 201
+param set-default SIM_GZ_SV_FUNC2 202
+param set-default SIM_GZ_SV_FUNC3 203
+param set-default SIM_GZ_SV_FUNC4 204
+param set-default SIM_GZ_SV_FUNC5 205
+param set-default SIM_GZ_SV_FUNC6 206

ROMFS/px4fmu_common/init.d-posix/airframes/CMakeLists.txt

@@ -79,6 +79,7 @@ px4_add_romfs_files(
 	4004_gz_standard_vtol
 	4005_gz_x500_vision
 	4006_gz_px4vision
+	4008_gz_advanced_plane
 
 	6011_gazebo-classic_typhoon_h480
 	6011_gazebo-classic_typhoon_h480.post

ROMFS/px4fmu_common/init.d-posix/px4-rc.simulator

@@ -38,6 +38,7 @@ if [ "$PX4_SIMULATOR" = "sihsim" ] || [ "$(param show -q SYS_AUTOSTART)" -eq "0"
 
 elif [ "$PX4_SIMULATOR" = "gz" ] || [ "$(param show -q SIM_GZ_EN)" -eq "1" ]; then
 
+
 	# set local coordinate frame reference
 	if [ -n "${PX4_HOME_LAT}" ]; then
 		param set SIM_GZ_HOME_LAT ${PX4_HOME_LAT}
@@ -59,36 +60,40 @@ elif [ "$PX4_SIMULATOR" = "gz" ] || [ "$(param show -q SIM_GZ_EN)" -eq "1" ]; th
 		. ../gz_env.sh
 	fi
 
-	# "gz sim" only avaiilable in Garden and later
-	GZ_SIM_VERSIONS=$(gz sim --versions 2>&1)
-	if [ $? -eq 0 ] && [ "${GZ_SIM_VERSIONS}" != "" ]
-	then
-		# "gz sim" from Garden on
-		gz_command="gz"
-		gz_sub_command="sim"
-	else
-		echo "ERROR [init] Gazebo gz please install gz-garden"
-		exit 1
-	fi
+	# Only start up Gazebo if STANDALONE set to false
+	if [ "$STANDALONE" != '1' ]; then
 
-	# look for running ${gz_command} gazebo world
-	gz_world=$( ${gz_command} topic -l | grep -m 1 -e "^/world/.*/clock" | sed 's/\/world\///g; s/\/clock//g' )
-
-	# shellcheck disable=SC2153
-	if [ -z "${gz_world}" ] && [ -n "${PX4_GZ_WORLDS}" ] && [ -n "${PX4_GZ_WORLD}" ]; then
-
-		echo "INFO  [init] starting gazebo with world: ${PX4_GZ_WORLDS}/${PX4_GZ_WORLD}.sdf"
-
-		${gz_command} ${gz_sub_command} --verbose=1 -r -s "${PX4_GZ_WORLDS}/${PX4_GZ_WORLD}.sdf" &
-
-		if [ -z "${HEADLESS}" ]; then
-			# HEADLESS not set, starting gui
-			${gz_command} ${gz_sub_command} -g &
+		# "gz sim" only avaiilable in Garden and later
+		GZ_SIM_VERSIONS=$(gz sim --versions 2>&1)
+		if [ $? -eq 0 ] && [ "${GZ_SIM_VERSIONS}" != "" ]
+		then
+			# "gz sim" from Garden on
+			gz_command="gz"
+			gz_sub_command="sim"
+		else
+			echo "ERROR [init] Gazebo gz please install gz-garden"
+			exit 1
 		fi
 
-	else
-		echo "INFO  [init] gazebo already running world: ${gz_world}"
-		PX4_GZ_WORLD=${gz_world}
+		# look for running ${gz_command} gazebo world
+		gz_world=$( ${gz_command} topic -l | grep -m 1 -e "^/world/.*/clock" | sed 's/\/world\///g; s/\/clock//g' )
+
+		# shellcheck disable=SC2153
+		if [ -z "${gz_world}" ] && [ -n "${PX4_GZ_WORLDS}" ] && [ -n "${PX4_GZ_WORLD}" ]; then
+
+			echo "INFO  [init] starting gazebo with world: ${PX4_GZ_WORLDS}/${PX4_GZ_WORLD}.sdf"
+
+			${gz_command} ${gz_sub_command} --verbose=1 -r -s "${PX4_GZ_WORLDS}/${PX4_GZ_WORLD}.sdf" &
+
+			if [ -z "${HEADLESS}" ]; then
+				# HEADLESS not set, starting gui
+				${gz_command} ${gz_sub_command} -g &
+			fi
+
+		else
+			echo "INFO  [init] gazebo already running world: ${gz_world}"
+			PX4_GZ_WORLD=${gz_world}
+		fi
 	fi
 
 	# start gz_bridge

ROMFS/px4fmu_common/init.d/rcS

@@ -537,6 +537,10 @@ else
 			cyphal start
 		fi
 	fi
+	if param greater -s ZENOH_ENABLE 0
+	then
+		zenoh start
+	fi
 
 #
 # End of autostart.

SECURITY.md

@@ -0,0 +1,25 @@
+# Security Policy
+
+## Supported Versions
+
+The following is a list of versions the development team is currently supporting.
+
+| Version | Supported          |
+| ------- | ------------------ |
+| 1.4.x   | :white_check_mark: |
+| 1.3.3   | :white_check_mark: |
+| < 1.3   | :x:                |
+
+## Reporting a Vulnerability
+
+We currently only receive security vulnerability reports through GitHub.
+
+To begin a report, please go to the top-level repository, for example, PX4/PX4-Autopilot,
+and click on the Security tab. If you are on mobile, click the ... dropdown menu, and then click Security.
+
+Click Report a Vulnerability to open the advisory form. Fill in the advisory details form.
+Make sure your title is descriptive, and the development team can find all of the relevant details needed
+to verify on the description box. We recommend you add as much data as possible. We welcome logs,
+screenshots, photos, and videos, anything that can help us verify and identify the issues being reported.
+
+At the bottom of the form, click Submit report. The maintainer team will be notified and will get back to you ASAP.

Tools/astyle/files_to_check_code_style.sh

@@ -25,5 +25,9 @@ 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/lib/heatshrink/heatshrink -prune -o \
     -path src/modules/uxrce_dds_client/Micro-XRCE-DDS-Client -prune -o \
+    -path src/lib/cdrstream/cyclonedds -prune -o \
+    -path src/lib/cdrstream/rosidl -prune -o \
+    -path src/modules/zenoh/zenoh-pico -prune -o \
     -type f \( -name "*.c" -o -name "*.h" -o -name "*.cpp" -o -name "*.hpp" \) | grep $PATTERN

Tools/msg/px_generate_uorb_compressed_fields.py

@@ -0,0 +1,232 @@
+#!/usr/bin/env python3
+#############################################################################
+#
+#   Copyright (C) 2023 PX4 Pro 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.
+#
+#############################################################################
+
+"""
+Generates cpp source + header files with compressed uorb topic fields from json files
+"""
+
+import argparse
+import json
+import struct
+from operator import itemgetter
+import sys
+import os
+
+sys.path.append(os.path.join(os.path.dirname(os.path.realpath(__file__)), '../../src/lib/heatshrink'))
+import heatshrink_encode
+
+
+def parse_json_files(json_files: [str]) -> dict:
+    """Read list of json files into a dict"""
+    definitions = {}
+    for json_file in json_files:
+        with open(json_file, encoding='utf-8') as file_handle:
+            definition = json.load(file_handle)
+            assert definition['name'] not in definitions
+            definitions[definition['name']] = definition
+            definitions[definition['name']]['completed'] = False
+
+    return definitions
+
+
+def get_ordered_list_by_dependency(name: str, definitions: dict) -> [str]:
+    """Iterate dependency graph and create an ordered list"""
+    if definitions[name]['completed']:
+        return []
+    ret = []
+    # Get nested types first (DFS)
+    for dependency in definitions[name]['dependencies']:
+        ret.extend(get_ordered_list_by_dependency(dependency, definitions))
+
+    ret.append(name)
+    definitions[name]['completed'] = True
+    return ret
+
+
+def get_field_definitions(names: [str], definitions: dict) -> (bytes, [str]):
+    """Get byte array with all definitions"""
+    ret = bytes()
+    formats_list = []
+
+    for name in names:
+        # Format as '<# orb_ids><orb_id0...><# orb_ids dependencies<orb_id_dependency0...><fields><null>'
+        assert len(definitions[name]['orb_ids']) < 255
+        assert len(definitions[name]['dependencies']) < 255
+        ret += struct.pack('<B', len(definitions[name]['orb_ids']))
+        for orb_id in definitions[name]['orb_ids']:
+            assert orb_id < (1 << 16)
+            ret += struct.pack('<H', orb_id)
+        # Dependencies
+        ret += struct.pack('<B', len(definitions[name]['dependencies']))
+        for dependent_message_name in definitions[name]['dependencies']:
+            # Get ORB ID by looking up the name in all definitions
+            dependent_orb_id_list = [definitions[k]['main_orb_id'] for k in definitions if
+                                     definitions[k]['name'] == dependent_message_name]
+            assert len(dependent_orb_id_list) == 1
+            orb_id = dependent_orb_id_list[0]
+            assert (1 << 16) > orb_id >= 0
+            ret += struct.pack('<H', orb_id)
+
+        ret += bytes(definitions[name]['fields'], 'latin1')
+        ret += b'\0'
+
+        formats_list.append(definitions[name]['fields'])
+
+    return ret, formats_list
+
+
+def write_fields_to_cpp_file(file_name: str, compressed_fields: bytes):
+    fields_str = ', '.join(str(c) for c in compressed_fields)
+    with open(file_name, 'w') as file_handle:
+        file_handle.write('''
+// Auto-generated from px4_generate_uorb_compressed_fields.py
+#include <uORB/topics/uORBMessageFieldsGenerated.hpp>
+
+namespace uORB {
+
+static const uint8_t compressed_fields[] = {
+    {FIELDS}
+};
+
+const uint8_t* orb_compressed_message_formats()
+{
+    return compressed_fields;
+}
+unsigned orb_compressed_message_formats_size()
+{
+    return sizeof(compressed_fields) / sizeof(compressed_fields[0]);
+}
+
+} // namespace uORB
+'''.replace('{FIELDS}', fields_str))
+
+
+def c_encode(s, encoding='ascii'):
+    result = ''
+    for c in s:
+        if not (32 <= ord(c) < 127) or c in ('\\', '"'):
+            result += '\\%03o' % ord(c)
+        else:
+            result += c
+    return '"' + result + '"'
+
+
+def write_fields_to_hpp_file(file_name: str, definitions: dict, window_length: int, lookahead_length: int,
+                             format_list: [str]):
+    max_tokenized_field_length, max_tokenized_field_length_msg = max(
+        ((len(definitions[k]['fields']), k) for k in definitions), key=itemgetter(0))
+    max_num_orb_ids = max(len(definitions[k]['orb_ids']) for k in definitions)
+    max_num_orb_id_dependencies = max(len(definitions[k]['dependencies']) for k in definitions)
+
+    with open(file_name, 'w') as file_handle:
+        file_handle.write('''
+// Auto-generated from px4_generate_uorb_compressed_fields.py
+#include <cstdint>
+
+namespace uORB {
+
+ /**
+  * Get compressed string of all uorb message format definitions
+  */
+const uint8_t* orb_compressed_message_formats();
+
+/**
+ * Get length of compressed message format definitions
+ */
+unsigned orb_compressed_message_formats_size();
+
+static constexpr unsigned orb_tokenized_fields_max_length = {MAX_TOKENIZED_FIELD_LENGTH}; // {MAX_TOKENIZED_FIELD_LENGTH_MSG}
+static constexpr unsigned orb_compressed_max_num_orb_ids = {MAX_NUM_ORB_IDS};
+static constexpr unsigned orb_compressed_max_num_orb_id_dependencies = {MAX_NUM_ORB_ID_DEPENDENCIES};
+
+static constexpr unsigned orb_compressed_heatshrink_window_length = {WINDOW_LENGTH};
+static constexpr unsigned orb_compressed_heatshrink_lookahead_length = {LOOKAHEAD_LENGTH};
+
+#define ORB_DECOMPRESSED_MESSAGE_FIELDS {{DECOMPRESSED_MESSAGE_FIELDS}}
+
+} // namespace uORB
+'''
+                          .replace('{MAX_TOKENIZED_FIELD_LENGTH}', str(max_tokenized_field_length))
+                          .replace('{MAX_TOKENIZED_FIELD_LENGTH_MSG}', max_tokenized_field_length_msg)
+                          .replace('{MAX_NUM_ORB_IDS}', str(max_num_orb_ids))
+                          .replace('{MAX_NUM_ORB_ID_DEPENDENCIES}', str(max_num_orb_id_dependencies))
+                          .replace('{WINDOW_LENGTH}', str(window_length))
+                          .replace('{LOOKAHEAD_LENGTH}', str(lookahead_length))
+                          .replace('{DECOMPRESSED_MESSAGE_FIELDS}', ','.join(c_encode(x) for x in format_list))
+                          )
+
+
+def main():
+    parser = argparse.ArgumentParser(description='Generate compressed uorb topic fields')
+    parser.add_argument('-f', dest='file',
+                        help="json input files",
+                        nargs="+")
+    parser.add_argument('--source-output-file', dest='output_cpp',
+                        help='cpp output file to generate')
+    parser.add_argument('--header-output-file', dest='output_hpp',
+                        help='hpp output file to generate')
+    parser.add_argument('-v', '--verbose',
+                        action='store_true',
+                        help="verbose output")
+    args = parser.parse_args()
+
+    if args.file is not None:
+        definitions = parse_json_files(args.file)
+
+        # Get array of all field definitions
+        names = []
+        for definition in definitions:
+            names.extend(get_ordered_list_by_dependency(definitions[definition]['name'], definitions))
+        names.reverse()  # Dependent definitions must be after
+        assert len(names) == len(definitions)
+        for definition in definitions:  # sanity check
+            assert definitions[definition]['completed']
+        field_definitions, format_list = get_field_definitions(names, definitions)
+
+        # Compress
+        window_size = 8  # Larger value = better compression; memory requirement (for decompression): 2 ^ window_size
+        lookahead = 4
+        compressed_field_definitions = heatshrink_encode.encode(field_definitions, window_size, lookahead)
+
+        if args.verbose:
+            print(
+                f'Field definitions: size: {len(field_definitions)}, reduction from compression: {len(field_definitions) - len(compressed_field_definitions)}')
+
+        # Write cpp & hpp file
+        write_fields_to_cpp_file(args.output_cpp, compressed_field_definitions)
+        write_fields_to_hpp_file(args.output_hpp, definitions, window_size, lookahead, format_list)
+
+
+if __name__ == "__main__":
+    main()

Tools/msg/px_generate_uorb_topic_files.py

@@ -70,9 +70,8 @@ __license__ = "BSD"
 __email__ = "thomasgubler@gmail.com"
 
 
-TEMPLATE_FILE = ['msg.h.em', 'msg.cpp.em']
-TOPICS_LIST_TEMPLATE_FILE = ['uORBTopics.hpp.em', 'uORBTopics.cpp.em']
-OUTPUT_FILE_EXT = ['.h', '.cpp']
+TEMPLATE_FILE = ['msg.h.em', 'msg.cpp.em', 'uorb_idl_header.h.em', 'msg.json.em']
+TOPICS_LIST_TEMPLATE_FILE = ['uORBTopics.hpp.em', 'uORBTopics.cpp.em', None, None]
 INCL_DEFAULT = ['std_msgs:./msg/std_msgs']
 PACKAGE = 'px4'
 TOPICS_TOKEN = '# TOPICS '
@@ -105,7 +104,7 @@ def get_topics(filename):
     return result
 
 
-def generate_output_from_file(format_idx, filename, outputdir, package, templatedir, includepath):
+def generate_output_from_file(format_idx, filename, outputdir, package, templatedir, includepath, all_topics):
     """
     Converts a single .msg file to an uorb header/source file
     """
@@ -150,10 +149,12 @@ def generate_output_from_file(format_idx, filename, outputdir, package, template
     em_globals = {
         "name_snake_case": full_type_name_snake,
         "file_name_in": filename,
+        "file_base_name": file_base_name,
         "search_path": search_path,
         "msg_context": msg_context,
         "spec": spec,
         "topics": topics,
+        "all_topics": all_topics,
     }
 
     # Make sure output directory exists:
@@ -161,7 +162,11 @@ def generate_output_from_file(format_idx, filename, outputdir, package, template
         os.makedirs(outputdir)
 
     template_file = os.path.join(templatedir, TEMPLATE_FILE[format_idx])
-    output_file = os.path.join(outputdir, full_type_name_snake + OUTPUT_FILE_EXT[format_idx])
+    extension = os.path.splitext(os.path.splitext(TEMPLATE_FILE[format_idx])[0])[1]
+    if format_idx == 2:
+        output_file = os.path.join(outputdir, file_base_name + extension)
+    else:
+        output_file = os.path.join(outputdir, full_type_name_snake + extension)
 
     return generate_by_template(output_file, template_file, em_globals)
 
@@ -191,17 +196,13 @@ def generate_by_template(output_file, template_file, em_globals):
     return True
 
 
-def generate_topics_list_file_from_files(files, outputdir, template_filename, templatedir):
+def generate_topics_list_file_from_files(files, outputdir, template_filename, templatedir, all_topics):
     # generate cpp file with topics list
     filenames = []
     for filename in [os.path.basename(p) for p in files if os.path.basename(p).endswith(".msg")]:
         filenames.append(re.sub(r'(?<!^)(?=[A-Z])', '_', filename).lower())
 
-    topics = []
-    for msg_filename in files:
-        topics.extend(get_topics(msg_filename))
-
-    tl_globals = {"msgs": filenames, "topics": topics}
+    tl_globals = {"msgs": filenames, "all_topics": all_topics}
     tl_template_file = os.path.join(templatedir, template_filename)
     tl_out_file = os.path.join(outputdir, template_filename.replace(".em", ""))
 
@@ -217,8 +218,10 @@ if __name__ == "__main__":
     parser = argparse.ArgumentParser(description='Convert msg files to uorb headers/sources')
     parser.add_argument('--headers', help='Generate header files', action='store_true')
     parser.add_argument('--sources', help='Generate source files', action='store_true')
+    parser.add_argument('--uorb-idl-header', help='Generate uORB compatible idl header', action='store_true')
+    parser.add_argument('--json', help='Generate json files', action='store_true')
     parser.add_argument('-f', dest='file',
-                        help="files to convert (use only without -d)",
+                        help="files to convert",
                         nargs="+")
     parser.add_argument('-i', dest="include_paths",
                         help='Additional Include Paths', nargs="*",
@@ -241,13 +244,22 @@ if __name__ == "__main__":
         generate_idx = 0
     elif args.sources:
         generate_idx = 1
+    elif args.uorb_idl_header:
+        generate_idx = 2
+    elif args.json:
+        generate_idx = 3
     else:
-        print('Error: either --headers or --sources must be specified')
+        print('Error: either --headers, --sources or --json must be specified')
         exit(-1)
     if args.file is not None:
+        all_topics = []
+        for msg_filename in args.file:
+            all_topics.extend(get_topics(msg_filename))
+        all_topics.sort()
+
         for f in args.file:
-            generate_output_from_file(generate_idx, f, args.outputdir, args.package, args.templatedir, INCL_DEFAULT)
+            generate_output_from_file(generate_idx, f, args.outputdir, args.package, args.templatedir, INCL_DEFAULT, all_topics)
 
         # Generate topics list header and source file
-        if os.path.isfile(os.path.join(args.templatedir, TOPICS_LIST_TEMPLATE_FILE[generate_idx])):
-            generate_topics_list_file_from_files(args.file, args.outputdir, TOPICS_LIST_TEMPLATE_FILE[generate_idx], args.templatedir)
+        if TOPICS_LIST_TEMPLATE_FILE[generate_idx] is not None and os.path.isfile(os.path.join(args.templatedir, TOPICS_LIST_TEMPLATE_FILE[generate_idx])):
+            generate_topics_list_file_from_files(args.file, args.outputdir, TOPICS_LIST_TEMPLATE_FILE[generate_idx], args.templatedir, all_topics)

Tools/msg/px_generate_uorb_topic_helper.py

@@ -171,6 +171,61 @@ def get_children_fields(base_type, search_path):
     return spec_temp.parsed_fields()
 
 
+def get_message_fields_str_for_message_hash(msg_fields, search_path):
+    """
+    Get all fields (including for nested types) in the form of:
+'''
+uint64 timestamp
+uint8 esc_count
+uint8 esc_online_flags
+EscReport[8] esc
+uint64 timestamp
+uint32 esc_errorcount
+int32 esc_rpm
+float32 esc_voltage
+uint16 failures
+int8 esc_power
+'''
+    """
+    all_fields_str = ''
+    for field in msg_fields:
+        if field.is_header:
+            continue
+
+        type_name = field.type
+        # detect embedded types
+        sl_pos = type_name.find('/')
+        if sl_pos >= 0:
+            type_name = type_name[sl_pos + 1:]
+
+        all_fields_str += type_name + ' ' + field.name + '\n'
+
+        if sl_pos >= 0: # nested type, add all nested fields
+            children_fields = get_children_fields(field.base_type, search_path)
+            all_fields_str += get_message_fields_str_for_message_hash(children_fields, search_path)
+
+    return all_fields_str
+
+
+def hash_32_fnv1a(data: str):
+    hash_val = 0x811c9dc5
+    prime = 0x1000193
+    for i in range(len(data)):
+        value = ord(data[i])
+        hash_val = hash_val ^ value
+        hash_val *= prime
+        hash_val &= 0xffffffff
+    return hash_val
+
+
+def get_message_hash(msg_fields, search_path):
+    """
+    Get a 32 bit message hash over all fields
+    """
+    all_fields_str = get_message_fields_str_for_message_hash(msg_fields, search_path)
+    return hash_32_fnv1a(all_fields_str)
+
+
 def add_padding_bytes(fields, search_path):
     """
     Add padding fields before the embedded types, at the end and calculate the

Tools/msg/templates/cdrstream/uorb_idl_header.h.em

@@ -0,0 +1,65 @@
+@{
+import genmsg.msgs
+import re
+
+from px_generate_uorb_topic_helper import * # this is in Tools/
+
+uorb_struct = '%s_s'%name_snake_case
+uorb_struct_upper = name_snake_case.upper()
+}@
+
+/****************************************************************
+
+  PX4 Cyclone DDS IDL to C Translator compatible idl struct
+  Source:  @file_name_in
+  Compatible with Cyclone DDS: V0.11.0
+
+*****************************************************************/
+#ifndef DDSC_IDL_UORB_@(uorb_struct_upper)_H
+#define DDSC_IDL_UORB_@(uorb_struct_upper)_H
+
+#include "dds/ddsc/dds_public_impl.h"
+#include "dds/cdr/dds_cdrstream.h"
+#include <uORB/topics/@(name_snake_case).h>
+
+@##############################
+@# Includes for dependencies
+@##############################
+@{
+for field in spec.parsed_fields():
+    if (not field.is_builtin):
+        if (not field.is_header):
+            (package, name) = genmsg.names.package_resource_name(field.base_type)
+            package = package or spec.package # convert '' to package
+
+            print('#include "%s.h"'%(name))
+            name = re.sub(r'(?<!^)(?=[A-Z])', '_', name).lower()
+            print('#include <uORB/topics/%s.h>'%(name))
+}@
+
+
+#ifdef __cplusplus
+extern "C" {
+#endif
+
+@{
+for field in spec.parsed_fields():
+    if (not field.is_builtin):
+        if (not field.is_header):
+            (package, name) = genmsg.names.package_resource_name(field.base_type)
+            package = package or spec.package # convert '' to package
+
+            print('typedef px4_msg_%s px4_msg_px4__msg__%s;' % (name,name))
+}@
+
+
+
+typedef struct @uorb_struct px4_msg_@(file_base_name);
+
+extern const struct dds_cdrstream_desc px4_msg_@(file_base_name)_cdrstream_desc;
+
+#ifdef __cplusplus
+}
+#endif
+
+#endif /* DDSC_IDL_UORB_@(uorb_struct_upper)_H */

Tools/msg/templates/uorb/msg.cpp.em

@@ -14,6 +14,7 @@
 @#  - spec (msggen.MsgSpec) Parsed specification of the .msg file
 @#  - search_path (dict) search paths for genmsg
 @#  - topics (List of String) topic names
+@#  - all_topics (List of String) all generated topic names (sorted)
 @###############################################
 /****************************************************************************
  *
@@ -57,9 +58,9 @@ from px_generate_uorb_topic_helper import * # this is in Tools/
 
 uorb_struct = '%s_s'%name_snake_case
 
+message_hash = get_message_hash(spec.parsed_fields(), search_path)
 sorted_fields = sorted(spec.parsed_fields(), key=sizeof_field_type, reverse=True)
 struct_size, padding_end_size = add_padding_bytes(sorted_fields, search_path)
-topic_fields = ["%s %s" % (convert_type(field.type, True), field.name) for field in sorted_fields]
 }@
 
 #include <inttypes.h>
@@ -72,12 +73,9 @@ topic_fields = ["%s %s" % (convert_type(field.type, True), field.name) for field
 #include <lib/matrix/matrix/math.hpp>
 #include <lib/mathlib/mathlib.h>
 
-@# join all msg files in one line e.g: "float[3] position;float[3] velocity;bool armed"
-@# This is used for the logger
-constexpr char __orb_@(name_snake_case)_fields[] = "@( ";".join(topic_fields) );";
-
 @[for topic in topics]@
-ORB_DEFINE(@topic, struct @uorb_struct, @(struct_size-padding_end_size), __orb_@(name_snake_case)_fields, static_cast<orb_id_size_t>(ORB_ID::@topic));
+static_assert(static_cast<orb_id_size_t>(ORB_ID::@topic) == @(all_topics.index(topic)), "ORB_ID index mismatch");
+ORB_DEFINE(@topic, struct @uorb_struct, @(struct_size-padding_end_size), @(message_hash)u, static_cast<orb_id_size_t>(ORB_ID::@topic));
 @[end for]
 
 void print_message(const orb_metadata *meta, const @uorb_struct& message)

Tools/msg/templates/uorb/msg.json.em

@@ -0,0 +1,49 @@
+@###############################################
+@#
+@# PX4 ROS compatible message source code
+@# generation for C++
+@#
+@# EmPy template for generating <msg>.h files
+@# Based on the original template for ROS
+@#
+@###############################################
+@# Start of Template
+@#
+@# Context:
+@#  - file_name_in (String) Source file
+@#  - spec (msggen.MsgSpec) Parsed specification of the .msg file
+@#  - search_path (dict) search paths for genmsg
+@#  - topics (List of String) topic names
+@#  - all_topics (List of String) all generated topic names (sorted)
+@###############################################
+
+@{
+import genmsg.msgs
+import json
+
+from px_generate_uorb_topic_helper import * # this is in Tools/
+
+uorb_struct = '%s_s'%name_snake_case
+
+sorted_fields = sorted(spec.parsed_fields(), key=sizeof_field_type, reverse=True)
+struct_size, padding_end_size = add_padding_bytes(sorted_fields, search_path)
+topic_fields = ["%s %s" % (convert_type(field.type, True), field.name) for field in sorted_fields]
+
+dependencies = []
+for field in spec.parsed_fields():
+    if not field.is_header:
+        type_name = field.type
+        # detect embedded types
+        sl_pos = type_name.find('/')
+        if sl_pos >= 0: # nested type
+            dependencies.append(field.base_type)
+}@
+
+{
+@# join all msg files in one line e.g: "float[3] position;float[3] velocity;bool armed;"
+		"fields": @( json.dumps(bytearray(";".join(topic_fields)+";", 'utf-8').decode('unicode_escape')) ),
+		"orb_ids": @( json.dumps([ all_topics.index(topic) for topic in topics]) ),
+		"main_orb_id": @( all_topics.index(name_snake_case) if name_snake_case in all_topics else -1 ),
+		"dependencies": @( json.dumps(list(set(dependencies))) ),
+		"name": "@( spec.full_name )"
+}

Tools/msg/templates/uorb/uORBTopics.cpp.em

@@ -8,7 +8,7 @@
 @#
 @# Context:
 @#  - msgs (List) list of all msg files
-@#  - multi_topics (List) list of all multi-topic names
+@#  - all_topics (List) list of all topic names (sorted)
 @###############################################
 /****************************************************************************
  *
@@ -50,9 +50,7 @@ msg_names = list(set([mn.replace(".msg", "") for mn in msgs])) # set() filters d
 msg_names.sort()
 msgs_count = len(msg_names)
 
-topic_names = list(set(topics)) # set() filters duplicates
-topic_names.sort()
-topics_count = len(topics)
+topics_count = len(all_topics)
 
 }@
 @[for msg_name in msg_names]@
@@ -60,8 +58,8 @@ topics_count = len(topics)
 @[end for]
 
 const constexpr struct orb_metadata *const uorb_topics_list[ORB_TOPICS_COUNT] = {
-@[for idx, topic_name in enumerate(topic_names, 1)]@
-	ORB_ID(@(topic_name))@[if idx != topic_names], @[end if]
+@[for idx, topic_name in enumerate(all_topics, 1)]@
+	ORB_ID(@(topic_name))@[if idx != all_topics], @[end if]
 @[end for]
 };
 

Tools/msg/templates/uorb/uORBTopics.hpp.em

@@ -7,7 +7,8 @@
 @# Start of Template
 @#
 @# Context:
-@#  - topics (List) list of all topic names
+@#  - msgs (List) list of all msg files
+@#  - all_topics (List) list of all topic names (sorted)
 @###############################################
 /****************************************************************************
  *
@@ -43,9 +44,7 @@
  ****************************************************************************/
 
 @{
-topics_count = len(topics)
-topic_names_all = list(set(topics)) # set() filters duplicates
-topic_names_all.sort()
+topics_count = len(all_topics)
 }@
 
 #pragma once
@@ -63,7 +62,7 @@ static constexpr size_t orb_topics_count() { return ORB_TOPICS_COUNT; }
 extern const struct orb_metadata *const *orb_get_topics() __EXPORT;
 
 enum class ORB_ID : orb_id_size_t {
-@[for idx, topic_name in enumerate(topic_names_all)]@
+@[for idx, topic_name in enumerate(all_topics)]@
 	@(topic_name) = @(idx),
 @[end for]
 	INVALID

Tools/px4events/srcparser.py

@@ -250,6 +250,22 @@ class SourceParser(object):
                             event.group = "arming_check"
                         event.prepend_arguments([('navigation_mode_group_t', 'modes'),
                                 ('uint8_t', 'health_component_index')])
+                    elif call in ['reporter.healthFailureExt', 'reporter.armingCheckFailureExt']: # from ROS2
+                        assert len(args_split) == num_args + 3, \
+                            "Unexpected Number of arguments for: {:}, {:}".format(args_split, num_args)
+                        m = self.re_event_id.search(args_split[0])
+                        if m:
+                            _, event_name = m.group(1, 2)
+                        else:
+                            raise Exception("Could not extract event ID from {:}".format(args_split[0]))
+                        event.name = event_name
+                        event.message = args_split[2][1:-1]
+                        if 'health' in call:
+                            event.group = "health"
+                        else:
+                            event.group = "arming_check"
+                        event.prepend_arguments([('navigation_mode_group_t', 'modes'),
+                                ('uint8_t', 'health_component_index')])
                     else:
                         raise Exception("unknown event method call: {}, args: {}".format(call, args))
 

Tools/setup/ubuntu.sh

@@ -165,7 +165,7 @@ if [[ $INSTALL_NUTTX == "true" ]]; then
 	source $HOME/.profile # load changed path for the case the script is reran before relogin
 	if [ $(which arm-none-eabi-gcc) ]; then
 		GCC_VER_STR=$(arm-none-eabi-gcc --version)
-		GCC_FOUND_VER=$(echo $GCC_VER_STR | grep -c "${NUTTX_GCC_VERSION}")
+		GCC_FOUND_VER=$(echo $GCC_VER_STR | grep -c "${NUTTX_GCC_VERSION}" || true)
 	fi
 
 	if [[ "$GCC_FOUND_VER" == "1" ]]; then

Tools/simulation/gz/models/advanced_plane/model.config

@@ -0,0 +1,14 @@
+<?xml version="1.0"?>
+<model>
+  <name>Advanced Plane</name>
+  <version>1.0</version>
+  <sdf version='1.5'>model.sdf</sdf>
+
+  <author>
+   <name>Karthik Srivatsan</name>
+  </author>
+
+  <description>
+    This is a model of a standard plane, which uses the advanced liftdrag plugin.
+  </description>
+</model>

Tools/simulation/gz/models/advanced_plane/model.sdf

@@ -0,0 +1,578 @@
+<?xml version="1.0"?>
+<sdf version='1.5'>
+  <model name='plane'>
+    <pose>0 0 0.246 0 0 0</pose>
+    <link name='base_link'>
+      <pose>0 0 0 0 0 0</pose>
+      <inertial>
+        <pose>0 0 0 0 0 0</pose>
+        <mass>1</mass>
+        <inertia>
+          <ixx>0.197563</ixx>
+          <ixy>0</ixy>
+          <ixz>0</ixz>
+          <iyy>0.1458929</iyy>
+          <iyz>0</iyz>
+          <izz>0.1477</izz>
+        </inertia>
+      </inertial>
+      <collision name='base_link_collision'>
+        <pose>0 0 -0.07 0 0 0</pose>
+        <geometry>
+          <box>
+            <size>0.47 0.47 0.11</size>
+          </box>
+        </geometry>
+        <surface>
+          <contact>
+            <ode>
+              <max_vel>10</max_vel>
+              <min_depth>0.01</min_depth>
+            </ode>
+          </contact>
+          <friction>
+            <ode/>
+          </friction>
+        </surface>
+      </collision>
+      <visual name='base_link_visual'>
+        <pose>0.07 0 -0.08 0 0 0</pose>
+        <geometry>
+          <mesh>
+            <scale>0.1 0.1 0.1</scale>
+            <uri>https://fuel.gazebosim.org/1.0/PX4/models/Advanced%20Plane/tip/files/meshes/body.dae</uri>
+          </mesh>
+        </geometry>
+        <material>
+	  <ambient>.175 .175 .175 1.0</ambient>
+          <diffuse>.175 .175 .175 1.0</diffuse>
+        </material>
+      </visual>
+      <gravity>1</gravity>
+      <velocity_decay/>
+      <self_collide>0</self_collide>
+      <sensor name="imu_sensor" type="imu">
+        <always_on>1</always_on>
+        <update_rate>250</update_rate>
+      </sensor>
+      <sensor name="air_pressure_sensor" type="air_pressure">
+        <always_on>1</always_on>
+        <update_rate>50</update_rate>
+        <air_pressure>
+          <pressure>
+            <noise type="gaussian">
+              <mean>0</mean>
+              <stddev>0.01</stddev>
+            </noise>
+          </pressure>
+        </air_pressure>
+      </sensor>
+    </link>
+    <link name='rotor_puller'>
+      <pose>0.3 0 0.0 0 1.57 0</pose>
+      <inertial>
+        <pose>0 0 0 0 0 0</pose>
+        <mass>0.005</mass>
+        <inertia>
+          <ixx>9.75e-07</ixx>
+          <ixy>0</ixy>
+          <ixz>0</ixz>
+          <iyy>0.000166704</iyy>
+          <iyz>0</iyz>
+          <izz>0.000167604</izz>
+        </inertia>
+      </inertial>
+      <collision name='rotor_puller_collision'>
+        <pose>0.0 0 0.0 0 0 0</pose>
+        <geometry>
+          <cylinder>
+            <length>0.005</length>
+            <radius>0.1</radius>
+          </cylinder>
+        </geometry>
+        <surface>
+          <contact>
+            <ode/>
+          </contact>
+          <friction>
+            <ode/>
+          </friction>
+        </surface>
+      </collision>
+      <visual name='rotor_puller_visual'>
+        <pose>0 0 -0.09 0 0 0</pose>
+        <geometry>
+          <mesh>
+            <scale>1 1 1</scale>
+            <uri>https://fuel.gazebosim.org/1.0/PX4/models/Advanced%20Plane/tip/files/meshes/iris_prop_ccw.dae</uri>
+          </mesh>
+        </geometry>
+        <material>
+	  <ambient>.175 .175 .175 1.0</ambient>
+          <diffuse>.175 .175 .175 1.0</diffuse>
+        </material>
+      </visual>
+      <gravity>1</gravity>
+      <velocity_decay/>
+      <self_collide>0</self_collide>
+    </link>
+    <joint name='rotor_puller_joint' type='revolute'>
+      <child>rotor_puller</child>
+      <parent>base_link</parent>
+      <axis>
+        <xyz>1 0 0</xyz>
+        <limit>
+          <lower>-1e+16</lower>
+          <upper>1e+16</upper>
+        </limit>
+        <dynamics>
+          <spring_reference>0</spring_reference>
+          <spring_stiffness>0</spring_stiffness>
+        </dynamics>
+        <use_parent_model_frame>1</use_parent_model_frame>
+      </axis>
+    </joint>
+
+    <link name="left_elevon">
+      <inertial>
+        <mass>0.00000001</mass>
+        <inertia>
+          <ixx>0.000001</ixx>
+          <ixy>0.0</ixy>
+          <iyy>0.000001</iyy>
+          <ixz>0.0</ixz>
+          <iyz>0.0</iyz>
+          <izz>0.000001</izz>
+        </inertia>
+        <pose>0 0.3 0 0.00 0 0.0</pose>
+      </inertial>
+      <visual name='left_elevon_visual'>
+        <pose>0.07 0.0 -0.08 0.00 0 0.0</pose>
+        <geometry>
+          <mesh>
+            <scale>0.1 0.1 0.1</scale>
+            <uri>https://fuel.gazebosim.org/1.0/PX4/models/Advanced%20Plane/tip/files/meshes/left_aileron.dae</uri>
+          </mesh>
+        </geometry>
+        <material>
+          <ambient>1 0 0 1.0</ambient>
+          <diffuse>1 0 0 1.0</diffuse>
+        </material>
+      </visual>
+    </link>
+    <link name="right_elevon">
+      <inertial>
+        <mass>0.00000001</mass>
+        <inertia>
+          <ixx>0.000001</ixx>
+          <ixy>0.0</ixy>
+          <iyy>0.000001</iyy>
+          <ixz>0.0</ixz>
+          <iyz>0.0</iyz>
+          <izz>0.000001</izz>
+        </inertia>
+        <pose>0 -0.3 0 0.00 0 0.0</pose>
+      </inertial>
+      <visual name='right_elevon_visual'>
+        <pose>0.07 0.0 -0.08 0.00 0 0.0</pose>
+        <geometry>
+          <mesh>
+            <scale>0.1 0.1 0.1</scale>
+            <uri>https://fuel.gazebosim.org/1.0/PX4/models/Advanced%20Plane/tip/files/meshes/right_aileron.dae</uri>
+          </mesh>
+        </geometry>
+        <material>
+          <ambient>1 0 0 1.0</ambient>
+          <diffuse>1 0 0 1.0</diffuse>
+        </material>
+      </visual>
+    </link>
+    <link name="left_flap">
+      <inertial>
+        <mass>0.00000001</mass>
+        <inertia>
+          <ixx>0.000001</ixx>
+          <ixy>0.0</ixy>
+          <iyy>0.000001</iyy>
+          <ixz>0.0</ixz>
+          <iyz>0.0</iyz>
+          <izz>0.000001</izz>
+        </inertia>
+        <pose>0 0.15 0 0.00 0 0.0</pose>
+      </inertial>
+      <visual name='left_flap_visual'>
+        <pose>0.07 0.0 -0.08 0.00 0 0.0</pose>
+        <geometry>
+          <mesh>
+            <scale>0.1 0.1 0.1</scale>
+            <uri>https://fuel.gazebosim.org/1.0/PX4/models/Advanced%20Plane/tip/files/meshes/left_flap.dae</uri>
+          </mesh>
+        </geometry>
+        <material>
+          <ambient>1 0 0 1.0</ambient>
+          <diffuse>1 0 0 1.0</diffuse>
+        </material>
+      </visual>
+    </link>
+    <link name="right_flap">
+      <inertial>
+        <mass>0.00000001</mass>
+        <inertia>
+          <ixx>0.000001</ixx>
+          <ixy>0.0</ixy>
+          <iyy>0.000001</iyy>
+          <ixz>0.0</ixz>
+          <iyz>0.0</iyz>
+          <izz>0.000001</izz>
+        </inertia>
+        <pose>0 -0.15 0 0.00 0 0.0</pose>
+      </inertial>
+      <visual name='right_flap_visual'>
+        <pose>0.07 0.0 -0.08 0.00 0 0.0</pose>
+        <geometry>
+          <mesh>
+            <scale>0.1 0.1 0.1</scale>
+            <uri>https://fuel.gazebosim.org/1.0/PX4/models/Advanced%20Plane/tip/files/meshes/right_flap.dae</uri>
+          </mesh>
+        </geometry>
+        <material>
+          <ambient>1 0 0 1.0</ambient>
+          <diffuse>1 0 0 1.0</diffuse>
+        </material>
+      </visual>
+    </link>
+    <link name="elevator">
+      <inertial>
+        <mass>0.00000001</mass>
+        <inertia>
+          <ixx>0.000001</ixx>
+          <ixy>0.0</ixy>
+          <iyy>0.000001</iyy>
+          <ixz>0.0</ixz>
+          <iyz>0.0</iyz>
+          <izz>0.000001</izz>
+        </inertia>
+        <pose> -0.5 0 0 0.00 0 0.0</pose>
+      </inertial>
+      <visual name='elevator_visual'>
+        <pose>0.07 0.0 -0.08 0.00 0 0.0</pose>
+        <geometry>
+          <mesh>
+            <scale>0.1 0.1 0.1</scale>
+            <uri>https://fuel.gazebosim.org/1.0/PX4/models/Advanced%20Plane/tip/files/meshes/elevators.dae</uri>
+          </mesh>
+        </geometry>
+        <material>
+          <ambient>1 0 0 1.0</ambient>
+          <diffuse>1 0 0 1.0</diffuse>
+        </material>
+      </visual>
+    </link>
+    <link name="rudder">
+      <inertial>
+        <mass>0.00000001</mass>
+        <inertia>
+          <ixx>0.000001</ixx>
+          <ixy>0.0</ixy>
+          <iyy>0.000001</iyy>
+          <ixz>0.0</ixz>
+          <iyz>0.0</iyz>
+          <izz>0.000001</izz>
+        </inertia>
+        <pose>-0.5 0 0.05 0 0 0 </pose>
+      </inertial>
+      <visual name='rudder_visual'>
+        <pose>0.07 0.0 -0.08 0.00 0 0.0</pose>
+        <geometry>
+          <mesh>
+            <scale>0.1 0.1 0.1</scale>
+            <uri>https://fuel.gazebosim.org/1.0/PX4/models/Advanced%20Plane/tip/files/meshes/rudder.dae</uri>
+          </mesh>
+        </geometry>
+        <material>
+          <ambient>1 0 0 1.0</ambient>
+          <diffuse>1 0 0 1.0</diffuse>
+        </material>
+      </visual>
+    </link>
+    <joint name='servo_0' type='revolute'>
+      <parent>base_link</parent>
+      <child>left_elevon</child>
+      <pose>-0.07 0.4 0.08 0.00 0 0.0</pose>
+      <axis>
+        <xyz>0 1 0</xyz>
+        <limit>
+          <!-- -30/+30 deg. -->
+          <lower>-0.53</lower>
+          <upper>0.53</upper>
+        </limit>
+        <dynamics>
+          <damping>1.000</damping>
+        </dynamics>
+      </axis>
+      <physics>
+        <ode>
+          <implicit_spring_damper>1</implicit_spring_damper>
+        </ode>
+      </physics>
+    </joint>
+    <plugin
+      filename="gz-sim-joint-position-controller-system" name="gz::sim::systems::JointPositionController">
+      <joint_name>servo_0</joint_name>
+      <sub_topic>servo_0</sub_topic>
+      <p_gain>10</p_gain>
+      <i_gain>0</i_gain>
+      <d_gain>0</d_gain>
+    </plugin>
+    <joint name='servo_1' type='revolute'>
+      <parent>base_link</parent>
+      <child>right_elevon</child>
+      <pose>-0.07 -0.4 0.08 0.00 0 0.0</pose>
+      <axis>
+        <xyz>0 1 0</xyz>
+        <limit>
+          <!-- -30/+30 deg. -->
+          <lower>-0.53</lower>
+          <upper>0.53</upper>
+        </limit>
+        <dynamics>
+          <damping>1.000</damping>
+        </dynamics>
+      </axis>
+      <physics>
+        <ode>
+          <implicit_spring_damper>1</implicit_spring_damper>
+        </ode>
+      </physics>
+    </joint>
+    <plugin
+      filename="gz-sim-joint-position-controller-system" name="gz::sim::systems::JointPositionController">
+      <joint_name>servo_1</joint_name>
+      <sub_topic>servo_1</sub_topic>
+      <p_gain>10</p_gain>
+      <i_gain>0</i_gain>
+      <d_gain>0</d_gain>
+    </plugin>
+    <joint name='servo_4' type='revolute'>
+      <parent>base_link</parent>
+      <child>left_flap</child>
+      <pose>-0.07 0.2 0.08 0.00 0 0.0</pose>
+      <axis>
+        <xyz>0 1 0</xyz>
+        <limit>
+          <!-- -30/+30 deg. -->
+          <lower>-0.53</lower>
+          <upper>0.53</upper>
+        </limit>
+        <dynamics>
+          <damping>1.000</damping>
+        </dynamics>
+      </axis>
+      <physics>
+        <ode>
+          <implicit_spring_damper>1</implicit_spring_damper>
+        </ode>
+      </physics>
+    </joint>
+    <plugin
+      filename="gz-sim-joint-position-controller-system" name="gz::sim::systems::JointPositionController">
+      <joint_name>servo_4</joint_name>
+      <sub_topic>servo_4</sub_topic>
+      <p_gain>10</p_gain>
+      <i_gain>0</i_gain>
+      <d_gain>0</d_gain>
+    </plugin>
+    <joint name='servo_5' type='revolute'>
+      <parent>base_link</parent>
+      <child>right_flap</child>
+      <pose>-0.07 -0.2 0.08 0.00 0 0.0</pose>
+      <axis>
+        <xyz>0 1 0</xyz>
+        <limit>
+          <!-- -30/+30 deg. -->
+          <lower>-0.53</lower>
+          <upper>0.53</upper>
+        </limit>
+        <dynamics>
+          <damping>1.000</damping>
+        </dynamics>
+      </axis>
+      <physics>
+        <ode>
+          <implicit_spring_damper>1</implicit_spring_damper>
+        </ode>
+      </physics>
+    </joint>
+    <plugin
+      filename="gz-sim-joint-position-controller-system" name="gz::sim::systems::JointPositionController">
+      <joint_name>servo_5</joint_name>
+      <sub_topic>servo_5</sub_topic>
+      <p_gain>10</p_gain>
+      <i_gain>0</i_gain>
+      <d_gain>0</d_gain>
+    </plugin>
+    <joint name='servo_2' type='revolute'>
+      <parent>base_link</parent>
+      <child>elevator</child>
+      <pose> -0.5 0 0 0 0 0</pose>
+      <axis>
+        <xyz>0 1 0</xyz>
+        <limit>
+          <!-- -30/+30 deg. -->
+          <lower>-0.53</lower>
+          <upper>0.53</upper>
+        </limit>
+        <dynamics>
+          <damping>1.000</damping>
+        </dynamics>
+      </axis>
+      <physics>
+        <ode>
+          <implicit_spring_damper>1</implicit_spring_damper>
+        </ode>
+      </physics>
+    </joint>
+    <plugin
+      filename="gz-sim-joint-position-controller-system" name="gz::sim::systems::JointPositionController">
+      <joint_name>servo_2</joint_name>
+      <sub_topic>servo_2</sub_topic>
+      <p_gain>10</p_gain>
+      <i_gain>0</i_gain>
+      <d_gain>0</d_gain>
+    </plugin>
+    <joint name='servo_3' type='revolute'>
+      <parent>base_link</parent>
+      <child>rudder</child>
+      <pose>-0.5 0 0.05 0.00 0 0.0</pose>
+      <axis>
+        <xyz>0 0 1</xyz>
+        <limit>
+          <!-- -30/+30 deg. -->
+          <lower>-0.53</lower>
+          <upper>0.53</upper>
+        </limit>
+        <dynamics>
+          <damping>1.000</damping>
+        </dynamics>
+      </axis>
+      <physics>
+        <ode>
+          <implicit_spring_damper>1</implicit_spring_damper>
+        </ode>
+      </physics>
+    </joint>
+    <plugin
+      filename="gz-sim-joint-position-controller-system" name="gz::sim::systems::JointPositionController">
+      <joint_name>servo_3</joint_name>
+      <sub_topic>servo_3</sub_topic>
+      <p_gain>10</p_gain>
+      <i_gain>0</i_gain>
+      <d_gain>0</d_gain>
+    </plugin>
+
+    <plugin filename="gz-sim-advanced-lift-drag-system" name="gz::sim::systems::AdvancedLiftDrag">
+      <a0>0.0</a0>
+      <CL0>0.15188</CL0>
+      <AR>6.5</AR>
+      <eff>0.97</eff>
+      <CLa>5.015</CLa>
+      <CD0>0.029</CD0>
+      <Cem0>0.075</Cem0>
+      <Cema>-0.463966</Cema>
+      <CYb>-0.258244</CYb>
+      <Cellb>-0.039250</Cellb>
+      <Cenb>0.100826</Cenb>
+      <CDp>0.0</CDp>
+      <CYp>0.065861</CYp>
+      <CLp>0.0</CLp>
+      <Cellp>-0.487407</Cellp>
+      <Cemp>0.0</Cemp>
+      <Cenp>-0.040416</Cenp>
+      <CDq>0.055166</CDq>
+      <CYq>0.0</CYq>
+      <CLq>7.971792</CLq>
+      <Cellq>0.0</Cellq>
+      <Cemq>-12.140140</Cemq>
+      <Cenq>0.0</Cenq>
+      <CDr>0.0</CDr>
+      <CYr>0.230299</CYr>
+      <CLr>0.0</CLr>
+      <Cellr>0.078165</Cellr>
+      <Cemr>0.0</Cemr>
+      <Cenr>-0.089947</Cenr>
+      <alpha_stall>0.3391428111</alpha_stall>
+      <CLa_stall>-3.85</CLa_stall>
+      <CDa_stall>-0.9233984055</CDa_stall>
+      <Cema_stall>0</Cema_stall>
+      <cp>-0.12 0.0 0.0</cp>
+      <area>0.34</area>
+      <mac>0.22</mac>
+      <air_density>1.2041</air_density>
+      <forward>1 0 0</forward>
+      <upward>0 0 1</upward>
+      <link_name>base_link</link_name>
+      <num_ctrl_surfaces>4</num_ctrl_surfaces>
+      <control_surface>
+        <name>servo_0</name>
+        <index>0</index>
+        <direction>1</direction>
+        <CD_ctrl>-0.000059</CD_ctrl>
+        <CY_ctrl>0.000171</CY_ctrl>
+        <CL_ctrl>-0.011940</CL_ctrl>
+        <Cell_ctrl>-0.003331</Cell_ctrl>
+        <Cem_ctrl>0.001498</Cem_ctrl>
+        <Cen_ctrl>-0.000057</Cen_ctrl>
+      </control_surface>
+      <control_surface>
+        <name>servo_1</name>
+        <direction>1</direction>
+        <index>1</index>
+        <CD_ctrl>-0.000059</CD_ctrl>
+        <CY_ctrl>-0.000171</CY_ctrl>
+        <CL_ctrl>-0.011940</CL_ctrl>
+        <Cell_ctrl>0.003331</Cell_ctrl>
+        <Cem_ctrl>0.001498</Cem_ctrl>
+        <Cen_ctrl>0.000057</Cen_ctrl>
+      </control_surface>
+      <control_surface>
+        <name>servo_2</name>
+        <direction>-1</direction>
+        <index>2</index>
+        <CD_ctrl>0.000274</CD_ctrl>
+        <CY_ctrl>0</CY_ctrl>
+        <CL_ctrl>0.010696</CL_ctrl>
+        <Cell_ctrl>0.0</Cell_ctrl>
+        <Cem_ctrl>-0.025798</Cem_ctrl>
+        <Cen_ctrl>0.0</Cen_ctrl>
+      </control_surface>
+      <control_surface>
+        <name>servo_3</name>
+        <direction>1</direction>
+        <index>3</index>
+        <CD_ctrl>0.0</CD_ctrl>
+        <CY_ctrl>-0.003913</CY_ctrl>
+        <CL_ctrl>0.0</CL_ctrl>
+        <Cell_ctrl>-0.000257</Cell_ctrl>
+        <Cem_ctrl>0.0</Cem_ctrl>
+        <Cen_ctrl>0.001613</Cen_ctrl>
+      </control_surface>
+    </plugin>
+    <plugin filename="gz-sim-multicopter-motor-model-system" name="gz::sim::systems::MulticopterMotorModel">
+      <jointName>rotor_puller_joint</jointName>
+      <linkName>rotor_puller</linkName>
+      <turningDirection>cw</turningDirection>
+      <timeConstantUp>0.0125</timeConstantUp>
+      <timeConstantDown>0.025</timeConstantDown>
+      <maxRotVelocity>3500</maxRotVelocity>
+      <motorConstant>8.54858e-06</motorConstant>
+      <momentConstant>0.01</momentConstant>
+      <commandSubTopic>command/motor_speed</commandSubTopic>
+      <motorNumber>0</motorNumber>
+      <rotorDragCoefficient>8.06428e-05</rotorDragCoefficient>
+      <rollingMomentCoefficient>1e-06</rollingMomentCoefficient>
+      <rotorVelocitySlowdownSim>10</rotorVelocitySlowdownSim>
+      <motorType>velocity</motorType>
+
+    </plugin>
+  </model>
+</sdf>

Tools/simulation/gz/models/omnicopter/model.sdf

@@ -2,7 +2,7 @@
 <sdf version='1.9'>
   <include>
       <name>omnicopter</name>
-      <pose>0 0 0 0 0 0</pose>
+      <pose>0 0 0.2 0 0 0</pose>
       <uri>https://fuel.gazebosim.org/1.0/PX4/models/Omnicopter</uri>
     </include>
 </sdf>

Tools/simulation/gz/models/rc_cessna/model.sdf

@@ -69,7 +69,7 @@
         <geometry>
           <mesh>
             <scale>0.1 0.1 0.1</scale>
-            <uri>model://rc_cessna/meshes/body.dae</uri>
+            <uri>https://fuel.gazebosim.org/1.0/PX4/models/RC%20Cessna/tip/files/meshes/body.dae</uri>
           </mesh>
         </geometry>
         <material>
@@ -184,7 +184,7 @@
         <geometry>
           <mesh>
             <scale>1 1 1</scale>
-            <uri>model://rc_cessna/meshes/iris_prop_ccw.dae</uri>
+            <uri>https://fuel.gazebosim.org/1.0/PX4/models/RC%20Cessna/tip/files/meshes/iris_prop_ccw.dae</uri>
           </mesh>
         </geometry>
         <material>
@@ -229,7 +229,7 @@
         <geometry>
           <mesh>
             <scale>0.1 0.1 0.1</scale>
-            <uri>model://rc_cessna/meshes/left_aileron.dae</uri>
+            <uri>https://fuel.gazebosim.org/1.0/PX4/models/RC%20Cessna/tip/files/meshes/left_aileron.dae</uri>
           </mesh>
         </geometry>
         <material>
@@ -385,7 +385,7 @@
         <geometry>
           <mesh>
             <scale>0.1 0.1 0.1</scale>
-            <uri>model://rc_cessna/meshes/right_aileron.dae</uri>
+            <uri>https://fuel.gazebosim.org/1.0/PX4/models/RC%20Cessna/tip/files/meshes/right_aileron.dae</uri>
           </mesh>
         </geometry>
         <material>
@@ -412,7 +412,7 @@
         <geometry>
           <mesh>
             <scale>0.1 0.1 0.1</scale>
-            <uri>model://rc_cessna/meshes/left_flap.dae</uri>
+            <uri>https://fuel.gazebosim.org/1.0/PX4/models/RC%20Cessna/tip/files/meshes/left_flap.dae</uri>
           </mesh>
         </geometry>
         <material>
@@ -439,7 +439,7 @@
         <geometry>
           <mesh>
             <scale>0.1 0.1 0.1</scale>
-            <uri>model://rc_cessna/meshes/right_flap.dae</uri>
+            <uri>https://fuel.gazebosim.org/1.0/PX4/models/RC%20Cessna/tip/files/meshes/right_flap.dae</uri>
           </mesh>
         </geometry>
         <material>
@@ -466,7 +466,7 @@
         <geometry>
           <mesh>
             <scale>0.1 0.1 0.1</scale>
-            <uri>model://rc_cessna/meshes/elevators.dae</uri>
+            <uri>https://fuel.gazebosim.org/1.0/PX4/models/RC%20Cessna/tip/files/meshes/elevators.dae</uri>
           </mesh>
         </geometry>
         <material>
@@ -493,7 +493,7 @@
         <geometry>
           <mesh>
             <scale>0.1 0.1 0.1</scale>
-            <uri>model://rc_cessna/meshes/rudder.dae</uri>
+            <uri>https://fuel.gazebosim.org/1.0/PX4/models/RC%20Cessna/tip/files/meshes/rudder.dae</uri>
           </mesh>
         </geometry>
         <material>

Tools/simulation/gz/models/standard_vtol/model.sdf

@@ -43,7 +43,7 @@
         <geometry>
           <mesh>
             <scale>0.001 0.001 0.001</scale>
-            <uri>model://standard_vtol/meshes/x8_wing.dae</uri>
+            <uri>https://fuel.gazebosim.org/1.0/PX4/models/Standard%20VTOL/tip/files/meshes/x8_wing.dae</uri>
           </mesh>
         </geometry>
         <material>
@@ -183,7 +183,7 @@
         <geometry>
           <mesh>
             <scale>1 1 1</scale>
-            <uri>model://standard_vtol/meshes/iris_prop_ccw.dae</uri>
+            <uri>https://fuel.gazebosim.org/1.0/PX4/models/Standard%20VTOL/tip/files/meshes/iris_prop_ccw.dae</uri>
           </mesh>
         </geometry>
         <material>
@@ -246,7 +246,7 @@
         <geometry>
           <mesh>
             <scale>1 1 1</scale>
-            <uri>model://standard_vtol/meshes/iris_prop_ccw.dae</uri>
+            <uri>https://fuel.gazebosim.org/1.0/PX4/models/Standard%20VTOL/tip/files/meshes/iris_prop_ccw.dae</uri>
           </mesh>
         </geometry>
         <material>
@@ -309,7 +309,7 @@
         <geometry>
           <mesh>
             <scale>1 1 1</scale>
-            <uri>model://standard_vtol/meshes/iris_prop_ccw.dae</uri>
+            <uri>https://fuel.gazebosim.org/1.0/PX4/models/Standard%20VTOL/tip/files/meshes/iris_prop_ccw.dae</uri>
           </mesh>
         </geometry>
         <material>
@@ -372,7 +372,7 @@
         <geometry>
           <mesh>
             <scale>1 1 1</scale>
-            <uri>model://standard_vtol/meshes/iris_prop_ccw.dae</uri>
+            <uri>https://fuel.gazebosim.org/1.0/PX4/models/Standard%20VTOL/tip/files/meshes/iris_prop_ccw.dae</uri>
           </mesh>
         </geometry>
         <material>
@@ -436,7 +436,7 @@
         <geometry>
           <mesh>
             <scale>0.8 0.8 0.8</scale>
-            <uri>model://standard_vtol/meshes/iris_prop_ccw.dae</uri>
+            <uri>https://fuel.gazebosim.org/1.0/PX4/models/Standard%20VTOL/tip/files/meshes/iris_prop_ccw.dae</uri>
           </mesh>
         </geometry>
         <material>
@@ -483,7 +483,7 @@
         <geometry>
           <mesh>
             <scale>0.001 0.001 0.001</scale>
-            <uri>model://standard_vtol/meshes/x8_elevon_left.dae</uri>
+            <uri>https://fuel.gazebosim.org/1.0/PX4/models/Standard%20VTOL/tip/files/meshes/x8_elevon_left.dae</uri>
           </mesh>
         </geometry>
         <material>
@@ -510,7 +510,7 @@
         <geometry>
           <mesh>
             <scale>0.001 0.001 0.001</scale>
-            <uri>model://standard_vtol/meshes/x8_elevon_right.dae</uri>
+            <uri>https://fuel.gazebosim.org/1.0/PX4/models/Standard%20VTOL/tip/files/meshes/x8_elevon_right.dae</uri>
           </mesh>
         </geometry>
         <material>

Tools/simulation/gz/models/x500/model.sdf

@@ -23,7 +23,7 @@
         <geometry>
           <mesh>
             <scale>1 1 1</scale>
-            <uri>model://x500/meshes/NXP-HGD-CF.dae</uri>
+            <uri>https://fuel.gazebosim.org/1.0/PX4/models/x500/1/files/meshes/NXP-HGD-CF.dae</uri>
           </mesh>
         </geometry>
       </visual>
@@ -32,7 +32,7 @@
         <geometry>
           <mesh>
             <scale>1 1 1</scale>
-            <uri>model://x500/meshes/5010Base.dae</uri>
+            <uri>https://fuel.gazebosim.org/1.0/PX4/models/x500/1/files/meshes/5010Base.dae</uri>
           </mesh>
         </geometry>
       </visual>
@@ -41,7 +41,7 @@
         <geometry>
           <mesh>
             <scale>1 1 1</scale>
-            <uri>model://x500/meshes/5010Base.dae</uri>
+            <uri>https://fuel.gazebosim.org/1.0/PX4/models/x500/1/files/meshes/5010Base.dae</uri>
           </mesh>
         </geometry>
       </visual>
@@ -50,7 +50,7 @@
         <geometry>
           <mesh>
             <scale>1 1 1</scale>
-            <uri>model://x500/meshes/5010Base.dae</uri>
+            <uri>https://fuel.gazebosim.org/1.0/PX4/models/x500/1/files/meshes/5010Base.dae</uri>
           </mesh>
         </geometry>
       </visual>
@@ -59,7 +59,7 @@
         <geometry>
           <mesh>
             <scale>1 1 1</scale>
-            <uri>model://x500/meshes/5010Base.dae</uri>
+            <uri>https://fuel.gazebosim.org/1.0/PX4/models/x500/1/files/meshes/5010Base.dae</uri>
           </mesh>
         </geometry>
       </visual>
@@ -77,7 +77,7 @@
           <specular>1.0 1.0 1.0</specular>
           <pbr>
             <metal>
-              <albedo_map>model://x500/materials/textures/nxp.png</albedo_map>
+              <albedo_map>https://fuel.gazebosim.org/1.0/PX4/models/x500/1/files/materials/textures/nxp.png</albedo_map>
             </metal>
           </pbr>
         </material>
@@ -96,7 +96,7 @@
           <specular>1.0 1.0 1.0</specular>
           <pbr>
             <metal>
-              <albedo_map>model://x500/materials/textures/nxp.png</albedo_map>
+              <albedo_map>https://fuel.gazebosim.org/1.0/PX4/models/x500/1/files/materials/textures/nxp.png</albedo_map>
             </metal>
           </pbr>
         </material>
@@ -115,7 +115,7 @@
           <specular>1.0 1.0 1.0</specular>
           <pbr>
             <metal>
-              <albedo_map>model://x500/materials/textures/rd.png</albedo_map>
+              <albedo_map>https://fuel.gazebosim.org/1.0/PX4/models/x500/1/files/materials/textures/rd.png</albedo_map>
             </metal>
           </pbr>
         </material>
@@ -250,7 +250,7 @@
         <geometry>
           <mesh>
             <scale>0.8461538461538461 0.8461538461538461 0.8461538461538461</scale>
-            <uri>model://x500/meshes/1345_prop_ccw.stl</uri>
+            <uri>https://fuel.gazebosim.org/1.0/PX4/models/x500/1/files/meshes/1345_prop_ccw.stl</uri>
           </mesh>
         </geometry>
         <material>
@@ -265,7 +265,7 @@
         <geometry>
           <mesh>
             <scale>1 1 1</scale>
-            <uri>model://x500/meshes/5010Bell.dae</uri>
+            <uri>https://fuel.gazebosim.org/1.0/PX4/models/x500/1/files/meshes/5010Bell.dae</uri>
           </mesh>
         </geometry>
       </visual>
@@ -322,7 +322,7 @@
         <geometry>
           <mesh>
             <scale>0.8461538461538461 0.8461538461538461 0.8461538461538461</scale>
-            <uri>model://x500/meshes/1345_prop_ccw.stl</uri>
+            <uri>https://fuel.gazebosim.org/1.0/PX4/models/x500/1/files/meshes/1345_prop_ccw.stl</uri>
           </mesh>
         </geometry>
         <material>
@@ -337,7 +337,7 @@
         <geometry>
           <mesh>
             <scale>1 1 1</scale>
-            <uri>model://x500/meshes/5010Bell.dae</uri>
+            <uri>https://fuel.gazebosim.org/1.0/PX4/models/x500/1/files/meshes/5010Bell.dae</uri>
           </mesh>
         </geometry>
       </visual>
@@ -394,7 +394,7 @@
         <geometry>
           <mesh>
             <scale>0.8461538461538461 0.8461538461538461 0.8461538461538461</scale>
-            <uri>model://x500/meshes/1345_prop_cw.stl</uri>
+            <uri>https://fuel.gazebosim.org/1.0/PX4/models/x500/1/files/meshes/1345_prop_cw.stl</uri>
           </mesh>
         </geometry>
         <material>
@@ -409,7 +409,7 @@
         <geometry>
           <mesh>
             <scale>1 1 1</scale>
-            <uri>model://x500/meshes/5010Bell.dae</uri>
+            <uri>https://fuel.gazebosim.org/1.0/PX4/models/x500/1/files/meshes/5010Bell.dae</uri>
           </mesh>
         </geometry>
       </visual>
@@ -466,7 +466,7 @@
         <geometry>
           <mesh>
             <scale>0.8461538461538461 0.8461538461538461 0.8461538461538461</scale>
-            <uri>model://x500/meshes/1345_prop_cw.stl</uri>
+            <uri>https://fuel.gazebosim.org/1.0/PX4/models/x500/1/files/meshes/1345_prop_cw.stl</uri>
           </mesh>
         </geometry>
         <material>
@@ -481,7 +481,7 @@
         <geometry>
           <mesh>
             <scale>1 1 1</scale>
-            <uri>model://x500/meshes/5010Bell.dae</uri>
+            <uri>https://fuel.gazebosim.org/1.0/PX4/models/x500/1/files/meshes/5010Bell.dae</uri>
           </mesh>
         </geometry>
       </visual>

Tools/simulation/gz/tools/avl_automation/README.md

@@ -0,0 +1,149 @@
+## Purpose
+
+The idea of this tool is to automate the writing of the Advanced Lift Drag plugin by automatizing the coefficient generation and requiring minimal user calculations.
+
+## Setup
+
+In order to run this tool, it is necessary to follow these steps:
+
+1. Download AVL 3.36 from <https://web.mit.edu/drela/Public/web/avl/>. The file for AVL version 3.36 can be found about halfway down the page.
+2. After downloading, extract AVL and move it to the home directory using:
+
+```shell
+sudo tar -xf avl3.36.tgz
+mv ./Avl /home/
+```
+
+Follow the README.md found in Avl to finish the setup process for AVL (requires to set up plotlib and eispack libraries). I recommend using the gfortran compile option. This might require you to install gfortran. This can be done by running:
+
+```shell
+sudo apt update
+sudo apt install gfortran
+```
+
+When running the Makefile for AVL, you might encounter an Error 1 message stating that there is a directory missing. This does not prevent AVL from working for our purposes. Once the process described in the AVL README is completed, AVL is ready to be used. No further set up is required on the side of the AVL or the tool.
+If you want to move the location of the AVL directory, this can simply be done by passing the `--avl_path` flag to the `input_avl.py` file, using the desired directory location for the flag (don't forget to place a "/" behind the last part of the path). Running this will automatically also adjust the paths where necessary.
+
+## Run
+
+To run the tool all that is needed is to modify the `input.yml` to the plane that you desire and then run `python input_avl.py <your_custom_yaml_file>.yml` Note that you require to have the yaml and argparse packages in your python environment to run this. An example template has been provided in the form of the `input.yml` that implements a standard plane with two ailerons, an elevator and a rudder. This example template can be run using: `python input_avl.py --yaml_file input.yml`.
+Once the script has been executed, the generated .avl, .sdf and a plot of the proposed control surfaces can be found in <your-planes-name> directory. The sdf file is the generated Advanced Lift Drag Plugin that can be copied and pasted straight into a model.sdf file, which can then be run in Gazebo.
+
+## Functionality
+
+The tool first asks the user for a range of vehicle specific parameters that are needed in order to specify the geometry and physical properties of the plane. The user has the choice to define a completely custom model, or alternatively select a predefined model template (such as a Cessna or a VTOL), which has a known number of control surfaces, and then provide only some physical properties, without having to define the entire model themselves. The input_avl.py file takes the provided parameter and creates an .avl file from this that can be read by AVL (the program). This happens in the process.sh file. The necessary output generated by AVL will be saved in two files: custom_vehicle_body_axis_derivatives.txt and custom_vehicle_stability_derivatives.txt. These two files contain the parameters that are required in order to populate the Advanced Lift Drag Plugin. Finally, avl_out_parse.py reads the generated .txt files and accordingly assigns parameters to the correct element in sdf. Once this is done, it is only a question of copy and pasting the generated Advanced Lift Drag plugin (found as <custom_plane>.sdf into the desired model.sdf file. )
+
+
+## Usability
+
+The current implementation provides a minimal working example. More accurate measurements can be made by adjusting the chosen number of vortices along span and chord according to desired preferences. A good starting point for this can be found here: <https://www.redalyc.org/pdf/6735/673571173005.pdf>. Furthermore, one can also more accurately model a vehicle by using a larger number of sections. In the current .yml file, only a left and right edge are defined for each surface yielding exactly one section, but the code supports expanding this to any number of desired sections.
+
+## IMPORTANT POINTS TO NOTE
+
+- A control surface in AVL is always defined from left to right. This means you need to first provide the left edge of a surface and then the right edge. If you do this the opposite way around, a surface will essentially be defined upside down.
+- The tool is designed to only support at most two control surfaces of any type on any one vehicle. Having more surfaces than that can lead to faulty behavior.
+- Another important point is that these scripts make use of the match, case syntax, which was only introduced in Python in version 3.10.
+- The primary reference resource for AVL can be found at <https://web.mit.edu/drela/Public/web/avl/AVL_User_Primer.pdf>. This document was written by the creators of AVL and contains all the variables that could be required in defining the control surfaces.
+- AVL cannot predict stall values. As such these need to be calculated/estimated another way. In the current implementation, default stall values have been taken from PX4's Advanced Plane. These should naturally be changed for new/different models.
+
+## Parameter Assignment
+
+Below is a comprehensive list on how the parameters are assigned at output and what files in AVL they are taken from. I am by no means an AVL expert, so please verify that these are actually the correct parameters required by the Advanced Lift Drag Plugin. For an explanation of what the parameters do, please see take a look at the Advanced Lift Drag Plugin.
+
+(name-in-AVL) -> (name-in-plugin)
+
+From the stability derivatives log file, the following advanced lift drag plugin parameters are taken:
+
+Alpha	-> alpha	The angle of attack
+
+Cmtot	-> Cem0		Pitching moment coefficient at zero angle of attack
+
+CLtot	-> CL0		Lift Coefficient at zero angle of attack
+
+CDtot	-> CD0		Drag coefficient at zero angle of attack
+
+CLa	-> CLa		dCL/da (slope of CL-alpha curve)
+
+CYa	-> CYa		dCy/da (sideforce slope wrt alpha)
+
+Cla	-> Cella	dCl/da (roll moment slope wrt alpha)
+
+Cma	-> Cema		dCm/da (pitching moment slope wrt alpha - before stall)
+
+Cna	-> Cena		dCn/da (yaw moment slope wrt alpha)
+
+CLb	-> CLb		dCL/dbeta (lift coefficient slope wrt beta)
+
+CYb	-> CYb		dCY/dbeta (side force slope wrt beta)
+
+Clb	-> Cellb	dCl/dbeta (roll moment slope wrt beta)
+
+Cmb	-> Cemb		dCm/dbeta (pitching moment slope wrt beta)
+
+Cnb	-> Cenb		dCn/dbeta (yaw moment slope wrt beta)
+
+
+From the body axis derivatives log file, the following advanced lift drag plugin parameters are taken:
+
+e	-> eff		Wing efficiency (Oswald efficiency factor for a 3D wing)
+
+CXp	-> CDp		dCD/dp (drag coefficient slope wrt roll rate)
+
+CYp	-> CYp		dCY/dp (sideforce slope wrt roll rate)
+
+CZp	-> CLp		dCL/dp (lift coefficient slope wrt roll rate)
+
+Clp	-> Cellp	dCl/dp (roll moment slope wrt roll rate)
+
+Cmp	-> Cemp		dCm/dp (pitching moment slope wrt roll rate)
+
+Cmp	-> Cenp		dCn/dp (yaw moment slope wrt roll rate)
+
+CXq	-> CDq		dCD/dq (drag coefficient slope wrt pitching rate)
+
+CYq	-> CYq		dCY/dq (side force slope wrt pitching rate)
+
+CZq	-> CLq		dCL/dq (lift coefficient slope wrt pitching rate)
+
+Clq	-> Cellq	dCl/dq (roll moment slope wrt pitching rate)
+
+Cmq	-> Cemq		dCm/dq (pitching moment slope wrt pitching rate)
+
+Cnq	-> Cenq		dCn/dq (yaw moment slope wrt pitching rate)
+
+CXr	-> CDr		dCD/dr (drag coefficient slope wrt yaw rate)
+
+CYr	-> CYr		dCY/dr (side force slope wrt yaw rate)
+
+CZr	-> CLr		dCL/dr (lift coefficient slope wrt yaw rate)
+
+Clr	-> Cellr	dCl/dr (roll moment slope wrt yaw rate)
+
+Cmr	-> Cemr		dCm/dr (pitching moment slope wrt yaw rate)
+
+Cnr	-> Cenr		dCn/dr (yaw moment slope wrt yaw rate)
+
+
+Furthermore, every control surface also has six own parameters, which are also derived from this log file. {i} below ranges from 1 to the number of unique control surface types in the model.
+
+CXd{i}	-> CD_ctrl	Effect of the control surface's deflection on drag
+
+CYd{i}	-> CY_ctrl	Effect of the control surface's deflection on side force
+
+CZd{i}	-> CL_ctrl	Effect of the control surface's deflection on lift
+
+Cld{i}	-> Cell_ctrl	Effect of the control surface's deflection on roll moment
+
+Cmd{i}	-> Cem_ctrl	Effect of the control surface's deflection on pitching moment
+
+Cnd{i}	-> Cen_ctrl	Effect of the control surface's deflection on yaw moment
+
+
+## Future Work
+
+The tool, while self-contained, could be expanded into multiple directions.
+
+1. Currently hinge positions and gains are set at default levels, and these could, if desired be further customized for more control.
+2. More vehicles could be added to provide default templates that require less input. At the moment, only "custom" works completely.
+3. Fuselage modelling could be included to further improve the accuracy of calculated coefficients.
+4. At the moment only NACA airfoils are provided as a way to generate cambered surfaces. An alternative to this would be to use custom airfoil files.

Tools/simulation/gz/tools/avl_automation/avl_out_parse.py

@@ -0,0 +1,342 @@
+#!/usr/bin/env
+
+import argparse
+import shutil
+import fileinput
+import subprocess
+import os
+from typing import TextIO
+
+
+"""
+Get the desired coefficient from the AVL output files by looking through the file line by line and picking it out when encountered.
+
+Args:
+	file (TextIO): The file from which the desired coefficient should be read.
+    token (str): The coefficient which to look for.
+
+Return:
+	value (str): The value associated with the desired coefficient.
+
+"""
+def get_coef(file: TextIO,token: str) -> str:
+
+    linesplit = []
+    for line in file:
+        if f' {token} ' in line:
+            linesplit = line.split()
+            break
+
+    index = 0
+    for i,v in enumerate(linesplit):
+        if v == token:
+            index = i
+    value = linesplit[index+2]
+    return value
+
+
+
+"""
+Write all gathered, model-wide coefficients to the sdf file.
+
+Args:
+	file (TextIO): The file to which the desired coefficient should be written.
+    token_str (str): The coefficients for which the associated value should be written.
+    token (str): The value which should be placed in the avl.
+
+Return:
+	None.
+
+"""
+def write_coef(file: TextIO, token_str: str, token: str):
+    old_line = f'<{token_str}></{token_str}>'
+    new_line = f'<{token_str}>{token}</{token_str}>'
+    with fileinput.FileInput(file, inplace=True) as output_file:
+        for line in output_file:
+            print(line.replace(old_line, new_line), end='')
+
+
+
+"""
+Write all gathered, control surface specific parameters to the sdf file.
+
+Args:
+	file (TextIO): The file to which the desired coefficients should be written.
+    ctrl_surface_vec (list): A vector that contains all 6 necessary coefficient values for the control surface in question.
+    index (str): The model-wide index number of the control surface in question.
+	direction (str): The direction in which the control surface can be actuated.
+
+Return:
+	None.
+"""
+def ctrl_surface_coef(file: TextIO,ctrl_surface_vec: list,index: str, direction: str):
+
+    extracted_text = ''
+    with open("./templates/control_surface.sdf",'r') as open_file:
+        for line in open_file:
+            extracted_text += line
+        open_file.close()
+
+	# Insert necessary coefficient values, index and direction in correct sdf location.
+    extracted_text = extracted_text.replace("<name></name>",f'<name>servo_{index}</name>')
+    extracted_text = extracted_text.replace("<index></index>",f'<index>{index}</index>')
+    extracted_text = extracted_text.replace("<direction></direction>",f'<directon>{direction}</direction>')
+    extracted_text = extracted_text.replace("<CD_ctrl></CD_ctrl>",f'<CD_ctrl>{ctrl_surface_vec[0]}</CD_ctrl>')
+    extracted_text = extracted_text.replace("<CY_ctrl></CY_ctrl>",f'<CY_ctrl>{ctrl_surface_vec[1]}</CY_ctrl>')
+    extracted_text = extracted_text.replace("<CL_ctrl></CL_ctrl>",f'<CL_ctrl>{ctrl_surface_vec[2]}</CL_ctrl>')
+    extracted_text = extracted_text.replace("<Cell_ctrl></Cell_ctrl>",f'<Cell_ctrl>{ctrl_surface_vec[3]}</Cell_ctrl>')
+    extracted_text = extracted_text.replace("<Cem_ctrl></Cem_ctrl>",f'<Cem_ctrl>{ctrl_surface_vec[4]}</Cem_ctrl>')
+    extracted_text = extracted_text.replace("<Cen_ctrl></Cen_ctrl>",f'<Cen_ctrl>{ctrl_surface_vec[5]}</Cen_ctrl>')
+
+
+    # Create model specific template
+    with open(file,'a') as plugin_file:
+        plugin_file.write(extracted_text + "\n")
+        plugin_file.close()
+
+"""
+Read out the necessary log files to gather the desired parameters and write them to the sdf plugin file.
+Arguments provided here are passed in the input_avl.py file.
+
+Args:
+	file_name (TextIO): The file to which the desired coefficients should be written.
+    vehicle_type (str): The type of vehicle in use.
+    AR (str): The calculated aspect ratio.
+    mac (str): The calculated mean aerodynamic chord.
+    ref_pt_x (str): The x coordinate of the reference point, at which forces and moments are applied.
+    ref_pt_y (str): The y coordinate of the reference point, at which forces and moments are applied.
+    ref_pt_z (str): The z coordinate of the reference point, at which forces and moments are applied.
+    num_ctrl_surfaces (str): The number of control surfaces that the model uses.
+    area (str): The wing surface area.
+	ctrl_surface_order (list): A list containing the types of control surfaces, in theorder in which
+    	they have been defined in the .avl file.
+    avl_path (str): A string containing the directory where the AVL directory should be moved to.
+
+Return:
+	None.
+"""
+
+def main(file_name: TextIO, vehicle_type: str, AR: str, mac: str, ref_pt_x: str, ref_pt_y: str, ref_pt_z: str, num_ctrl_surfaces: str, area: str, ctrl_surface_order: list, avl_path:str):
+
+	# Set current path for user
+    curr_path = subprocess.run(['pwd'], stdout=subprocess.PIPE, stderr=subprocess.PIPE, text=True)
+
+    if curr_path.returncode == 0:
+        # Save the output in a variable
+        savedir = curr_path.stdout.strip()
+    else:
+        raise LookupError("Invalid path to directory. Check both the avl_automation directory and the Avl directory are positioned correctly.")
+
+	# Set the file directory path from where the AVL output logs can be read.
+    filedir = f'{avl_path}Avl/runs/'
+
+	# Read out all necessary parameters from the stability and body axis derivatives files.
+    with open(f'{filedir}custom_vehicle_stability_derivatives.txt','r+') as stability_file:
+        original_position = stability_file.tell()
+
+        # As plane is modelled at 0 degree AoA, the total coefficients should(?) correspond to the
+        # 0 degree coefficients required by the plugin.
+        alpha = get_coef(stability_file,"Alpha")
+        Cem0 = get_coef(stability_file,"Cmtot")
+        CL0 = get_coef(stability_file,"CLtot")
+        CD0 = get_coef(stability_file,"CDtot")
+
+        CLa = get_coef(stability_file,"CLa")
+        CYa = get_coef(stability_file,"CYa")
+        Cella = get_coef(stability_file,"Cla")
+        Cema = get_coef(stability_file,"Cma")
+        Cena = get_coef(stability_file,"Cna")
+
+        stability_file.seek(original_position)
+
+        CLb = get_coef(stability_file,"CLb")
+        CYb = get_coef(stability_file,"CYb")
+        Cellb = get_coef(stability_file,"Clb")
+        Cemb = get_coef(stability_file,"Cmb")
+        Cenb = get_coef(stability_file,"Cnb")
+        stability_file.close()
+
+    with open(f'{filedir}custom_vehicle_body_axis_derivatives.txt') as bodyax_file:
+        original_position = bodyax_file.tell()
+
+        eff = get_coef(bodyax_file,"e")
+
+        bodyax_file.seek(original_position)
+
+        CDp = get_coef(bodyax_file,"CXp")
+        CYp = get_coef(bodyax_file,"CYp")
+        CLp = get_coef(bodyax_file,"CZp")
+        Cellp = get_coef(bodyax_file,"Clp")
+        Cemp = get_coef(bodyax_file,"Cmp")
+        Cenp = get_coef(bodyax_file,"Cnp")
+
+        bodyax_file.seek(original_position)
+
+        CDq = get_coef(bodyax_file,"CXq")
+        CYq = get_coef(bodyax_file,"CYq")
+        CLq = get_coef(bodyax_file,"CZq")
+        Cellq = get_coef(bodyax_file,"Clq")
+        Cemq = get_coef(bodyax_file,"Cmq")
+        Cenq = get_coef(bodyax_file,"Cnq")
+
+        bodyax_file.seek(original_position)
+
+        CDr = get_coef(bodyax_file,"CXr")
+        CYr = get_coef(bodyax_file,"CYr")
+        CLr = get_coef(bodyax_file,"CZr")
+        Cellr = get_coef(bodyax_file,"Clr")
+        Cemr = get_coef(bodyax_file,"Cmr")
+        Cenr = get_coef(bodyax_file,"Cnr")
+        bodyax_file.close()
+
+    plane_type = vehicle_type
+    ctrl_surface_mat = []
+
+	# Maybe in the future you want more types of set aircraft. Thus us a case differentiator.
+    match plane_type:
+
+        case "custom":
+            ctrl_surface_vec = []
+            with open(f'{filedir}custom_vehicle_body_axis_derivatives.txt') as bodyax_file:
+                original_position = bodyax_file.tell()
+                for i in range(1,(len(set(ctrl_surface_order)))+1):
+                    ctrl_surface_vec = []
+                    ctrl_surface_vec.append(get_coef(bodyax_file,f'CXd{i}'))
+                    ctrl_surface_vec.append(get_coef(bodyax_file,f'CYd{i}'))
+                    ctrl_surface_vec.append(get_coef(bodyax_file,f'CZd{i}'))
+                    ctrl_surface_vec.append(get_coef(bodyax_file,f'Cld{i}'))
+                    ctrl_surface_vec.append(get_coef(bodyax_file,f'Cmd{i}'))
+                    ctrl_surface_vec.append(get_coef(bodyax_file,f'Cnd{i}'))
+                    bodyax_file.seek(original_position)
+                    ctrl_surface_mat.append(ctrl_surface_vec)
+
+
+	# SPECIFY STALL PARAMETERS BASED ON AIRCRAFT TYPE (IF PROVIDED)
+    if not os.path.exists(f'{savedir}/{file_name}'):
+        os.makedirs(f'{savedir}/{file_name}')
+    file_name = f'{savedir}/{file_name}/{file_name}.sdf'
+    shutil.copy(f'{savedir}/templates/advanced_lift_drag_template.sdf',file_name)
+
+    # Get argument coefficients taken directly from the input file.
+    write_coef(file_name,"a0",alpha)
+    write_coef(file_name,"CL0",CL0)
+    write_coef(file_name,"CD0",CD0)
+    write_coef(file_name,"Cem0",Cem0)
+    write_coef(file_name,"AR",AR)
+    write_coef(file_name,"area",area)
+    write_coef(file_name,"mac",mac)
+    write_coef(file_name,"air_density",1.2041) # TODO: Provide custom air density option
+    write_coef(file_name,"forward","1 0 0")
+    write_coef(file_name,"upward","0 0 1")
+    write_coef(file_name,"link_name","base_link")
+    write_coef(file_name,"cp",f'{ref_pt_x} {ref_pt_y} {ref_pt_z}')
+    write_coef(file_name,"num_ctrl_surfaces",num_ctrl_surfaces)
+
+    write_coef(file_name,"CLa",CLa)
+    write_coef(file_name,"CYa",CYa)
+    write_coef(file_name,"Cella",Cella)
+    write_coef(file_name,"Cema",Cema)
+    write_coef(file_name,"Cena",Cena)
+    write_coef(file_name,"CLb",CLb)
+    write_coef(file_name,"CYb",CYb)
+    write_coef(file_name,"Cellb",Cellb)
+    write_coef(file_name,"Cemb",Cemb)
+    write_coef(file_name,"Cenb",Cenb)
+
+    write_coef(file_name,"CDp",CDp)
+    write_coef(file_name,"CYp",CYp)
+    write_coef(file_name,"CLp",CLp)
+    write_coef(file_name,"Cellp",Cellp)
+    write_coef(file_name,"Cemp",Cemp)
+    write_coef(file_name,"Cenp",Cenp)
+    write_coef(file_name,"CDq",CDq)
+    write_coef(file_name,"CYq",CYq)
+    write_coef(file_name,"CLq",CLq)
+    write_coef(file_name,"Cellq",Cellq)
+    write_coef(file_name,"Cemq",Cemq)
+    write_coef(file_name,"Cenq",Cenq)
+    write_coef(file_name,"CDr",CDr)
+    write_coef(file_name,"CYr",CYr)
+    write_coef(file_name,"CLr",CLr)
+    write_coef(file_name,"Cellr",Cellr)
+    write_coef(file_name,"Cemr",Cemr)
+    write_coef(file_name,"Cenr",Cenr)
+
+    write_coef(file_name,"eff",eff)
+
+    # TODO: Improve this for custom stall values
+    # Note: Currently these stall values are simply taken from advanced_plane presets.
+
+    write_coef(file_name,"alpha_stall","0.3391428111")
+    write_coef(file_name,"CLa_stall","-3.85")
+    write_coef(file_name,"CDa_stall","-0.9233984055")
+    write_coef(file_name,"Cema_stall","0")
+
+    # Check whether a particular type of control surface has been seen before. If it has,
+    # then the current control surface is the (right) counterpart.
+
+    # ASSUMPTION: There is the assumption that an vehicle will only ever have two of any
+    # particular type of control surface. (left and right). If this is not the case, the negation
+    # below will likely not work correctly.
+    type_seen = list()
+
+    # Dictionary containing the directions that each type of control surface can move.
+    ctrl_direction = {"aileron": 1,"elevator": -1,"rudder": 1}
+
+	# More set types in the future?
+    match plane_type:
+
+        case "custom":
+            for i, ctrl_surface in enumerate(ctrl_surface_order):
+
+                # Check whether a particular type of control surface has been seen before. If it has,
+                # then the current control surface is the (right) counterpart. Depending on the exact
+                # nature of the encountered type you then need to negate the correct parameters.
+                if ctrl_surface in type_seen:
+                    # Work out what the corresponding index for the first encounter of the ctrl surface is.
+                    seen_index = type_seen.index(ctrl_surface)
+
+                    if ctrl_surface == 'aileron':
+                        #Change for right wing aileron by flipping sign
+                        ctrl_surface_mat[seen_index][3] = -float(ctrl_surface_mat[0][3])
+                        ctrl_surface_mat[seen_index][5] = -float(ctrl_surface_mat[0][5])
+
+                    # Split Elevators are assumed to never run differentially. Feel free to add a
+                    # condition if your plane does require differential elevator action.
+
+                else:
+                    # If a ctrl surface has not been encountered add it to the type_seen list and
+                    # set the index to the length of the list - 1 as this corresponds to the newest
+                    # unseen element in ctrl_surface_mat .
+                    type_seen.append(ctrl_surface)
+                    seen_index = len(type_seen) - 1
+
+                ctrl_surface_coef(file_name,ctrl_surface_mat[seen_index],i,ctrl_direction[ctrl_surface])
+
+
+    # close the sdf file with plugin
+    with open(file_name,'a') as plugin_file:
+        plugin_file.write("</plugin>")
+        plugin_file.close()
+
+
+if __name__ == '__main__':
+    parser = argparse.ArgumentParser()
+
+    parser.add_argument("file_name", help="The file to which the desired coefficients should be written.")
+    parser.add_argument("vehicle_type", help="The type of vehicle in use.")
+    parser.add_argument("AR", help="The calculated aspect ratio.")
+    parser.add_argument("mac", help="The calculated mean aerodynamic chord.")
+    parser.add_argument("ref_pt_x", help="The x coordinate of the reference point, at which forces and moments are applied.")
+    parser.add_argument("ref_pt_y", help="The y coordinate of the reference point, at which forces and moments are applied.")
+    parser.add_argument("ref_pt_z", help="The z coordinate of the reference point, at which forces and moments are applied.")
+    parser.add_argument("num_ctrl_surfaces", help="The number of control surfaces that the model uses.")
+    parser.add_argument("area", help= "The wing surface area.")
+    parser.add_argument("ctrl_surface_order", help=" A list containing the types of control surfaces, in theorder in which \
+    	they have been defined in the .avl file.")
+    parser.add_argument("avl_path",help="A string containing the directory where the AVL directory should be moved to.")
+
+    args = parser.parse_args()
+
+    main(args.file_name,args.vehicle_type,args.AR,args.mac,args.ref_pt_x,args.ref_pt_y,
+         args.ref_pt_z,args.num_ctrl_surfaces,args.area,args.ctrl_surface_order,args.avl_path)

Tools/simulation/gz/tools/avl_automation/avl_steps.txt

@@ -0,0 +1,10 @@
+oper
+x
+n custom_plane
+st custom_vehicle_stability_derivatives.txt
+sb custom_vehicle_body_axis_derivatives.txt
+g
+h
+
+
+quit

Tools/simulation/gz/tools/avl_automation/input.yml

@@ -0,0 +1,142 @@
+# Enter a name for your vehicle
+vehicle_name: plane_example_2
+
+# Enter the type of airframe you would like to use:
+frame_type: custom
+
+# First define some model-wide parameters for custom models:
+reference_area: 12
+wing_span: 15
+# Provide a reference point at which the forces and moments generated will act.
+reference_point:
+  X: 0
+  Y: 0
+  Z: 0
+
+#Provide information on each of the Control Surfaces
+num_ctrl_surfaces: 4
+control_surfaces:
+  - name: right_wing
+    type: aileron
+    nchord: 1
+    cspace: 1
+    nspan: 16
+    sspace: -2
+    angle: 4
+    translation:
+      X: 0
+      Y: 0
+      Z: 0
+    naca: 2412
+    sections:
+    - name: section_1
+      position:
+        X: -0.25
+        Y: 0
+        Z: 0
+      chord: 1
+      ainc: 0
+      nspan: 8
+      sspace: 1
+    - name: section_2
+      position:
+        X: -0.175
+        Y: 5
+        Z: 0.5
+      chord: 0.7
+      ainc: 0
+      nspan: 0
+      sspace: 0
+
+
+  - name: left_wing
+    type: aileron
+    nchord: 1
+    cspace: 1
+    nspan: 16
+    sspace: -2
+    angle: 4
+    translation:
+      X: 0
+      Y: 0
+      Z: 0
+    naca: 2412
+    sections:
+    - name: section_1
+      position:
+        X: -0.175
+        Y: -5
+        Z: 0.5
+      chord: 0.7
+      ainc: 0
+      nspan: 0
+      sspace: 0
+    - name: section_2
+      position:
+        X: -0.25
+        Y: 0
+        Z: 0
+      chord: 1
+      ainc: 0
+      nspan: 8
+      sspace: 1
+
+  - name: elevator
+    type: elevator
+    nchord: 1
+    cspace: 1
+    nspan: 7
+    sspace: -2
+    translation:
+      X: 6
+      Y: 0
+      Z: 0.5
+    sections:
+    - name: section_1
+      position:
+        X: -0.1
+        Y: 0
+        Z: 0
+      chord: 0.4
+      ainc: 0
+      nspan: 7
+      sspace: -1.25
+    - name: section_2
+      position:
+        X: -0.075
+        Y: 2
+        Z: 0
+      chord: 0.3
+      ainc: 0
+      nspan: 0
+      sspace: 0
+
+  - name: fin
+    type: rudder
+    nchord: 1
+    cspace: 1
+    nspan: 10
+    sspace: 1
+    translation:
+      X: 6
+      Y: 0
+      Z: 0.5
+    sections:
+    - name: section_1
+      position:
+        X: -0.1
+        Y: 0
+        Z: 0
+      chord: 0.4
+      ainc: 0
+      nspan: 7
+      sspace: -1.25
+    - name: section_2
+      position:
+        X: -0.075
+        Y: 0
+        Z: 1
+      chord: 0.3
+      ainc: 0
+      nspan: 0
+      sspace: 0

Tools/simulation/gz/tools/avl_automation/input_avl.py

@@ -0,0 +1,314 @@
+#!/usr/bin/env
+
+import argparse
+import avl_out_parse
+import os
+import yaml
+import subprocess
+import shutil
+
+"""
+Write individual airfoil section definitions to the .avl file.
+Sections are defined through a 3D point in space and assigned properties such as chord, angle of incidence etc.
+AVL then links them up to the other sections of a particular surface. You can define any number of sections for
+a particular surface, but there always have to be at least two (a left and right edge).
+
+Args:
+	plane_name (str): The name of the vehicle.
+	x (str): The x coordinate of the section.
+	y (str): The y coordinate of the section.
+	z (str): The z coordinate of the section.
+	chord (str): Chord in this section of the surface. Trailing edge is at x + chord, y, z.
+	ainc (str): Angle of incidence for this section. Taken as a rotation (RH rule) about the surface's
+		spanwise axis projected onto the Y-Z plane.
+	nspan (str): Number of spanwise vortices in until the next section.
+	sspan (str): Controls the spanwise spacing of the vortices.
+	naca_number (str): The chosen NACA number that will define the cambered properties of this section
+		of the surface. For help picking an airfoil go to: http://airfoiltools.com/airfoil/naca4digit.
+	ctrl_surface_type: The selected type of control surface. This should be consistent along the entirety of
+		the surface. (Question: Flap and Aileron along the same airfoil?)
+
+Return:
+	None.
+
+"""
+def write_section(plane_name: str,x: str,y: str,z: str,chord: str,ainc: str,nspan: str,sspace: str,naca_number: str,ctrl_surf_type: str):
+
+	with open(f'{plane_name}.avl','a') as avl_file:
+		avl_file.write("SECTION \n")
+		avl_file.write("!Xle    Yle    Zle     Chord   Ainc  Nspanwise  Sspace \n")
+		avl_file.write(f'{x}   {y}    {z}    {chord}    {ainc}     {nspan}    {sspace} \n')
+		if naca_number != "0000":
+			avl_file.write("NACA \n")
+			avl_file.write(f'{naca_number} \n')
+		avl_file.close()
+
+	match ctrl_surf_type:
+		case 'aileron':
+			#TODO provide custom options for gain and hinge positions
+			with open(f'{plane_name}.avl','a') as avl_file:
+				avl_file.write("CONTROL \n")
+				avl_file.write("aileron  1.0  0.0  0.0  0.0  0.0  -1 \n")
+				avl_file.close()
+
+		case 'elevator':
+			with open(f'{plane_name}.avl','a') as avl_file:
+				avl_file.write("CONTROL \n")
+				avl_file.write("elevator  1.0  0.0  0.0  0.0  0.0  1 \n")
+				avl_file.close()
+
+		case 'rudder':
+			with open(f'{plane_name}.avl','a') as avl_file:
+				avl_file.write("CONTROL \n")
+				avl_file.write("rudder  1.0  0.0  0.0  0.0  0.0  1 \n")
+				avl_file.close()
+
+
+
+"""
+Read the provided yaml file and generate the corresponding .avl file that can be read into AVL.
+Also calls AVL and the avl_out_parse.py file that generates the sdf plugin.
+
+Args:
+	yaml_file: Path to the input yaml file
+	avl_path: Set the avl_path to provide a desired directory for where Avl should be located.
+
+Return:
+	None
+
+"""
+def main():
+	user = os.environ.get('USER')
+	# This will find Avl on a users machine.
+	for root, dirs, _ in os.walk(f'/home/{user}/'):
+		if "Avl" in dirs:
+			target_directory_path = os.path.join(root, "Avl")
+			break
+	parent_directory_path = os.path.dirname(target_directory_path)
+	filedir = f'{parent_directory_path}/'
+	print(filedir)
+
+	parser = argparse.ArgumentParser()
+	parser.add_argument("--yaml_file", help="Path to input yaml file.")
+	parser.add_argument("--avl_path", default=filedir, help="Provide an absolute AVL path. If this argument is passed, AVL will be moved there and the files will adjust their paths accordingly.")
+	inputs = parser.parse_args()
+
+
+	# If the user passes the avl_path argument then move Avl to that location:
+	if inputs.avl_path != filedir:
+
+		#Check if the directory is already there
+		if os.path.exists(f'{inputs.avl_path}/Avl') and os.path.isdir(f'{inputs.avl_path}/Avl'):
+			print("Avl is already at desired location")
+		else:
+			shutil.move(f'{filedir}Avl',inputs.avl_path)
+
+		# Adjust paths to AVL in process.sh
+		print("Adjusting paths")
+		with open("./process.sh", "r") as file:
+			all_lines = file.readlines()
+			file.close()
+
+		it = 0
+		for line in all_lines:
+			if "cp $DIR_PATH/$CUSTOM_MODEL.avl" in line:
+				new_line = f'cp $DIR_PATH/$CUSTOM_MODEL.avl {inputs.avl_path}Avl/runs\n'
+				all_lines[it] = new_line
+
+			if "/Avl/runs/plot.ps $DIR_PATH/" in line:
+				new_line =f'mv {inputs.avl_path}Avl/runs/plot.ps $DIR_PATH/\n'
+				all_lines[it] = new_line
+
+			if "cd" in line and "/Avl/runs" in line:
+				new_line = f'cd {inputs.avl_path}Avl/runs\n'
+				all_lines[it] = new_line
+			it += 1
+
+		with open("./process.sh", "w") as file:
+			file.writelines(all_lines)
+			file.close()
+
+
+	with open(inputs.yaml_file,'r') as yaml_file:
+		yaml_data = yaml.safe_load(yaml_file)
+
+	airframes = ['cessna','standard_vtol','custom']
+	plane_name = yaml_data['vehicle_name']
+	frame_type = yaml_data['frame_type']
+	if not frame_type in airframes:
+		raise ValueError("\nThis is not a valid airframe, please choose a valid airframe. \n")
+
+	# Parameters that need to be provided:
+	# General
+	# - Reference Area (Sref)
+	# - Wing span (Bref) (wing span squared / area = aspect ratio which is a required parameter for the sdf file)
+	# - Reference point (X,Y,Zref) point at which moments and forces are calculated
+	#Control Surface specific
+	# - type (select from options; aileron,elevator,rudder)
+	# - nchord
+	# - cspace
+	# - nspanwise
+	# - sspace
+	# - x,y,z 1. (section)
+	# - chord 1. (section)
+	# - ainc 1. (section)
+	# - Nspan 1. (optional for section)
+	# - sspace 1. (optional for section)
+	# - x,y,z 2. (section)
+	# - chord 2. (section)
+	# - ainc 2. (section)
+	# - Nspan 2. (optional for section)
+	# - sspace 2. (optional for section)
+
+	# TODO: Find out if elevons are defined
+	ctrl_surface_types = ['aileron','elevator','rudder']
+	# - Reference Chord (Cref) (= area/wing span)
+	delineation = '!***************************************'
+	sec_demark = '#--------------------------------------------------'
+	num_ctrl_surfaces = 0
+	ctrl_surface_order = []
+	area = 0
+	span = 0
+
+	ref_pt_x = None
+	ref_pt_y = None
+	ref_pt_z = None
+
+	# Future work: Provide some pre-worked frames for a Cessna and standard VTOL if there is a need for it
+	match frame_type:
+
+		case "custom":
+
+			# These parameters are consistent across all models.
+			# At the moment we do not use any symmetry axis for mirroring.
+			with open(f'{plane_name}.avl','w') as avl_file:
+				avl_file.write(f'{delineation} \n')
+				avl_file.write(f'!{plane_name} input dataset \n')
+				avl_file.write(f'{delineation} \n')
+				avl_file.write(f'{plane_name} \n')
+				avl_file.write('!Mach \n0.0 \n')
+				avl_file.write('!IYsym    IZsym    Zsym \n')
+				avl_file.write('0     0     0 \n')
+				avl_file.close()
+
+			# First define some model-specific parameters for custom models
+			area = yaml_data["reference_area"]
+			span = yaml_data["wing_span"]
+			ref_pt_x = yaml_data["reference_point"]["X"]
+			ref_pt_y = yaml_data["reference_point"]["Y"]
+			ref_pt_z = yaml_data["reference_point"]["Z"]
+
+			if(span != 0 and area != 0):
+				ref_chord = float(area)/float(span)
+			else:
+				raise ValueError("Invalid reference chord value. Check area and wing span values.")
+
+			# Write the gathered model-wide parameters into the .avl file
+			with open(f'{plane_name}.avl','a') as avl_file:
+				avl_file.write('!Sref    Cref    Bref \n')
+				avl_file.write(f'{area}     {str(ref_chord)}     {span} \n')
+				avl_file.write('!Xref    Yref    Zref \n')
+				avl_file.write(f'{ref_pt_x}     {ref_pt_y}      {ref_pt_z} \n')
+				avl_file.close()
+
+			num_ctrl_surfaces = yaml_data["num_ctrl_surfaces"]
+			for i, control_surface in enumerate(yaml_data["control_surfaces"]):
+
+				# Wings always need to be defined from left to right
+				ctrl_surf_name = control_surface['name']
+				ctrl_surf_type = control_surface['type']
+				if ctrl_surf_type not in ctrl_surface_types:
+					raise ValueError(f'The selected type is invalid. Available types are: {ctrl_surface_types}')
+
+				# The order of control surfaces becomes important in the output parsing
+				# to correctly assign derivatives to particular surfaces.
+				ctrl_surface_order.append(ctrl_surf_type)
+
+				nchord = control_surface["nchord"]
+				cspace = control_surface["cspace"]
+				nspanwise = control_surface["nspan"]
+				sspace = control_surface["sspace"]
+
+				# TODO: Add more control surface types that also require Angles.
+				if ctrl_surf_type.lower() == 'aileron':
+					angle = control_surface["angle"]
+
+				#Translation of control surface, will move the whole surface to specified position
+				tx = control_surface["translation"]["X"]
+				ty = control_surface["translation"]["Y"]
+				tz = control_surface["translation"]["Z"]
+
+				# Write common part of this surface to .avl file
+				with open(f'{plane_name}.avl','a') as avl_file:
+					avl_file.write(sec_demark)
+					avl_file.write("\nSURFACE \n")
+					avl_file.write(f'{ctrl_surf_name} \n')
+					avl_file.write("!Nchordwise     Cspace      Nspanwise       Sspace \n")
+					avl_file.write(f'{nchord}       {cspace}        {nspanwise}     {sspace} \n')
+
+					# If we have a elevator, we can duplicate the defined control surface along the y-axis of the model
+					# as both sides are generally modelled and controlled as one in simulation. Adjust for split elevators if desired.
+					if ctrl_surf_type.lower() == 'elevator':
+						avl_file.write("\nYDUPLICATE\n")
+						avl_file.write("0.0\n\n")
+
+					# Elevators and Rudders do not require an angle of incidence.
+					if ctrl_surf_type.lower() == 'aileron':
+						avl_file.write("ANGLE \n")
+						avl_file.write(f'{angle} \n')
+
+					# Translate the surface to a particular position in space.
+					avl_file.write("TRANSLATE \n")
+					avl_file.write(f'{tx}    {ty}    {tz} \n')
+					avl_file.close()
+
+
+				# Define NACA airfoil shape.
+				# For help picking an airfoil go to: http://airfoiltools.com/airfoil/naca4digit
+				# NOTE: AVL can only use 4-digit NACA codes.
+				if ctrl_surf_type.lower() == "aileron":
+					naca_number = control_surface["naca"]
+				else:
+					# Provide a default NACA number for unused airfoils
+					naca_number = '0000'
+
+				# Iterating over each defined section for the control surface. There need to be at least
+				# two in order to define a left and right edge, but there is no upper limit.
+				# CRITICAL: ALWAYS DEFINE YOUR SECTION FROM LEFT TO RIGHT
+				for j, section in enumerate(control_surface["sections"]):
+
+					print(f'Defining {j}. section of {i+1}. control surface \n')
+					y = section["position"]["Y"]
+					z = section["position"]["Z"]
+					x = section["position"]["X"]
+					chord = section["chord"]
+					ainc = section["ainc"]
+					nspan = section["nspan"]
+					write_section(plane_name,x,y,z,chord,ainc,nspan,sspace,naca_number,ctrl_surf_type)
+
+				print(f'\nPARAMETER DEFINITION FOR {i+1}. CONTROL SURFACE COMPLETED \n')
+
+
+   	# Calculation of Aspect Ratio (AR) and Mean Aerodynamic Chord (mac)
+	AR = str((float(span)*float(span))/float(area))
+	mac = str((2/3)*(float(area)/float(span)))
+
+	# Call shell script that will pass the generated .avl file to AVL
+	os.system(f'./process.sh {plane_name}')
+
+	# Call main function of avl parse script to parse the generated AVL files.
+	avl_out_parse.main(plane_name,frame_type,AR,mac,ref_pt_x,ref_pt_y,ref_pt_z,num_ctrl_surfaces,area,ctrl_surface_order,inputs.avl_path)
+
+	# Finally move all generated files to a new directory and show the generated geometry image:
+	result = subprocess.run(['pwd'], stdout=subprocess.PIPE, stderr=subprocess.PIPE, text=True)
+
+	if result.returncode == 0:
+		# Save the output in a variable
+		current_path = result.stdout.strip()
+
+	# Run image plot from avl_automation directory.
+	os.system(f'mv ./{plane_name}.* ./{plane_name}' )
+	os.system(f'evince {current_path}/{plane_name}/{plane_name}.ps')
+
+if __name__ == '__main__':
+	main()

Tools/simulation/gz/tools/avl_automation/process.sh

@@ -0,0 +1,27 @@
+#!/bin/bash
+CUSTOM_MODEL=$1
+DIR_PATH=$(pwd)
+
+cp $DIR_PATH/$CUSTOM_MODEL.avl /home/$USER/Avl/runs/
+cd
+cd /home/$USER/Avl/runs
+
+old_stability_derivatives="custom_vehicle_stability_derivatives.txt"
+old_body_ax_derivatives="custom_vehicle_body_axis_derivatives.txt"
+
+if [ -e "$old_stability_derivatives" ]; then
+    # Delete old stability derivative file
+    rm "$old_stability_derivatives"
+fi
+if [ -e "$old_body_ax_derivatives" ]; then
+    # Delete old body_axis derivative file
+    rm "$old_body_ax_derivatives"
+fi
+
+#avl_steps.txt can be used to run commands on the AVL commandline.
+../bin/avl $CUSTOM_MODEL.avl < $DIR_PATH/avl_steps.txt
+echo "\n"
+
+#After completion move the plot to avl_automation directory
+mv /home/$USER/Avl/runs/plot.ps $DIR_PATH/
+mv $DIR_PATH/plot.ps $DIR_PATH/$CUSTOM_MODEL.ps

Tools/simulation/gz/tools/avl_automation/templates/advanced_lift_drag_template.sdf

@@ -0,0 +1,43 @@
+<plugin filename="gz-sim-advanced-lift-drag-system" name="gz::sim::systems::AdvancedLiftDrag">
+      <a0></a0>
+      <CL0></CL0>
+      <AR></AR>
+      <eff></eff>
+      <CLa></CLa>
+      <CD0></CD0>
+      <Cem0></Cem0>
+      <Cema></Cema>
+      <CYb></CYb>
+      <Cellb></Cellb>
+      <Cenb></Cenb>
+      <CDp></CDp>
+      <CYp></CYp>
+      <CLp></CLp>
+      <Cellp></Cellp>
+      <Cemp></Cemp>
+      <Cenp></Cenp>
+      <CDq></CDq>
+      <CYq></CYq>
+      <CLq></CLq>
+      <Cellq></Cellq>
+      <Cemq></Cemq>
+      <Cenq></Cenq>
+      <CDr></CDr>
+      <CYr></CYr>
+      <CLr></CLr>
+      <Cellr></Cellr>
+      <Cemr></Cemr>
+      <Cenr></Cenr>
+      <alpha_stall></alpha_stall>
+      <CLa_stall></CLa_stall>
+      <CDa_stall></CDa_stall>
+      <Cema_stall></Cema_stall>
+      <cp></cp>
+      <area></area>
+      <mac></mac>
+      <air_density></air_density>
+      <forward></forward>
+      <upward></upward>
+      <link_name></link_name>
+      <num_ctrl_surfaces></num_ctrl_surfaces>
+  

Tools/simulation/gz/tools/avl_automation/templates/control_surface.sdf

@@ -0,0 +1,11 @@
+    <control_surface>
+        <name></name>
+        <index></index>
+        <direction></direction>
+        <CD_ctrl></CD_ctrl>
+        <CY_ctrl></CY_ctrl>
+        <CL_ctrl></CL_ctrl>
+        <Cell_ctrl></Cell_ctrl>
+        <Cem_ctrl></Cem_ctrl>
+        <Cen_ctrl></Cen_ctrl>
+    </control_surface>

Tools/simulation/gz/worlds/windy.sdf

@@ -0,0 +1,151 @@
+<sdf version='1.9'>
+  <world name='windy'>
+    <physics type="ode">
+      <max_step_size>0.004</max_step_size>
+      <real_time_factor>1.0</real_time_factor>
+      <real_time_update_rate>250</real_time_update_rate>
+    </physics>
+    <plugin name='gz::sim::systems::Physics' filename='gz-sim-physics-system'/>
+    <plugin name='gz::sim::systems::UserCommands' filename='gz-sim-user-commands-system'/>
+    <plugin name='gz::sim::systems::SceneBroadcaster' filename='gz-sim-scene-broadcaster-system'/>
+    <plugin name='gz::sim::systems::Contact' filename='gz-sim-contact-system'/>
+    <plugin name='gz::sim::systems::Imu' filename='gz-sim-imu-system'/>
+    <plugin name='gz::sim::systems::AirPressure' filename='gz-sim-air-pressure-system'/>
+    <plugin name='gz::sim::systems::ApplyLinkWrench' filename='gz-sim-apply-link-wrench-system'/>
+    <plugin name='gz::sim::systems::Sensors' filename='gz-sim-sensors-system'>
+      <render_engine>ogre2</render_engine>
+    </plugin>
+    <gui fullscreen='false'>
+      <plugin name='3D View' filename='GzScene3D'>
+        <gz-gui>
+          <title>3D View</title>
+          <property type='bool' key='showTitleBar'>0</property>
+          <property type='string' key='state'>docked</property>
+        </gz-gui>
+        <engine>ogre2</engine>
+        <scene>scene</scene>
+        <ambient_light>0.5984631152222222 0.5984631152222222 0.5984631152222222</ambient_light>
+        <background_color>0.8984631152222222 0.8984631152222222 0.8984631152222222</background_color>
+        <camera_pose>-6 0 6 0 0.5 0</camera_pose>
+      </plugin>
+      <plugin name='World control' filename='WorldControl'>
+        <gz-gui>
+          <title>World control</title>
+          <property type='bool' key='showTitleBar'>0</property>
+          <property type='bool' key='resizable'>0</property>
+          <property type='double' key='height'>72</property>
+          <property type='double' key='width'>121</property>
+          <property type='double' key='z'>1</property>
+          <property type='string' key='state'>floating</property>
+          <anchors target='3D View'>
+            <line own='left' target='left'/>
+            <line own='bottom' target='bottom'/>
+          </anchors>
+        </gz-gui>
+        <play_pause>1</play_pause>
+        <step>1</step>
+        <start_paused>1</start_paused>
+      </plugin>
+      <plugin name='World stats' filename='WorldStats'>
+        <gz-gui>
+          <title>World stats</title>
+          <property type='bool' key='showTitleBar'>0</property>
+          <property type='bool' key='resizable'>0</property>
+          <property type='double' key='height'>110</property>
+          <property type='double' key='width'>290</property>
+          <property type='double' key='z'>1</property>
+          <property type='string' key='state'>floating</property>
+          <anchors target='3D View'>
+            <line own='right' target='right'/>
+            <line own='bottom' target='bottom'/>
+          </anchors>
+        </gz-gui>
+        <sim_time>1</sim_time>
+        <real_time>1</real_time>
+        <real_time_factor>1</real_time_factor>
+        <iterations>1</iterations>
+      </plugin>
+      <plugin name='Entity tree' filename='EntityTree'/>
+    </gui>
+    <gravity>0 0 -9.8</gravity>
+    <magnetic_field>6e-06 2.3e-05 -4.2e-05</magnetic_field>
+    <atmosphere type='adiabatic'/>
+    <scene>
+      <grid>false</grid>
+      <ambient>0.4 0.4 0.4 1</ambient>
+      <background>0.7 0.7 0.7 1</background>
+      <shadows>true</shadows>
+    </scene>
+    <model name='ground_plane'>
+      <static>true</static>
+      <link name='link'>
+        <collision name='collision'>
+          <geometry>
+            <plane>
+              <normal>0 0 1</normal>
+              <size>1 1</size>
+            </plane>
+          </geometry>
+          <surface>
+            <friction>
+              <ode/>
+            </friction>
+            <bounce/>
+            <contact/>
+          </surface>
+        </collision>
+        <visual name='visual'>
+          <geometry>
+            <plane>
+              <normal>0 0 1</normal>
+              <size>100 100</size>
+            </plane>
+          </geometry>
+          <material>
+            <ambient>0.8 0.8 0.8 1</ambient>
+            <diffuse>0.8 0.8 0.8 1</diffuse>
+            <specular>0.8 0.8 0.8 1</specular>
+          </material>
+        </visual>
+        <pose>0 0 0 0 -0 0</pose>
+        <inertial>
+          <pose>0 0 0 0 -0 0</pose>
+          <mass>1</mass>
+          <inertia>
+            <ixx>1</ixx>
+            <ixy>0</ixy>
+            <ixz>0</ixz>
+            <iyy>1</iyy>
+            <iyz>0</iyz>
+            <izz>1</izz>
+          </inertia>
+        </inertial>
+        <enable_wind>true</enable_wind>
+      </link>
+      <pose>0 0 0 0 -0 0</pose>
+      <self_collide>false</self_collide>
+    </model>
+    <light name='sunUTC' type='directional'>
+      <pose>0 0 500 0 -0 0</pose>
+      <cast_shadows>true</cast_shadows>
+      <intensity>1</intensity>
+      <direction>0.001 0.625 -0.78</direction>
+      <diffuse>0.904 0.904 0.904 1</diffuse>
+      <specular>0.271 0.271 0.271 1</specular>
+      <attenuation>
+        <range>2000</range>
+        <linear>0</linear>
+        <constant>1</constant>
+        <quadratic>0</quadratic>
+      </attenuation>
+      <spot>
+        <inner_angle>0</inner_angle>
+        <outer_angle>0</outer_angle>
+        <falloff>0</falloff>
+      </spot>
+    </light>
+    <wind>
+	<linear_velocity>10 10 10</linear_velocity>
+    </wind>
+  </world>
+</sdf>

Tools/zenoh/px_generate_zenoh_topic_files.py

@@ -0,0 +1,175 @@
+#!/usr/bin/env python3
+#############################################################################
+#
+#   Copyright (C) 2013-2022 PX4 Pro 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.
+#
+#############################################################################
+
+"""
+px_generate_zenoh_topic_files.py
+Generates c/cpp header/source files for use with zenoh
+message files
+"""
+
+import os
+import argparse
+import re
+import sys
+
+try:
+    import em
+except ImportError as e:
+    print("Failed to import em: " + str(e))
+    print("")
+    print("You may need to install it using:")
+    print("    pip3 install --user empy")
+    print("")
+    sys.exit(1)
+
+try:
+    import genmsg.template_tools
+except ImportError as e:
+    print("Failed to import genmsg: " + str(e))
+    print("")
+    print("You may need to install it using:")
+    print("    pip3 install --user pyros-genmsg")
+    print("")
+    sys.exit(1)
+
+
+__author__ = "Sergey Belash, Thomas Gubler, Beat Kueng"
+__copyright__ = "Copyright (C) 2013-2022 PX4 Development Team."
+__license__ = "BSD"
+__email__ = "thomasgubler@gmail.com"
+
+ZENOH_TEMPLATE_FILE = ['Kconfig.topics.em', 'uorb_pubsub_factory.hpp.em']
+TOPICS_TOKEN = '# TOPICS '
+
+
+def get_topics(filename):
+    """
+    Get TOPICS names from a "# TOPICS" line
+    """
+    ofile = open(filename, 'r')
+    text = ofile.read()
+    result = []
+    for each_line in text.split('\n'):
+        if each_line.startswith(TOPICS_TOKEN):
+            topic_names_str = each_line.strip()
+            topic_names_str = topic_names_str.replace(TOPICS_TOKEN, "")
+            topic_names_list = topic_names_str.split(" ")
+            for topic in topic_names_list:
+                # topic name PascalCase (file name) to snake_case (topic name)
+                topic_name = re.sub(r'(?<!^)(?=[A-Z])', '_', topic).lower()
+                result.append(topic_name)
+    ofile.close()
+
+    if len(result) == 0:
+        # topic name PascalCase (file name) to snake_case (topic name)
+        file_base_name = os.path.basename(filename).replace(".msg", "")
+        topic_name = re.sub(r'(?<!^)(?=[A-Z])', '_', file_base_name).lower()
+        result.append(topic_name)
+
+    return result
+
+def generate_by_template(output_file, template_file, em_globals):
+    """
+    Invokes empy intepreter to geneate output_file by the
+    given template_file and predefined em_globals dict
+    """
+    # check if folder exists:
+    folder_name = os.path.dirname(output_file)
+    if not os.path.exists(folder_name):
+        os.makedirs(folder_name)
+
+    ofile = open(output_file, 'w')
+    # todo, reuse interpreter
+    interpreter = em.Interpreter(output=ofile, globals=em_globals, options={
+                                 em.RAW_OPT: True, em.BUFFERED_OPT: True})
+    try:
+        interpreter.file(open(template_file))
+    except OSError as e:
+        ofile.close()
+        os.remove(output_file)
+        raise
+    interpreter.shutdown()
+    ofile.close()
+    return True
+
+
+def generate_topics_list_file_from_files(files, outputdir, template_filename, templatedir):
+    # generate cpp file with topics list
+    filenames = []
+    for filename in [os.path.basename(p) for p in files if os.path.basename(p).endswith(".msg")]:
+        filenames.append(re.sub(r'(?<!^)(?=[A-Z])', '_', filename).lower())
+
+    datatypes = []
+    for filename in [os.path.basename(p) for p in files if os.path.basename(p).endswith(".msg")]:
+        datatypes.append(re.sub(r'(?<!^)(?=[A-Z])', '_', filename).lower().replace(".msg",""))
+
+    full_base_names = []
+    for filename in [os.path.basename(p) for p in files if os.path.basename(p).endswith(".msg")]:
+        full_base_names.append(filename.replace(".msg",""))
+
+    topics = []
+    for msg_filename in files:
+        topics.extend(get_topics(msg_filename))
+
+    tl_globals = {"msgs": filenames, "topics": topics, "datatypes": datatypes, "full_base_names": full_base_names}
+    tl_template_file = os.path.join(templatedir, template_filename)
+    tl_out_file = os.path.join(outputdir, template_filename.replace(".em", ""))
+
+    generate_by_template(tl_out_file, tl_template_file, tl_globals)
+
+if __name__ == "__main__":
+    parser = argparse.ArgumentParser(description='Convert msg files to uorb headers/sources')
+    parser.add_argument('--zenoh-config', help='Generate Zenoh Kconfig file', action='store_true')
+    parser.add_argument('--zenoh-pub-sub', help='Generate Zenoh Pubsub factory', action='store_true')
+    parser.add_argument('-f', dest='file',
+                        help="files to convert (use only without -d)",
+                        nargs="+")
+    parser.add_argument('-e', dest='templatedir',
+                        help='directory with template files',)
+    parser.add_argument('-o', dest='outputdir',
+                        help='output directory for header files')
+    parser.add_argument('-p', dest='prefix', default='',
+                        help='string added as prefix to the output file '
+                        ' name when converting directories')
+    args = parser.parse_args()
+
+    if args.zenoh_config:
+        generate_topics_list_file_from_files(args.file, args.outputdir, ZENOH_TEMPLATE_FILE[0], args.templatedir)
+        exit(0)
+    elif args.zenoh_pub_sub:
+        generate_topics_list_file_from_files(args.file, args.outputdir, ZENOH_TEMPLATE_FILE[1], args.templatedir)
+        exit(0)
+    else:
+        print('Error: either --headers or --sources must be specified')
+        exit(-1)

Tools/zenoh/templates/zenoh/Kconfig.topics.em

@@ -0,0 +1,25 @@
+@{
+topics_count = len(topics)
+topic_names_all = list(set(topics)) # set() filters duplicates
+topic_names_all.sort()
+
+datatypes = list(set(datatypes)) # set() filters duplicates
+datatypes.sort()
+}@
+
+menu "Zenoh publishers/subscribers"
+	depends on MODULES_ZENOH
+@[for idx, topic_name in enumerate(datatypes)]@
+	config ZENOH_PUBSUB_@(topic_name.upper())
+		bool "@(topic_name)"
+		default n
+
+@[end for]
+
+config ZENOH_PUBSUB_ALL_SELECTION
+	bool
+	default y if ZENOH_PUBSUB_ALL
+@[for idx, topic_name in enumerate(datatypes)]@
+	select ZENOH_PUBSUB_@(topic_name.upper())
+@[end for]
+endmenu

Tools/zenoh/templates/zenoh/uorb_pubsub_factory.hpp.em

@@ -0,0 +1,154 @@
+@###############################################
+@#
+@# EmPy template for generating u.hpp file
+@# for logging purposes
+@#
+@###############################################
+@# Start of Template
+@#
+@# Context:
+@#  - topics (List) list of all topic names
+@###############################################
+@{
+
+topic_dict = dict(zip(datatypes, full_base_names))
+
+topics_count = len(topics)
+topic_names_all = list(set(topics)) # set() filters duplicates
+topic_names_all.sort()
+
+datatypes = list(set(datatypes)) # set() filters duplicates
+datatypes.sort()
+
+full_base_names = list(set(full_base_names)) # set() filters duplicates
+full_base_names.sort()
+
+
+}@
+/****************************************************************************
+ *
+ *   Copyright (c) 2023 PX4 Development Team. All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright
+ *    notice, this list of conditions and the following disclaimer.
+ * 2. Redistributions in binary form must reproduce the above copyright
+ *    notice, this list of conditions and the following disclaimer in
+ *    the documentation and/or other materials provided with the
+ *    distribution.
+ * 3. Neither the name PX4 nor the names of its contributors may be
+ *    used to endorse or promote products derived from this software
+ *    without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
+ * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
+ * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *
+ ****************************************************************************/
+
+/**
+ * uorb_pubsub_factory.hpp
+ *
+ * Defines generic, templatized uORB over Zenoh / ROS2
+ */
+
+#pragma once
+
+#include <publishers/uorb_publisher.hpp>
+#include <uORB/topics/uORBTopics.hpp>
+@[for idx, topic_name in enumerate(full_base_names)]@
+#include <px4/msg/@(topic_name).h>
+@[end for]
+
+@[for idx, topic_name in enumerate(datatypes)]@
+@{
+type_topic_count = len([e for e in topic_names_all if e.startswith(topic_name)])
+}@
+#ifdef CONFIG_ZENOH_PUBSUB_@(topic_name.upper())
+#  define CONFIG_ZENOH_PUBSUB_@(topic_name.upper())_COUNT @(type_topic_count)
+#else
+#  define CONFIG_ZENOH_PUBSUB_@(topic_name.upper())_COUNT 0
+#endif
+@[end for]
+
+#define ZENOH_PUBSUB_COUNT \
+@[for idx, topic_name in enumerate(datatypes)]@
+        CONFIG_ZENOH_PUBSUB_@(topic_name.upper())_COUNT + \
+@[end for]        0
+
+typedef struct {
+	const uint32_t *ops;
+	const orb_metadata* orb_meta;
+} UorbPubSubTopicBinder;
+
+const UorbPubSubTopicBinder _topics[ZENOH_PUBSUB_COUNT] {
+@{
+uorb_id_idx = 0
+}@
+@[for idx, topic_name in enumerate(datatypes)]@
+#ifdef CONFIG_ZENOH_PUBSUB_@(topic_name.upper())
+@{
+topic_names = [e for e in topic_names_all if e.startswith(topic_name)]
+}@
+@[for topic_name_inst in topic_names]@
+		{
+		  px4_msg_@(topic_dict[topic_name])_cdrstream_desc.ops.ops,
+		  ORB_ID(@(topic_name_inst))
+		},
+@{
+uorb_id_idx += 1
+}@
+@[end for]#endif
+@[end for]
+};
+
+uORB_Zenoh_Publisher* genPublisher(const orb_metadata *meta) {
+    for (auto &pub : _topics) {
+        if(pub.orb_meta->o_id == meta->o_id) {
+            return new uORB_Zenoh_Publisher(meta, pub.ops);
+        }
+    }
+    return NULL;
+}
+
+
+uORB_Zenoh_Publisher* genPublisher(const char *name) {
+    for (auto &pub : _topics) {
+        if(strcmp(pub.orb_meta->o_name, name) == 0) {
+            return new uORB_Zenoh_Publisher(pub.orb_meta, pub.ops);
+        }
+    }
+    return NULL;
+}
+
+
+Zenoh_Subscriber* genSubscriber(const orb_metadata *meta) {
+    for (auto &sub : _topics) {
+        if(sub.orb_meta->o_id == meta->o_id) {
+            return new uORB_Zenoh_Subscriber(meta, sub.ops);
+        }
+    }
+    return NULL;
+}
+
+
+Zenoh_Subscriber* genSubscriber(const char *name) {
+    for (auto &sub : _topics) {
+        if(strcmp(sub.orb_meta->o_name, name) == 0) {
+            return new uORB_Zenoh_Subscriber(sub.orb_meta, sub.ops);
+        }
+    }
+    return NULL;
+}

boards/airmind/mindpx-v2/default.px4board

@@ -16,8 +16,8 @@ CONFIG_DRIVERS_GPS=y
 CONFIG_DRIVERS_IMU_INVENSENSE_ICM20948=y
 CONFIG_DRIVERS_IMU_INVENSENSE_MPU6000=y
 CONFIG_DRIVERS_IMU_INVENSENSE_MPU6500=y
-CONFIG_DRIVERS_IMU_L3GD20=y
-CONFIG_DRIVERS_IMU_LSM303D=y
+CONFIG_DRIVERS_IMU_ST_L3GD20=y
+CONFIG_DRIVERS_IMU_ST_LSM303D=y
 CONFIG_DRIVERS_IRLOCK=y
 CONFIG_COMMON_LIGHT=y
 CONFIG_COMMON_MAGNETOMETER=y

boards/ark/can-flow/nuttx-config/nsh/defconfig

@@ -27,7 +27,6 @@
 # CONFIG_NSH_DISABLE_LOOPS is not set
 # CONFIG_NSH_DISABLE_LS is not set
 # CONFIG_NSH_DISABLE_MKDIR is not set
-# CONFIG_NSH_DISABLE_MKFATFS is not set
 # CONFIG_NSH_DISABLE_MOUNT is not set
 # CONFIG_NSH_DISABLE_MV is not set
 # CONFIG_NSH_DISABLE_PS is not set
@@ -66,17 +65,8 @@ CONFIG_DEBUG_HARDFAULT_ALERT=y
 CONFIG_DEBUG_SYMBOLS=y
 CONFIG_DEBUG_TCBINFO=y
 CONFIG_DEFAULT_SMALL=y
-CONFIG_DEV_FIFO_SIZE=0
-CONFIG_DEV_PIPE_MAXSIZE=1024
-CONFIG_DEV_PIPE_SIZE=70
 CONFIG_FDCLONE_STDIO=y
-CONFIG_FS_BINFS=y
 CONFIG_FS_CROMFS=y
-CONFIG_FS_FAT=y
-CONFIG_FS_FATTIME=y
-CONFIG_FS_PROCFS=y
-CONFIG_FS_PROCFS_INCLUDE_PROGMEM=y
-CONFIG_FS_PROCFS_REGISTER=y
 CONFIG_FS_ROMFS=y
 CONFIG_GRAN=y
 CONFIG_GRAN_INTR=y
@@ -88,14 +78,9 @@ CONFIG_INIT_STACKSIZE=2624
 CONFIG_LIBC_FLOATINGPOINT=y
 CONFIG_LIBC_LONG_LONG=y
 CONFIG_LIBC_MAX_EXITFUNS=1
-CONFIG_LIBC_STRERROR=y
 CONFIG_MEMSET_64BIT=y
 CONFIG_MEMSET_OPTSPEED=y
 CONFIG_MM_REGIONS=2
-CONFIG_MTD=y
-CONFIG_MTD_BYTE_WRITE=y
-CONFIG_MTD_PARTITION=y
-CONFIG_MTD_RAMTRON=y
 CONFIG_NAME_MAX=40
 CONFIG_NSH_ARCHINIT=y
 CONFIG_NSH_ARGCAT=y
@@ -108,31 +93,20 @@ CONFIG_NSH_NESTDEPTH=8
 CONFIG_NSH_QUOTE=y
 CONFIG_NSH_ROMFSETC=y
 CONFIG_NSH_ROMFSSECTSIZE=128
-CONFIG_NSH_STRERROR=y
 CONFIG_NSH_VARS=y
-CONFIG_PIPES=y
 CONFIG_PREALLOC_TIMERS=50
-CONFIG_PRIORITY_INHERITANCE=y
 CONFIG_PTHREAD_MUTEX_ROBUST=y
 CONFIG_PTHREAD_STACK_MIN=512
-CONFIG_RAMTRON_EMULATE_PAGE_SHIFT=5
-CONFIG_RAMTRON_EMULATE_SECTOR_SHIFT=5
-CONFIG_RAMTRON_SETSPEED=y
 CONFIG_RAM_SIZE=262144
 CONFIG_RAM_START=0x20000000
 CONFIG_RAW_BINARY=y
-CONFIG_RTC_DATETIME=y
 CONFIG_SCHED_HPWORK=y
 CONFIG_SCHED_HPWORKPRIORITY=254
 CONFIG_SCHED_HPWORKSTACKSIZE=3000
 CONFIG_SCHED_INSTRUMENTATION=y
 CONFIG_SCHED_INSTRUMENTATION_EXTERNAL=y
 CONFIG_SCHED_INSTRUMENTATION_SWITCH=y
-CONFIG_SCHED_LPWORK=y
-CONFIG_SCHED_LPWORKPRIORITY=50
-CONFIG_SCHED_LPWORKSTACKSIZE=1632
 CONFIG_SCHED_WAITPID=y
-CONFIG_SEM_PREALLOCHOLDERS=32
 CONFIG_SERIAL_TERMIOS=y
 CONFIG_SIG_DEFAULT=y
 CONFIG_SIG_SIGALRM_ACTION=y
@@ -151,11 +125,6 @@ CONFIG_STM32_FLASH_PREFETCH=y
 CONFIG_STM32_FLOWCONTROL_BROKEN=y
 CONFIG_STM32_JTAG_SW_ENABLE=y
 CONFIG_STM32_PWR=y
-CONFIG_STM32_RTC=y
-CONFIG_STM32_RTC_HSECLOCK=y
-CONFIG_STM32_RTC_MAGIC=0xfacefeee
-CONFIG_STM32_RTC_MAGIC_REG=1
-CONFIG_STM32_RTC_MAGIC_TIME_SET=0xfacefeef
 CONFIG_STM32_SERIALBRK_BSDCOMPAT=y
 CONFIG_STM32_SERIAL_DISABLE_REORDERING=y
 CONFIG_STM32_SPI1=y

boards/ark/can-flow/src/init.c

@@ -156,8 +156,7 @@ __EXPORT int board_app_initialize(uintptr_t arg)
 	led_on(LED_BLUE);
 
 	/* Configure the HW based on the manifest */
-
-	px4_platform_configure();
+	//px4_platform_configure();
 
 	return OK;
 }

boards/ark/can-gps/nuttx-config/nsh/defconfig

@@ -27,7 +27,6 @@
 # CONFIG_NSH_DISABLE_LOOPS is not set
 # CONFIG_NSH_DISABLE_LS is not set
 # CONFIG_NSH_DISABLE_MKDIR is not set
-# CONFIG_NSH_DISABLE_MKFATFS is not set
 # CONFIG_NSH_DISABLE_MOUNT is not set
 # CONFIG_NSH_DISABLE_MV is not set
 # CONFIG_NSH_DISABLE_PS is not set
@@ -66,17 +65,8 @@ CONFIG_DEBUG_HARDFAULT_ALERT=y
 CONFIG_DEBUG_SYMBOLS=y
 CONFIG_DEBUG_TCBINFO=y
 CONFIG_DEFAULT_SMALL=y
-CONFIG_DEV_FIFO_SIZE=0
-CONFIG_DEV_PIPE_MAXSIZE=1024
-CONFIG_DEV_PIPE_SIZE=70
 CONFIG_FDCLONE_STDIO=y
-CONFIG_FS_BINFS=y
 CONFIG_FS_CROMFS=y
-CONFIG_FS_FAT=y
-CONFIG_FS_FATTIME=y
-CONFIG_FS_PROCFS=y
-CONFIG_FS_PROCFS_INCLUDE_PROGMEM=y
-CONFIG_FS_PROCFS_REGISTER=y
 CONFIG_FS_ROMFS=y
 CONFIG_GRAN=y
 CONFIG_GRAN_INTR=y
@@ -90,14 +80,9 @@ CONFIG_INIT_STACKSIZE=2624
 CONFIG_LIBC_FLOATINGPOINT=y
 CONFIG_LIBC_LONG_LONG=y
 CONFIG_LIBC_MAX_EXITFUNS=1
-CONFIG_LIBC_STRERROR=y
 CONFIG_MEMSET_64BIT=y
 CONFIG_MEMSET_OPTSPEED=y
 CONFIG_MM_REGIONS=2
-CONFIG_MTD=y
-CONFIG_MTD_BYTE_WRITE=y
-CONFIG_MTD_PARTITION=y
-CONFIG_MTD_RAMTRON=y
 CONFIG_NAME_MAX=40
 CONFIG_NSH_ARCHINIT=y
 CONFIG_NSH_ARGCAT=y
@@ -110,37 +95,25 @@ CONFIG_NSH_NESTDEPTH=8
 CONFIG_NSH_QUOTE=y
 CONFIG_NSH_ROMFSETC=y
 CONFIG_NSH_ROMFSSECTSIZE=128
-CONFIG_NSH_STRERROR=y
 CONFIG_NSH_VARS=y
-CONFIG_PIPES=y
 CONFIG_PREALLOC_TIMERS=50
-CONFIG_PRIORITY_INHERITANCE=y
 CONFIG_PTHREAD_MUTEX_ROBUST=y
 CONFIG_PTHREAD_STACK_MIN=512
-CONFIG_RAMTRON_EMULATE_PAGE_SHIFT=5
-CONFIG_RAMTRON_EMULATE_SECTOR_SHIFT=5
-CONFIG_RAMTRON_SETSPEED=y
 CONFIG_RAM_SIZE=262144
 CONFIG_RAM_START=0x20000000
 CONFIG_RAW_BINARY=y
-CONFIG_RTC_DATETIME=y
 CONFIG_SCHED_HPWORK=y
 CONFIG_SCHED_HPWORKPRIORITY=249
 CONFIG_SCHED_HPWORKSTACKSIZE=1280
 CONFIG_SCHED_INSTRUMENTATION=y
 CONFIG_SCHED_INSTRUMENTATION_EXTERNAL=y
 CONFIG_SCHED_INSTRUMENTATION_SWITCH=y
-CONFIG_SCHED_LPWORK=y
-CONFIG_SCHED_LPWORKPRIORITY=50
-CONFIG_SCHED_LPWORKSTACKSIZE=1632
 CONFIG_SCHED_WAITPID=y
-CONFIG_SEM_PREALLOCHOLDERS=32
 CONFIG_SERIAL_TERMIOS=y
 CONFIG_SIG_DEFAULT=y
 CONFIG_SIG_SIGALRM_ACTION=y
 CONFIG_SIG_SIGUSR1_ACTION=y
 CONFIG_SIG_SIGUSR2_ACTION=y
-CONFIG_SIG_SIGWORK=4
 CONFIG_STACK_COLORATION=y
 CONFIG_START_DAY=30
 CONFIG_START_MONTH=11
@@ -155,11 +128,6 @@ CONFIG_STM32_I2C1=y
 CONFIG_STM32_I2C2=y
 CONFIG_STM32_JTAG_SW_ENABLE=y
 CONFIG_STM32_PWR=y
-CONFIG_STM32_RTC=y
-CONFIG_STM32_RTC_HSECLOCK=y
-CONFIG_STM32_RTC_MAGIC=0xfacefeee
-CONFIG_STM32_RTC_MAGIC_REG=1
-CONFIG_STM32_RTC_MAGIC_TIME_SET=0xfacefeef
 CONFIG_STM32_SERIALBRK_BSDCOMPAT=y
 CONFIG_STM32_SERIAL_DISABLE_REORDERING=y
 CONFIG_STM32_SPI1=y

boards/ark/can-gps/src/init.c

@@ -156,8 +156,7 @@ __EXPORT int board_app_initialize(uintptr_t arg)
 #endif // FLASH_BASED_PARAMS
 
 	/* Configure the HW based on the manifest */
-
-	px4_platform_configure();
+	//px4_platform_configure();
 
 	return OK;
 }

boards/ark/can-rtk-gps/nuttx-config/nsh/defconfig

@@ -27,7 +27,6 @@
 # CONFIG_NSH_DISABLE_LOOPS is not set
 # CONFIG_NSH_DISABLE_LS is not set
 # CONFIG_NSH_DISABLE_MKDIR is not set
-# CONFIG_NSH_DISABLE_MKFATFS is not set
 # CONFIG_NSH_DISABLE_MOUNT is not set
 # CONFIG_NSH_DISABLE_MV is not set
 # CONFIG_NSH_DISABLE_PS is not set
@@ -66,17 +65,8 @@ CONFIG_DEBUG_HARDFAULT_ALERT=y
 CONFIG_DEBUG_SYMBOLS=y
 CONFIG_DEBUG_TCBINFO=y
 CONFIG_DEFAULT_SMALL=y
-CONFIG_DEV_FIFO_SIZE=0
-CONFIG_DEV_PIPE_MAXSIZE=1024
-CONFIG_DEV_PIPE_SIZE=70
 CONFIG_FDCLONE_STDIO=y
-CONFIG_FS_BINFS=y
 CONFIG_FS_CROMFS=y
-CONFIG_FS_FAT=y
-CONFIG_FS_FATTIME=y
-CONFIG_FS_PROCFS=y
-CONFIG_FS_PROCFS_INCLUDE_PROGMEM=y
-CONFIG_FS_PROCFS_REGISTER=y
 CONFIG_FS_ROMFS=y
 CONFIG_GRAN=y
 CONFIG_GRAN_INTR=y
@@ -90,14 +80,9 @@ CONFIG_INIT_STACKSIZE=2624
 CONFIG_LIBC_FLOATINGPOINT=y
 CONFIG_LIBC_LONG_LONG=y
 CONFIG_LIBC_MAX_EXITFUNS=1
-CONFIG_LIBC_STRERROR=y
 CONFIG_MEMSET_64BIT=y
 CONFIG_MEMSET_OPTSPEED=y
 CONFIG_MM_REGIONS=2
-CONFIG_MTD=y
-CONFIG_MTD_BYTE_WRITE=y
-CONFIG_MTD_PARTITION=y
-CONFIG_MTD_RAMTRON=y
 CONFIG_NAME_MAX=40
 CONFIG_NSH_ARCHINIT=y
 CONFIG_NSH_ARGCAT=y
@@ -110,31 +95,20 @@ CONFIG_NSH_NESTDEPTH=8
 CONFIG_NSH_QUOTE=y
 CONFIG_NSH_ROMFSETC=y
 CONFIG_NSH_ROMFSSECTSIZE=128
-CONFIG_NSH_STRERROR=y
 CONFIG_NSH_VARS=y
-CONFIG_PIPES=y
 CONFIG_PREALLOC_TIMERS=50
-CONFIG_PRIORITY_INHERITANCE=y
 CONFIG_PTHREAD_MUTEX_ROBUST=y
 CONFIG_PTHREAD_STACK_MIN=512
-CONFIG_RAMTRON_EMULATE_PAGE_SHIFT=5
-CONFIG_RAMTRON_EMULATE_SECTOR_SHIFT=5
-CONFIG_RAMTRON_SETSPEED=y
 CONFIG_RAM_SIZE=262144
 CONFIG_RAM_START=0x20000000
 CONFIG_RAW_BINARY=y
-CONFIG_RTC_DATETIME=y
 CONFIG_SCHED_HPWORK=y
 CONFIG_SCHED_HPWORKPRIORITY=249
 CONFIG_SCHED_HPWORKSTACKSIZE=1280
 CONFIG_SCHED_INSTRUMENTATION=y
 CONFIG_SCHED_INSTRUMENTATION_EXTERNAL=y
 CONFIG_SCHED_INSTRUMENTATION_SWITCH=y
-CONFIG_SCHED_LPWORK=y
-CONFIG_SCHED_LPWORKPRIORITY=50
-CONFIG_SCHED_LPWORKSTACKSIZE=1632
 CONFIG_SCHED_WAITPID=y
-CONFIG_SEM_PREALLOCHOLDERS=32
 CONFIG_SERIAL_TERMIOS=y
 CONFIG_SIG_DEFAULT=y
 CONFIG_SIG_SIGALRM_ACTION=y
@@ -155,11 +129,6 @@ CONFIG_STM32_I2C1=y
 CONFIG_STM32_I2C2=y
 CONFIG_STM32_JTAG_SW_ENABLE=y
 CONFIG_STM32_PWR=y
-CONFIG_STM32_RTC=y
-CONFIG_STM32_RTC_HSECLOCK=y
-CONFIG_STM32_RTC_MAGIC=0xfacefeee
-CONFIG_STM32_RTC_MAGIC_REG=1
-CONFIG_STM32_RTC_MAGIC_TIME_SET=0xfacefeef
 CONFIG_STM32_SERIALBRK_BSDCOMPAT=y
 CONFIG_STM32_SERIAL_DISABLE_REORDERING=y
 CONFIG_STM32_SPI1=y

boards/ark/can-rtk-gps/src/init.c

@@ -162,8 +162,7 @@ __EXPORT int board_app_initialize(uintptr_t arg)
 #endif // FLASH_BASED_PARAMS
 
 	/* Configure the HW based on the manifest */
-
-	px4_platform_configure();
+	//px4_platform_configure();
 
 	return OK;
 }

boards/ark/cannode/nuttx-config/nsh/defconfig

@@ -27,7 +27,6 @@
 # CONFIG_NSH_DISABLE_LOOPS is not set
 # CONFIG_NSH_DISABLE_LS is not set
 # CONFIG_NSH_DISABLE_MKDIR is not set
-# CONFIG_NSH_DISABLE_MKFATFS is not set
 # CONFIG_NSH_DISABLE_MOUNT is not set
 # CONFIG_NSH_DISABLE_MV is not set
 # CONFIG_NSH_DISABLE_PS is not set
@@ -66,17 +65,8 @@ CONFIG_DEBUG_HARDFAULT_ALERT=y
 CONFIG_DEBUG_SYMBOLS=y
 CONFIG_DEBUG_TCBINFO=y
 CONFIG_DEFAULT_SMALL=y
-CONFIG_DEV_FIFO_SIZE=0
-CONFIG_DEV_PIPE_MAXSIZE=1024
-CONFIG_DEV_PIPE_SIZE=70
 CONFIG_FDCLONE_STDIO=y
-CONFIG_FS_BINFS=y
 CONFIG_FS_CROMFS=y
-CONFIG_FS_FAT=y
-CONFIG_FS_FATTIME=y
-CONFIG_FS_PROCFS=y
-CONFIG_FS_PROCFS_INCLUDE_PROGMEM=y
-CONFIG_FS_PROCFS_REGISTER=y
 CONFIG_FS_ROMFS=y
 CONFIG_GRAN=y
 CONFIG_GRAN_INTR=y
@@ -90,14 +80,9 @@ CONFIG_INIT_STACKSIZE=2624
 CONFIG_LIBC_FLOATINGPOINT=y
 CONFIG_LIBC_LONG_LONG=y
 CONFIG_LIBC_MAX_EXITFUNS=1
-CONFIG_LIBC_STRERROR=y
 CONFIG_MEMSET_64BIT=y
 CONFIG_MEMSET_OPTSPEED=y
 CONFIG_MM_REGIONS=2
-CONFIG_MTD=y
-CONFIG_MTD_BYTE_WRITE=y
-CONFIG_MTD_PARTITION=y
-CONFIG_MTD_RAMTRON=y
 CONFIG_NAME_MAX=40
 CONFIG_NSH_ARCHINIT=y
 CONFIG_NSH_ARGCAT=y
@@ -110,31 +95,20 @@ CONFIG_NSH_NESTDEPTH=8
 CONFIG_NSH_QUOTE=y
 CONFIG_NSH_ROMFSETC=y
 CONFIG_NSH_ROMFSSECTSIZE=128
-CONFIG_NSH_STRERROR=y
 CONFIG_NSH_VARS=y
-CONFIG_PIPES=y
 CONFIG_PREALLOC_TIMERS=50
-CONFIG_PRIORITY_INHERITANCE=y
 CONFIG_PTHREAD_MUTEX_ROBUST=y
 CONFIG_PTHREAD_STACK_MIN=512
-CONFIG_RAMTRON_EMULATE_PAGE_SHIFT=5
-CONFIG_RAMTRON_EMULATE_SECTOR_SHIFT=5
-CONFIG_RAMTRON_SETSPEED=y
 CONFIG_RAM_SIZE=262144
 CONFIG_RAM_START=0x20000000
 CONFIG_RAW_BINARY=y
-CONFIG_RTC_DATETIME=y
 CONFIG_SCHED_HPWORK=y
 CONFIG_SCHED_HPWORKPRIORITY=254
 CONFIG_SCHED_HPWORKSTACKSIZE=3000
 CONFIG_SCHED_INSTRUMENTATION=y
 CONFIG_SCHED_INSTRUMENTATION_EXTERNAL=y
 CONFIG_SCHED_INSTRUMENTATION_SWITCH=y
-CONFIG_SCHED_LPWORK=y
-CONFIG_SCHED_LPWORKPRIORITY=50
-CONFIG_SCHED_LPWORKSTACKSIZE=1632
 CONFIG_SCHED_WAITPID=y
-CONFIG_SEM_PREALLOCHOLDERS=32
 CONFIG_SERIAL_TERMIOS=y
 CONFIG_SIG_DEFAULT=y
 CONFIG_SIG_SIGALRM_ACTION=y
@@ -155,11 +129,6 @@ CONFIG_STM32_FLOWCONTROL_BROKEN=y
 CONFIG_STM32_I2C1=y
 CONFIG_STM32_JTAG_SW_ENABLE=y
 CONFIG_STM32_PWR=y
-CONFIG_STM32_RTC=y
-CONFIG_STM32_RTC_HSECLOCK=y
-CONFIG_STM32_RTC_MAGIC=0xfacefeee
-CONFIG_STM32_RTC_MAGIC_REG=1
-CONFIG_STM32_RTC_MAGIC_TIME_SET=0xfacefeef
 CONFIG_STM32_SERIALBRK_BSDCOMPAT=y
 CONFIG_STM32_SERIAL_DISABLE_REORDERING=y
 CONFIG_STM32_SPI1=y

boards/ark/cannode/src/init.c

@@ -180,8 +180,7 @@ __EXPORT int board_app_initialize(uintptr_t arg)
 #endif // FLASH_BASED_PARAMS
 
 	/* Configure the HW based on the manifest */
-
-	px4_platform_configure();
+	//px4_platform_configure();
 
 	return OK;
 }

boards/ark/fmu-v6x/init/rc.board_sensors

@@ -81,13 +81,16 @@ fi
 if ver hwtypecmp ARKV6X000001 ARKV6X001001 ARKV6X002001 ARKV6X003001 ARKV6X004001 ARKV6X005001 ARKV6X006001 ARKV6X007001
 then
 	# Internal SPI bus IIM42653 with SPIX measured frequency of 32.051kHz
-	iim42653 -R 3 -s -b 1 -C 32051 start
+	#iim42653 -R 3 -s -b 1 -C 32051 start
+	iim42653 -R 3 -s -b 1 start
 
 	# Internal SPI bus IIM42653 with SPIX measured frequency of 32.051kHz
-	iim42653 -R 9 -s -b 2 -C 32051 start
+	#iim42653 -R 9 -s -b 2 -C 32051 start
+	iim42653 -R 9 -s -b 2 start
 
 	# Internal SPI bus IIM42653 with SPIX measured frequency of 32.051kHz
-	iim42653 -R 6 -s -b 3 -C 32051 start
+	#iim42653 -R 6 -s -b 3 -C 32051 start
+	iim42653 -R 6 -s -b 3 start
 fi
 
 # Internal magnetometer on I2C

boards/ark/fmu-v6x/src/board_config.h

@@ -247,7 +247,6 @@
 /* PWM
  */
 #define DIRECT_PWM_OUTPUT_CHANNELS   8
-#define BOARD_PWM_FREQ	             1024000
 
 #define GPIO_FMU_CH1                    /* PI0  */ (GPIO_INPUT|GPIO_PULLDOWN|GPIO_PORTI|GPIO_PIN0)
 #define GPIO_FMU_CH2                    /* PH12 */ (GPIO_INPUT|GPIO_PULLDOWN|GPIO_PORTH|GPIO_PIN12)

boards/ark/fmu-v6x/src/spix_sync.c

@@ -82,9 +82,7 @@
 #define rDMAR(_tmr)     REG(_tmr, STM32_GTIM_DMAR_OFFSET)
 #define rBDTR(_tmr)     REG(_tmr, STM32_ATIM_BDTR_OFFSET)
 
-#if !defined(BOARD_PWM_FREQ)
-#define BOARD_PWM_FREQ 1000000
-#endif
+#define BOARD_SPIX_SYNC_PWM_FREQ 1024000
 
 unsigned
 spix_sync_timer_get_period(unsigned timer)
@@ -129,11 +127,11 @@ static void spix_sync_timer_init_timer(unsigned timer, unsigned rate)
 		 * Otherwise, other frequencies are attainable by adjusting .clock_freq accordingly.
 		 */
 
-		rPSC(timer) = (spix_sync_timers[timer].clock_freq / BOARD_PWM_FREQ) - 1;
+		rPSC(timer) = (spix_sync_timers[timer].clock_freq / BOARD_SPIX_SYNC_PWM_FREQ) - 1;
 
 		/* configure the timer to update at the desired rate */
 
-		rARR(timer) = (BOARD_PWM_FREQ / rate) - 1;
+		rARR(timer) = (BOARD_SPIX_SYNC_PWM_FREQ / rate) - 1;
 
 		/* generate an update event; reloads the counter and all registers */
 		rEGR(timer) = GTIM_EGR_UG;

boards/av/x-v1/default.px4board

@@ -15,8 +15,8 @@ CONFIG_DRIVERS_CAMERA_TRIGGER=y
 CONFIG_COMMON_DIFFERENTIAL_PRESSURE=y
 CONFIG_COMMON_DISTANCE_SENSOR=y
 CONFIG_DRIVERS_GPS=y
-CONFIG_DRIVERS_IMU_ADIS16477=y
-CONFIG_DRIVERS_IMU_ADIS16497=y
+CONFIG_DRIVERS_IMU_ANALOG_DEVICES_ADIS16477=y
+CONFIG_DRIVERS_IMU_ANALOG_DEVICES_ADIS16497=y
 CONFIG_DRIVERS_IMU_INVENSENSE_ICM20948=y
 CONFIG_DRIVERS_IRLOCK=y
 CONFIG_COMMON_LIGHT=y

boards/bitcraze/crazyflie/default.px4board

@@ -15,7 +15,7 @@ CONFIG_MODULES_COMMANDER=y
 CONFIG_MODULES_CONTROL_ALLOCATOR=y
 CONFIG_MODULES_DATAMAN=y
 CONFIG_MODULES_EKF2=y
-# CONFIG_EKF2_GNSS_YAW is not set
+# CONFIG_EKF2_GNSS is not set
 # CONFIG_EKF2_SIDESLIP is not set
 CONFIG_MODULES_EVENTS=y
 CONFIG_MODULES_FLIGHT_MODE_MANAGER=y

boards/bitcraze/crazyflie21/default.px4board

@@ -14,7 +14,7 @@ CONFIG_MODULES_COMMANDER=y
 CONFIG_MODULES_CONTROL_ALLOCATOR=y
 CONFIG_MODULES_DATAMAN=y
 CONFIG_MODULES_EKF2=y
-# CONFIG_EKF2_GNSS_YAW is not set
+# CONFIG_EKF2_GNSS is not set
 # CONFIG_EKF2_MAGNETOMETER is not set
 # CONFIG_EKF2_SIDESLIP is not set
 CONFIG_MODULES_EVENTS=y

boards/matek/h743-slim/default.px4board

@@ -12,6 +12,7 @@ CONFIG_DRIVERS_DSHOT=y
 CONFIG_DRIVERS_GPS=y
 CONFIG_DRIVERS_IMU_INVENSENSE_ICM20602=y
 CONFIG_DRIVERS_IMU_INVENSENSE_ICM42605=y
+CONFIG_DRIVERS_IMU_INVENSENSE_ICM42688P=y
 CONFIG_DRIVERS_IMU_INVENSENSE_MPU6000=y
 CONFIG_COMMON_MAGNETOMETER=y
 CONFIG_COMMON_OPTICAL_FLOW=y

boards/matek/h743-slim/init/rc.board_sensors

@@ -5,15 +5,25 @@
 
 board_adc start
 
-# Internal SPI bus ICM-42605
-if ! icm42605 -R 14 -s start
+# Different board versions have different IMUs, so we try all known options
+
+# Internal SPI bus ICM-42688P (SPI1) on V3 board, PITCH180 orientation
+if ! icm42688p -s -b 1 -R 12 start
 then
-	# internal SPI bus ICM-20602
-	icm20602 -R 12 -s start
+	# Internal SPI bus MPU-6000 on V1.0 and V1.5 boards
+	mpu6000 -s -b 1 -R 12 start
 fi
 
-# Internal SPI bus MPU-6000
-mpu6000 -R 12 -s start
+# Internal SPI bus ICM-42688P (SPI4) on V3 board, PITCH180_YAW90 orientation
+if ! icm42688p -s -b 4 -R 26 start
+then
+	# Internal SPI bus ICM-42605 on V1.5 board, ROTATION_ROLL_180_YAW_270 orientation
+	if ! icm42605 -s -b 4 -R 14 start
+	then
+		# Internal SPI bus ICM-20602 on V1.0 board, PITCH180 orientation
+		icm20602 -s -b 4 -R 12 start
+	fi
+fi
 
 # Internal baro
 dps310 -I start -a 118

boards/matek/h743-slim/src/spi.cpp

@@ -38,7 +38,11 @@
 
 constexpr px4_spi_bus_t px4_spi_buses[SPI_BUS_MAX_BUS_ITEMS] = {
 	initSPIBus(SPI::Bus::SPI1, {
+		// Matek H743 Slim V1.0 and V1.5
 		initSPIDevice(DRV_IMU_DEVTYPE_MPU6000, SPI::CS{GPIO::PortC, GPIO::Pin15}, SPI::DRDY{GPIO::PortB, GPIO::Pin2}),
+
+		// Matek H743 Slim V3
+		initSPIDevice(DRV_IMU_DEVTYPE_ICM42688P, SPI::CS{GPIO::PortC, GPIO::Pin15}, SPI::DRDY{GPIO::PortB, GPIO::Pin2}),
 	}),
 	initSPIBus(SPI::Bus::SPI2, {
 		initSPIDevice(DRV_OSD_DEVTYPE_ATXXXX, SPI::CS{GPIO::PortB, GPIO::Pin12}),
@@ -48,8 +52,14 @@ constexpr px4_spi_bus_t px4_spi_buses[SPI_BUS_MAX_BUS_ITEMS] = {
 		initSPIConfigExternal(SPI::CS{GPIO::PortE, GPIO::Pin2}),
 	}),
 	initSPIBus(SPI::Bus::SPI4, {
-		initSPIDevice(DRV_IMU_DEVTYPE_ICM42605, SPI::CS{GPIO::PortC, GPIO::Pin13}),
+		// Matek H743 Slim V1.0
 		initSPIDevice(DRV_IMU_DEVTYPE_ICM20602, SPI::CS{GPIO::PortE, GPIO::Pin11}),
+
+		// Matek H743 Slim V1.5
+		initSPIDevice(DRV_IMU_DEVTYPE_ICM42605, SPI::CS{GPIO::PortC, GPIO::Pin13}),
+
+		// Matek H743 Slim V3
+		initSPIDevice(DRV_IMU_DEVTYPE_ICM42688P, SPI::CS{GPIO::PortC, GPIO::Pin13}),
 	}),
 };
 

boards/modalai/voxl2-slpi/src/drivers/icm42688p/InvenSense_ICM42688P_registers.hpp

@@ -133,6 +133,15 @@ enum INT_CONFIG_BIT : uint8_t {
 	INT1_POLARITY       = Bit0,
 };
 
+// INTF_CONFIG0
+enum INTF_CONFIG0_BIT : uint8_t {
+	FIFO_HOLD_LAST_DATA_EN  = Bit7,
+	FIFO_COUNT_REC          = Bit6,
+	FIFO_COUNT_ENDIAN       = Bit5,
+	SENSOR_DATA_ENDIAN      = Bit4,
+	UI_SIFS_CFG_DISABLE_I2C = Bit1 | Bit0,
+};
+
 // FIFO_CONFIG
 enum FIFO_CONFIG_BIT : uint8_t {
 	// 7:6 FIFO_MODE

boards/nxp/fmuk66-v3/default.px4board

@@ -15,8 +15,8 @@ CONFIG_COMMON_DIFFERENTIAL_PRESSURE=y
 CONFIG_COMMON_DISTANCE_SENSOR=y
 CONFIG_DRIVERS_DISTANCE_SENSOR_SRF05=y
 CONFIG_DRIVERS_GPS=y
-CONFIG_DRIVERS_IMU_FXAS21002C=y
-CONFIG_DRIVERS_IMU_FXOS8701CQ=y
+CONFIG_DRIVERS_IMU_NXP_FXAS21002C=y
+CONFIG_DRIVERS_IMU_NXP_FXOS8701CQ=y
 CONFIG_DRIVERS_IMU_INVENSENSE_ICM20948=y
 CONFIG_DRIVERS_IRLOCK=y
 CONFIG_DRIVERS_LIGHTS_RGBLED=y

boards/nxp/mr-canhubk3/nuttx-config/nsh/defconfig

@@ -85,6 +85,7 @@ CONFIG_DEFAULT_SMALL=y
 CONFIG_DEV_FIFO_SIZE=0
 CONFIG_DEV_PIPE_MAXSIZE=1024
 CONFIG_DEV_PIPE_SIZE=70
+CONFIG_DEV_URANDOM=y
 CONFIG_ETH0_PHY_TJA1103=y
 CONFIG_FAT_DMAMEMORY=y
 CONFIG_FAT_LCNAMES=y
@@ -175,6 +176,7 @@ CONFIG_NET_CAN_SOCK_OPTS=y
 CONFIG_NET_ETH_PKTSIZE=1518
 CONFIG_NET_ICMP=y
 CONFIG_NET_ICMP_SOCKET=y
+CONFIG_NET_IGMP=y
 CONFIG_NET_NACTIVESOCKETS=16
 CONFIG_NET_SOLINGER=y
 CONFIG_NET_TCP=y

boards/nxp/mr-canhubk3/zenoh.px4board

@@ -0,0 +1,58 @@
+# CONFIG_BOARD_ROMFSROOT is not set
+CONFIG_BOARD_SERIAL_GPS1="/dev/ttyS1"
+CONFIG_BOARD_SERIAL_GPS2="/dev/ttyS4"
+CONFIG_BOARD_SERIAL_RC="/dev/ttyS5"
+CONFIG_BOARD_SERIAL_TEL1="/dev/ttyS2"
+CONFIG_BOARD_SERIAL_TEL4="/dev/ttyS3"
+CONFIG_BOARD_UAVCAN_INTERFACES=1
+CONFIG_COMMON_LIGHT=y
+CONFIG_DRIVERS_ADC_BOARD_ADC=y
+CONFIG_DRIVERS_GPS=y
+CONFIG_DRIVERS_IRLOCK=y
+CONFIG_DRIVERS_MAGNETOMETER_ISENTEK_IST8310=y
+CONFIG_DRIVERS_MAGNETOMETER_LIS3MDL=y
+CONFIG_DRIVERS_RC_INPUT=y
+CONFIG_DRIVERS_SAFETY_BUTTON=y
+CONFIG_DRIVERS_UAVCAN=y
+CONFIG_EXAMPLES_FAKE_GPS=y
+CONFIG_MODULES_AIRSPEED_SELECTOR=y
+CONFIG_MODULES_ATTITUDE_ESTIMATOR_Q=y
+CONFIG_MODULES_BATTERY_STATUS=y
+CONFIG_MODULES_CAMERA_FEEDBACK=y
+CONFIG_MODULES_COMMANDER=y
+CONFIG_MODULES_ESC_BATTERY=y
+CONFIG_MODULES_EVENTS=y
+CONFIG_MODULES_FW_ATT_CONTROL=y
+CONFIG_MODULES_FW_AUTOTUNE_ATTITUDE_CONTROL=y
+CONFIG_MODULES_FW_POS_CONTROL=y
+CONFIG_MODULES_FW_RATE_CONTROL=y
+CONFIG_MODULES_GYRO_CALIBRATION=y
+CONFIG_MODULES_GYRO_FFT=y
+CONFIG_MODULES_LANDING_TARGET_ESTIMATOR=y
+CONFIG_MODULES_LAND_DETECTOR=y
+CONFIG_MODULES_LOAD_MON=y
+CONFIG_MODULES_LOCAL_POSITION_ESTIMATOR=y
+CONFIG_MODULES_LOGGER=y
+CONFIG_MODULES_MAG_BIAS_ESTIMATOR=y
+CONFIG_MODULES_MANUAL_CONTROL=y
+CONFIG_MODULES_MC_ATT_CONTROL=y
+CONFIG_MODULES_MC_AUTOTUNE_ATTITUDE_CONTROL=y
+CONFIG_MODULES_MC_HOVER_THRUST_ESTIMATOR=y
+CONFIG_MODULES_MC_POS_CONTROL=y
+CONFIG_MODULES_MC_RATE_CONTROL=y
+CONFIG_MODULES_NAVIGATOR=y
+CONFIG_MODULES_RC_UPDATE=y
+CONFIG_MODULES_ROVER_POS_CONTROL=y
+CONFIG_MODULES_TEMPERATURE_COMPENSATION=y
+CONFIG_MODULES_VTOL_ATT_CONTROL=y
+CONFIG_MODULES_ZENOH=y
+CONFIG_SYSTEMCMDS_ACTUATOR_TEST=y
+CONFIG_SYSTEMCMDS_DMESG=y
+CONFIG_SYSTEMCMDS_DUMPFILE=y
+CONFIG_SYSTEMCMDS_HARDFAULT_LOG=y
+CONFIG_SYSTEMCMDS_NETMAN=y
+CONFIG_SYSTEMCMDS_NSHTERM=y
+CONFIG_SYSTEMCMDS_PERF=y
+CONFIG_SYSTEMCMDS_REFLECT=y
+CONFIG_SYSTEMCMDS_SERIAL_TEST=y
+CONFIG_SYSTEMCMDS_TUNE_CONTROL=y

boards/nxp/ucans32k146/init/rc.board_defaults

@@ -12,3 +12,5 @@ if param compare -s CYPHAL_ENABLE 1
 then
     cyphal start
 fi
+
+rgbled_ncp5623c start -X -a 56

boards/nxp/ucans32k146/src/boot.c

@@ -45,6 +45,7 @@
 #include <nuttx/board.h>
 
 #include "board_config.h"
+#include <drivers/bootloaders/boot_app_shared.h>
 
 /****************************************************************************
  * Public Functions
@@ -61,15 +62,30 @@
  *
  ****************************************************************************/
 
+static bootloader_app_shared_t can0_config;
+
+int weak_function board_app_shared_read(bootloader_app_shared_t *shared, eRole_t role)
+{
+	int rv = -EBADR;
+
+	if (can0_config.signature != 0) {
+		*shared = can0_config;
+		rv = OK;
+	}
+
+	return rv;
+}
+
 void s32k1xx_board_initialize(void)
 {
+	can0_config.signature = 0;
+	bootloader_app_shared_read(&can0_config, BootLoader);
 #ifdef CONFIG_ARCH_LEDS
 	/* Configure on-board LEDs if LED support has been selected. */
 
 	board_autoled_initialize();
 #endif
 	ucans32k_timer_initialize();
-
 }
 
 /****************************************************************************

boards/px4/fmu-v2/default.px4board

@@ -11,8 +11,8 @@ CONFIG_DRIVERS_ADC_BOARD_ADC=y
 CONFIG_DRIVERS_BAROMETER_MS5611=y
 CONFIG_DRIVERS_GPS=y
 CONFIG_DRIVERS_IMU_INVENSENSE_MPU6000=y
-CONFIG_DRIVERS_IMU_L3GD20=y
-CONFIG_DRIVERS_IMU_LSM303D=y
+CONFIG_DRIVERS_IMU_ST_L3GD20=y
+CONFIG_DRIVERS_IMU_ST_LSM303D=y
 CONFIG_DRIVERS_LIGHTS_RGBLED=y
 CONFIG_DRIVERS_MAGNETOMETER_HMC5883=y
 CONFIG_DRIVERS_PWM_OUT=y

boards/px4/fmu-v3/default.px4board

@@ -21,8 +21,8 @@ CONFIG_DRIVERS_IMU_INVENSENSE_ICM20608G=y
 CONFIG_DRIVERS_IMU_INVENSENSE_ICM20948=y
 CONFIG_DRIVERS_IMU_INVENSENSE_MPU6000=y
 CONFIG_DRIVERS_IMU_INVENSENSE_MPU9250=y
-CONFIG_DRIVERS_IMU_L3GD20=y
-CONFIG_DRIVERS_IMU_LSM303D=y
+CONFIG_DRIVERS_IMU_ST_L3GD20=y
+CONFIG_DRIVERS_IMU_ST_LSM303D=y
 CONFIG_COMMON_INS=y
 CONFIG_DRIVERS_IRLOCK=y
 CONFIG_COMMON_LIGHT=y

boards/px4/fmu-v5x/default.px4board

@@ -111,3 +111,4 @@ CONFIG_SYSTEMCMDS_UORB=y
 CONFIG_SYSTEMCMDS_USB_CONNECTED=y
 CONFIG_SYSTEMCMDS_VER=y
 CONFIG_SYSTEMCMDS_WORK_QUEUE=y
+CONFIG_MAVLINK_DIALECT="development"

boards/px4/fmu-v6x/default.px4board

@@ -77,6 +77,7 @@ CONFIG_MODULES_SENSORS=y
 CONFIG_MODULES_TEMPERATURE_COMPENSATION=y
 CONFIG_MODULES_UXRCE_DDS_CLIENT=y
 CONFIG_MODULES_VTOL_ATT_CONTROL=y
+CONFIG_MODE_NAVIGATOR_VTOL_TAKEOFF=y
 CONFIG_SYSTEMCMDS_ACTUATOR_TEST=y
 CONFIG_SYSTEMCMDS_BSONDUMP=y
 CONFIG_SYSTEMCMDS_DMESG=y

boards/px4/fmu-v6x/init/rc.board_sensors

@@ -81,32 +81,32 @@ else
 			fi
 		fi
 	fi
-fi
 
-# Internal SPI bus ICM42688p
-if ver hwtypecmp V6X009010 V6X010010
-then
-	icm42688p -R 12 -s start
-else
-	if ver hwtypecmp V6X000010
-	then
-		icm42688p -R 14 -s start
-	else
-		icm42688p -R 6 -s start
-	fi
-fi
-
-if ver hwtypecmp V6X000003 V6X001003 V6X003003 V6X000004 V6X001004 V6X004003 V6X004004 V6X005003 V6X005004
-then
-	# Internal SPI bus ICM-42670-P (hard-mounted)
-	icm42670p -R 10 -s start
-else
+	# Internal SPI bus ICM42688p
 	if ver hwtypecmp V6X009010 V6X010010
 	then
-		icm20602 -R 6 -s start
+		icm42688p -R 12 -s start
 	else
-		# Internal SPI bus ICM-20649 (hard-mounted)
-		icm20649 -R 14 -s start
+		if ver hwtypecmp V6X000010
+		then
+			icm42688p -R 14 -s start
+		else
+			icm42688p -R 6 -s start
+		fi
+	fi
+
+	if ver hwtypecmp V6X000003 V6X001003 V6X003003 V6X000004 V6X001004 V6X004003 V6X004004 V6X005003 V6X005004
+	then
+		# Internal SPI bus ICM-42670-P (hard-mounted)
+		icm42670p -R 10 -s start
+	else
+		if ver hwtypecmp V6X009010 V6X010010
+		then
+			icm20602 -R 6 -s start
+		else
+			# Internal SPI bus ICM-20649 (hard-mounted)
+			icm20649 -R 14 -s start
+		fi
 	fi
 fi
 

boards/px4/fmu-v6x/zenoh.px4board

@@ -0,0 +1,4 @@
+# CONFIG_BOARD_UAVCAN_TIMER_OVERRIDE is not set
+CONFIG_DRIVERS_UAVCAN=n
+CONFIG_MODULES_UXRCE_DDS_CLIENT=n
+CONFIG_MODULES_ZENOH=y

boards/px4/sitl/default.px4board

@@ -18,6 +18,7 @@ CONFIG_MODULES_FLIGHT_MODE_MANAGER=y
 CONFIG_MODULES_FW_ATT_CONTROL=y
 CONFIG_MODULES_FW_AUTOTUNE_ATTITUDE_CONTROL=y
 CONFIG_MODULES_FW_POS_CONTROL=y
+CONFIG_FIGURE_OF_EIGHT=y
 CONFIG_MODULES_FW_RATE_CONTROL=y
 CONFIG_MODULES_GIMBAL=y
 CONFIG_MODULES_GYRO_CALIBRATION=y

boards/px4/sitl/src/sitl_led.c

@@ -50,7 +50,7 @@ __END_DECLS
 
 static bool _led_state[2] = { false, false };
 
-__EXPORT void led_init()
+__EXPORT void led_init(void)
 {
 	PX4_DEBUG("LED_INIT");
 }

boards/px4/sitl/zenoh.px4board

@@ -0,0 +1,3 @@
+CONFIG_MODULES_UXRCE_DDS_CLIENT=n
+CONFIG_MODULES_ZENOH=y
+CONFIG_ZENOH_CONFIG_PATH="./zenoh"

boards/scumaker/pilotpi/default.px4board

@@ -16,6 +16,8 @@ CONFIG_DRIVERS_MAGNETOMETER_HMC5883=y
 CONFIG_DRIVERS_MAGNETOMETER_ISENTEK_IST8310=y
 CONFIG_DRIVERS_MAGNETOMETER_QMC5883L=y
 CONFIG_DRIVERS_PCA9685_PWM_OUT=y
+CONFIG_PCA9685_USE_EXTERNAL_CRYSTAL=y
+CONFIG_PCA9685_EXTERNAL_CRYSTAL_FREQ=25000000
 CONFIG_COMMON_RC=y
 CONFIG_DRIVERS_RC_INPUT=y
 CONFIG_DRIVERS_SMART_BATTERY_BATMON=y

cmake/kconfig.cmake

@@ -73,12 +73,18 @@ if(EXISTS ${BOARD_DEFCONFIG})
 			# Find the value
 			string(REPLACE "${Name}=" "" Value ${NameAndValue})
 
-			if(Value)
-				# remove extra quotes
-				string(REPLACE "\"" "" Value ${Value})
+			# remove extra quotes
+			string(REPLACE "\"" "" Value ${Value})
 
-				# Set the variable
-				set(${Name} ${Value} CACHE INTERNAL "BOARD DEFCONFIG: ${Name}" FORCE)
+			# Set the variable
+			set(${Name} ${Value} CACHE INTERNAL "BOARD DEFCONFIG: ${Name}" FORCE)
+
+		else()
+			# Find boolean not set
+			string(REGEX MATCH " (CONFIG[^ ]+) is not set" Name ${NameAndValue})
+
+			if(${CMAKE_MATCH_1})
+				set(${CMAKE_MATCH_1} "" CACHE INTERNAL "BOARD DEFCONFIG: ${CMAKE_MATCH_1}" FORCE)
 			endif()
 		endif()
 

msg/ArmingCheckReply.msg

@@ -0,0 +1,34 @@
+uint64 timestamp # time since system start (microseconds)
+
+uint8 request_id
+uint8 registration_id
+
+uint8 HEALTH_COMPONENT_INDEX_NONE = 0
+uint8 HEALTH_COMPONENT_INDEX_AVOIDANCE = 19
+
+uint8 health_component_index      # HEALTH_COMPONENT_INDEX_*
+bool health_component_is_present
+bool health_component_warning
+bool health_component_error
+
+bool can_arm_and_run              # whether arming is possible, and if it's a navigation mode, if it can run
+
+uint8 num_events
+
+Event[5] events
+
+# Mode requirements
+bool mode_req_angular_velocity
+bool mode_req_attitude
+bool mode_req_local_alt
+bool mode_req_local_position
+bool mode_req_local_position_relaxed
+bool mode_req_global_position
+bool mode_req_mission
+bool mode_req_home_position
+bool mode_req_prevent_arming
+bool mode_req_manual_control
+
+
+uint8 ORB_QUEUE_LENGTH = 4
+

msg/ArmingCheckRequest.msg

@@ -0,0 +1,7 @@
+uint64 timestamp # time since system start (microseconds)
+
+# broadcast message to request all registered arming checks to be reported
+
+uint8 request_id
+
+

msg/CMakeLists.txt

@@ -49,6 +49,8 @@ set(msg_files
 	Airspeed.msg
 	AirspeedValidated.msg
 	AirspeedWind.msg
+	ArmingCheckReply.msg
+	ArmingCheckRequest.msg
 	AutotuneAttitudeControlStatus.msg
 	BatteryStatus.msg
 	ButtonEvent.msg
@@ -58,6 +60,7 @@ set(msg_files
 	CellularStatus.msg
 	CollisionConstraints.msg
 	CollisionReport.msg
+	ConfigOverrides.msg
 	ControlAllocatorStatus.msg
 	Cpuload.msg
 	DatamanRequest.msg
@@ -85,6 +88,7 @@ set(msg_files
 	EstimatorStatus.msg
 	EstimatorStatusFlags.msg
 	Event.msg
+	FigureEightStatus.msg
 	FailsafeFlags.msg
 	FailureDetectorStatus.msg
 	FollowTarget.msg
@@ -129,6 +133,8 @@ set(msg_files
 	ManualControlSwitches.msg
 	MavlinkLog.msg
 	MavlinkTunnel.msg
+	MessageFormatRequest.msg
+	MessageFormatResponse.msg
 	Mission.msg
 	MissionResult.msg
 	MountOrientation.msg
@@ -160,6 +166,8 @@ set(msg_files
 	RateCtrlStatus.msg
 	RcChannels.msg
 	RcParameterMap.msg
+	RegisterExtComponentReply.msg
+	RegisterExtComponentRequest.msg
 	Rpm.msg
 	RtlTimeEstimate.msg
 	SatelliteInfo.msg
@@ -197,6 +205,7 @@ set(msg_files
 	UavcanParameterValue.msg
 	UlogStream.msg
 	UlogStreamAck.msg
+	UnregisterExtComponent.msg
 	VehicleAcceleration.msg
 	VehicleAirData.msg
 	VehicleAngularAccelerationSetpoint.msg
@@ -252,6 +261,7 @@ set(msg_source_out_path ${CMAKE_CURRENT_BINARY_DIR}/topics_sources)
 set(uorb_headers)
 set(uorb_sources)
 set(uorb_ucdr_headers)
+set(uorb_json_files)
 foreach(msg_file ${msg_files})
 	get_filename_component(msg ${msg_file} NAME_WE)
 
@@ -263,11 +273,9 @@ foreach(msg_file ${msg_files})
 	list(APPEND uorb_headers ${msg_out_path}/${msg}.h)
 	list(APPEND uorb_sources ${msg_source_out_path}/${msg}.cpp)
 	list(APPEND uorb_ucdr_headers ${ucdr_out_path}/${msg}.h)
+	list(APPEND uorb_json_files ${msg_source_out_path}/${msg}.json)
 endforeach()
 
-# 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
@@ -291,6 +299,44 @@ add_custom_command(
 	)
 add_custom_target(uorb_headers DEPENDS ${uorb_headers})
 
+add_custom_command(
+	OUTPUT
+		${uorb_json_files}
+	COMMAND ${PYTHON_EXECUTABLE} ${PX4_SOURCE_DIR}/Tools/msg/px_generate_uorb_topic_files.py
+		--json
+		-f ${msg_files}
+		-i ${CMAKE_CURRENT_SOURCE_DIR}
+		-o ${msg_source_out_path}
+		-e ${PX4_SOURCE_DIR}/Tools/msg/templates/uorb
+	DEPENDS
+		${msg_files}
+		${PX4_SOURCE_DIR}/Tools/msg/templates/uorb/msg.json.em
+		${PX4_SOURCE_DIR}/Tools/msg/px_generate_uorb_topic_files.py
+		${PX4_SOURCE_DIR}/Tools/msg/px_generate_uorb_topic_helper.py
+	COMMENT "Generating uORB json files"
+	WORKING_DIRECTORY ${CMAKE_CURRENT_SOURCE_DIR}
+	VERBATIM
+)
+add_custom_target(uorb_json_files DEPENDS ${uorb_json_files})
+
+set(uorb_message_fields_cpp_file ${msg_source_out_path}/uORBMessageFieldsGenerated.cpp)
+set(uorb_message_fields_header_file ${msg_out_path}/uORBMessageFieldsGenerated.hpp)
+add_custom_command(
+	OUTPUT
+		${uorb_message_fields_cpp_file}
+		${uorb_message_fields_header_file}
+	COMMAND ${PYTHON_EXECUTABLE} ${PX4_SOURCE_DIR}/Tools/msg/px_generate_uorb_compressed_fields.py
+		-f ${uorb_json_files}
+		--source-output-file ${uorb_message_fields_cpp_file}
+		--header-output-file ${uorb_message_fields_header_file}
+	DEPENDS
+		uorb_json_files
+		${PX4_SOURCE_DIR}/Tools/msg/px_generate_uorb_compressed_fields.py
+	COMMENT "Generating uORB compressed fields"
+	WORKING_DIRECTORY ${CMAKE_CURRENT_SOURCE_DIR}
+	VERBATIM
+)
+
 # Generate microcdr headers
 add_custom_command(
 	OUTPUT ${uorb_ucdr_headers}
@@ -333,6 +379,131 @@ add_custom_command(
 	VERBATIM
 	)
 
-add_library(uorb_msgs ${uorb_headers} ${msg_out_path}/uORBTopics.hpp ${uorb_sources} ${msg_source_out_path}/uORBTopics.cpp)
+add_library(uorb_msgs ${uorb_headers} ${msg_out_path}/uORBTopics.hpp ${uorb_sources} ${msg_source_out_path}/uORBTopics.cpp ${uorb_message_fields_cpp_file})
 target_link_libraries(uorb_msgs PRIVATE m)
 add_dependencies(uorb_msgs prebuild_targets uorb_headers)
+
+if(CONFIG_LIB_CDRSTREAM)
+	set(uorb_cdr_idl)
+	set(uorb_cdr_msg)
+	set(uorb_cdr_idl_uorb)
+	set(idl_include_path ${PX4_BINARY_DIR}/uORB/idl)
+	set(idl_out_path ${idl_include_path}/px4/msg)
+	set(idl_uorb_path ${PX4_BINARY_DIR}/msg/px4/msg)
+
+	# Make sure that CycloneDDS has been checkout out
+	execute_process(COMMAND git submodule sync src/lib/cdrstream/cyclonedds
+			WORKING_DIRECTORY ${PX4_SOURCE_DIR} )
+	execute_process(COMMAND git submodule update --init --force src/lib/cdrstream/cyclonedds
+			WORKING_DIRECTORY ${PX4_SOURCE_DIR} )
+
+	# CycloneDDS-tools doesn't ship with the cdrstream-desc feature thus we've to compile idlc from source
+	MESSAGE(STATUS "Configuring idlc :" ${CMAKE_CURRENT_BINARY_DIR}/idlc)
+	file(MAKE_DIRECTORY ${CMAKE_CURRENT_BINARY_DIR}/idlc)
+	execute_process(COMMAND ${CMAKE_COMMAND} ${PX4_SOURCE_DIR}/src/lib/cdrstream/cyclonedds
+			-DCMAKE_C_COMPILER=/usr/bin/gcc
+			-DBUILD_EXAMPLES=OFF
+			WORKING_DIRECTORY ${CMAKE_CURRENT_BINARY_DIR}/idlc
+			RESULT_VARIABLE CMD_ERROR
+			OUTPUT_FILE CMD_OUTPUT )
+	MESSAGE(STATUS "Building idlc :" ${CMAKE_CURRENT_BINARY_DIR}/idlc)
+	execute_process(COMMAND ${CMAKE_COMMAND} --build . --target idlc
+			WORKING_DIRECTORY ${CMAKE_CURRENT_BINARY_DIR}/idlc
+			RESULT_VARIABLE CMD_ERROR
+			OUTPUT_FILE CMD_OUTPUT )
+	list(APPEND CMAKE_PROGRAM_PATH "${CMAKE_CURRENT_BINARY_DIR}/idlc/bin")
+
+	# Copy .msg files
+	foreach(msg_file ${msg_files})
+		get_filename_component(msg ${msg_file} NAME_WE)
+		configure_file(${PX4_SOURCE_DIR}/msg/${msg}.msg ${idl_out_path}/${msg}.msg COPYONLY)
+		list(APPEND uorb_cdr_idl ${idl_out_path}/${msg}.idl)
+		list(APPEND uorb_cdr_msg ${idl_out_path}/${msg}.msg)
+		list(APPEND uorb_cdr_idl_uorb ${idl_uorb_path}/${msg}.h)
+	endforeach()
+
+	# Generate IDL from .msg using rosidl_adapter
+	# Note this a submodule inside PX4 hence no ROS2 installation required
+	add_custom_command(
+		OUTPUT ${uorb_cdr_idl}
+		COMMAND ${CMAKE_COMMAND}
+		        -E env "PYTHONPATH=${PX4_SOURCE_DIR}/src/lib/cdrstream/rosidl/rosidl_adapter:${PX4_SOURCE_DIR}/src/lib/cdrstream/rosidl/rosidl_cli"
+			${PYTHON_EXECUTABLE} ${PX4_SOURCE_DIR}/src/lib/cdrstream/msg2idl.py
+			${uorb_cdr_msg}
+		DEPENDS
+			${uorb_cdr_msg}
+			git_cyclonedds
+		COMMENT "Generating IDL from uORB topic headers"
+		WORKING_DIRECTORY ${CMAKE_CURRENT_SOURCE_DIR}
+		VERBATIM
+		)
+
+	# Generate C definitions from IDL
+	set(CYCLONEDDS_DIR ${PX4_SOURCE_DIR}/src/lib/cdrstream/cyclonedds)
+	include("${CYCLONEDDS_DIR}/cmake/Modules/Generate.cmake")
+	idlc_generate(TARGET uorb_cdrstream
+                  FEATURES "cdrstream-desc"
+                  FILES ${uorb_cdr_idl}
+                  INCLUDES ${idl_include_path}
+                  BASE_DIR ${idl_include_path}
+                  WARNINGS no-implicit-extensibility)
+	target_link_libraries(uorb_cdrstream INTERFACE cdr)
+
+	# Generate and overwrite IDL header with custom headers for uORB operatability
+	# We typedef the IDL struct the uORB struct so that the IDL offset calculate
+	# the offset of internal uORB struct for serialization/deserialization
+
+	# In the future we might want to turn this around let the IDL struct be the leading ABI
+	# However we need to remove the padding for logging and remove the re-ordering of fields
+
+	add_custom_target(
+		uorb_idl_header
+		COMMAND ${PYTHON_EXECUTABLE} ${PX4_SOURCE_DIR}/Tools/msg/px_generate_uorb_topic_files.py
+			--uorb-idl-header
+			-f ${msg_files}
+			-i ${CMAKE_CURRENT_SOURCE_DIR}
+			-o ${idl_uorb_path}
+			-e ${PX4_SOURCE_DIR}/Tools/msg/templates/cdrstream
+		DEPENDS
+			uorb_cdrstream
+			${msg_files}
+			${PX4_SOURCE_DIR}/Tools/msg/templates/cdrstream/uorb_idl_header.h.em
+			${PX4_SOURCE_DIR}/Tools/msg/px_generate_uorb_topic_files.py
+			${PX4_SOURCE_DIR}/Tools/msg/px_generate_uorb_topic_helper.py
+		COMMENT "Generating uORB compatible IDL headers"
+		WORKING_DIRECTORY ${CMAKE_CURRENT_SOURCE_DIR}
+		VERBATIM
+	)
+	add_dependencies(uorb_msgs uorb_idl_header)
+
+	# Compile all CDR compatible message defnitions
+	target_link_libraries(uorb_msgs PRIVATE uorb_cdrstream )
+endif()
+
+if(CONFIG_MODULES_ZENOH)
+	# Update kconfig file for topics
+	execute_process(COMMAND ${PYTHON_EXECUTABLE} ${PX4_SOURCE_DIR}/Tools/zenoh/px_generate_zenoh_topic_files.py
+			--zenoh-config
+			-f ${msg_files}
+			-o ${PX4_SOURCE_DIR}/src/modules/zenoh/
+			-e ${PX4_SOURCE_DIR}/Tools/zenoh/templates/zenoh
+		)
+	add_custom_command(
+		OUTPUT
+			${PX4_BINARY_DIR}/src/modules/zenoh/uorb_pubsub_factory.hpp
+		COMMAND ${PYTHON_EXECUTABLE} ${PX4_SOURCE_DIR}/Tools/zenoh/px_generate_zenoh_topic_files.py
+			--zenoh-pub-sub
+			-f ${msg_files}
+			-o ${PX4_BINARY_DIR}/src/modules/zenoh/
+			-e ${PX4_SOURCE_DIR}/Tools/zenoh/templates/zenoh
+		DEPENDS
+			${msg_files}
+			${PX4_SOURCE_DIR}/Tools/zenoh/templates/zenoh/uorb_pubsub_factory.hpp.em
+			${PX4_SOURCE_DIR}/Tools/zenoh/px_generate_zenoh_topic_files.py
+		COMMENT "Generating Zenoh Topic Code"
+		WORKING_DIRECTORY ${CMAKE_CURRENT_SOURCE_DIR}
+		VERBATIM
+		)
+		add_library(zenoh_topics ${PX4_BINARY_DIR}/src/modules/zenoh/uorb_pubsub_factory.hpp)
+		set_target_properties(zenoh_topics PROPERTIES LINKER_LANGUAGE CXX)
+endif()

msg/ConfigOverrides.msg

@@ -0,0 +1,19 @@
+# Configurable overrides by (external) modes or mode executors
+uint64 timestamp		# time since system start (microseconds)
+
+bool disable_auto_disarm         # Prevent the drone from automatically disarming after landing (if configured)
+
+bool defer_failsafes             # Defer all failsafes that can be deferred (until the flag is cleared)
+int16 defer_failsafes_timeout_s  # Maximum time a failsafe can be deferred. 0 = system default, -1 = no timeout
+
+
+int8 SOURCE_TYPE_MODE = 0
+int8 SOURCE_TYPE_MODE_EXECUTOR = 1
+int8 source_type
+
+uint8 source_id                  # ID depending on source_type
+
+uint8 ORB_QUEUE_LENGTH = 4
+
+# TOPICS config_overrides config_overrides_request
+

msg/EstimatorAidSource2d.msg

@@ -19,3 +19,4 @@ bool fused                   # true if the sample was successfully fused
 
 # TOPICS estimator_aid_src_ev_pos estimator_aid_src_fake_pos estimator_aid_src_gnss_pos
 # TOPICS estimator_aid_src_aux_vel estimator_aid_src_optical_flow estimator_aid_src_terrain_optical_flow
+# TOPICS estimator_aid_src_drag

msg/EstimatorInnovations.msg

@@ -19,7 +19,6 @@ float32 baro_vpos	# barometer height innovation (m) and innovation variance (m**
 
 # Auxiliary velocity
 float32[2] aux_hvel	# horizontal auxiliary velocity innovation from landing target measurement (m/sec) and innovation variance ((m/sec)**2)
-float32    aux_vvel	# vertical auxiliary velocity innovation from landing target measurement (m/sec) and innovation variance ((m/sec)**2)
 
 # Optical flow
 float32[2] flow		# flow innvoation (rad/sec) and innovation variance ((rad/sec)**2)

msg/EstimatorStates.msg

@@ -4,4 +4,4 @@ uint64 timestamp_sample         # the timestamp of the raw data (microseconds)
 float32[24] states		# Internal filter states
 uint8 n_states		# Number of states effectively used
 
-float32[24] covariances	# Diagonal Elements of Covariance Matrix
+float32[23] covariances	# Diagonal Elements of Covariance Matrix

msg/FigureEightStatus.msg

@@ -0,0 +1,8 @@
+uint64 timestamp # time since system start (microseconds)
+float32 major_radius 	# Major axis radius of the figure eight [m]. Positive values orbit clockwise, negative values orbit counter-clockwise.
+float32 minor_radius 	# Minor axis radius of the figure eight [m].
+float32 orientation 	# Orientation of the major axis of the figure eight [rad].
+uint8 frame      # The coordinate system of the fields: x, y, z.
+int32 x          # X coordinate of center point. Coordinate system depends on frame field: local = x position in meters * 1e4, global = latitude in degrees * 1e7.
+int32 y        	 # Y coordinate of center point. Coordinate system depends on frame field: local = y position in meters * 1e4, global = latitude in degrees * 1e7.
+float32 z        # Altitude of center point. Coordinate system depends on frame field.

msg/MessageFormatRequest.msg

@@ -0,0 +1,10 @@
+uint64 timestamp # time since system start (microseconds)
+
+# Request to PX4 to get the hash of a message, to check for message compatibility
+
+uint16 LATEST_PROTOCOL_VERSION = 1 # Current version of this protocol. Increase this whenever the MessageFormatRequest or MessageFormatResponse changes.
+
+uint16 protocol_version           # Must be set to LATEST_PROTOCOL_VERSION. Do not change this field, it must be the first field after the timestamp
+
+char[50] topic_name  # E.g. /fmu/in/vehicle_command
+

msg/MessageFormatResponse.msg

@@ -0,0 +1,11 @@
+uint64 timestamp # time since system start (microseconds)
+
+# Response from PX4 with the format of a message
+
+uint16 protocol_version           # Must be set to LATEST_PROTOCOL_VERSION. Do not change this field, it must be the first field after the timestamp
+
+char[50] topic_name  # E.g. /fmu/in/vehicle_command
+
+bool success
+uint32 message_hash # hash over all message fields
+

msg/PositionSetpoint.msg

@@ -9,6 +9,9 @@ uint8 SETPOINT_TYPE_TAKEOFF=3	# takeoff setpoint
 uint8 SETPOINT_TYPE_LAND=4	# land setpoint, altitude must be ignored, descend until landing
 uint8 SETPOINT_TYPE_IDLE=5	# do nothing, switch off motors or keep at idle speed (MC)
 
+uint8 LOITER_TYPE_ORBIT=0 	# Circular pattern
+uint8 LOITER_TYPE_FIGUREEIGHT=1 # Pattern resembling an 8
+
 bool valid			# true if setpoint is valid
 uint8 type			# setpoint type to adjust behavior of position controller
 
@@ -25,8 +28,11 @@ bool yaw_valid			# true if yaw setpoint valid
 float32 yawspeed		# yawspeed (only for multirotors, in rad/s)
 bool yawspeed_valid		# true if yawspeed setpoint valid
 
-float32 loiter_radius		# loiter radius (only for fixed wing), in m
+float32 loiter_radius		# loiter major axis radius in m
+float32 loiter_minor_radius	# loiter minor axis radius (used for non-circular loiter shapes) in m
 bool loiter_direction_counter_clockwise # loiter direction is clockwise by default and can be changed using this field
+float32 loiter_orientation 	# Orientation of the major axis with respect to true north in rad [-pi,pi)
+uint8 	loiter_pattern		# loitern pattern to follow
 
 float32 acceptance_radius   # navigation acceptance_radius if we're doing waypoint navigation
 

msg/RegisterExtComponentReply.msg

@@ -0,0 +1,14 @@
+uint64 timestamp # time since system start (microseconds)
+
+uint64 request_id          # ID from the request
+char[25] name              # name from the request
+
+uint16 px4_ros2_api_version
+
+bool success
+int8 arming_check_id      # arming check registration ID (-1 if invalid)
+int8 mode_id              # assigned mode ID (-1 if invalid)
+int8 mode_executor_id     # assigned mode executor ID (-1 if invalid)
+
+uint8 ORB_QUEUE_LENGTH = 2
+

msg/RegisterExtComponentRequest.msg

@@ -0,0 +1,21 @@
+# Request to register an external component
+uint64 timestamp # time since system start (microseconds)
+
+uint64 request_id                  # ID, set this to a random value
+char[25] name                      # either the requested mode name, or component name
+
+uint16 LATEST_PX4_ROS2_API_VERSION = 1 # API version compatibility. Increase this on a breaking semantic change. Changes to any message field are detected separately and do not require an API version change.
+
+uint16 px4_ros2_api_version   # Set to LATEST_PX4_ROS2_API_VERSION
+
+# Components to be registered
+bool register_arming_check
+bool register_mode                 # registering a mode also requires arming_check to be set
+bool register_mode_executor        # registering an executor also requires a mode to be registered (which is the owned mode by the executor)
+
+bool enable_replace_internal_mode  # set to true if an internal mode should be replaced
+uint8 replace_internal_mode        # vehicle_status::NAVIGATION_STATE_*
+bool activate_mode_immediately     # switch to the registered mode (can only be set in combination with an executor)
+
+
+uint8 ORB_QUEUE_LENGTH = 2

msg/TecsStatus.msg

@@ -26,7 +26,4 @@ float32 throttle_sp			# Current throttle setpoint [-]
 float32 pitch_sp_rad			# Current pitch setpoint [rad]
 float32 throttle_trim			# estimated throttle value [0,1] required to fly level at equivalent_airspeed_sp in the current atmospheric conditions
 
-# TECS mode
-uint8 mode
-uint8 TECS_MODE_NORMAL = 0
-uint8 TECS_MODE_UNDERSPEED = 1
+float32 underspeed_ratio		# 0: no underspeed, 1: maximal underspeed. Controller takes measures to avoid stall proportional to ratio if >0.

msg/UnregisterExtComponent.msg

@@ -0,0 +1,10 @@
+uint64 timestamp # time since system start (microseconds)
+
+char[25] name                      # either the mode name, or component name
+
+int8 arming_check_id      # arming check registration ID (-1 if not registered)
+int8 mode_id              # assigned mode ID (-1 if not registered)
+int8 mode_executor_id     # assigned mode executor ID (-1 if not registered)
+
+
+

msg/VehicleCommand.msg

@@ -15,6 +15,7 @@ uint16 VEHICLE_CMD_NAV_LAND = 21			# Land at location |Empty| Empty| Empty| Desi
 uint16 VEHICLE_CMD_NAV_TAKEOFF = 22			# Takeoff from ground / hand |Minimum pitch (if airspeed sensor present), desired pitch without sensor| Empty| Empty| Yaw angle (if magnetometer present), ignored without magnetometer| Latitude| Longitude| Altitude|
 uint16 VEHICLE_CMD_NAV_PRECLAND = 23			# Attempt a precision landing
 uint16 VEHICLE_CMD_DO_ORBIT = 34			# Start orbiting on the circumference of a circle defined by the parameters. |Radius [m] |Velocity [m/s] |Yaw behaviour |Empty |Latitude/X |Longitude/Y |Altitude/Z |
+uint16 VEHICLE_CMD_DO_FIGUREEIGHT = 35        		# Start flying on the outline of a figure eight defined by the parameters. |Major Radius [m] |Minor Radius [m] |Velocity [m/s] |Orientation |Latitude/X |Longitude/Y |Altitude/Z |
 uint16 VEHICLE_CMD_NAV_ROI = 80				# Sets the region of interest (ROI) for a sensor set or the vehicle itself. This can then be used by the vehicles control system to control the vehicle attitude and the attitude of various sensors such as cameras. |Region of interest mode. (see MAV_ROI enum)| MISSION index/ target ID. (see MAV_ROI enum)| ROI index (allows a vehicle to manage multiple ROI's)| Empty| x the location of the fixed ROI (see MAV_FRAME)| y| z|
 uint16 VEHICLE_CMD_NAV_PATHPLANNING = 81		# Control autonomous path planning on the MAV. |0: Disable local obstacle avoidance / local path planning (without resetting map), 1: Enable local path planning, 2: Enable and reset local path planning| 0: Disable full path planning (without resetting map), 1: Enable, 2: Enable and reset map/occupancy grid, 3: Enable and reset planned route, but not occupancy grid| Empty| Yaw angle at goal, in compass degrees, [0..360]| Latitude/X of goal| Longitude/Y of goal| Altitude/Z of goal|
 uint16 VEHICLE_CMD_NAV_VTOL_TAKEOFF = 84		# Takeoff from ground / hand and transition to fixed wing |Minimum pitch (if airspeed sensor present), desired pitch without sensor| Empty| Empty| Yaw angle (if magnetometer present), ignored without magnetometer| Latitude| Longitude| Altitude|
@@ -70,6 +71,7 @@ uint16 VEHICLE_CMD_PREFLIGHT_UAVCAN = 243		# UAVCAN configuration. If param 1 ==
 uint16 VEHICLE_CMD_PREFLIGHT_STORAGE = 245		# Request storage of different parameter values and logs. This command will be only accepted if in pre-flight mode. |Parameter storage: 0: READ FROM FLASH/EEPROM, 1: WRITE CURRENT TO FLASH/EEPROM| Mission storage: 0: READ FROM FLASH/EEPROM, 1: WRITE CURRENT TO FLASH/EEPROM| Reserved| Reserved| Empty| Empty| Empty|
 uint16 VEHICLE_CMD_PREFLIGHT_REBOOT_SHUTDOWN = 246	# Request the reboot or shutdown of system components. |0: Do nothing for autopilot, 1: Reboot autopilot, 2: Shutdown autopilot.| 0: Do nothing for onboard computer, 1: Reboot onboard computer, 2: Shutdown onboard computer.| Reserved| Reserved| Empty| Empty| Empty|
 uint16 VEHICLE_CMD_OBLIQUE_SURVEY=260			# Mission command to set a Camera Auto Mount Pivoting Oblique Survey for this flight|Camera trigger distance (meters)| Shutter integration time (ms)| Camera minimum trigger interval| Number of positions| Roll| Pitch| Empty|
+uint16 VEHICLE_CMD_DO_SET_STANDARD_MODE=262		# Enable the specified standard MAVLink mode |MAV_STANDARD_MODE|
 uint16 VEHICLE_CMD_GIMBAL_DEVICE_INFORMATION = 283	# Command to ask information about a low level gimbal
 
 uint16 VEHICLE_CMD_MISSION_START = 300			# start running a mission |first_item: the first mission item to run| last_item:  the last mission item to run (after this item is run, the mission ends)|
@@ -102,6 +104,7 @@ uint16 VEHICLE_CMD_DO_WINCH = 42600			# Command to operate winch.
 # PX4 vehicle commands (beyond 16 bit mavlink commands)
 uint32 VEHICLE_CMD_PX4_INTERNAL_START    = 65537        # start of PX4 internal only vehicle commands (> UINT16_MAX)
 uint32 VEHICLE_CMD_SET_GPS_GLOBAL_ORIGIN = 100000       # Sets the GPS coordinates of the vehicle local origin (0,0,0) position. |Empty|Empty|Empty|Empty|Latitude|Longitude|Altitude|
+uint32 VEHICLE_CMD_SET_NAV_STATE = 100001               # Change mode by specifying nav_state directly. |nav_state|Empty|Empty|Empty|Empty|Empty|Empty|
 
 uint8 VEHICLE_MOUNT_MODE_RETRACT = 0			# Load and keep safe position (Roll,Pitch,Yaw) from permanent memory and stop stabilization |
 uint8 VEHICLE_MOUNT_MODE_NEUTRAL = 1			# Load and keep neutral position (Roll,Pitch,Yaw) from permanent memory. |
@@ -173,8 +176,10 @@ uint32 command			# Command ID
 uint8 target_system		# System which should execute the command
 uint8 target_component		# Component which should execute the command, 0 for all components
 uint8 source_system		# System sending the command
-uint8 source_component		# Component sending the command
+uint16 source_component	# Component / mode executor sending the command
 uint8 confirmation		# 0: First transmission of this command. 1-255: Confirmation transmissions (e.g. for kill command)
 bool from_external
 
-# TOPICS vehicle_command gimbal_v1_command
+uint16 COMPONENT_MODE_EXECUTOR_START = 1000
+
+# TOPICS vehicle_command gimbal_v1_command vehicle_command_mode_executor

msg/VehicleCommandAck.msg

@@ -28,6 +28,6 @@ uint8 result						# Command result
 uint8 result_param1					# Also used as progress[%], it can be set with the reason why the command was denied, or the progress percentage when result is MAV_RESULT_IN_PROGRESS
 int32 result_param2					# Additional parameter of the result, example: which parameter of MAV_CMD_NAV_WAYPOINT caused it to be denied.
 uint8 target_system
-uint8 target_component
+uint16 target_component 				# Target component / mode executor
 
 bool from_external					# Indicates if the command came from an external source

msg/VehicleControlMode.msg

@@ -15,3 +15,8 @@ bool flag_control_altitude_enabled		# true if altitude is controlled
 bool flag_control_climb_rate_enabled		# true if climb rate is controlled
 bool flag_control_termination_enabled		# true if flighttermination is enabled
 bool flag_control_allocation_enabled		# true if control allocation is enabled
+
+# TODO: use dedicated topic for external requests
+uint8 source_id                  # Mode ID (nav_state)
+
+# TOPICS vehicle_control_mode config_control_setpoints

msg/VehicleStatus.msg

@@ -52,7 +52,20 @@ uint8 NAVIGATION_STATE_AUTO_FOLLOW_TARGET = 19  # Auto Follow
 uint8 NAVIGATION_STATE_AUTO_PRECLAND = 20       # Precision land with landing target
 uint8 NAVIGATION_STATE_ORBIT = 21               # Orbit in a circle
 uint8 NAVIGATION_STATE_AUTO_VTOL_TAKEOFF = 22   # Takeoff, transition, establish loiter
-uint8 NAVIGATION_STATE_MAX = 23
+uint8 NAVIGATION_STATE_EXTERNAL1 = 23
+uint8 NAVIGATION_STATE_EXTERNAL2 = 24
+uint8 NAVIGATION_STATE_EXTERNAL3 = 25
+uint8 NAVIGATION_STATE_EXTERNAL4 = 26
+uint8 NAVIGATION_STATE_EXTERNAL5 = 27
+uint8 NAVIGATION_STATE_EXTERNAL6 = 28
+uint8 NAVIGATION_STATE_EXTERNAL7 = 29
+uint8 NAVIGATION_STATE_EXTERNAL8 = 30
+uint8 NAVIGATION_STATE_MAX = 31
+
+uint8 executor_in_charge                        # Current mode executor in charge (0=Autopilot)
+
+uint32 valid_nav_states_mask                    # Bitmask for all valid nav_state values
+uint32 can_set_nav_states_mask                  # Bitmask for all modes that a user can select
 
 # Bitmask of detected failures
 uint16 failure_detector_status
@@ -78,8 +91,13 @@ uint8 VEHICLE_TYPE_FIXED_WING = 2
 uint8 VEHICLE_TYPE_ROVER = 3
 uint8 VEHICLE_TYPE_AIRSHIP = 4
 
+uint8 FAILSAFE_DEFER_STATE_DISABLED = 0
+uint8 FAILSAFE_DEFER_STATE_ENABLED = 1
+uint8 FAILSAFE_DEFER_STATE_WOULD_FAILSAFE = 2 # Failsafes deferred, but would trigger a failsafe
+
 bool failsafe # true if system is in failsafe state (e.g.:RTL, Hover, Terminate, ...)
 bool failsafe_and_user_took_over # true if system is in failsafe state but the user took over control
+uint8 failsafe_defer_state # one of FAILSAFE_DEFER_STATE_*
 
 # Link loss
 bool gcs_connection_lost              # datalink to GCS lost

platforms/common/CMakeLists.txt

@@ -33,7 +33,7 @@
 
 set(SRCS)
 
-if(NOT "${PX4_PLATFORM}" MATCHES "qurt" AND NOT "${PX4_BOARD}" MATCHES "io-v2" AND NOT "${PX4_BOARD_LABEL}" MATCHES "bootloader")
+if(NOT "${PX4_PLATFORM}" MATCHES "qurt" AND NOT "${PX4_PLATFORM}" MATCHES "ros2" AND NOT "${PX4_BOARD}" MATCHES "io-v2" AND NOT "${PX4_BOARD_LABEL}" MATCHES "bootloader")
 	list(APPEND SRCS
 		px4_log.cpp
 		px4_log_history.cpp

platforms/common/px4_log.cpp

@@ -136,7 +136,7 @@ __EXPORT void px4_log_modulename(int level, const char *module_name, const char
 #if defined(PX4_LOG_COLORIZED_OUTPUT)
 
 		if (use_color) {
-			// alway reset color
+			// always reset color
 			const ssize_t sz = math::min(pos, max_length - (ssize_t)strlen(PX4_ANSI_COLOR_RESET) - (ssize_t)1);
 			pos += snprintf(buf + sz, math::max(max_length - sz, (ssize_t)0), "%s\n", PX4_ANSI_COLOR_RESET);
 

platforms/common/px4_work_queue/WorkItemSingleShot.cpp

@@ -41,6 +41,7 @@ WorkItemSingleShot::WorkItemSingleShot(const px4::wq_config_t &config, worker_me
 	: px4::WorkItem("<single_shot>", config), _argument(argument), _method(method)
 {
 	px4_sem_init(&_sem, 0, 0);
+	px4_sem_setprotocol(&_sem, SEM_PRIO_NONE);
 }
 
 WorkItemSingleShot::WorkItemSingleShot(const px4::WorkItem &work_item, worker_method method, void *argument)

platforms/common/uORB/CMakeLists.txt

@@ -34,6 +34,10 @@
 # this includes the generated topics directory
 include_directories(${CMAKE_CURRENT_BINARY_DIR})
 
+if(CONFIG_LIB_CDRSTREAM)
+	include_directories(${CMAKE_CURRENT_SOURCE_DIR}/src/lib/cdrstream/cyclonedds/src/core/ddsc/include)
+endif()
+
 set(SRCS)
 
 set(SRCS_COMMON
@@ -50,6 +54,8 @@ set(SRCS_COMMON
 	uORBCommon.hpp
 	uORBCommunicator.hpp
 	uORBManager.hpp
+	uORBMessageFields.cpp
+	uORBMessageFields.hpp
 	uORBUtils.cpp
 	uORBUtils.hpp
 	uORBDeviceMaster.hpp
@@ -72,7 +78,7 @@ if (NOT DEFINED CONFIG_BUILD_FLAT AND "${PX4_PLATFORM}" MATCHES "nuttx")
 		${SRCS_COMMON}
 		${SRCS_KERNEL}
 		)
-	target_link_libraries(uORB_kernel PRIVATE cdev uorb_msgs nuttx_mm)
+	target_link_libraries(uORB_kernel PRIVATE cdev uorb_msgs nuttx_mm heatshrink)
 	target_compile_options(uORB_kernel PRIVATE ${MAX_CUSTOM_OPT_LEVEL} -D__KERNEL__)
 
 	# User side library in nuttx kernel/protected build
@@ -98,9 +104,11 @@ else()
 	target_link_libraries(uORB PRIVATE cdev)
 endif()
 
-target_link_libraries(uORB PRIVATE uorb_msgs)
+target_link_libraries(uORB PRIVATE uorb_msgs heatshrink)
 target_compile_options(uORB PRIVATE ${MAX_CUSTOM_OPT_LEVEL})
 
 if(PX4_TESTING)
 	add_subdirectory(uORB_tests)
 endif()
+
+px4_add_functional_gtest(SRC uORBMessageFieldsTest.cpp LINKLIBS uORB)

platforms/common/uORB/Subscription.cpp

@@ -76,7 +76,7 @@ void Subscription::unsubscribe()
 bool Subscription::ChangeInstance(uint8_t instance)
 {
 	if (instance != _instance) {
-		if (uORB::Manager::orb_device_node_exists(_orb_id, _instance)) {
+		if (uORB::Manager::orb_device_node_exists(_orb_id, instance)) {
 			// if desired new instance exists, unsubscribe from current
 			unsubscribe();
 			_instance = instance;

platforms/common/uORB/uORB.cpp

@@ -40,6 +40,7 @@
 
 #include "uORBManager.hpp"
 #include "uORBCommon.hpp"
+#include "uORBMessageFields.hpp"
 
 
 #include <lib/drivers/device/Device.hpp>
@@ -196,7 +197,7 @@ int orb_get_interval(int handle, unsigned *interval)
 
 const char *orb_get_c_type(unsigned char short_type)
 {
-	// this matches with the uorb o_fields generator
+	// this matches with the uorb type_map_short python data
 	switch (short_type) {
 	case 0x82: return "int8_t";
 
@@ -239,25 +240,29 @@ void orb_print_message_internal(const orb_metadata *meta, const void *data, bool
 	const uint8_t *data_ptr = (const uint8_t *)data;
 	int data_offset = 0;
 
-	for (int format_idx = 0; meta->o_fields[format_idx] != 0;) {
-		const char *end_field = strchr(meta->o_fields + format_idx, ';');
+	// Find message format
+	char format_buffer[128];
+	uORB::MessageFormatReader format_reader(format_buffer, sizeof(format_buffer));
 
-		if (!end_field) {
-			PX4_ERR("Format error in %s", meta->o_fields);
-			return;
-		}
+	if (!format_reader.readUntilFormat(meta->o_id)) {
+		PX4_ERR("Failed to get uorb format");
+		return;
+	}
 
-		const char *c_type = orb_get_c_type(meta->o_fields[format_idx]);
-		const int end_field_idx = end_field - meta->o_fields;
+	int field_length = 0;
+
+	while (format_reader.readNextField(field_length)) {
+
+		const char *c_type = orb_get_c_type(format_buffer[0]);
 
 		int array_idx = -1;
 		int field_name_idx = -1;
 
-		for (int field_idx = format_idx; field_idx != end_field_idx; ++field_idx) {
-			if (meta->o_fields[field_idx] == '[') {
+		for (int field_idx = 0; field_idx < field_length; ++field_idx) {
+			if (format_buffer[field_idx] == '[') {
 				array_idx = field_idx + 1;
 
-			} else if (meta->o_fields[field_idx] == ' ') {
+			} else if (format_buffer[field_idx] == ' ') {
 				field_name_idx = field_idx + 1;
 				break;
 			}
@@ -266,19 +271,10 @@ void orb_print_message_internal(const orb_metadata *meta, const void *data, bool
 		int array_size = 1;
 
 		if (array_idx >= 0) {
-			array_size = strtol(meta->o_fields + array_idx, nullptr, 10);
+			array_size = strtol(format_buffer + array_idx, nullptr, 10);
 		}
 
-		char field_name[80];
-		size_t field_name_len = end_field_idx - field_name_idx;
-
-		if (field_name_len >= sizeof(field_name)) {
-			PX4_ERR("field name too long %s (max: %u)", meta->o_fields, (unsigned)sizeof(field_name));
-			return;
-		}
-
-		memcpy(field_name, meta->o_fields + field_name_idx, field_name_len);
-		field_name[field_name_len] = '\0';
+		const char *field_name = format_buffer + field_name_idx;
 
 		if (c_type) { // built-in type
 			bool dont_print = false;
@@ -458,17 +454,10 @@ void orb_print_message_internal(const orb_metadata *meta, const void *data, bool
 
 		} else {
 
-			// extract the topic name
-			char topic_name[80];
-			const size_t topic_name_len = array_size > 1 ? array_idx - format_idx - 1 : field_name_idx - format_idx - 1;
-
-			if (topic_name_len >= sizeof(topic_name)) {
-				PX4_ERR("topic name too long in %s (max: %u)", meta->o_name, (unsigned)sizeof(topic_name));
-				return;
-			}
-
-			memcpy(topic_name, meta->o_fields + format_idx, topic_name_len);
-			topic_name[topic_name_len] = '\0';
+			// Get the topic name
+			const size_t topic_name_len = array_size > 1 ? array_idx - 1 : field_name_idx - 1;
+			format_buffer[topic_name_len] = '\0';
+			const char *topic_name = format_buffer;
 
 			// find the metadata
 			const orb_metadata *const *topics = orb_get_topics();
@@ -499,7 +488,5 @@ void orb_print_message_internal(const orb_metadata *meta, const void *data, bool
 				data_offset += found_topic->o_size;
 			}
 		}
-
-		format_idx = end_field_idx + 1;
 	}
 }

platforms/common/uORB/uORB.h

@@ -51,7 +51,7 @@ struct orb_metadata {
 	const char    *o_name;              /**< unique object name */
 	const uint16_t o_size;              /**< object size */
 	const uint16_t o_size_no_padding;   /**< object size w/o padding at the end (for logger) */
-	const char    *o_fields;            /**< semicolon separated list of fields (with type) */
+	uint32_t message_hash;	/**< Hash over all fields for message compatibility checks */
 	orb_id_size_t  o_id;                /**< ORB_ID enum */
 };
 
@@ -100,15 +100,15 @@ typedef const struct orb_metadata *orb_id_t;
  * @param _name		The name of the topic.
  * @param _struct	The structure the topic provides.
  * @param _size_no_padding	Struct size w/o padding at the end
- * @param _fields	All fields in a semicolon separated list e.g: "float[3] position;bool armed"
+ * @param _message_hash	32 bit message hash over all fields
  * @param _orb_id_enum	ORB ID enum e.g.: ORB_ID::vehicle_status
  */
-#define ORB_DEFINE(_name, _struct, _size_no_padding, _fields, _orb_id_enum)		\
+#define ORB_DEFINE(_name, _struct, _size_no_padding, _message_hash, _orb_id_enum)		\
 	const struct orb_metadata __orb_##_name = {	\
 		#_name,					\
 		sizeof(_struct),		\
 		_size_no_padding,			\
-		_fields,				\
+		_message_hash,				\
 		_orb_id_enum				\
 	}; struct hack
 
@@ -236,7 +236,7 @@ extern int	orb_set_interval(int handle, unsigned interval) __EXPORT;
 extern int	orb_get_interval(int handle, unsigned *interval) __EXPORT;
 
 /**
- * Returns the C type string from a short type in o_fields metadata, or nullptr
+ * Returns the C type string from a short type in message fields metadata, or nullptr
  * if not a short type
  */
 const char *orb_get_c_type(unsigned char short_type);
@@ -248,7 +248,6 @@ const char *orb_get_c_type(unsigned char short_type);
  */
 void orb_print_message_internal(const struct orb_metadata *meta, const void *data, bool print_topic_name);
 
-
 __END_DECLS
 
 /* Diverse uORB header defines */ //XXX: move to better location

platforms/common/uORB/uORBMessageFields.cpp

@@ -0,0 +1,342 @@
+/****************************************************************************
+ *
+ *   Copyright (c) 2023 PX4 Development Team. All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright
+ *    notice, this list of conditions and the following disclaimer.
+ * 2. Redistributions in binary form must reproduce the above copyright
+ *    notice, this list of conditions and the following disclaimer in
+ *    the documentation and/or other materials provided with the
+ *    distribution.
+ * 3. Neither the name PX4 nor the names of its contributors may be
+ *    used to endorse or promote products derived from this software
+ *    without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
+ * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
+ * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *
+ ****************************************************************************/
+
+
+#include "uORBMessageFields.hpp"
+
+#include <px4_platform_common/log.h>
+
+namespace uORB
+{
+
+MessageFormatReader::State MessageFormatReader::readMore()
+{
+	if (_state == State::Complete || _state == State::Failure) {
+		return _state;
+	}
+
+	if (_buffer_length == _buffer_capacity) {
+		_state = State::Failure;
+		PX4_ERR("buffer too small");
+		return _state;
+	}
+
+	const uint8_t *compressed_formats = orb_compressed_message_formats();
+	const unsigned compressed_formats_size = orb_compressed_message_formats_size();
+
+	if (_buffer_length == 0 && _compressed_formats_idx == compressed_formats_size) {
+		_state = State::Complete;
+		return _state;
+	}
+
+	const unsigned max_num_iterations = 5; // Safeguard, we're not expected to do more than a few iterations
+
+	for (unsigned iteration = 0; iteration < max_num_iterations; ++iteration) {
+		switch (_state) {
+		case State::ReadOrbIDs: {
+				int num_orb_ids = _buffer[0];
+				const unsigned orb_ids_size = 1 + num_orb_ids * sizeof(orb_id_size_t);
+
+				if (_buffer_length > orb_ids_size) {
+					int num_dependent_orb_ids = _buffer[orb_ids_size];
+					const unsigned orb_ids_dependent_size = 1 + num_dependent_orb_ids * sizeof(orb_id_size_t);
+
+					if (_buffer_length >= orb_ids_size + orb_ids_dependent_size) {
+
+						orb_id_size_t orb_id;
+						_state = State::ReadingFormat;
+						_format_length = 0;
+
+						_orb_ids.clear();
+
+						for (int i = 0; i < num_orb_ids; ++i) {
+							memcpy(&orb_id, &_buffer[1 + sizeof(orb_id_size_t) * i], sizeof(orb_id_size_t));
+							_orb_ids.push_back(orb_id);
+						}
+
+						_orb_ids_dependencies.clear();
+
+						for (int i = 0; i < num_dependent_orb_ids; ++i) {
+							memcpy(&orb_id, &_buffer[orb_ids_size + 1 + sizeof(orb_id_size_t) * i],
+							       sizeof(orb_id_size_t));
+							_orb_ids_dependencies.push_back(orb_id);
+						}
+
+						memmove(_buffer, _buffer + orb_ids_size + orb_ids_dependent_size,
+							_buffer_length - orb_ids_size - orb_ids_dependent_size);
+						_buffer_length -= orb_ids_size + orb_ids_dependent_size;
+
+						return State::ReadOrbIDs;
+					}
+				}
+
+				if (_buffer_length == _buffer_capacity) {
+					_state = State::Failure;
+					PX4_ERR("buffer too small");
+					return _state;
+				}
+
+				break;
+			}
+
+		case State::ReadingFormat: {
+				const bool got_new_data = _format_length < _buffer_length;
+
+				for (; _format_length < _buffer_length; ++_format_length) {
+					if (_buffer[_format_length] == '\0') {
+						_state = State::FormatComplete;
+						return _state;
+					}
+				}
+
+				if (got_new_data) {
+					return _state;
+				}
+			}
+			break;
+
+		case State::FormatComplete:
+			if (_format_length != 0) {
+				PX4_ERR("Invalid API calls"); // Missing call to clearFormatFromBuffer or clearFormatAndRestoreLeftover
+				_state = State::Failure;
+				return _state;
+			}
+
+			_state = State::ReadOrbIDs;
+			break;
+
+		case State::Failure:
+		case State::Complete:
+			return _state;
+		}
+
+		// Decompress more data
+		size_t count = 0;
+
+		if (heatshrink_decoder_sink(&_hsd, &compressed_formats[_compressed_formats_idx],
+					    compressed_formats_size - _compressed_formats_idx, &count) < 0) {
+			_state = State::Failure;
+			return _state;
+		}
+
+		_compressed_formats_idx += count;
+
+		if (_compressed_formats_idx == compressed_formats_size) {
+			const HSD_finish_res fres = heatshrink_decoder_finish(&_hsd);
+
+			if (fres != HSDR_FINISH_MORE && fres != HSDR_FINISH_DONE) {
+				_state = State::Failure;
+				return _state;
+			}
+		}
+
+		const HSD_poll_res pres = heatshrink_decoder_poll(&_hsd, reinterpret_cast<uint8_t *>(&_buffer[_buffer_length]),
+					  _buffer_capacity - _buffer_length, &count);
+		_buffer_length += count;
+
+		if (HSDR_POLL_EMPTY != pres && HSDR_POLL_MORE != pres) {
+			_state = State::Failure;
+			return _state;
+		}
+
+		if (_compressed_formats_idx == compressed_formats_size) {
+			const HSD_finish_res fres = heatshrink_decoder_finish(&_hsd);
+
+			if (HSDR_FINISH_DONE != fres && HSDR_FINISH_MORE != fres) {
+				_state = State::Failure;
+				return _state;
+			}
+		}
+
+	}
+
+	// Not expected to get here
+	PX4_ERR("logic error");
+	_state = State::Failure;
+	return _state;
+}
+
+void MessageFormatReader::clearFormatFromBuffer()
+{
+	if (_state == State::FormatComplete) {
+		++_format_length; // Include null char
+		memmove(_buffer, _buffer + _format_length, _buffer_length - _format_length);
+		_buffer_length -= _format_length;
+
+	} else {
+		// Full buffer is occupied with format
+		_buffer_length = 0;
+	}
+
+	_format_length = 0;
+}
+
+int MessageFormatReader::expandMessageFormat(char *format, unsigned len, unsigned buf_len)
+{
+	++len; // Include null char
+
+	int format_idx = 0;
+
+	while (format[format_idx] != 0) {
+
+		const char *c_type = orb_get_c_type(format[format_idx]);
+
+		if (c_type) {
+			// Replace 1 char type with expanded c_type
+			const int c_type_len = (int)strlen(c_type);
+
+			if (len + c_type_len - 1 > buf_len) {
+				return -1;
+			}
+
+			memmove(format + format_idx + c_type_len, format + format_idx + 1, len - format_idx - 1);
+			memcpy(format + format_idx, c_type, c_type_len);
+			format_idx += c_type_len - 1;
+			len += c_type_len - 1;
+		}
+
+		// Go to next field
+		const char *end_field = strchr(format + format_idx, ';');
+
+		if (!end_field) {
+			PX4_ERR("Format error in %s", format);
+			return -1;
+		}
+
+		format_idx = (int)(end_field - format + 1);
+	}
+
+	if (format_idx + 1 != (int)len) {
+		PX4_ERR("logic error");
+		return -1;
+	}
+
+	return format_idx;
+}
+
+bool MessageFormatReader::readUntilFormat(orb_id_size_t orb_id)
+{
+	bool done = false;
+	bool found_format = false;
+
+	while (!done && !found_format) {
+		switch (readMore()) {
+		case State::ReadOrbIDs:
+			for (const orb_id_size_t current_orb_id : orbIDs()) {
+				if (current_orb_id == orb_id) {
+					found_format = true;
+				}
+			}
+
+			break;
+
+		case State::ReadingFormat:
+		case State::FormatComplete:
+			clearFormatFromBuffer();
+			break;
+
+		case State::Complete:
+		case State::Failure:
+			done = true;
+			break;
+
+		default:
+			break;
+		}
+	}
+
+	return found_format;
+}
+
+bool MessageFormatReader::readNextField(int &field_length)
+{
+	if (field_length > 0) {
+		// Move left-over part to beginning
+		++field_length; // include null
+		memmove(_buffer, _buffer + field_length, _buffer_length - field_length);
+		_buffer_length -= field_length;
+		_format_length -= field_length;
+	}
+
+	auto findFieldEnd = [&]() {
+		// Find ';'
+		bool found = false;
+
+		for (field_length = 0; field_length < (int)_format_length; ++field_length) {
+			if (_buffer[field_length] == ';') {
+				_buffer[field_length] = '\0';
+				found = true;
+				break;
+			}
+		}
+
+		return found;
+	};
+
+	// We might still have a field in the buffer
+	if (findFieldEnd()) {
+		return true;
+	}
+
+	bool done = false;
+	bool ret = false;
+
+	while (!done) {
+		switch (readMore()) {
+		case State::ReadingFormat:
+			if (findFieldEnd()) {
+				ret = true;
+				done = true;
+			}
+
+			break;
+
+		case State::FormatComplete: {
+				ret = findFieldEnd(); // Expected to return true here
+				done = true;
+				break;
+			}
+
+		case State::ReadOrbIDs: // Arrived at the next format -> we're done
+		case State::Complete:
+		case State::Failure:
+			done = true;
+			break;
+		}
+	}
+
+	return ret;
+}
+
+
+} // namespace uORB