Merge remote-tracking branch 'px4/main' into pr-ekf2_output_predictor_init_cleanup

2023-10-26 10:59:20 -04:00 · 2023-10-26 10:59:20 -04:00 · 0290a619e1
parent b1727055f9 12b291b82f
commit 0290a619e1
655 changed files with 35082 additions and 14632 deletions
--- a/.ci/Jenkinsfile-compile
+++ b/.ci/Jenkinsfile-compile
@ -117,7 +117,8 @@ pipeline {
                     "spracing_h7extreme_default",
                     "thepeach_k1_default",
                     "thepeach_r1_default",
-                     "uvify_core_default"
+                     "uvify_core_default",
+                     "siyi_n7_default"
            ],
            image: docker_images.nuttx,
            archive: true
--- a/.devcontainer/devcontainer.json
+++ b/.devcontainer/devcontainer.json
@ -29,7 +29,8 @@
 		"twxs.cmake",
 		"uavcan.dsdl",
 		"wholroyd.jinja",
-		"zixuanwang.linkerscript"
+		"zixuanwang.linkerscript",
+		"ms-vscode.makefile-tools"
 	],

 	"containerUser": "user",
--- a/.github/workflows/compile_nuttx.yml
+++ b/.github/workflows/compile_nuttx.yml
@ -73,7 +73,8 @@ jobs:
          raspberrypi_pico,
          sky-drones_smartap-airlink,
          spracing_h7extreme,
-          uvify_core
+          uvify_core,
+          siyi_n7
          ]
    steps:
    - uses: actions/checkout@v1
--- a/.gitmodules
+++ b/.gitmodules
@ -62,3 +62,13 @@
 	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
--- a/.vscode/extensions.json
+++ b/.vscode/extensions.json
@ -18,6 +18,7 @@
        "twxs.cmake",
        "uavcan.dsdl",
        "wholroyd.jinja",
-        "zixuanwang.linkerscript"
+        "zixuanwang.linkerscript",
+        "ms-vscode.makefile-tools"
    ]
 }
--- a/.vscode/settings.json
+++ b/.vscode/settings.json
@ -127,5 +127,6 @@
        "terminal.integrated.scrollback": 15000,
        "yaml.schemas": {
                "${workspaceFolder}/validation/module_schema.yaml": "${workspaceFolder}/src/modules/*/module.yaml"
-        }
+        },
+        "ros.distro": "humble"
 }
--- a/CMakeLists.txt
+++ b/CMakeLists.txt
@ -414,6 +414,8 @@ endif()
 #
 add_library(parameters_interface INTERFACE)
 add_library(kernel_parameters_interface INTERFACE)
+add_library(events_interface INTERFACE)
+add_library(kernel_events_interface INTERFACE)

 include(px4_add_library)
 add_subdirectory(src/lib EXCLUDE_FROM_ALL)
@ -440,8 +442,11 @@ add_subdirectory(src/lib/parameters EXCLUDE_FROM_ALL)
 if(${PX4_PLATFORM} STREQUAL "nuttx" AND NOT CONFIG_BUILD_FLAT)
 	target_link_libraries(parameters_interface INTERFACE usr_parameters)
 	target_link_libraries(kernel_parameters_interface INTERFACE parameters)
+	target_link_libraries(events_interface INTERFACE usr_events)
+	target_link_libraries(kernel_events_interface INTERFACE events)
 else()
 	target_link_libraries(parameters_interface INTERFACE parameters)
+	target_link_libraries(events_interface INTERFACE events)
 endif()

 # firmware added last to generate the builtin for included modules
--- a/2
+++ b/2
@ -205,3 +205,5 @@ menu "platforms"
 depends on PLATFORM_QURT || PLATFORM_POSIX
 source "platforms/common/Kconfig"
 endmenu
+
+source "src/lib/*/Kconfig"
--- a/README.md
+++ b/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

--- a/ROMFS/px4fmu_common/init.d-posix/airframes/1010_gazebo-classic_iris_opt_flow
+++ b/ROMFS/px4fmu_common/init.d-posix/airframes/1010_gazebo-classic_iris_opt_flow
@ -31,6 +31,7 @@ param set-default PWM_MAIN_FUNC4 104

 # EKF2
 param set-default EKF2_GPS_CTRL 0
+param set-default EKF2_HGT_REF 0
 param set-default EKF2_EVP_NOISE 0.05
 param set-default EKF2_EVA_NOISE 0.05
 param set-default EKF2_OF_CTRL 1
@ -39,6 +40,9 @@ param set-default EKF2_OF_CTRL 1
 param set-default LPE_FUSION 242
 param set-default LPE_FAKE_ORIGIN 1

+# Commander
+# param set-default COM_HOME_EN 0 # Disable setting of home position
+
 param set-default MPC_ALT_MODE 2

 param set-default SENS_FLOW_ROT 6
--- a/ROMFS/px4fmu_common/init.d-posix/airframes/1014_gazebo-classic_iris_obs_avoid
+++ b/ROMFS/px4fmu_common/init.d-posix/airframes/1014_gazebo-classic_iris_obs_avoid
--- a/ROMFS/px4fmu_common/init.d-posix/airframes/1014_gazebo-classic_iris_obs_avoid.post
+++ b/ROMFS/px4fmu_common/init.d-posix/airframes/1014_gazebo-classic_iris_obs_avoid.post
--- a/ROMFS/px4fmu_common/init.d-posix/airframes/1015_gazebo-classic_iris_depth_camera
+++ b/ROMFS/px4fmu_common/init.d-posix/airframes/1015_gazebo-classic_iris_depth_camera
@ -0,0 +1,8 @@
+#!/bin/sh
+#
+# @name 3DR Iris Quadrotor with a depth camera (forward-facing)
+#
+# @type Quadrotor Wide
+#
+
+. ${R}etc/init.d-posix/airframes/10015_gazebo-classic_iris
--- a/ROMFS/px4fmu_common/init.d-posix/airframes/1016_gazebo-classic_iris_downward_depth_camera
+++ b/ROMFS/px4fmu_common/init.d-posix/airframes/1016_gazebo-classic_iris_downward_depth_camera
@ -0,0 +1,8 @@
+#!/bin/sh
+#
+# @name 3DR Iris Quadrotor with a depth camera (downward-facing)
+#
+# @type Quadrotor Wide
+#
+
+. ${R}etc/init.d-posix/airframes/10015_gazebo-classic_iris
--- a/ROMFS/px4fmu_common/init.d-posix/airframes/1032_gazebo-classic_plane_catapult
+++ b/ROMFS/px4fmu_common/init.d-posix/airframes/1032_gazebo-classic_plane_catapult
@ -7,6 +7,7 @@

 param set-default FW_LAUN_DETCN_ON 1
 param set-default FW_THR_IDLE 0.1 # needs to be running before throw as that's how gazebo detects arming
+param set-default FW_LAUN_AC_THLD 10

 param set-default FW_LND_ANG 8

--- a/ROMFS/px4fmu_common/init.d-posix/airframes/1039_gazebo-classic_advanced_plane
+++ b/ROMFS/px4fmu_common/init.d-posix/airframes/1039_gazebo-classic_advanced_plane
@ -8,7 +8,6 @@


 param set-default FW_LND_ANG 8
-param set-default FW_THR_LND_MAX 0

 param set-default NPFG_PERIOD 12

@ -36,7 +35,6 @@ param set-default NAV_DLL_ACT 2

 param set-default RWTO_TKOFF 1

-#param set-default SYS_CTRL_ALLOC 1
 param set-default CA_AIRFRAME 1

 param set-default CA_ROTOR_COUNT 1
@ -61,7 +59,3 @@ param set-default PWM_MAIN_FUNC7 202
 param set-default PWM_MAIN_FUNC8 203
 param set-default PWM_MAIN_FUNC9 206
 param set-default PWM_MAIN_REV 256
-
-
-set MIXER_FILE etc/mixers-sitl/plane_sitl.main.mix
-set MIXER custom
--- a/ROMFS/px4fmu_common/init.d-posix/airframes/CMakeLists.txt
+++ b/ROMFS/px4fmu_common/init.d-posix/airframes/CMakeLists.txt
@ -39,8 +39,10 @@ px4_add_romfs_files(
 	1012_gazebo-classic_iris_rplidar
 	1013_gazebo-classic_iris_vision
 	1013_gazebo-classic_iris_vision.post
-	1015_gazebo-classic_iris_obs_avoid
-	1015_gazebo-classic_iris_obs_avoid.post
+	1014_gazebo-classic_iris_obs_avoid
+	1014_gazebo-classic_iris_obs_avoid.post
+	1015_gazebo-classic_iris_depth_camera
+	1016_gazebo-classic_iris_downward_depth_camera
 	1017_gazebo-classic_iris_opt_flow_mockup
 	1019_gazebo-classic_iris_dual_gps
 	1021_gazebo-classic_uuv_hippocampus
--- a/ROMFS/px4fmu_common/init.d/airframes/4019_x500_v2
+++ b/ROMFS/px4fmu_common/init.d/airframes/4019_x500_v2
@ -38,5 +38,3 @@ param set-default PWM_MAIN_FUNC1 101
 param set-default PWM_MAIN_FUNC2 102
 param set-default PWM_MAIN_FUNC3 103
 param set-default PWM_MAIN_FUNC4 104
-param set-default PWM_MAIN_TIM0 -4
-
--- a/ROMFS/px4fmu_common/init.d/airframes/4900_crazyflie
+++ b/ROMFS/px4fmu_common/init.d/airframes/4900_crazyflie
@ -43,7 +43,6 @@ param set-default MC_ROLLRATE_P 0.07
 param set-default MC_YAW_P 3

 param set-default MPC_THR_HOVER 0.7
-param set-default MPC_THR_MAX 1
 param set-default MPC_Z_P 1.5
 param set-default MPC_Z_VEL_P_ACC 8
 param set-default MPC_Z_VEL_I_ACC 6
--- a/ROMFS/px4fmu_common/init.d/airframes/4901_crazyflie21
+++ b/ROMFS/px4fmu_common/init.d/airframes/4901_crazyflie21
@ -42,7 +42,6 @@ param set-default MC_ROLLRATE_P 0.07
 param set-default MC_YAW_P 3

 param set-default MPC_THR_HOVER 0.7
-param set-default MPC_THR_MAX 1
 param set-default MPC_Z_P 1.5
 param set-default MPC_Z_VEL_P_ACC 8
 param set-default MPC_Z_VEL_I_ACC 6
--- a/ROMFS/px4fmu_common/init.d/rc.fw_defaults
+++ b/ROMFS/px4fmu_common/init.d/rc.fw_defaults
@ -34,6 +34,7 @@ param set-default EKF2_REQ_EPV 10
 param set-default EKF2_REQ_HDRIFT 0.5
 param set-default EKF2_REQ_SACC 1
 param set-default EKF2_REQ_VDRIFT 1.0
+param set-default EKF2_RNG_QLTY_T 3.0

 param set-default RTL_TYPE 1
 param set-default RTL_RETURN_ALT 100
--- a/ROMFS/px4fmu_common/init.d/rc.sensors
+++ b/ROMFS/px4fmu_common/init.d/rc.sensors
@ -172,6 +172,12 @@ then
 	ms5525dso start -X
 fi

+# TE ASP5033 differential pressure sensor external I2C
+if param compare -s SENS_EN_ASP5033 1
+then
+	asp5033 start -X
+fi
+
 # SHT3x temperature and hygrometer sensor, external I2C
 if param compare -s SENS_EN_SHT3X 1
 then
--- a/ROMFS/px4fmu_common/init.d/rcS
+++ b/ROMFS/px4fmu_common/init.d/rcS
@ -93,6 +93,14 @@ then
 		fi
 	fi

+	# Check for an update of the ext_autostart folder, and replace the old one with it
+	if [ -e /fs/microsd/ext_autostart_new ]
+	then
+		echo "Updating external autostart files"
+		rm -r $SDCARD_EXT_PATH
+		mv /fs/microsd/ext_autostart_new $SDCARD_EXT_PATH
+	fi
+
 	set PARAM_FILE /fs/microsd/params
 	set PARAM_BACKUP_FILE "/fs/microsd/parameters_backup.bson"
 fi
@ -529,6 +537,10 @@ else
 			cyphal start
 		fi
 	fi
+	if param greater -s ZENOH_ENABLE 0
+	then
+		zenoh start
+	fi

 #
 # End of autostart.
--- a/Tools/astyle/files_to_check_code_style.sh
+++ b/Tools/astyle/files_to_check_code_style.sh
@ -26,4 +26,7 @@ exec find boards msg src platforms test \
    -path src/lib/crypto/libtomcrypt -prune -o \
    -path src/lib/crypto/libtommath -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/drivers/zenoh/zenoh-pico -prune -o \
    -type f \( -name "*.c" -o -name "*.h" -o -name "*.cpp" -o -name "*.hpp" \) | grep $PATTERN
--- a/Tools/auterion/remote_update_fmu.sh
+++ b/Tools/auterion/remote_update_fmu.sh
@ -0,0 +1,115 @@
+#!/bin/bash
+# Flash PX4 to a device running AuterionOS in the local network
+if [ "$1" == "-h" ] || [ "$1" == "--help" ] || [ $# -lt 2 ]; then
+	echo "Usage: $0 -f <firmware.px4|.elf> [-c <configuration_dir>] -d <IP/Device> [-u <user>] [-p <ssh_port>] [--revert]"
+	exit 1
+fi
+
+ssh_port=22
+ssh_user=root
+
+while getopts ":f:c:d:p:u:r" opt; do
+	case ${opt} in
+		f )
+			if [ -n "$OPTARG" ]; then
+				firmware_file="$OPTARG"
+			else
+				echo "ERROR: -f requires a non-empty option argument."
+				exit 1
+			fi
+			;;
+		c )
+			if [ -f "$OPTARG/rc.autostart" ]; then
+				config_dir="$OPTARG"
+			else
+				echo "ERROR: -c configuration directory is empty or does not contain a valid rc.autostart"
+				exit 1
+			fi
+			;;
+		d )
+			if [ "$OPTARG" ]; then
+				device="$OPTARG"
+			else
+				echo "ERROR: -d requires a non-empty option argument."
+				exit 1
+			fi
+			;;
+		p )
+			if [[ "$OPTARG" =~ ^[0-9]+$ ]]; then
+				ssh_port="$OPTARG"
+			else
+				echo "ERROR: -p ssh_port must be a number."
+				exit 1
+			fi
+			;;
+		u )
+			if [ "$OPTARG" ]; then
+				ssh_user="$OPTARG"
+			else
+				echo "ERROR: -u requires a non-empty option argument."
+				exit 1
+			fi
+			;;
+		r )
+			revert=true
+			;;
+	esac
+done
+
+if [ -z "$device" ]; then
+	echo "Error: missing device"
+	exit 1
+fi
+
+target_dir=/shared_container_dir/fmu
+target_file_name="update-dev.tar"
+
+if [ "$revert" == true ]; then
+	# revert to the release version which was originally deployed
+	cmd="cp $target_dir/update.tar $target_dir/$target_file_name"
+	ssh -t -p $ssh_port $ssh_user@$device "$cmd"
+else
+	# create custom update-dev.tar
+	tmp_dir="$(mktemp -d)"
+	config_path=""
+	firmware_path=""
+
+	if [ -d "$config_dir" ]; then
+		cp -r "$config_dir" "$tmp_dir/config"
+		config_path=config
+	fi
+
+	if [ -f "$firmware_file" ]; then
+		extension="${firmware_file##*.}"
+		cp "$firmware_file" "$tmp_dir/firmware.$extension"
+		if [ "$extension" == "elf" ]; then
+			# ensure the file is stripped to reduce file size
+			arm-none-eabi-strip "$tmp_dir/firmware.$extension"
+		fi
+		firmware_path="firmware.$extension"
+	fi
+
+	pushd "$tmp_dir" &>/dev/null
+
+	if [ -z $firmware_path ] && [ -z $config_path ]; then
+		exit 1
+	fi
+
+	tar_name="tar"
+
+	if [ -x "$(command -v gtar)" ]; then
+		# check if gnu-tar is installed on macOS and use that instead
+		tar_name="gtar"
+	fi
+
+	$tar_name -C "$tmp_dir" --sort=name --owner=root:0 --group=root:0 --mtime='2019-01-01 00:00:00' -cvf $target_file_name $firmware_path $config_path
+
+	# send it to the target to start flashing
+	scp -P $ssh_port "$target_file_name" $ssh_user@"$device":$target_dir
+	popd &>/dev/null
+	rm -rf "$tmp_dir"
+fi
+
+# grab status output for flashing progress
+cmd="tail --follow=name $target_dir/update_status 2>/dev/null || true"
+ssh -t -p $ssh_port $ssh_user@$device "$cmd"
--- a/Tools/auterion/upload_skynode.sh
+++ b/Tools/auterion/upload_skynode.sh
@ -0,0 +1,51 @@
+#!/usr/bin/env bash
+
+DIR="$(dirname $(readlink -f $0))"
+DEFAULT_AUTOPILOT_HOST=10.41.1.1
+DEFAULT_AUTOPILOT_PORT=33333
+DEFAULT_AUTOPILOT_USER=auterion
+
+for i in "$@"
+do
+    case $i in
+        --file=*)
+        PX4_BINARY_FILE="${i#*=}"
+        ;;
+        --default-ip=*)
+        DEFAULT_AUTOPILOT_HOST="${i#*=}"
+        ;;
+        --default-port=*)
+        DEFAULT_AUTOPILOT_PORT="${i#*=}"
+        ;;
+        --default-user=*)
+        DEFAULT_AUTOPILOT_USER="${i#*=}"
+        ;;
+        --revert)
+        REVERT_AUTOPILOT_ARGUMENT=-r
+        ;;
+        --wifi)
+        DEFAULT_AUTOPILOT_HOST=10.41.0.1
+        ;;
+        *)
+            # unknown option
+        ;;
+    esac
+done
+
+# allow these to be overridden
+[ -z "$AUTOPILOT_HOST" ] && AUTOPILOT_HOST=$DEFAULT_AUTOPILOT_HOST
+[ -z "$AUTOPILOT_PORT" ] && AUTOPILOT_PORT=$DEFAULT_AUTOPILOT_PORT
+[ -z "$AUTOPILOT_USER" ] && AUTOPILOT_USER=$DEFAULT_AUTOPILOT_USER
+
+ARGUMENTS=()
+ARGUMENTS+=(-d "$AUTOPILOT_HOST")
+ARGUMENTS+=(-p "$AUTOPILOT_PORT")
+ARGUMENTS+=(-u "$AUTOPILOT_USER")
+ARGUMENTS+=(${PX4_BINARY_FILE:+-f "$PX4_BINARY_FILE"})
+ARGUMENTS+=($REVERT_AUTOPILOT_ARGUMENT)
+
+echo "Flashing $AUTOPILOT_HOST ..."
+
+"$DIR"/remote_update_fmu.sh "${ARGUMENTS[@]}"
+
+exit 0
--- a/Tools/msg/px_generate_uorb_topic_files.py
+++ b/Tools/msg/px_generate_uorb_topic_files.py
@ -70,9 +70,9 @@ __license__ = "BSD"
 __email__ = "thomasgubler@gmail.com"


-TEMPLATE_FILE = ['msg.h.em', 'msg.cpp.em']
+TEMPLATE_FILE = ['msg.h.em', 'msg.cpp.em', 'uorb_idl_header.h.em']
 TOPICS_LIST_TEMPLATE_FILE = ['uORBTopics.hpp.em', 'uORBTopics.cpp.em']
-OUTPUT_FILE_EXT = ['.h', '.cpp']
+OUTPUT_FILE_EXT = ['.h', '.cpp', '.h']
 INCL_DEFAULT = ['std_msgs:./msg/std_msgs']
 PACKAGE = 'px4'
 TOPICS_TOKEN = '# TOPICS '
@ -150,6 +150,7 @@ 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,
@ -161,7 +162,10 @@ 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])
+    if format_idx == 2:
+        output_file = os.path.join(outputdir, file_base_name + OUTPUT_FILE_EXT[format_idx])
+    else:
+        output_file = os.path.join(outputdir, full_type_name_snake + OUTPUT_FILE_EXT[format_idx])

    return generate_by_template(output_file, template_file, em_globals)

@ -217,6 +221,7 @@ 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('-f', dest='file',
                        help="files to convert (use only without -d)",
                        nargs="+")
@ -241,6 +246,11 @@ if __name__ == "__main__":
        generate_idx = 0
    elif args.sources:
        generate_idx = 1
+    elif args.uorb_idl_header:
+        for f in args.file:
+            print(f)
+            generate_output_from_file(2, f, args.outputdir, args.package, args.templatedir, INCL_DEFAULT)
+        exit(0)
    else:
        print('Error: either --headers or --sources must be specified')
        exit(-1)
--- a/Tools/msg/templates/cdrstream/uorb_idl_header.h.em
+++ b/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 */
--- a/Tools/msg/templates/ucdr/msg.h.em
+++ b/Tools/msg/templates/ucdr/msg.h.em
@ -124,8 +124,9 @@ static inline constexpr int ucdr_topic_size_@(topic)()
 	return @(struct_size);
 }

-bool ucdr_serialize_@(topic)(const @(uorb_struct)& topic, ucdrBuffer& buf, int64_t time_offset = 0)
+bool ucdr_serialize_@(topic)(const void* data, ucdrBuffer& buf, int64_t time_offset = 0)
 {
+	const @(uorb_struct)& topic = *static_cast<const @(uorb_struct)*>(data);
@{
 for field_type, field_name, field_size, padding in fields:
 	if padding > 0:
--- a/Tools/setup/ubuntu.sh
+++ b/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
--- a/Tools/simulation/gz/tools/avl_automation/README.md
+++ b/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.
--- a/Tools/simulation/gz/tools/avl_automation/avl_out_parse.py
+++ b/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)
--- a/Tools/simulation/gz/tools/avl_automation/avl_steps.txt
+++ b/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
--- a/Tools/simulation/gz/tools/avl_automation/input.yml
+++ b/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
--- a/Tools/simulation/gz/tools/avl_automation/input_avl.py
+++ b/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()
--- a/Tools/simulation/gz/tools/avl_automation/process.sh
+++ b/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
--- a/Tools/simulation/gz/tools/avl_automation/templates/advanced_lift_drag_template.sdf
+++ b/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>
+  
--- a/Tools/simulation/gz/tools/avl_automation/templates/control_surface.sdf
+++ b/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>
--- a/Tools/zenoh/px_generate_zenoh_topic_files.py
+++ b/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)
--- a/Tools/zenoh/templates/zenoh/Kconfig.topics.em
+++ b/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
--- a/Tools/zenoh/templates/zenoh/uorb_pubsub_factory.hpp.em
+++ b/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;
+}
--- a/boards/airmind/mindpx-v2/nuttx-config/nsh/defconfig
+++ b/boards/airmind/mindpx-v2/nuttx-config/nsh/defconfig
@ -106,7 +106,7 @@ CONFIG_I2C=y
 CONFIG_I2C_RESET=y
 CONFIG_IDLETHREAD_STACKSIZE=750
 CONFIG_INIT_ENTRYPOINT="nsh_main"
-CONFIG_INIT_STACKSIZE=3094
+CONFIG_INIT_STACKSIZE=3194
 CONFIG_LIBC_FLOATINGPOINT=y
 CONFIG_LIBC_LONG_LONG=y
 CONFIG_LIBC_MAX_EXITFUNS=1
@ -140,6 +140,8 @@ 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
--- a/boards/ark/can-flow/nuttx-config/nsh/defconfig
+++ b/boards/ark/can-flow/nuttx-config/nsh/defconfig
@ -115,6 +115,8 @@ 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
--- a/boards/ark/can-flow/src/boot_config.h
+++ b/boards/ark/can-flow/src/boot_config.h
@ -65,7 +65,7 @@
 #define OPT_PREFERRED_NODE_ID ANY_NODE_ID

 //todo:wrap OPT_x in in ifdefs for command line definitions
-#define OPT_TBOOT_MS                    5000
+#define OPT_TBOOT_MS                    3000
 #define OPT_NODE_STATUS_RATE_MS         800
 #define OPT_NODE_INFO_RATE_MS           50
 #define OPT_BL_NUMBER_TIMERS            7
@ -93,7 +93,7 @@
 */
 #define OPT_WAIT_FOR_GETNODEINFO                    0
 #define OPT_WAIT_FOR_GETNODEINFO_JUMPER_GPIO        1
-#define OPT_WAIT_FOR_GETNODEINFO_JUMPER_GPIO_INVERT 0
+#define OPT_WAIT_FOR_GETNODEINFO_JUMPER_GPIO_INVERT 1

 #define OPT_ENABLE_WD           1

--- a/boards/ark/can-flow/uavcan_board_identity
+++ b/boards/ark/can-flow/uavcan_board_identity
@ -1,6 +1,6 @@
 # UAVCAN boot loadable Module ID
-set(uavcanblid_sw_version_major 0)
-set(uavcanblid_sw_version_minor 1)
+set(uavcanblid_sw_version_major ${PX4_VERSION_MAJOR})
+set(uavcanblid_sw_version_minor ${PX4_VERSION_MINOR})
 add_definitions(
 	-DAPP_VERSION_MAJOR=${uavcanblid_sw_version_major}
 	-DAPP_VERSION_MINOR=${uavcanblid_sw_version_minor}
@ -14,4 +14,4 @@ add_definitions(
 	-DHW_UAVCAN_NAME=${uavcanblid_name}
 	-DHW_VERSION_MAJOR=${uavcanblid_hw_version_major}
 	-DHW_VERSION_MINOR=${uavcanblid_hw_version_minor}
-)
+)
--- a/boards/ark/can-gps/nuttx-config/nsh/defconfig
+++ b/boards/ark/can-gps/nuttx-config/nsh/defconfig
@ -117,6 +117,8 @@ 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
--- a/boards/ark/can-gps/src/boot_config.h
+++ b/boards/ark/can-gps/src/boot_config.h
@ -65,7 +65,7 @@
 #define OPT_PREFERRED_NODE_ID ANY_NODE_ID

 //todo:wrap OPT_x in in ifdefs for command line definitions
-#define OPT_TBOOT_MS                    5000
+#define OPT_TBOOT_MS                    3000
 #define OPT_NODE_STATUS_RATE_MS         800
 #define OPT_NODE_INFO_RATE_MS           50
 #define OPT_BL_NUMBER_TIMERS            7
@ -93,7 +93,7 @@
 */
 #define OPT_WAIT_FOR_GETNODEINFO                    0
 #define OPT_WAIT_FOR_GETNODEINFO_JUMPER_GPIO        1
-#define OPT_WAIT_FOR_GETNODEINFO_JUMPER_GPIO_INVERT 0
+#define OPT_WAIT_FOR_GETNODEINFO_JUMPER_GPIO_INVERT 1

 #define OPT_ENABLE_WD           1

--- a/boards/ark/can-gps/uavcan_board_identity
+++ b/boards/ark/can-gps/uavcan_board_identity
@ -1,6 +1,6 @@
 # UAVCAN boot loadable Module ID
-set(uavcanblid_sw_version_major 0)
-set(uavcanblid_sw_version_minor 1)
+set(uavcanblid_sw_version_major ${PX4_VERSION_MAJOR})
+set(uavcanblid_sw_version_minor ${PX4_VERSION_MINOR})
 add_definitions(
 	-DAPP_VERSION_MAJOR=${uavcanblid_sw_version_major}
 	-DAPP_VERSION_MINOR=${uavcanblid_sw_version_minor}
--- a/boards/ark/can-rtk-gps/nuttx-config/nsh/defconfig
+++ b/boards/ark/can-rtk-gps/nuttx-config/nsh/defconfig
@ -117,6 +117,8 @@ 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
--- a/boards/ark/can-rtk-gps/src/boot_config.h
+++ b/boards/ark/can-rtk-gps/src/boot_config.h
@ -65,7 +65,7 @@
 #define OPT_PREFERRED_NODE_ID ANY_NODE_ID

 //todo:wrap OPT_x in in ifdefs for command line definitions
-#define OPT_TBOOT_MS                    5000
+#define OPT_TBOOT_MS                    3000
 #define OPT_NODE_STATUS_RATE_MS         800
 #define OPT_NODE_INFO_RATE_MS           50
 #define OPT_BL_NUMBER_TIMERS            7
@ -93,7 +93,7 @@
 */
 #define OPT_WAIT_FOR_GETNODEINFO                    0
 #define OPT_WAIT_FOR_GETNODEINFO_JUMPER_GPIO        1
-#define OPT_WAIT_FOR_GETNODEINFO_JUMPER_GPIO_INVERT 0
+#define OPT_WAIT_FOR_GETNODEINFO_JUMPER_GPIO_INVERT 1

 #define OPT_ENABLE_WD           1

--- a/boards/ark/can-rtk-gps/uavcan_board_identity
+++ b/boards/ark/can-rtk-gps/uavcan_board_identity
@ -1,6 +1,6 @@
 # UAVCAN boot loadable Module ID
-set(uavcanblid_sw_version_major 0)
-set(uavcanblid_sw_version_minor 1)
+set(uavcanblid_sw_version_major ${PX4_VERSION_MAJOR})
+set(uavcanblid_sw_version_minor ${PX4_VERSION_MINOR})
 add_definitions(
 	-DAPP_VERSION_MAJOR=${uavcanblid_sw_version_major}
 	-DAPP_VERSION_MINOR=${uavcanblid_sw_version_minor}
--- a/boards/ark/cannode/nuttx-config/nsh/defconfig
+++ b/boards/ark/cannode/nuttx-config/nsh/defconfig
@ -117,6 +117,8 @@ 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
--- a/boards/ark/cannode/src/boot_config.h
+++ b/boards/ark/cannode/src/boot_config.h
@ -65,7 +65,7 @@
 #define OPT_PREFERRED_NODE_ID ANY_NODE_ID

 //todo:wrap OPT_x in in ifdefs for command line definitions
-#define OPT_TBOOT_MS                    5000
+#define OPT_TBOOT_MS                    3000
 #define OPT_NODE_STATUS_RATE_MS         800
 #define OPT_NODE_INFO_RATE_MS           50
 #define OPT_BL_NUMBER_TIMERS            7
@ -92,8 +92,12 @@
 *
 */
 #define OPT_WAIT_FOR_GETNODEINFO                    0
-#define OPT_WAIT_FOR_GETNODEINFO_JUMPER_GPIO        1
-#define OPT_WAIT_FOR_GETNODEINFO_JUMPER_GPIO_INVERT 0
+/* The ARK CANnode uses PH1 for GPIO_BOOT_CONFIG but it is not
+ * compatible with px4_arch_gpioread as Port H = 7 which is greater
+ * than STM32_NPORTS
+ * #define OPT_WAIT_FOR_GETNODEINFO_JUMPER_GPIO        0
+ */
+#define OPT_WAIT_FOR_GETNODEINFO_JUMPER_GPIO_INVERT 1

 #define OPT_ENABLE_WD           1

--- a/boards/ark/cannode/uavcan_board_identity
+++ b/boards/ark/cannode/uavcan_board_identity
@ -1,6 +1,6 @@
 # UAVCAN boot loadable Module ID
-set(uavcanblid_sw_version_major 0)
-set(uavcanblid_sw_version_minor 1)
+set(uavcanblid_sw_version_major ${PX4_VERSION_MAJOR})
+set(uavcanblid_sw_version_minor ${PX4_VERSION_MINOR})
 add_definitions(
 	-DAPP_VERSION_MAJOR=${uavcanblid_sw_version_major}
 	-DAPP_VERSION_MINOR=${uavcanblid_sw_version_minor}
--- a/boards/ark/fmu-v6x/default.px4board
+++ b/boards/ark/fmu-v6x/default.px4board
@ -20,6 +20,7 @@ CONFIG_DRIVERS_HEATER=y
 CONFIG_DRIVERS_IMU_ANALOG_DEVICES_ADIS16507=y
 CONFIG_DRIVERS_IMU_INVENSENSE_ICM42688P=y
 CONFIG_DRIVERS_IMU_INVENSENSE_IIM42652=y
+CONFIG_DRIVERS_IMU_INVENSENSE_IIM42653=y
 CONFIG_COMMON_LIGHT=y
 CONFIG_COMMON_MAGNETOMETER=y
 CONFIG_COMMON_OPTICAL_FLOW=y
--- a/boards/ark/fmu-v6x/init/rc.board_defaults
+++ b/boards/ark/fmu-v6x/init/rc.board_defaults
@ -17,12 +17,21 @@ param set-default SENS_EN_INA228 0
 param set-default SENS_EN_INA226 1

 param set-default SENS_EN_THERMAL 1
-param set-default SENS_TEMP_ID 2818058
 param set-default SENS_IMU_TEMP 10.0
 #param set-default SENS_IMU_TEMP_FF 0.0
 #param set-default SENS_IMU_TEMP_I 0.025
 #param set-default SENS_IMU_TEMP_P 1.0

+if ver hwtypecmp ARKV6X000000 ARKV6X001000 ARKV6X002000 ARKV6X003000 ARKV6X004000 ARKV6X005000 ARKV6X006000 ARKV6X007000
+then
+	param set-default SENS_TEMP_ID 2818058
+fi
+
+if ver hwtypecmp ARKV6X000001 ARKV6X001001 ARKV6X002001 ARKV6X003001 ARKV6X004001 ARKV6X005001 ARKV6X006001 ARKV6X007001
+then
+	param set-default SENS_TEMP_ID 3014666
+fi
+
 if ver hwtypecmp ARKV6X001000 ARKV6X001001 ARKV6X001002 ARKV6X001003 ARKV6X001004 ARKV6X001005 ARKV6X001006 ARKV6X001007
 then
 	param set-default SYS_USE_IO 0
--- a/boards/ark/fmu-v6x/init/rc.board_sensors
+++ b/boards/ark/fmu-v6x/init/rc.board_sensors
@ -3,10 +3,12 @@
 # ARK FMUARKV6X specific board sensors init
 #------------------------------------------------------------------------------
 set HAVE_PM2 yes
+set HAVE_PM3 yes

 if ver hwtypecmp ARKV6X005000 ARKV6X005001 ARKV6X005002 ARKV6X005003 ARKV6X005004
 then
 	set HAVE_PM2 no
+	set HAVE_PM3 no
 fi

 if param compare -s ADC_ADS1115_EN 1
@ -25,36 +27,68 @@ then
 	then
 		ina226 -X -b 2 -t 2 -k start
 	fi
+
+	if [ $HAVE_PM3 = yes ]
+	then
+		ina226 -X -b 3 -t 2 -k start
+	fi
 fi

 if param compare SENS_EN_INA228 1
 then
 	# Start Digital power monitors
 	ina228 -X -b 1 -t 1 -k start
+
 	if [ $HAVE_PM2 = yes ]
 	then
 		ina228 -X -b 2 -t 2 -k start
 	fi
+
+	if [ $HAVE_PM3 = yes ]
+	then
+		ina228 -X -b 3 -t 2 -k start
+	fi
 fi

 if param compare SENS_EN_INA238 1
 then
 	# Start Digital power monitors
 	ina238 -X -b 1 -t 1 -k start
+
 	if [ $HAVE_PM2 = yes ]
 	then
 		ina238 -X -b 2 -t 2 -k start
 	fi
+
+	if [ $HAVE_PM3 = yes ]
+	then
+		ina238 -X -b 3 -t 2 -k start
+	fi
 fi

-# Internal SPI bus IIM42652 with SPIX measured frequency of 32.051kHz
-iim42652 -R 3 -s -b 1 -C 32051 start
+if ver hwtypecmp ARKV6X000000 ARKV6X001000 ARKV6X002000 ARKV6X003000 ARKV6X004000 ARKV6X005000 ARKV6X006000 ARKV6X007000
+then
+	# Internal SPI bus IIM42652 with SPIX measured frequency of 32.051kHz
+	iim42652 -R 3 -s -b 1 -C 32051 start

-# Internal SPI bus ICM42688p with SPIX measured frequency of 32.051kHz
-icm42688p -R 9 -s -b 2 -C 32051 start
+	# Internal SPI bus ICM42688p with SPIX measured frequency of 32.051kHz
+	icm42688p -R 9 -s -b 2 -C 32051 start

-# Internal SPI bus ICM42688p with SPIX measured frequency of 32.051kHz
-icm42688p -R 6 -s -b 3 -C 32051 start
+	# Internal SPI bus ICM42688p with SPIX measured frequency of 32.051kHz
+	icm42688p -R 6 -s -b 3 -C 32051 start
+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
+
+	# Internal SPI bus IIM42653 with SPIX measured frequency of 32.051kHz
+	iim42653 -R 9 -s -b 2 -C 32051 start
+
+	# Internal SPI bus IIM42653 with SPIX measured frequency of 32.051kHz
+	iim42653 -R 6 -s -b 3 -C 32051 start
+fi

 # Internal magnetometer on I2C
 bmm150 -I start
@ -63,3 +97,4 @@ bmm150 -I start
 bmp388 -I start

 unset HAVE_PM2
+unset HAVE_PM3
--- a/boards/ark/fmu-v6x/nuttx-config/bootloader/defconfig
+++ b/boards/ark/fmu-v6x/nuttx-config/bootloader/defconfig
@ -49,7 +49,7 @@ CONFIG_HAVE_CXX=y
 CONFIG_HAVE_CXXINITIALIZE=y
 CONFIG_IDLETHREAD_STACKSIZE=750
 CONFIG_INIT_ENTRYPOINT="bootloader_main"
-CONFIG_INIT_STACKSIZE=3094
+CONFIG_INIT_STACKSIZE=3194
 CONFIG_LIBC_FLOATINGPOINT=y
 CONFIG_LIBC_LONG_LONG=y
 CONFIG_LIBC_STRERROR=y
--- a/boards/ark/fmu-v6x/nuttx-config/nsh/defconfig
+++ b/boards/ark/fmu-v6x/nuttx-config/nsh/defconfig
@ -118,7 +118,7 @@ CONFIG_I2C=y
 CONFIG_I2C_RESET=y
 CONFIG_IDLETHREAD_STACKSIZE=750
 CONFIG_INIT_ENTRYPOINT="nsh_main"
-CONFIG_INIT_STACKSIZE=3094
+CONFIG_INIT_STACKSIZE=3194
 CONFIG_IOB_NBUFFERS=24
 CONFIG_IOB_THROTTLE=0
 CONFIG_IPCFG_BINARY=y
@ -189,6 +189,8 @@ 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=245760
 CONFIG_RAM_START=0x20010000
--- a/boards/ark/fmu-v6x/src/board_config.h
+++ b/boards/ark/fmu-v6x/src/board_config.h
@ -211,23 +211,18 @@
 #define GPIO_HW_VER_SENSE      /* PH3 */  GPIO_ADC3_INP14
 #define HW_INFO_INIT_PREFIX    "ARKV6X"

-#define BOARD_NUM_SPI_CFG_HW_VERSIONS 2 // Rev 0 and Rev 3,4 Sensor sets
+#define BOARD_NUM_SPI_CFG_HW_VERSIONS 8 // Rev 0 and Rev 1
 //                 Base/FMUM
-#define ARKV6X00   HW_VER_REV(0x0,0x0) // ARKV6X,                 Rev 0
-#define ARKV6X01   HW_VER_REV(0x0,0x1) // ARKV6X,     BMI388 I2C2 Rev 1
-#define ARKV6X03   HW_VER_REV(0x0,0x3) // ARKV6X,     Sensor Set  Rev 3
-#define ARKV6X04   HW_VER_REV(0x0,0x4) // ARKV6X,     Sensor Set  Rev 4
-#define ARKV6X10   HW_VER_REV(0x1,0x0) // NO PX4IO,               Rev 0
-#define ARKV6X13   HW_VER_REV(0x1,0x3) // NO PX4IO,   Sensor Set  Rev 3
-#define ARKV6X14   HW_VER_REV(0x1,0x4) // NO PX4IO,   Sensor Set  Rev 4
-//#define ARKV6X40   HW_VER_REV(0x4,0x0) // ARKV6X,                    HB CM4 base Rev 0 // never shipped
-//#define ARKV6X41   HW_VER_REV(0x4,0x1) // ARKV6X,     BMI388 I2C2    HB CM4 base Rev 1 // never shipped
-#define ARKV6X43   HW_VER_REV(0x4,0x3) // ARKV6X,     Sensor Set     HB CM4 base Rev 3
-#define ARKV6X44   HW_VER_REV(0x4,0x4) // ARKV6X,     Sensor Set     HB CM4 base Rev 4
-#define ARKV6X50   HW_VER_REV(0x5,0x0) // ARKV6X, ARKV6X Rev 0 with HB Mini Rev 5
-//#define ARKV6X51   HW_VER_REV(0x5,0x1) // ARKV6X,     BMI388 I2C2    HB Mini Rev 1 // never shipped
-#define ARKV6X53   HW_VER_REV(0x5,0x3) // ARKV6X,     Sensor Set     HB Mini Rev 3
-#define ARKV6X54   HW_VER_REV(0x5,0x4) // ARKV6X,     Sensor Set     HB Mini Rev 4
+#define ARKV6X00   HW_VER_REV(0x0,0x0) // ARKV6X,     Sensor Set  Rev 0
+#define ARKV6X01   HW_VER_REV(0x0,0x1) // ARKV6X,     Sensor Set  Rev 1
+//#define ARKV6X03   HW_VER_REV(0x0,0x3) // ARKV6X,     Sensor Set  Rev 3
+//#define ARKV6X04   HW_VER_REV(0x0,0x4) // ARKV6X,     Sensor Set  Rev 4
+#define ARKV6X10   HW_VER_REV(0x1,0x0) // NO PX4IO,   Sensor Set  Rev 0
+#define ARKV6X11   HW_VER_REV(0x1,0x1) // NO PX4IO,   Sensor Set  Rev 1
+#define ARKV6X40   HW_VER_REV(0x4,0x0) // ARKV6X,     Sensor Set  Rev 0   HB CM4 base Rev 3
+#define ARKV6X41   HW_VER_REV(0x4,0x1) // ARKV6X,     Sensor Set  Rev 1   HB CM4 base Rev 4
+#define ARKV6X50   HW_VER_REV(0x5,0x0) // ARKV6X,     Sensor Set  Rev 0   HB Mini Rev 5
+#define ARKV6X51   HW_VER_REV(0x5,0x1) // ARKV6X,     Sensor Set  Rev 1   HB Mini Rev 1 // never shipped

 #define UAVCAN_NUM_IFACES_RUNTIME  1

--- a/boards/ark/fmu-v6x/src/manifest.c
+++ b/boards/ark/fmu-v6x/src/manifest.c
@ -160,21 +160,14 @@ static const px4_hw_mft_item_t hw_mft_list_v0650[] = {

 static px4_hw_mft_list_entry_t mft_lists[] = {
 //  ver_rev
-	{ARKV6X00, hw_mft_list_v0600, arraySize(hw_mft_list_v0600)},
-	{ARKV6X01, hw_mft_list_v0600, arraySize(hw_mft_list_v0600)}, // BMP388 moved to I2C2
-	{ARKV6X03, hw_mft_list_v0600, arraySize(hw_mft_list_v0600)}, // BMP388 moved to I2C2, Sensor Set 3
-	//{ARKV6X40, hw_mft_list_v0640, arraySize(hw_mft_list_v0640)}, //                       HB CM4 base           // never shipped
-	//{ARKV6X41, hw_mft_list_v0640, arraySize(hw_mft_list_v0640)}, // BMP388 moved to I2C2  HB CM4 base           // never shipped
-	{ARKV6X43, hw_mft_list_v0640, arraySize(hw_mft_list_v0640)}, // BMP388 moved to I2C2, HB CM4 base Sensor Set 3
-	{ARKV6X44, hw_mft_list_v0640, arraySize(hw_mft_list_v0640)}, // BMP388 moved to I2C2, HB CM4 base Sensor Set 4
-	{ARKV6X50, hw_mft_list_v0650, arraySize(hw_mft_list_v0650)}, // ARKV6X Rev 0 with HB Mini Rev 5
-	//{ARKV6X51, hw_mft_list_v0650, arraySize(hw_mft_list_v0650)}, // BMP388 moved to I2C2  HB Mini               // never shipped
-	{ARKV6X53, hw_mft_list_v0650, arraySize(hw_mft_list_v0650)}, // BMP388 moved to I2C2, HB Mini Sensor Set 3
-	{ARKV6X54, hw_mft_list_v0650, arraySize(hw_mft_list_v0650)}, // BMP388 moved to I2C2, HB Mini Sensor Set 4
-	{ARKV6X10, hw_mft_list_v0610, arraySize(hw_mft_list_v0610)}, // No PX4IO
-	{ARKV6X13, hw_mft_list_v0610, arraySize(hw_mft_list_v0610)}, // No PX4IO BMP388 moved to I2C2, Sensor Set 3
-	{ARKV6X04, hw_mft_list_v0600, arraySize(hw_mft_list_v0600)}, // BMP388 moved to I2C2, Sensor Set 4
-	{ARKV6X14, hw_mft_list_v0610, arraySize(hw_mft_list_v0610)}, // No PX4IO BMP388 moved to I2C2, Sensor Set 4
+	{ARKV6X00, hw_mft_list_v0600, arraySize(hw_mft_list_v0600)}, // ARKV6X,     Sensor Set  Rev 0
+	{ARKV6X01, hw_mft_list_v0600, arraySize(hw_mft_list_v0600)}, // ARKV6X,     Sensor Set  Rev 1
+	{ARKV6X10, hw_mft_list_v0610, arraySize(hw_mft_list_v0610)}, // NO PX4IO,   Sensor Set  Rev 0
+	{ARKV6X11, hw_mft_list_v0610, arraySize(hw_mft_list_v0610)}, // NO PX4IO,   Sensor Set  Rev 1
+	{ARKV6X40, hw_mft_list_v0640, arraySize(hw_mft_list_v0640)}, // ARKV6X,     Sensor Set  Rev 0   HB CM4 base Rev 3
+	{ARKV6X41, hw_mft_list_v0640, arraySize(hw_mft_list_v0640)}, // ARKV6X,     Sensor Set  Rev 1   HB CM4 base Rev 4
+	{ARKV6X50, hw_mft_list_v0650, arraySize(hw_mft_list_v0650)}, // ARKV6X,     Sensor Set  Rev 0   HB Mini Rev 5
+	{ARKV6X51, hw_mft_list_v0650, arraySize(hw_mft_list_v0650)}, // ARKV6X,     Sensor Set  Rev 1   HB Mini Rev 1 // never shipped
 };

 /************************************************************************************
--- a/boards/ark/fmu-v6x/src/mtd.cpp
+++ b/boards/ark/fmu-v6x/src/mtd.cpp
@ -34,7 +34,7 @@
 #include <nuttx/spi/spi.h>
 #include <px4_platform_common/px4_manifest.h>
 //                                                              KiB BS    nB
-static const px4_mft_device_t spi5 = {             // FM25V02A on FMUM 32K 512 X 64
+static const px4_mft_device_t spi5 = {             // FM25V02A on FMUM native: 32K X 8, emulated as (1024 Blocks of 32)
 	.bus_type = px4_mft_device_t::SPI,
 	.devid    = SPIDEV_FLASH(0)
 };
@ -45,18 +45,12 @@ static const px4_mft_device_t i2c3 = {             // 24LC64T on Base  8K 32 X 2

 static const px4_mtd_entry_t fmum_fram = {
 	.device = &spi5,
-	.npart = 2,
+	.npart = 1,
 	.partd = {
 		{
 			.type = MTD_PARAMETERS,
 			.path = "/fs/mtd_params",
-			.nblocks = 32
-		},
-		{
-			.type = MTD_WAYPOINTS,
-			.path = "/fs/mtd_waypoints",
-			.nblocks = 32
-
+			.nblocks = (32768 / (1 << CONFIG_RAMTRON_EMULATE_SECTOR_SHIFT))
 		}
 	},
 };
--- a/boards/ark/fmu-v6x/src/spi.cpp
+++ b/boards/ark/fmu-v6x/src/spi.cpp
@ -59,7 +59,30 @@ constexpr px4_spi_bus_all_hw_t px4_spi_buses_all_hw[BOARD_NUM_SPI_CFG_HW_VERSION
 		}),
 	}),

-	initSPIHWVersion(ARKV6X01, { // Placeholder
+	initSPIHWVersion(ARKV6X01, {
+		initSPIBus(SPI::Bus::SPI1, {
+			initSPIDevice(DRV_IMU_DEVTYPE_IIM42653, SPI::CS{GPIO::PortI, GPIO::Pin9}, SPI::DRDY{GPIO::PortF, GPIO::Pin2}),
+		}, {GPIO::PortI, GPIO::Pin11}),
+		initSPIBus(SPI::Bus::SPI2, {
+			initSPIDevice(DRV_IMU_DEVTYPE_IIM42653, SPI::CS{GPIO::PortH, GPIO::Pin5}, SPI::DRDY{GPIO::PortA, GPIO::Pin10}),
+		}, {GPIO::PortF, GPIO::Pin4}),
+		initSPIBus(SPI::Bus::SPI3, {
+			initSPIDevice(DRV_IMU_DEVTYPE_IIM42653, SPI::CS{GPIO::PortI, GPIO::Pin4}, SPI::DRDY{GPIO::PortI, GPIO::Pin6}),
+		}, {GPIO::PortE, GPIO::Pin7}),
+		//  initSPIBus(SPI::Bus::SPI4, {
+		//    // no devices
+		// TODO: if enabled, remove GPIO_VDD_3V3_SENSORS4_EN from board_config.h
+		//  }, {GPIO::PortG, GPIO::Pin8}),
+		initSPIBus(SPI::Bus::SPI5, {
+			initSPIDevice(SPIDEV_FLASH(0), SPI::CS{GPIO::PortG, GPIO::Pin7})
+		}),
+		initSPIBusExternal(SPI::Bus::SPI6, {
+			initSPIConfigExternal(SPI::CS{GPIO::PortI, GPIO::Pin10}, SPI::DRDY{GPIO::PortD, GPIO::Pin11}),
+			initSPIConfigExternal(SPI::CS{GPIO::PortA, GPIO::Pin15}, SPI::DRDY{GPIO::PortD, GPIO::Pin12}),
+		}),
+	}),
+
+	initSPIHWVersion(ARKV6X10, {
 		initSPIBus(SPI::Bus::SPI1, {
 			initSPIDevice(DRV_IMU_DEVTYPE_IIM42652, SPI::CS{GPIO::PortI, GPIO::Pin9}, SPI::DRDY{GPIO::PortF, GPIO::Pin2}),
 		}, {GPIO::PortI, GPIO::Pin11}),
@ -81,6 +104,121 @@ constexpr px4_spi_bus_all_hw_t px4_spi_buses_all_hw[BOARD_NUM_SPI_CFG_HW_VERSION
 			initSPIConfigExternal(SPI::CS{GPIO::PortA, GPIO::Pin15}, SPI::DRDY{GPIO::PortD, GPIO::Pin12}),
 		}),
 	}),
+
+	initSPIHWVersion(ARKV6X11, {
+		initSPIBus(SPI::Bus::SPI1, {
+			initSPIDevice(DRV_IMU_DEVTYPE_IIM42653, SPI::CS{GPIO::PortI, GPIO::Pin9}, SPI::DRDY{GPIO::PortF, GPIO::Pin2}),
+		}, {GPIO::PortI, GPIO::Pin11}),
+		initSPIBus(SPI::Bus::SPI2, {
+			initSPIDevice(DRV_IMU_DEVTYPE_IIM42653, SPI::CS{GPIO::PortH, GPIO::Pin5}, SPI::DRDY{GPIO::PortA, GPIO::Pin10}),
+		}, {GPIO::PortF, GPIO::Pin4}),
+		initSPIBus(SPI::Bus::SPI3, {
+			initSPIDevice(DRV_IMU_DEVTYPE_IIM42653, SPI::CS{GPIO::PortI, GPIO::Pin4}, SPI::DRDY{GPIO::PortI, GPIO::Pin6}),
+		}, {GPIO::PortE, GPIO::Pin7}),
+		//  initSPIBus(SPI::Bus::SPI4, {
+		//    // no devices
+		// TODO: if enabled, remove GPIO_VDD_3V3_SENSORS4_EN from board_config.h
+		//  }, {GPIO::PortG, GPIO::Pin8}),
+		initSPIBus(SPI::Bus::SPI5, {
+			initSPIDevice(SPIDEV_FLASH(0), SPI::CS{GPIO::PortG, GPIO::Pin7})
+		}),
+		initSPIBusExternal(SPI::Bus::SPI6, {
+			initSPIConfigExternal(SPI::CS{GPIO::PortI, GPIO::Pin10}, SPI::DRDY{GPIO::PortD, GPIO::Pin11}),
+			initSPIConfigExternal(SPI::CS{GPIO::PortA, GPIO::Pin15}, SPI::DRDY{GPIO::PortD, GPIO::Pin12}),
+		}),
+	}),
+
+	initSPIHWVersion(ARKV6X40, {
+		initSPIBus(SPI::Bus::SPI1, {
+			initSPIDevice(DRV_IMU_DEVTYPE_IIM42652, SPI::CS{GPIO::PortI, GPIO::Pin9}, SPI::DRDY{GPIO::PortF, GPIO::Pin2}),
+		}, {GPIO::PortI, GPIO::Pin11}),
+		initSPIBus(SPI::Bus::SPI2, {
+			initSPIDevice(DRV_IMU_DEVTYPE_ICM42688P, SPI::CS{GPIO::PortH, GPIO::Pin5}, SPI::DRDY{GPIO::PortA, GPIO::Pin10}),
+		}, {GPIO::PortF, GPIO::Pin4}),
+		initSPIBus(SPI::Bus::SPI3, {
+			initSPIDevice(DRV_IMU_DEVTYPE_ICM42688P, SPI::CS{GPIO::PortI, GPIO::Pin4}, SPI::DRDY{GPIO::PortI, GPIO::Pin6}),
+		}, {GPIO::PortE, GPIO::Pin7}),
+		//  initSPIBus(SPI::Bus::SPI4, {
+		//    // no devices
+		// TODO: if enabled, remove GPIO_VDD_3V3_SENSORS4_EN from board_config.h
+		//  }, {GPIO::PortG, GPIO::Pin8}),
+		initSPIBus(SPI::Bus::SPI5, {
+			initSPIDevice(SPIDEV_FLASH(0), SPI::CS{GPIO::PortG, GPIO::Pin7})
+		}),
+		initSPIBusExternal(SPI::Bus::SPI6, {
+			initSPIConfigExternal(SPI::CS{GPIO::PortI, GPIO::Pin10}, SPI::DRDY{GPIO::PortD, GPIO::Pin11}),
+			initSPIConfigExternal(SPI::CS{GPIO::PortA, GPIO::Pin15}, SPI::DRDY{GPIO::PortD, GPIO::Pin12}),
+		}),
+	}),
+
+	initSPIHWVersion(ARKV6X41, {
+		initSPIBus(SPI::Bus::SPI1, {
+			initSPIDevice(DRV_IMU_DEVTYPE_IIM42653, SPI::CS{GPIO::PortI, GPIO::Pin9}, SPI::DRDY{GPIO::PortF, GPIO::Pin2}),
+		}, {GPIO::PortI, GPIO::Pin11}),
+		initSPIBus(SPI::Bus::SPI2, {
+			initSPIDevice(DRV_IMU_DEVTYPE_IIM42653, SPI::CS{GPIO::PortH, GPIO::Pin5}, SPI::DRDY{GPIO::PortA, GPIO::Pin10}),
+		}, {GPIO::PortF, GPIO::Pin4}),
+		initSPIBus(SPI::Bus::SPI3, {
+			initSPIDevice(DRV_IMU_DEVTYPE_IIM42653, SPI::CS{GPIO::PortI, GPIO::Pin4}, SPI::DRDY{GPIO::PortI, GPIO::Pin6}),
+		}, {GPIO::PortE, GPIO::Pin7}),
+		//  initSPIBus(SPI::Bus::SPI4, {
+		//    // no devices
+		// TODO: if enabled, remove GPIO_VDD_3V3_SENSORS4_EN from board_config.h
+		//  }, {GPIO::PortG, GPIO::Pin8}),
+		initSPIBus(SPI::Bus::SPI5, {
+			initSPIDevice(SPIDEV_FLASH(0), SPI::CS{GPIO::PortG, GPIO::Pin7})
+		}),
+		initSPIBusExternal(SPI::Bus::SPI6, {
+			initSPIConfigExternal(SPI::CS{GPIO::PortI, GPIO::Pin10}, SPI::DRDY{GPIO::PortD, GPIO::Pin11}),
+			initSPIConfigExternal(SPI::CS{GPIO::PortA, GPIO::Pin15}, SPI::DRDY{GPIO::PortD, GPIO::Pin12}),
+		}),
+	}),
+
+	initSPIHWVersion(ARKV6X50, {
+		initSPIBus(SPI::Bus::SPI1, {
+			initSPIDevice(DRV_IMU_DEVTYPE_IIM42652, SPI::CS{GPIO::PortI, GPIO::Pin9}, SPI::DRDY{GPIO::PortF, GPIO::Pin2}),
+		}, {GPIO::PortI, GPIO::Pin11}),
+		initSPIBus(SPI::Bus::SPI2, {
+			initSPIDevice(DRV_IMU_DEVTYPE_ICM42688P, SPI::CS{GPIO::PortH, GPIO::Pin5}, SPI::DRDY{GPIO::PortA, GPIO::Pin10}),
+		}, {GPIO::PortF, GPIO::Pin4}),
+		initSPIBus(SPI::Bus::SPI3, {
+			initSPIDevice(DRV_IMU_DEVTYPE_ICM42688P, SPI::CS{GPIO::PortI, GPIO::Pin4}, SPI::DRDY{GPIO::PortI, GPIO::Pin6}),
+		}, {GPIO::PortE, GPIO::Pin7}),
+		//  initSPIBus(SPI::Bus::SPI4, {
+		//    // no devices
+		// TODO: if enabled, remove GPIO_VDD_3V3_SENSORS4_EN from board_config.h
+		//  }, {GPIO::PortG, GPIO::Pin8}),
+		initSPIBus(SPI::Bus::SPI5, {
+			initSPIDevice(SPIDEV_FLASH(0), SPI::CS{GPIO::PortG, GPIO::Pin7})
+		}),
+		initSPIBusExternal(SPI::Bus::SPI6, {
+			initSPIConfigExternal(SPI::CS{GPIO::PortI, GPIO::Pin10}, SPI::DRDY{GPIO::PortD, GPIO::Pin11}),
+			initSPIConfigExternal(SPI::CS{GPIO::PortA, GPIO::Pin15}, SPI::DRDY{GPIO::PortD, GPIO::Pin12}),
+		}),
+	}),
+
+	initSPIHWVersion(ARKV6X51, {
+		initSPIBus(SPI::Bus::SPI1, {
+			initSPIDevice(DRV_IMU_DEVTYPE_IIM42653, SPI::CS{GPIO::PortI, GPIO::Pin9}, SPI::DRDY{GPIO::PortF, GPIO::Pin2}),
+		}, {GPIO::PortI, GPIO::Pin11}),
+		initSPIBus(SPI::Bus::SPI2, {
+			initSPIDevice(DRV_IMU_DEVTYPE_IIM42653, SPI::CS{GPIO::PortH, GPIO::Pin5}, SPI::DRDY{GPIO::PortA, GPIO::Pin10}),
+		}, {GPIO::PortF, GPIO::Pin4}),
+		initSPIBus(SPI::Bus::SPI3, {
+			initSPIDevice(DRV_IMU_DEVTYPE_IIM42653, SPI::CS{GPIO::PortI, GPIO::Pin4}, SPI::DRDY{GPIO::PortI, GPIO::Pin6}),
+		}, {GPIO::PortE, GPIO::Pin7}),
+		//  initSPIBus(SPI::Bus::SPI4, {
+		//    // no devices
+		// TODO: if enabled, remove GPIO_VDD_3V3_SENSORS4_EN from board_config.h
+		//  }, {GPIO::PortG, GPIO::Pin8}),
+		initSPIBus(SPI::Bus::SPI5, {
+			initSPIDevice(SPIDEV_FLASH(0), SPI::CS{GPIO::PortG, GPIO::Pin7})
+		}),
+		initSPIBusExternal(SPI::Bus::SPI6, {
+			initSPIConfigExternal(SPI::CS{GPIO::PortI, GPIO::Pin10}, SPI::DRDY{GPIO::PortD, GPIO::Pin11}),
+			initSPIConfigExternal(SPI::CS{GPIO::PortA, GPIO::Pin15}, SPI::DRDY{GPIO::PortD, GPIO::Pin12}),
+		}),
+	}),
 };

 static constexpr bool unused = validateSPIConfig(px4_spi_buses_all_hw);
--- a/boards/atl/mantis-edu/nuttx-config/nsh/defconfig
+++ b/boards/atl/mantis-edu/nuttx-config/nsh/defconfig
@ -112,7 +112,7 @@ CONFIG_I2C=y
 CONFIG_I2C_RESET=y
 CONFIG_IDLETHREAD_STACKSIZE=750
 CONFIG_INIT_ENTRYPOINT="nsh_main"
-CONFIG_INIT_STACKSIZE=3094
+CONFIG_INIT_STACKSIZE=3194
 CONFIG_LIBC_FLOATINGPOINT=y
 CONFIG_LIBC_LONG_LONG=y
 CONFIG_LIBC_MAX_EXITFUNS=1
@ -147,6 +147,8 @@ 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=245760
 CONFIG_RAM_START=0x20010000
--- a/boards/av/x-v1/nuttx-config/nsh/defconfig
+++ b/boards/av/x-v1/nuttx-config/nsh/defconfig
@ -120,7 +120,7 @@ CONFIG_I2C=y
 CONFIG_I2C_RESET=y
 CONFIG_IDLETHREAD_STACKSIZE=750
 CONFIG_INIT_ENTRYPOINT="nsh_main"
-CONFIG_INIT_STACKSIZE=3094
+CONFIG_INIT_STACKSIZE=3194
 CONFIG_IOB_NBUFFERS=24
 CONFIG_IOB_THROTTLE=0
 CONFIG_LIBC_LONG_LONG=y
--- a/boards/bitcraze/crazyflie/default.px4board
+++ b/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
--- a/boards/bitcraze/crazyflie/nuttx-config/nsh/defconfig
+++ b/boards/bitcraze/crazyflie/nuttx-config/nsh/defconfig
@ -105,7 +105,7 @@ CONFIG_HAVE_CXXINITIALIZE=y
 CONFIG_I2C_RESET=y
 CONFIG_IDLETHREAD_STACKSIZE=750
 CONFIG_INIT_ENTRYPOINT="nsh_main"
-CONFIG_INIT_STACKSIZE=3094
+CONFIG_INIT_STACKSIZE=3194
 CONFIG_LIBC_FLOATINGPOINT=y
 CONFIG_LIBC_LONG_LONG=y
 CONFIG_LIBC_MAX_EXITFUNS=1
--- a/boards/bitcraze/crazyflie/src/mtd.cpp
+++ b/boards/bitcraze/crazyflie/src/mtd.cpp
@ -42,18 +42,12 @@ static const px4_mft_device_t i2c1 = {             // 24AA64FT on Base  8K 32 X

 static const px4_mtd_entry_t fmu_eeprom = {
 	.device = &i2c1,
-	.npart = 2,
+	.npart = 1,
 	.partd = {
 		{
 			.type = MTD_PARAMETERS,
 			.path = "/fs/mtd_params",
 			.nblocks = 128
-		},
-		{
-			.type = MTD_WAYPOINTS,
-			.path = "/fs/mtd_waypoints",
-			.nblocks = 128
-
 		}
 	},
 };
--- a/boards/bitcraze/crazyflie21/default.px4board
+++ b/boards/bitcraze/crazyflie21/default.px4board
@ -14,7 +14,8 @@ 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
 CONFIG_MODULES_FLIGHT_MODE_MANAGER=y
@ -33,6 +34,7 @@ CONFIG_MODULES_NAVIGATOR=y
 CONFIG_MODULES_RC_UPDATE=y
 CONFIG_MODULES_SENSORS=y
 # CONFIG_SENSORS_VEHICLE_AIRSPEED is not set
+# CONFIG_SENSORS_VEHICLE_MAGNETOMETER is not set
 CONFIG_SYSTEMCMDS_DMESG=y
 CONFIG_SYSTEMCMDS_HARDFAULT_LOG=y
 CONFIG_SYSTEMCMDS_MTD=y
--- a/boards/bitcraze/crazyflie21/nuttx-config/nsh/defconfig
+++ b/boards/bitcraze/crazyflie21/nuttx-config/nsh/defconfig
@ -106,7 +106,7 @@ CONFIG_HAVE_CXXINITIALIZE=y
 CONFIG_I2C_RESET=y
 CONFIG_IDLETHREAD_STACKSIZE=750
 CONFIG_INIT_ENTRYPOINT="nsh_main"
-CONFIG_INIT_STACKSIZE=3094
+CONFIG_INIT_STACKSIZE=3194
 CONFIG_LIBC_FLOATINGPOINT=y
 CONFIG_LIBC_LONG_LONG=y
 CONFIG_LIBC_MAX_EXITFUNS=1
--- a/boards/cuav/can-gps-v1/nuttx-config/nsh/defconfig
+++ b/boards/cuav/can-gps-v1/nuttx-config/nsh/defconfig
@ -119,6 +119,8 @@ 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
--- a/boards/cuav/can-gps-v1/uavcan_board_identity
+++ b/boards/cuav/can-gps-v1/uavcan_board_identity
@ -1,6 +1,6 @@
 # UAVCAN boot loadable Module ID
-set(uavcanblid_sw_version_major 0)
-set(uavcanblid_sw_version_minor 1)
+set(uavcanblid_sw_version_major ${PX4_VERSION_MAJOR})
+set(uavcanblid_sw_version_minor ${PX4_VERSION_MINOR})
 add_definitions(
 	-DAPP_VERSION_MAJOR=${uavcanblid_sw_version_major}
 	-DAPP_VERSION_MINOR=${uavcanblid_sw_version_minor}
--- a/boards/cuav/nora/nuttx-config/bootloader/defconfig
+++ b/boards/cuav/nora/nuttx-config/bootloader/defconfig
@ -48,7 +48,7 @@ CONFIG_HAVE_CXX=y
 CONFIG_HAVE_CXXINITIALIZE=y
 CONFIG_IDLETHREAD_STACKSIZE=750
 CONFIG_INIT_ENTRYPOINT="bootloader_main"
-CONFIG_INIT_STACKSIZE=3094
+CONFIG_INIT_STACKSIZE=3194
 CONFIG_LIBC_FLOATINGPOINT=y
 CONFIG_LIBC_LONG_LONG=y
 CONFIG_LIBC_STRERROR=y
--- a/boards/cuav/nora/nuttx-config/nsh/defconfig
+++ b/boards/cuav/nora/nuttx-config/nsh/defconfig
@ -111,7 +111,7 @@ CONFIG_I2C=y
 CONFIG_I2C_RESET=y
 CONFIG_IDLETHREAD_STACKSIZE=750
 CONFIG_INIT_ENTRYPOINT="nsh_main"
-CONFIG_INIT_STACKSIZE=3094
+CONFIG_INIT_STACKSIZE=3194
 CONFIG_LIBC_FLOATINGPOINT=y
 CONFIG_LIBC_LONG_LONG=y
 CONFIG_LIBC_MAX_EXITFUNS=1
@ -147,6 +147,8 @@ 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=245760
 CONFIG_RAM_START=0x20010000
--- a/boards/cuav/x7pro/nuttx-config/bootloader/defconfig
+++ b/boards/cuav/x7pro/nuttx-config/bootloader/defconfig
@ -48,7 +48,7 @@ CONFIG_HAVE_CXX=y
 CONFIG_HAVE_CXXINITIALIZE=y
 CONFIG_IDLETHREAD_STACKSIZE=750
 CONFIG_INIT_ENTRYPOINT="bootloader_main"
-CONFIG_INIT_STACKSIZE=3094
+CONFIG_INIT_STACKSIZE=3194
 CONFIG_LIBC_FLOATINGPOINT=y
 CONFIG_LIBC_LONG_LONG=y
 CONFIG_LIBC_STRERROR=y
--- a/boards/cuav/x7pro/nuttx-config/nsh/defconfig
+++ b/boards/cuav/x7pro/nuttx-config/nsh/defconfig
@ -112,7 +112,7 @@ CONFIG_I2C=y
 CONFIG_I2C_RESET=y
 CONFIG_IDLETHREAD_STACKSIZE=750
 CONFIG_INIT_ENTRYPOINT="nsh_main"
-CONFIG_INIT_STACKSIZE=3094
+CONFIG_INIT_STACKSIZE=3194
 CONFIG_LIBC_LONG_LONG=y
 CONFIG_LIBC_MAX_EXITFUNS=1
 CONFIG_LIBC_STRERROR=y
@ -147,6 +147,8 @@ 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=245760
 CONFIG_RAM_START=0x20010000
--- a/boards/cuav/x7pro/nuttx-config/test/defconfig
+++ b/boards/cuav/x7pro/nuttx-config/test/defconfig
@ -112,7 +112,7 @@ CONFIG_I2C=y
 CONFIG_I2C_RESET=y
 CONFIG_IDLETHREAD_STACKSIZE=750
 CONFIG_INIT_ENTRYPOINT="nsh_main"
-CONFIG_INIT_STACKSIZE=3094
+CONFIG_INIT_STACKSIZE=3194
 CONFIG_LIBC_LONG_LONG=y
 CONFIG_LIBC_MAX_EXITFUNS=1
 CONFIG_LIBC_STRERROR=y
@ -147,6 +147,8 @@ 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=245760
 CONFIG_RAM_START=0x20010000
--- a/boards/cubepilot/cubeorange/default.px4board
+++ b/boards/cubepilot/cubeorange/default.px4board
@ -19,6 +19,7 @@ CONFIG_DRIVERS_IMU_ANALOG_DEVICES_ADIS16448=y
 CONFIG_DRIVERS_IMU_INVENSENSE_ICM20602=y
 CONFIG_DRIVERS_IMU_INVENSENSE_ICM20649=y
 CONFIG_DRIVERS_IMU_INVENSENSE_ICM20948=y
+CONFIG_COMMON_INS=y
 CONFIG_DRIVERS_IRLOCK=y
 CONFIG_COMMON_LIGHT=y
 CONFIG_COMMON_MAGNETOMETER=y
@ -49,7 +50,6 @@ CONFIG_MODULES_FW_POS_CONTROL=y
 CONFIG_MODULES_FW_RATE_CONTROL=y
 CONFIG_MODULES_GIMBAL=y
 CONFIG_MODULES_GYRO_CALIBRATION=y
-CONFIG_MODULES_GYRO_FFT=y
 CONFIG_MODULES_LAND_DETECTOR=y
 CONFIG_MODULES_LANDING_TARGET_ESTIMATOR=y
 CONFIG_MODULES_LOAD_MON=y
--- a/boards/cubepilot/cubeorange/nuttx-config/bootloader/defconfig
+++ b/boards/cubepilot/cubeorange/nuttx-config/bootloader/defconfig
@ -48,7 +48,7 @@ CONFIG_HAVE_CXX=y
 CONFIG_HAVE_CXXINITIALIZE=y
 CONFIG_IDLETHREAD_STACKSIZE=750
 CONFIG_INIT_ENTRYPOINT="bootloader_main"
-CONFIG_INIT_STACKSIZE=3094
+CONFIG_INIT_STACKSIZE=3194
 CONFIG_LIBC_FLOATINGPOINT=y
 CONFIG_LIBC_LONG_LONG=y
 CONFIG_LIBC_STRERROR=y
--- a/boards/cubepilot/cubeorange/nuttx-config/nsh/defconfig
+++ b/boards/cubepilot/cubeorange/nuttx-config/nsh/defconfig
@ -112,7 +112,7 @@ CONFIG_I2C=y
 CONFIG_I2C_RESET=y
 CONFIG_IDLETHREAD_STACKSIZE=750
 CONFIG_INIT_ENTRYPOINT="nsh_main"
-CONFIG_INIT_STACKSIZE=3094
+CONFIG_INIT_STACKSIZE=3194
 CONFIG_LIBC_FLOATINGPOINT=y
 CONFIG_LIBC_LONG_LONG=y
 CONFIG_LIBC_MAX_EXITFUNS=1
@ -148,6 +148,8 @@ 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=245760
 CONFIG_RAM_START=0x20010000
--- a/boards/cubepilot/cubeorange/nuttx-config/test/defconfig
+++ b/boards/cubepilot/cubeorange/nuttx-config/test/defconfig
@ -113,7 +113,7 @@ CONFIG_I2C=y
 CONFIG_I2C_RESET=y
 CONFIG_IDLETHREAD_STACKSIZE=750
 CONFIG_INIT_ENTRYPOINT="nsh_main"
-CONFIG_INIT_STACKSIZE=3094
+CONFIG_INIT_STACKSIZE=3194
 CONFIG_LIBC_LONG_LONG=y
 CONFIG_LIBC_MAX_EXITFUNS=1
 CONFIG_LIBC_STRERROR=y
@ -148,6 +148,8 @@ 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=245760
 CONFIG_RAM_START=0x20010000
--- a/boards/cubepilot/cubeorangeplus/default.px4board
+++ b/boards/cubepilot/cubeorangeplus/default.px4board
@ -20,6 +20,7 @@ CONFIG_DRIVERS_IMU_INVENSENSE_ICM20649=y
 CONFIG_DRIVERS_IMU_INVENSENSE_ICM20948=y
 CONFIG_DRIVERS_IMU_INVENSENSE_ICM42688P=y
 CONFIG_DRIVERS_IMU_INVENSENSE_ICM45686=y
+CONFIG_COMMON_INS=y
 CONFIG_DRIVERS_IRLOCK=y
 CONFIG_COMMON_LIGHT=y
 CONFIG_COMMON_MAGNETOMETER=y
--- a/boards/cubepilot/cubeorangeplus/init/rc.board_sensors
+++ b/boards/cubepilot/cubeorangeplus/init/rc.board_sensors
@ -8,16 +8,22 @@ board_adc start
 # 1. Isolated {ICM42688p, ICM20948(with mag)}, body-fixed {ICM20649}
 # 2. Isolated {ICM42688p, ICM42688p}, body-fixed  {ICM20649, ICM45686, AK09918}
 # 3. Isolated {ICM42688p, ICM42688p}, body-fixed  {ICM45686, AK09918}
+# 4. Isolated {ICM45686, ICM45686}, body-fixed  {ICM45686, AK09918}

 # SPI4 is isolated, SPI1 is body-fixed

 # SPI4, isolated
 ms5611 -s -b 4 start

-icm42688p -s -b 4 -R 10 start -c 15
-if ! icm20948 -s -b 4 -R 10 -M -q start
+if icm42688p -s -b 4 -R 10 -q start -c 15
 then
-	icm42688p -s -b 4 -R 6 start -c 13
+	if ! icm20948 -s -b 4 -R 10 -M -q start
+	then
+		icm42688p -s -b 4 -R 6 start -c 13
+	fi
+else
+	icm45686 -s -b 4 -R 10 start -c 15
+	icm45686 -s -b 4 -R 6 start -c 13
 fi

 # SPI1, body-fixed
--- a/boards/cubepilot/cubeorangeplus/nuttx-config/bootloader/defconfig
+++ b/boards/cubepilot/cubeorangeplus/nuttx-config/bootloader/defconfig
@ -49,7 +49,7 @@ CONFIG_HAVE_CXX=y
 CONFIG_HAVE_CXXINITIALIZE=y
 CONFIG_IDLETHREAD_STACKSIZE=750
 CONFIG_INIT_ENTRYPOINT="bootloader_main"
-CONFIG_INIT_STACKSIZE=3094
+CONFIG_INIT_STACKSIZE=3194
 CONFIG_LIBC_FLOATINGPOINT=y
 CONFIG_LIBC_LONG_LONG=y
 CONFIG_LIBC_STRERROR=y
--- a/boards/cubepilot/cubeorangeplus/nuttx-config/nsh/defconfig
+++ b/boards/cubepilot/cubeorangeplus/nuttx-config/nsh/defconfig
@ -112,7 +112,7 @@ CONFIG_I2C=y
 CONFIG_I2C_RESET=y
 CONFIG_IDLETHREAD_STACKSIZE=750
 CONFIG_INIT_ENTRYPOINT="nsh_main"
-CONFIG_INIT_STACKSIZE=3094
+CONFIG_INIT_STACKSIZE=3194
 CONFIG_LIBC_FLOATINGPOINT=y
 CONFIG_LIBC_LONG_LONG=y
 CONFIG_LIBC_MAX_EXITFUNS=1
@ -148,6 +148,8 @@ 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=245760
 CONFIG_RAM_START=0x20010000
--- a/boards/cubepilot/cubeorangeplus/nuttx-config/test/defconfig
+++ b/boards/cubepilot/cubeorangeplus/nuttx-config/test/defconfig
@ -113,7 +113,7 @@ CONFIG_I2C=y
 CONFIG_I2C_RESET=y
 CONFIG_IDLETHREAD_STACKSIZE=750
 CONFIG_INIT_ENTRYPOINT="nsh_main"
-CONFIG_INIT_STACKSIZE=3094
+CONFIG_INIT_STACKSIZE=3194
 CONFIG_LIBC_LONG_LONG=y
 CONFIG_LIBC_MAX_EXITFUNS=1
 CONFIG_LIBC_STRERROR=y
@ -148,6 +148,8 @@ 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=245760
 CONFIG_RAM_START=0x20010000
--- a/boards/cubepilot/cubeorangeplus/src/spi.cpp
+++ b/boards/cubepilot/cubeorangeplus/src/spi.cpp
@ -49,7 +49,9 @@ constexpr px4_spi_bus_t px4_spi_buses[SPI_BUS_MAX_BUS_ITEMS] = {
 	initSPIBus(SPI::Bus::SPI4, {
 		initSPIDevice(DRV_IMU_DEVTYPE_ICM20948, SPI::CS{GPIO::PortE, GPIO::Pin4}),  // MPU_EXT_CS
 		initSPIDevice(DRV_IMU_DEVTYPE_ICM42688P, SPI::CS{GPIO::PortC, GPIO::Pin15}), // ACCEL_EXT_CS
+		initSPIDevice(DRV_IMU_DEVTYPE_ICM45686, SPI::CS{GPIO::PortC, GPIO::Pin15}), // ACCEL_EXT_CS
 		initSPIDevice(DRV_IMU_DEVTYPE_ICM42688P, SPI::CS{GPIO::PortC, GPIO::Pin13}), // GYRO_EXT_CS
+		initSPIDevice(DRV_IMU_DEVTYPE_ICM45686, SPI::CS{GPIO::PortC, GPIO::Pin13}), // GYRO_EXT_CS
 		initSPIDevice(DRV_BARO_DEVTYPE_MS5611, 	SPI::CS{GPIO::PortC, GPIO::Pin14}), // BARO_EXT_CS
 	}),
 };
--- a/boards/cubepilot/cubeyellow/nuttx-config/nsh/defconfig
+++ b/boards/cubepilot/cubeyellow/nuttx-config/nsh/defconfig
@ -112,7 +112,7 @@ CONFIG_I2C=y
 CONFIG_I2C_RESET=y
 CONFIG_IDLETHREAD_STACKSIZE=750
 CONFIG_INIT_ENTRYPOINT="nsh_main"
-CONFIG_INIT_STACKSIZE=3094
+CONFIG_INIT_STACKSIZE=3194
 CONFIG_LIBC_FLOATINGPOINT=y
 CONFIG_LIBC_LONG_LONG=y
 CONFIG_LIBC_MAX_EXITFUNS=1
@ -147,6 +147,8 @@ 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=245760
 CONFIG_RAM_START=0x20010000
--- a/boards/diatone/mamba-f405-mk2/nuttx-config/nsh/defconfig
+++ b/boards/diatone/mamba-f405-mk2/nuttx-config/nsh/defconfig
@ -62,7 +62,7 @@ CONFIG_I2C=y
 CONFIG_I2C_RESET=y
 CONFIG_IDLETHREAD_STACKSIZE=750
 CONFIG_INIT_ENTRYPOINT="nsh_main"
-CONFIG_INIT_STACKSIZE=3094
+CONFIG_INIT_STACKSIZE=3194
 CONFIG_LIBC_MAX_EXITFUNS=1
 CONFIG_LIBC_STRERROR=y
 CONFIG_MEMSET_64BIT=y
--- a/boards/flywoo/gn-f405/nuttx-config/nsh/defconfig
+++ b/boards/flywoo/gn-f405/nuttx-config/nsh/defconfig
@ -109,7 +109,7 @@ CONFIG_I2C=y
 CONFIG_I2C_RESET=y
 CONFIG_IDLETHREAD_STACKSIZE=750
 CONFIG_INIT_ENTRYPOINT="nsh_main"
-CONFIG_INIT_STACKSIZE=3094
+CONFIG_INIT_STACKSIZE=3194
 CONFIG_LIBC_LONG_LONG=y
 CONFIG_LIBC_MAX_EXITFUNS=1
 CONFIG_LIBC_STRERROR=y
--- a/boards/freefly/can-rtk-gps/nuttx-config/nsh/defconfig
+++ b/boards/freefly/can-rtk-gps/nuttx-config/nsh/defconfig
@ -132,6 +132,8 @@ 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=245760
 CONFIG_RAM_START=0x20010000
--- a/boards/freefly/can-rtk-gps/uavcan_board_identity
+++ b/boards/freefly/can-rtk-gps/uavcan_board_identity
@ -1,6 +1,6 @@
 # UAVCAN boot loadable Module ID
-set(uavcanblid_sw_version_major 0)
-set(uavcanblid_sw_version_minor 1)
+set(uavcanblid_sw_version_major ${PX4_VERSION_MAJOR})
+set(uavcanblid_sw_version_minor ${PX4_VERSION_MINOR})
 add_definitions(
 	-DAPP_VERSION_MAJOR=${uavcanblid_sw_version_major}
 	-DAPP_VERSION_MINOR=${uavcanblid_sw_version_minor}
--- a/boards/holybro/can-gps-v1/nuttx-config/nsh/defconfig
+++ b/boards/holybro/can-gps-v1/nuttx-config/nsh/defconfig
@ -120,6 +120,8 @@ 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
--- a/boards/holybro/can-gps-v1/uavcan_board_identity
+++ b/boards/holybro/can-gps-v1/uavcan_board_identity
@ -1,6 +1,6 @@
 # UAVCAN boot loadable Module ID
-set(uavcanblid_sw_version_major 0)
-set(uavcanblid_sw_version_minor 1)
+set(uavcanblid_sw_version_major ${PX4_VERSION_MAJOR})
+set(uavcanblid_sw_version_minor ${PX4_VERSION_MINOR})
 add_definitions(
 	-DAPP_VERSION_MAJOR=${uavcanblid_sw_version_major}
 	-DAPP_VERSION_MINOR=${uavcanblid_sw_version_minor}
--- a/boards/holybro/durandal-v1/nuttx-config/bootloader/defconfig
+++ b/boards/holybro/durandal-v1/nuttx-config/bootloader/defconfig
@ -48,7 +48,7 @@ CONFIG_HAVE_CXX=y
 CONFIG_HAVE_CXXINITIALIZE=y
 CONFIG_IDLETHREAD_STACKSIZE=750
 CONFIG_INIT_ENTRYPOINT="bootloader_main"
-CONFIG_INIT_STACKSIZE=3094
+CONFIG_INIT_STACKSIZE=3194
 CONFIG_LIBC_FLOATINGPOINT=y
 CONFIG_LIBC_LONG_LONG=y
 CONFIG_LIBC_STRERROR=y
--- a/boards/holybro/durandal-v1/nuttx-config/nsh/defconfig
+++ b/boards/holybro/durandal-v1/nuttx-config/nsh/defconfig
@ -113,7 +113,7 @@ CONFIG_I2C=y
 CONFIG_I2C_RESET=y
 CONFIG_IDLETHREAD_STACKSIZE=750
 CONFIG_INIT_ENTRYPOINT="nsh_main"
-CONFIG_INIT_STACKSIZE=3094
+CONFIG_INIT_STACKSIZE=3194
 CONFIG_LIBC_FLOATINGPOINT=y
 CONFIG_LIBC_LONG_LONG=y
 CONFIG_LIBC_MAX_EXITFUNS=1
@ -149,6 +149,8 @@ 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=245760
 CONFIG_RAM_START=0x20010000
--- a/boards/holybro/kakutef7/default.px4board
+++ b/boards/holybro/kakutef7/default.px4board
@ -17,11 +17,19 @@ CONFIG_DRIVERS_OSD_ATXXXX=y
 CONFIG_DRIVERS_PWM_OUT=y
 CONFIG_DRIVERS_RC_INPUT=y
 CONFIG_DRIVERS_TELEMETRY_FRSKY_TELEMETRY=y
-CONFIG_MODULES_ATTITUDE_ESTIMATOR_Q=y
 CONFIG_MODULES_BATTERY_STATUS=y
 CONFIG_MODULES_COMMANDER=y
 CONFIG_MODULES_CONTROL_ALLOCATOR=y
 CONFIG_MODULES_DATAMAN=y
+CONFIG_MODULES_EKF2=y
+# CONFIG_EKF2_AUXVEL is not set
+# CONFIG_EKF2_BARO_COMPENSATION is not set
+# CONFIG_EKF2_DRAG_FUSION is not set
+# CONFIG_EKF2_EXTERNAL_VISION is not set
+# CONFIG_EKF2_GNSS_YAW is not set
+# CONFIG_EKF2_MAGNETOMETER is not set
+# CONFIG_EKF2_RANGE_FINDER is not set
+# CONFIG_EKF2_SIDESLIP is not set
 CONFIG_MODULES_FLIGHT_MODE_MANAGER=y
 CONFIG_MODULES_LAND_DETECTOR=y
 CONFIG_MODULES_LOGGER=y
@ -33,6 +41,9 @@ CONFIG_MODULES_MC_RATE_CONTROL=y
 CONFIG_MODULES_NAVIGATOR=y
 CONFIG_MODULES_RC_UPDATE=y
 CONFIG_MODULES_SENSORS=y
+# CONFIG_SENSORS_VEHICLE_AIRSPEED is not set
+# CONFIG_SENSORS_VEHICLE_MAGNETOMETER is not set
+# CONFIG_SENSORS_VEHICLE_OPTICAL_FLOW is not set
 CONFIG_SYSTEMCMDS_DMESG=y
 CONFIG_SYSTEMCMDS_PARAM=y
 CONFIG_SYSTEMCMDS_TOP=y
--- a/boards/holybro/kakutef7/init/rc.board_defaults
+++ b/boards/holybro/kakutef7/init/rc.board_defaults
@ -12,13 +12,11 @@ param set-default CBRK_SUPPLY_CHK 894281
 # Select the Generic 250 Racer by default
 param set-default SYS_AUTOSTART 4050

-# use the Q attitude estimator, it works w/o mag or GPS.
-param set-default SYS_MC_EST_GROUP 3
-param set-default ATT_W_ACC 0.4
-param set-default ATT_W_GYRO_BIAS 0
-
 param set-default SYS_HAS_MAG 0

+# enable gravity fusion
+param set-default EKF2_IMU_CONTROL 7
+
 # the startup tune is not great on a binary output buzzer, so disable it
 param set-default CBRK_BUZZER 782090

--- a/boards/holybro/kakutef7/nuttx-config/nsh/defconfig
+++ b/boards/holybro/kakutef7/nuttx-config/nsh/defconfig
@ -115,7 +115,7 @@ CONFIG_I2C=y
 CONFIG_I2C_RESET=y
 CONFIG_IDLETHREAD_STACKSIZE=750
 CONFIG_INIT_ENTRYPOINT="nsh_main"
-CONFIG_INIT_STACKSIZE=3094
+CONFIG_INIT_STACKSIZE=3194
 CONFIG_LIBC_FLOATINGPOINT=y
 CONFIG_LIBC_LONG_LONG=y
 CONFIG_LIBC_MAX_EXITFUNS=1
@ -150,6 +150,8 @@ 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=245760
 CONFIG_RAM_START=0x20010000
--- a/boards/holybro/kakuteh7/default.px4board
+++ b/boards/holybro/kakuteh7/default.px4board
@ -33,6 +33,12 @@ CONFIG_MODULES_COMMANDER=y
 CONFIG_MODULES_CONTROL_ALLOCATOR=y
 CONFIG_MODULES_DATAMAN=y
 CONFIG_MODULES_EKF2=y
+# CONFIG_EKF2_AUXVEL is not set
+# CONFIG_EKF2_BARO_COMPENSATION is not set
+# CONFIG_EKF2_DRAG_FUSION is not set
+# CONFIG_EKF2_EXTERNAL_VISION is not set
+# CONFIG_EKF2_GNSS_YAW is not set
+# CONFIG_EKF2_SIDESLIP is not set
 CONFIG_MODULES_ESC_BATTERY=y
 CONFIG_MODULES_EVENTS=y
 CONFIG_MODULES_FLIGHT_MODE_MANAGER=y
--- a/Show More
+++ b/Show More