Log camera topics

Eliminate intermediate variable
Add fw state for path following
2026-05-25 05:27:34 +08:00 · 2023-03-30 19:05:50 +02:00 · 2023-03-28 18:48:26 +02:00 · 2023-03-28 17:13:07 +02:00 · 2023-03-27 10:28:43 -07:00 · 2023-03-27 10:28:43 -07:00
398 changed files with 15430 additions and 10318 deletions
@@ -105,3 +105,6 @@ src/modules/simulator/simulator_config.h
 src/systemcmds/topic_listener/listener_generated.cpp

 !src/drivers/distance_sensor/broadcom/afbrs50/Lib/*
+
+# colcon
+log/
@@ -170,7 +170,7 @@
            ]
        },
        {
-            "label": "ign gazebo",
+            "label": "gazebo",
            "type": "shell",
            "options": {
                "cwd": "${workspaceFolder}",
@@ -178,7 +178,7 @@
                    "IGN_GAZEBO_RESOURCE_PATH": "${workspaceFolder}/Tools/simulation/gz/models",
                }
            },
-            "command": "ign gazebo -v 4 -r ${workspaceFolder}/Tools/simulation/gz/worlds/${input:gzWorld}.sdf",
+            "command": "gz sim -v 4 -r ${workspaceFolder}/Tools/simulation/gz/worlds/${input:gzWorld}.sdf",
            "isBackground": true,
            "presentation": {
                "echo": true,
@@ -191,7 +191,7 @@
                "close": false
            },
            "problemMatcher": [],
-            "dependsOn":["ign gazebo kill"]
+            "dependsOn":["gazebo kill"]
        },
        {
            "label": "gazebo-classic kill",
@@ -211,9 +211,9 @@
            "dependsOn":["px4_sitl_cleanup"]
        },
        {
-            "label": "ign gazebo kill",
+            "label": "gazebo kill",
            "type": "shell",
-            "command": "pkill -9 -f 'ign gazebo' || true",
+            "command": "pkill -9 -f 'gz sim' || true",
            "presentation": {
                "echo": true,
                "reveal": "never",
@@ -104,8 +104,9 @@ These boards fully comply with Pixhawk Standard, and are maintained by the PX4-A
 These boards are maintained to be compatible with PX4-Autopilot by the Manufacturers.

 * [ARK Electronics ARKV6X](https://docs.px4.io/main/en/flight_controller/arkv6x.html)
-* [Hex Cube Orange](https://docs.px4.io/main/en/flight_controller/cubepilot_cube_orange.html)
-* [Hex Cube Yellow](https://docs.px4.io/main/en/flight_controller/cubepilot_cube_yellow.html)
+* [CubePilot Cube Orange+](https://docs.px4.io/main/en/flight_controller/cubepilot_cube_orangeplus.html)
+* [CubePilot Cube Orange](https://docs.px4.io/main/en/flight_controller/cubepilot_cube_orange.html)
+* [CubePilot Cube Yellow](https://docs.px4.io/main/en/flight_controller/cubepilot_cube_yellow.html)
 * [Holybro Durandal](https://docs.px4.io/main/en/flight_controller/durandal.html)
 * [Airmind MindPX V2.8](http://www.mindpx.net/assets/accessories/UserGuide_MindPX.pdf)
 * [Airmind MindRacer V1.2](http://mindpx.net/assets/accessories/mindracer_user_guide_v1.2.pdf)
@@ -137,11 +137,6 @@ add_custom_command(
 	COMMENT "ROMFS: copying, generating airframes"
 	)

-if(EXISTS ${PX4_BOARD_DIR}/extras/${PX4_BOARD_VENDOR}_${PX4_BOARD_MODEL}_bootloader.bin)
-	set(BOARD_FIRMWARE_BIN "${PX4_BOARD_VENDOR}_${PX4_BOARD_MODEL}_bootloader.bin")
-	configure_file(${PX4_SOURCE_DIR}/platforms/nuttx/init/rc.board_bootloader_upgrade.in ${romfs_gen_root_dir}/init.d/rc.board_bootloader_upgrade @ONLY)
-endif()
-
 # copy extras into ROMFS
 set(extras_dependencies)

@@ -208,6 +203,24 @@ endforeach()
 set(OPTIONAL_BOARD_EXTRAS)
 file(GLOB OPTIONAL_BOARD_EXTRAS ${PX4_BOARD_DIR}/extras/*)

+# bootloader (optional)
+#  - if systemcmds/bl_update included and board bootloader available then generate rc.board_bootloader_upgrade and copy bootloader binary
+#  - otherwise remove bootloader binary from extras in final ROMFS
+foreach(board_extra_file ${OPTIONAL_BOARD_EXTRAS})
+	file(RELATIVE_PATH extra_file_base_name ${PX4_BOARD_DIR}/extras/ ${board_extra_file})
+	if(${extra_file_base_name} MATCHES "${PX4_BOARD_VENDOR}_${PX4_BOARD_MODEL}_bootloader.bin")
+		if(CONFIG_SYSTEMCMDS_BL_UPDATE)
+			# generate rc.board_bootloader_upgrade
+			set(BOARD_FIRMWARE_BIN "${PX4_BOARD_VENDOR}_${PX4_BOARD_MODEL}_bootloader.bin")
+			configure_file(${PX4_SOURCE_DIR}/platforms/nuttx/init/rc.board_bootloader_upgrade.in ${romfs_gen_root_dir}/init.d/rc.board_bootloader_upgrade @ONLY)
+		else()
+			# remove bootloader from extras
+			list(REMOVE_ITEM OPTIONAL_BOARD_EXTRAS ${board_extra_file})
+		endif()
+
+	endif()
+endforeach()
+
 foreach(board_extra_file ${OPTIONAL_BOARD_EXTRAS})

 	if(EXISTS "${board_extra_file}")
@@ -11,6 +11,8 @@
 PX4_SIMULATOR=${PX4_SIMULATOR:=sihsim}
 PX4_SIM_MODEL=${PX4_SIM_MODEL:=xvert}

+param set-default EKF2_FUSE_BETA 0 # side slip fusion is currently not supported for tailsitters
+
 param set-default SENS_EN_GPSSIM 1
 param set-default SENS_EN_BAROSIM 1
 param set-default SENS_EN_MAGSIM 1
@@ -25,7 +25,6 @@ param set-default FW_W_EN 1

 param set-default MIS_TAKEOFF_ALT 20
 param set-default MIS_DIST_1WP 2500
-param set-default MIS_DIST_WPS 10000

 param set-default NAV_ACC_RAD 15
 param set-default NAV_DLL_ACT 2
@@ -25,7 +25,6 @@ param set-default FW_W_EN 1

 param set-default MIS_TAKEOFF_ALT 20
 param set-default MIS_DIST_1WP 2500
-param set-default MIS_DIST_WPS 10000

 param set-default NAV_ACC_RAD 15
 param set-default NAV_DLL_ACT 2
@@ -25,7 +25,6 @@ param set-default FW_W_EN 1

 param set-default MIS_TAKEOFF_ALT 20
 param set-default MIS_DIST_1WP 2500
-param set-default MIS_DIST_WPS 10000

 param set-default NAV_ACC_RAD 15
 param set-default NAV_DLL_ACT 2
@@ -27,7 +27,6 @@ param set-default FW_W_EN 1
 param set-default MIS_LTRMIN_ALT 30
 param set-default MIS_TAKEOFF_ALT 20
 param set-default MIS_DIST_1WP 2500
-param set-default MIS_DIST_WPS 10000

 param set-default NAV_ACC_RAD 15
 param set-default NAV_DLL_ACT 2
@@ -9,6 +9,8 @@

 param set-default MAV_TYPE 20

+param set-default EKF2_FUSE_BETA 0 # side slip fusion is currently not supported for tailsitters
+
 param set-default CA_AIRFRAME 4

 param set-default CA_ROTOR_COUNT 4
@@ -0,0 +1,12 @@
+#!/bin/sh
+#
+# @name Gazebo x500 vision
+#
+# @type Quadrotor
+#
+
+. ${R}etc/init.d-posix/airframes/4001_gz_x500
+
+PX4_SIMULATOR=${PX4_SIMULATOR:=gz}
+PX4_GZ_WORLD=${PX4_GZ_WORLD:=default}
+PX4_SIM_MODEL=${PX4_SIM_MODEL:=x500_vision}
@@ -1,8 +1,6 @@

 mavlink start -x -u 14558 -r 4000 -f -m onboard -o 14530 -p

-# shellcheck disable=SC2154
-mavlink stream -r 10 -s MOUNT_ORIENTATION -u $udp_gcs_port_local
 # shellcheck disable=SC2154
 mavlink stream -r 50 -s ATTITUDE_QUATERNION -u $udp_offboard_port_local
 mavlink stream -r 10 -s MOUNT_ORIENTATION -u $udp_offboard_port_local
@@ -74,6 +74,7 @@ px4_add_romfs_files(
 	4002_gz_x500_depth
 	4003_gz_rc_cessna
 	4004_gz_standard_vtol
+	4005_gz_x500_vision

 	6011_gazebo-classic_typhoon_h480
 	6011_gazebo-classic_typhoon_h480.post
@@ -8,6 +8,14 @@ if [ "$PX4_SIMULATOR" = "sihsim" ] || [ "$(param show -q SYS_AUTOSTART)" -eq "0"

 	echo "INFO  [init] SIH simulator"

+	if [ -n "${PX4_HOME_LAT}" ]; then
+		param set SIH_LOC_LAT0 ${PX4_HOME_LAT}
+	fi
+
+	if [ -n "${PX4_HOME_LON}" ]; then
+		param set SIH_LOC_LON0 ${PX4_HOME_LON}
+	fi
+
 	if simulator_sih start; then

 		if param compare -s SENS_EN_BAROSIM 1
@@ -277,7 +277,7 @@ then
 	# Override namespace if environment variable is defined
 	microdds_ns="-n $PX4_MICRODDS_NS"
 fi
-microdds_client start -t udp -p 8888 $microdds_ns
+microdds_client start -t udp -h 127.0.0.1 -p 8888 $microdds_ns

 if param greater -s MNT_MODE_IN -1
 then
@@ -17,6 +17,8 @@

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

+param set-default EKF2_FUSE_BETA 0 # side slip fusion is currently not supported for tailsitters
+
 param set UAVCAN_ENABLE 0
 param set-default VT_ELEV_MC_LOCK 0
 param set-default VT_MOT_COUNT 2
@@ -79,7 +79,6 @@ param set-default MC_YAWRATE_MAX 20
 param set-default MC_AIRMODE 1

 param set-default MIS_DIST_1WP 100
-param set-default MIS_DIST_WPS 100000
 param set-default MIS_TAKEOFF_ALT 15

 param set-default MPC_XY_P 0.8
@@ -122,16 +121,12 @@ param set-default VT_TRANS_MIN_TM 15
 param set-default VT_B_TRANS_DUR 8
 param set-default VT_FWD_THRUST_SC 4
 param set-default VT_F_TRANS_DUR 1
-param set-default VT_B_REV_OUT 0.5
 param set-default VT_B_TRANS_THR 0.7
 param set-default VT_TRANS_TIMEOUT 22
 param set-default VT_F_TRANS_RAMP 4

 param set-default COM_RC_OVERRIDE 0

-
-
-
 param set-default CA_AIRFRAME 2

 param set-default CA_ROTOR_COUNT 5
@@ -12,6 +12,8 @@

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

+param set-default EKF2_FUSE_BETA 0 # side slip fusion is currently not supported for tailsitters
+
 param set-default CA_AIRFRAME 4
 param set-default CA_ROTOR_COUNT 2
 param set-default CA_ROTOR0_KM -0.05
@@ -54,14 +54,6 @@ param set-default CBRK_AIRSPD_CHK 162128
 # Differential drive acts like ackermann steering with a maximum turn angle of 180 degrees, or pi radians
 param set-default GND_MAX_ANG 3.1415

-param set-default RBCLW_BAUD 8
-param set-default RBCLW_COUNTS_REV 1200
-param set-default RBCLW_ADDRESS 128
-# 104 corresponds to Telem 4
-param set-default RBCLW_SER_CFG 104
-# Start this driver after setting parameters, because the driver uses some of those parameters.
-# roboclaw start /dev/ttyS3
-
 # Set geometry & output configration
 param set-default CA_AIRFRAME 6
 param set-default CA_R_REV 3
@@ -46,7 +46,6 @@ param set-default RTL_LAND_DELAY -1
 #
 param set-default NAV_ACC_RAD 10

-param set-default MIS_DIST_WPS 5000
 param set-default MIS_TAKEOFF_ALT 25
 param set-default MIS_TKO_LAND_REQ 2

@@ -23,7 +23,6 @@ param set-default EKF2_FUSE_BETA 1

 param set-default HTE_VXY_THR 2.0

-param set-default MIS_DIST_WPS 5000
 param set-default MIS_TKO_LAND_REQ 2

 param set-default MPC_ACC_HOR_MAX 2
@@ -0,0 +1,51 @@
+#!/usr/bin/env python3
+
+"""
+Converts IP address given in decimal dot notation to int32 to be used in XRCE_DDS_0_CFG parameter
+and vice-versa
+
+@author: Beniamino Pozzan (beniamino.pozzan@phd.unipd.it)
+"""
+
+
+import argparse
+
+parser = argparse.ArgumentParser(
+    prog = 'convert_ip',
+    description = 'converts IP to int32 and viceversa'
+    )
+parser.add_argument('input',
+                    type=str,
+                    help='IP address to convert')
+parser.add_argument('-r','--reverse',
+                    action='store_true',
+                    help='converts from int32 to dot decimal')
+
+args = parser.parse_args()
+
+if( args.reverse == False ):
+
+    try:
+        ip_parts = [int(x) for x in args.input.split('.')]
+    except:
+        raise ValueError("Not a valid IP")
+    if( len(ip_parts)!=4 ):
+        raise ValueError("Not a valid IP")
+    if( not all(x>=0 and x<255 for x in ip_parts) ):
+        raise ValueError("Not a valid IP")
+
+    ip = (ip_parts[0]<<24) + (ip_parts[1]<<16) + (ip_parts[2]<<8) + ip_parts[3]
+
+    if(ip & 0x80000000):
+        ip = -0x100000000 + ip
+
+    print(ip)
+
+else:
+    try:
+        ip = int(args.input)
+    except:
+        raise ValueError("Not a valid IP")
+    if(ip < 0):
+        ip = ip + 0x100000000
+    print('{}.{}.{}.{}'.format(ip>>24, (ip>>16)&0xff, (ip>>8)&0xff, ip&0xff))
@@ -7,7 +7,7 @@ import os
 class JsonOutput():
    def __init__(self, groups):
        all_json = {}
-        all_json['version'] = 1
+        all_json['version'] = 2
        component = {}
        all_json['components'] = {1: component} #1: autopilot component

@@ -102,7 +102,7 @@ class ModuleDocumentation(object):
    def _handle_usage_param_int(self, args):
        assert(len(args) == 6) # option_char, default_val, min_val, max_val, description, is_optional
        option_char = self._get_option_char(args[0])
-        default_val = int(args[1], 0)
+        default_val = self._get_int(args[1])
        description = self._get_string(args[4])
        if self._is_bool_true(args[5]):
            self._usage_string += "     [-%s <val>]  %s\n" % (option_char, description)
@@ -214,6 +214,9 @@ class ModuleDocumentation(object):
            f = f[:-1]
        return float(f)

+    def _get_int(self, argument):
+        return int(eval(argument))
+
    def _is_string(self, argument):
        return len(argument) > 0 and argument[0] == '"'

@@ -307,6 +310,8 @@ class SourceParser(object):
            r'//.*?$|/\*.*?\*/|\'(?:\\.|[^\\\'])*\'|"(?:\\.|[^\\"])*"',
            re.DOTALL | re.MULTILINE)

+        self._define_pattern = re.compile(r'#define\s+(\w+?)[^\S\r\n]+(.+?)\s*?\n')
+
    def Parse(self, scope, contents):
        """
        Incrementally parse program contents and append all found documentations
@@ -316,6 +321,9 @@ class SourceParser(object):
        # remove comments from source
        contents = self._comment_remover(contents)

+        # replace preprocessor defines defined in file directly
+        contents = self._define_replacer(contents)
+
        extracted_function_calls = [] # list of tuples: (FUNC_NAME, list(ARGS))

        start_index = 0
@@ -379,6 +387,15 @@ class SourceParser(object):
                return s
        return re.sub(self._comment_remove_pattern, replacer, text)

+    def _define_replacer(self, text):
+        """ check for C preprocesor #define in text and replace with argument"""
+        text = re.sub(r"\\\s*?\n"," ",text)
+        define_iter = self._define_pattern.finditer(text)
+        for define_pattern in define_iter:
+            text = re.sub(r"\b" +re.escape(str(define_pattern.groups()[0])) + r"\b", re.escape(str(define_pattern.groups()[1])), text)
+        return text
+
+
    def _do_consistency_check(self, contents, scope, module_doc):
        """
        check the documentation for consistency with the code (arguments to
@@ -35,7 +35,20 @@ function spawn_model() {
 	pushd "$working_dir" &>/dev/null
 	echo "starting instance $N in $(pwd)"
 	$build_path/bin/px4 -i $N -d "$build_path/etc" >out.log 2>err.log &
-	python3 ${src_path}/Tools/simulation/gazebo-classic/sitl_gazebo-classic/scripts/jinja_gen.py ${src_path}/Tools/simulation/gazebo-classic/sitl_gazebo-classic/models/${MODEL}/${MODEL}.sdf.jinja ${src_path}/Tools/simulation/gazebo-classic/sitl_gazebo-classic --mavlink_tcp_port $((4560+${N})) --mavlink_udp_port $((14560+${N})) --mavlink_id $((1+${N})) --gst_udp_port $((5600+${N})) --video_uri $((5600+${N})) --mavlink_cam_udp_port $((14530+${N})) --output-file /tmp/${MODEL}_${N}.sdf
+
+	set --
+	set -- ${@} ${src_path}/Tools/simulation/gazebo-classic/sitl_gazebo-classic/scripts/jinja_gen.py
+	set -- ${@} ${src_path}/Tools/simulation/gazebo-classic/sitl_gazebo-classic/models/${MODEL}/${MODEL}.sdf.jinja
+	set -- ${@} ${src_path}/Tools/simulation/gazebo-classic/sitl_gazebo-classic
+	set -- ${@} --mavlink_tcp_port $((4560+${N}))
+	set -- ${@} --mavlink_udp_port $((14560+${N}))
+	set -- ${@} --mavlink_id $((1+${N}))
+	set -- ${@} --gst_udp_port $((5600+${N}))
+	set -- ${@} --video_uri $((5600+${N}))
+	set -- ${@} --mavlink_cam_udp_port $((14530+${N}))
+	set -- ${@} --output-file /tmp/${MODEL}_${N}.sdf
+
+	python3 ${@}

 	echo "Spawning ${MODEL}_${N} at ${X} ${Y}"

@@ -106,7 +119,7 @@ if [ -z ${SCRIPT} ]; then
 	fi

 	while [ $n -lt $num_vehicles ]; do
-		spawn_model ${vehicle_model} $n
+		spawn_model ${vehicle_model} $(($n + 1))
 		n=$(($n + 1))
 	done
 else
@@ -127,7 +140,7 @@ else
 		m=0
 		while [ $m -lt ${target_number} ]; do
 			export PX4_SIM_MODEL=gazebo-classic_${target_vehicle}
-			spawn_model ${target_vehicle}${LABEL} $n $target_x $target_y
+			spawn_model ${target_vehicle}${LABEL} $(($n + 1)) $target_x $target_y
 			m=$(($m + 1))
 			n=$(($n + 1))
 		done
@@ -209,7 +209,6 @@
          <spring_reference>0</spring_reference>
          <spring_stiffness>0</spring_stiffness>
        </dynamics>
-        <use_parent_model_frame>1</use_parent_model_frame>
      </axis>
    </joint>
    <link name="left_elevon">
@@ -638,7 +637,6 @@
          <spring_reference>0</spring_reference>
          <spring_stiffness>0</spring_stiffness>
        </dynamics>
-        <use_parent_model_frame>1</use_parent_model_frame>
      </axis>
    </joint>
    <joint name="RightWheelJoint" type="revolute">
@@ -655,7 +653,6 @@
          <spring_reference>0</spring_reference>
          <spring_stiffness>0</spring_stiffness>
        </dynamics>
-        <use_parent_model_frame>1</use_parent_model_frame>
      </axis>
    </joint>
    <joint name="CenterWheelJoint" type="revolute">
@@ -672,7 +669,6 @@
          <spring_reference>0</spring_reference>
          <spring_stiffness>0</spring_stiffness>
        </dynamics>
-        <use_parent_model_frame>1</use_parent_model_frame>
      </axis>
    </joint>
    <plugin filename="gz-sim-lift-drag-system" name="gz::sim::systems::LiftDrag">
@@ -807,7 +803,7 @@
      <sub_topic>servo_3</sub_topic>
      <p_gain>10.0</p_gain>
    </plugin>
-    <plugin filename="ignition-gazebo-multicopter-motor-model-system" name="gz::sim::systems::MulticopterMotorModel">
+    <plugin filename="gz-sim-multicopter-motor-model-system" name="gz::sim::systems::MulticopterMotorModel">
      <jointName>rotor_puller_joint</jointName>
      <linkName>rotor_puller</linkName>
      <turningDirection>cw</turningDirection>
@@ -208,7 +208,6 @@
          <spring_reference>0</spring_reference>
          <spring_stiffness>0</spring_stiffness>
        </dynamics>
-        <use_parent_model_frame>1</use_parent_model_frame>
      </axis>
    </joint>
    <link name='rotor_1'>
@@ -272,7 +271,6 @@
          <spring_reference>0</spring_reference>
          <spring_stiffness>0</spring_stiffness>
        </dynamics>
-        <use_parent_model_frame>1</use_parent_model_frame>
      </axis>
    </joint>
    <link name='rotor_2'>
@@ -336,7 +334,6 @@
          <spring_reference>0</spring_reference>
          <spring_stiffness>0</spring_stiffness>
        </dynamics>
-        <use_parent_model_frame>1</use_parent_model_frame>
      </axis>
    </joint>
    <link name='rotor_3'>
@@ -400,7 +397,6 @@
          <spring_reference>0</spring_reference>
          <spring_stiffness>0</spring_stiffness>
        </dynamics>
-        <use_parent_model_frame>1</use_parent_model_frame>
      </axis>
    </joint>

@@ -466,7 +462,6 @@
          <spring_reference>0</spring_reference>
          <spring_stiffness>0</spring_stiffness>
        </dynamics>
-        <use_parent_model_frame>1</use_parent_model_frame>
      </axis>
    </joint>

@@ -302,7 +302,6 @@
          <spring_reference>0</spring_reference>
          <spring_stiffness>0</spring_stiffness>
        </dynamics>
-        <use_parent_model_frame>1</use_parent_model_frame>
      </axis>
    </joint>
    <link name="rotor_1">
@@ -375,7 +374,6 @@
          <spring_reference>0</spring_reference>
          <spring_stiffness>0</spring_stiffness>
        </dynamics>
-        <use_parent_model_frame>1</use_parent_model_frame>
      </axis>
    </joint>
    <link name="rotor_2">
@@ -448,7 +446,6 @@
          <spring_reference>0</spring_reference>
          <spring_stiffness>0</spring_stiffness>
        </dynamics>
-        <use_parent_model_frame>1</use_parent_model_frame>
      </axis>
    </joint>
    <link name="rotor_3">
@@ -521,10 +518,9 @@
          <spring_reference>0</spring_reference>
          <spring_stiffness>0</spring_stiffness>
        </dynamics>
-        <use_parent_model_frame>1</use_parent_model_frame>
      </axis>
    </joint>
-   <plugin filename="ignition-gazebo-multicopter-motor-model-system" name="ignition::gazebo::systems::MulticopterMotorModel">
+   <plugin filename="gz-sim-multicopter-motor-model-system" name="gz::sim::systems::MulticopterMotorModel">
      <jointName>rotor_0_joint</jointName>
      <linkName>rotor_0</linkName>
      <turningDirection>ccw</turningDirection>
@@ -0,0 +1,11 @@
+<?xml version="1.0"?>
+<model>
+  <name>x500-vision</name>
+  <version>1.0</version>
+  <sdf version="1.9">model.sdf</sdf>
+  <author>
+    <name>Jaeyoung Lim</name>
+    <email>jalim@ethz.ch</email>
+  </author>
+  <description>Model of the X500 with a odometry/external vision input.</description>
+</model>
@@ -0,0 +1,13 @@
+<?xml version="1.0" encoding="UTF-8"?>
+<sdf version='1.9'>
+  <model name='x500-vision'>
+    <include merge='true'>
+      <uri>x500</uri>
+    </include>
+      <plugin
+        filename="gz-sim-odometry-publisher-system"
+        name="gz::sim::systems::OdometryPublisher">
+        <dimensions>3</dimensions>
+      </plugin>
+  </model>
+</sdf>
@@ -5,22 +5,22 @@
      <real_time_factor>1.0</real_time_factor>
      <real_time_update_rate>250</real_time_update_rate>
    </physics>
-    <plugin name='ignition::gazebo::systems::Physics' filename='ignition-gazebo-physics-system'/>
-    <plugin name='ignition::gazebo::systems::UserCommands' filename='ignition-gazebo-user-commands-system'/>
-    <plugin name='ignition::gazebo::systems::SceneBroadcaster' filename='ignition-gazebo-scene-broadcaster-system'/>
-    <plugin name='ignition::gazebo::systems::Contact' filename='ignition-gazebo-contact-system'/>
-    <plugin name='ignition::gazebo::systems::Imu' filename='ignition-gazebo-imu-system'/>
-    <plugin name='ignition::gazebo::systems::AirPressure' filename='ignition-gazebo-air-pressure-system'/>
-    <plugin name='ignition::gazebo::systems::Sensors' filename='ignition-gazebo-sensors-system'>
+    <plugin name='gz::sim::systems::Physics' filename='gz-sim-physics-system'/>
+    <plugin name='gz::sim::systems::UserCommands' filename='gz-sim-user-commands-system'/>
+    <plugin name='gz::sim::systems::SceneBroadcaster' filename='gz-sim-scene-broadcaster-system'/>
+    <plugin name='gz::sim::systems::Contact' filename='gz-sim-contact-system'/>
+    <plugin name='gz::sim::systems::Imu' filename='gz-sim-imu-system'/>
+    <plugin name='gz::sim::systems::AirPressure' filename='gz-sim-air-pressure-system'/>
+    <plugin name='gz::sim::systems::Sensors' filename='gz-sim-sensors-system'>
      <render_engine>ogre2</render_engine>
    </plugin>
    <gui fullscreen='false'>
      <plugin name='3D View' filename='GzScene3D'>
-        <ignition-gui>
+        <gz-gui>
          <title>3D View</title>
          <property type='bool' key='showTitleBar'>0</property>
          <property type='string' key='state'>docked</property>
-        </ignition-gui>
+        </gz-gui>
        <engine>ogre2</engine>
        <scene>scene</scene>
        <ambient_light>0.5984631152222222 0.5984631152222222 0.5984631152222222</ambient_light>
@@ -28,7 +28,7 @@
        <camera_pose>-6 0 6 0 0.5 0</camera_pose>
      </plugin>
      <plugin name='World control' filename='WorldControl'>
-        <ignition-gui>
+        <gz-gui>
          <title>World control</title>
          <property type='bool' key='showTitleBar'>0</property>
          <property type='bool' key='resizable'>0</property>
@@ -40,13 +40,13 @@
            <line own='left' target='left'/>
            <line own='bottom' target='bottom'/>
          </anchors>
-        </ignition-gui>
+        </gz-gui>
        <play_pause>1</play_pause>
        <step>1</step>
        <start_paused>1</start_paused>
      </plugin>
      <plugin name='World stats' filename='WorldStats'>
-        <ignition-gui>
+        <gz-gui>
          <title>World stats</title>
          <property type='bool' key='showTitleBar'>0</property>
          <property type='bool' key='resizable'>0</property>
@@ -58,7 +58,7 @@
            <line own='right' target='right'/>
            <line own='bottom' target='bottom'/>
          </anchors>
-        </ignition-gui>
+        </gz-gui>
        <sim_time>1</sim_time>
        <real_time>1</real_time>
        <real_time_factor>1</real_time_factor>
@@ -110,10 +110,6 @@ class PubSub(object):

            log.debug("          ####:{}:  {}, {}".format( self._name, route_group, topic_group))

-            # # TODO: handle this case... but not sure where, yet
-            # if match == 'ORB_ID_VEHICLE_ATTITUDE_CONTROLS': # special case
-            #     match = orb_id+orb_id_vehicle_attitude_controls_topic
-
            # match has the form: '[ORB_ID(]<topic_name>'
            if route_group:
                if route_group == 'ORB_ID':
@@ -232,9 +228,6 @@ class Graph(object):

        self._topic_blacklist = set(kwargs.get('topic_blacklist',set()))

-        self._orb_id_vehicle_attitude_controls_topic = 'actuator_controls_0'
-        self._orb_id_vehicle_attitude_controls_re = re.compile(r'\#define\s+ORB_ID_VEHICLE_ATTITUDE_CONTROLS\s+([^,)]+)')
-
        self._warnings = [] # list of all ambiguous scan sites

        self._current_scope = [] # stack with current module (they can be nested)
@@ -510,18 +503,7 @@ class Graph(object):
                elif current_scope.name == 'uorb_tests': # skip this
                    return
                elif current_scope.name == 'uorb':
-
-                    # search and validate the ORB_ID_VEHICLE_ATTITUDE_CONTROLS define
-                    matches = self._orb_id_vehicle_attitude_controls_re.findall(content)
-                    for match in matches:
-                        if match != 'ORB_ID('+self._orb_id_vehicle_attitude_controls_topic:
-                            # if we land here, you need to change _orb_id_vehicle_attitude_controls_topic
-                            raise Exception(
-                                'The extracted define for ORB_ID_VEHICLE_ATTITUDE_CONTROLS '
-                                'is '+match+' but expected ORB_ID('+
-                                self._orb_id_vehicle_attitude_controls_topic)
-
-                    return # skip uorb module for the rest
+                    return # skip this

            line_number = 0
            for full_line in content.splitlines():
@@ -27,7 +27,6 @@ CONFIG_DRIVERS_POWER_MONITOR_INA226=y
 CONFIG_DRIVERS_PWM_INPUT=y
 CONFIG_DRIVERS_PWM_OUT=y
 CONFIG_DRIVERS_RC_INPUT=y
-CONFIG_DRIVERS_ROBOCLAW=y
 CONFIG_DRIVERS_SMART_BATTERY_BATMON=y
 CONFIG_COMMON_TELEMETRY=y
 CONFIG_DRIVERS_TONE_ALARM=y
@@ -13,6 +13,8 @@ CONFIG_DRIVERS_OPTICAL_FLOW_PAA3905=y
 CONFIG_DRIVERS_OPTICAL_FLOW_PAW3902=y
 CONFIG_BOARD_UAVCAN_INTERFACES=1
 CONFIG_DRIVERS_UAVCANNODE=y
+CONFIG_UAVCANNODE_FLOW_MEASUREMENT=y
+CONFIG_UAVCANNODE_RANGE_SENSOR_MEASUREMENT=y
 CONFIG_MODULES_GYRO_CALIBRATION=y
 CONFIG_MODULES_SENSORS=y
 # CONFIG_SENSORS_VEHICLE_AIRSPEED is not set
@@ -13,6 +13,13 @@ CONFIG_DRIVERS_SAFETY_BUTTON=y
 CONFIG_DRIVERS_TONE_ALARM=y
 CONFIG_BOARD_UAVCAN_INTERFACES=1
 CONFIG_DRIVERS_UAVCANNODE=y
+CONFIG_UAVCANNODE_BEEP_COMMAND=y
+CONFIG_UAVCANNODE_GNSS_FIX=y
+CONFIG_UAVCANNODE_LIGHTS_COMMAND=y
+CONFIG_UAVCANNODE_MAGNETIC_FIELD_STRENGTH=y
+CONFIG_UAVCANNODE_SAFETY_BUTTON=y
+CONFIG_UAVCANNODE_STATIC_PRESSURE=y
+CONFIG_UAVCANNODE_STATIC_TEMPERATURE=y
 CONFIG_MODULES_GYRO_CALIBRATION=y
 CONFIG_MODULES_MAG_BIAS_ESTIMATOR=y
 CONFIG_MODULES_SENSORS=y
@@ -13,6 +13,14 @@ CONFIG_DRIVERS_SAFETY_BUTTON=y
 CONFIG_DRIVERS_TONE_ALARM=y
 CONFIG_BOARD_UAVCAN_INTERFACES=1
 CONFIG_DRIVERS_UAVCANNODE=y
+CONFIG_UAVCANNODE_BEEP_COMMAND=y
+CONFIG_UAVCANNODE_GNSS_FIX=y
+CONFIG_UAVCANNODE_LIGHTS_COMMAND=y
+CONFIG_UAVCANNODE_MAGNETIC_FIELD_STRENGTH=y
+CONFIG_UAVCANNODE_RTK_DATA=y
+CONFIG_UAVCANNODE_SAFETY_BUTTON=y
+CONFIG_UAVCANNODE_STATIC_PRESSURE=y
+CONFIG_UAVCANNODE_STATIC_TEMPERATURE=y
 CONFIG_MODULES_GYRO_CALIBRATION=y
 CONFIG_MODULES_MAG_BIAS_ESTIMATOR=y
 CONFIG_MODULES_SENSORS=y
@@ -9,6 +9,7 @@ CONFIG_DRIVERS_BATT_SMBUS=y
 CONFIG_DRIVERS_BOOTLOADERS=y
 CONFIG_COMMON_DIFFERENTIAL_PRESSURE=y
 CONFIG_COMMON_DISTANCE_SENSOR=y
+CONFIG_DRIVERS_DSHOT=y
 CONFIG_DRIVERS_GPS=y
 CONFIG_COMMON_HYGROMETERS=y
 CONFIG_DRIVERS_IMU_ANALOG_DEVICES_ADIS16448=y
@@ -17,10 +18,27 @@ CONFIG_DRIVERS_IMU_INVENSENSE_ICM42688P=y
 CONFIG_DRIVERS_IRLOCK=y
 CONFIG_COMMON_LIGHT=y
 CONFIG_COMMON_MAGNETOMETER=y
+CONFIG_COMMON_OPTICAL_FLOW=y
 CONFIG_DRIVERS_PWM_OUT=y
 CONFIG_BOARD_UAVCAN_INTERFACES=1
 CONFIG_DRIVERS_UAVCANNODE=y
+CONFIG_UAVCANNODE_ARMING_STATUS=y
+CONFIG_UAVCANNODE_BEEP_COMMAND=y
+CONFIG_UAVCANNODE_ESC_RAW_COMMAND=y
+CONFIG_UAVCANNODE_ESC_STATUS=y
+CONFIG_UAVCANNODE_FLOW_MEASUREMENT=y
+CONFIG_UAVCANNODE_GNSS_FIX=y
+CONFIG_UAVCANNODE_HYGROMETER_MEASUREMENT=y
+CONFIG_UAVCANNODE_LIGHTS_COMMAND=y
+CONFIG_UAVCANNODE_MAGNETIC_FIELD_STRENGTH=y
+CONFIG_UAVCANNODE_RANGE_SENSOR_MEASUREMENT=y
+CONFIG_UAVCANNODE_RAW_AIR_DATA=y
+CONFIG_UAVCANNODE_RTK_DATA=y
+CONFIG_UAVCANNODE_SERVO_ARRAY_COMMAND=y
+CONFIG_UAVCANNODE_STATIC_PRESSURE=y
+CONFIG_UAVCANNODE_STATIC_TEMPERATURE=y
 CONFIG_MODULES_CONTROL_ALLOCATOR=y
+CONFIG_MODULES_SENSORS=y
 CONFIG_SYSTEMCMDS_ACTUATOR_TEST=y
 CONFIG_SYSTEMCMDS_I2CDETECT=y
 CONFIG_SYSTEMCMDS_LED_CONTROL=y
@@ -2,6 +2,7 @@
 #
 # board specific defaults
 #------------------------------------------------------------------------------
+
 pwm_out start

-control_allocator start
+dshot start
@@ -67,7 +67,6 @@ CONFIG_MODULES_ROVER_POS_CONTROL=y
 CONFIG_MODULES_SENSORS=y
 CONFIG_MODULES_VTOL_ATT_CONTROL=y
 CONFIG_SYSTEMCMDS_ACTUATOR_TEST=y
-CONFIG_SYSTEMCMDS_BL_UPDATE=y
 CONFIG_SYSTEMCMDS_BSONDUMP=y
 CONFIG_SYSTEMCMDS_DMESG=y
 CONFIG_SYSTEMCMDS_HARDFAULT_LOG=y
@@ -26,7 +26,6 @@ CONFIG_DRIVERS_PCA9685_PWM_OUT=y
 CONFIG_DRIVERS_POWER_MONITOR_INA226=y
 CONFIG_DRIVERS_PWM_OUT=y
 CONFIG_DRIVERS_RC_INPUT=y
-CONFIG_DRIVERS_ROBOCLAW=y
 CONFIG_DRIVERS_SMART_BATTERY_BATMON=y
 CONFIG_COMMON_TELEMETRY=y
 CONFIG_DRIVERS_UAVCAN=y
@@ -15,6 +15,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_SIDESLIP is not set
 CONFIG_MODULES_EVENTS=y
 CONFIG_MODULES_FLIGHT_MODE_MANAGER=y
 CONFIG_MODULES_LAND_DETECTOR=y
@@ -14,6 +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_SIDESLIP is not set
 CONFIG_MODULES_EVENTS=y
 CONFIG_MODULES_FLIGHT_MODE_MANAGER=y
 CONFIG_MODULES_LAND_DETECTOR=y
@@ -14,6 +14,13 @@ CONFIG_DRIVERS_SAFETY_BUTTON=y
 CONFIG_DRIVERS_TONE_ALARM=y
 CONFIG_BOARD_UAVCAN_INTERFACES=1
 CONFIG_DRIVERS_UAVCANNODE=y
+CONFIG_UAVCANNODE_BEEP_COMMAND=y
+CONFIG_UAVCANNODE_GNSS_FIX=y
+CONFIG_UAVCANNODE_LIGHTS_COMMAND=y
+CONFIG_UAVCANNODE_MAGNETIC_FIELD_STRENGTH=y
+CONFIG_UAVCANNODE_SAFETY_BUTTON=y
+CONFIG_UAVCANNODE_STATIC_PRESSURE=y
+CONFIG_UAVCANNODE_STATIC_TEMPERATURE=y
 CONFIG_MODULES_SENSORS=y
 # CONFIG_SENSORS_VEHICLE_AIRSPEED is not set
 # CONFIG_SENSORS_VEHICLE_AIR_DATA is not set
@@ -30,7 +30,6 @@ CONFIG_DRIVERS_PCA9685_PWM_OUT=y
 CONFIG_DRIVERS_POWER_MONITOR_INA226=y
 CONFIG_DRIVERS_PWM_OUT=y
 CONFIG_DRIVERS_RC_INPUT=y
-CONFIG_DRIVERS_ROBOCLAW=y
 CONFIG_DRIVERS_SAFETY_BUTTON=y
 CONFIG_DRIVERS_SMART_BATTERY_BATMON=y
 CONFIG_COMMON_TELEMETRY=y
@@ -76,7 +75,6 @@ CONFIG_MODULES_SIMULATION_SIMULATOR_SIH=y
 CONFIG_MODULES_TEMPERATURE_COMPENSATION=y
 CONFIG_MODULES_VTOL_ATT_CONTROL=y
 CONFIG_SYSTEMCMDS_ACTUATOR_TEST=y
-CONFIG_SYSTEMCMDS_BL_UPDATE=y
 CONFIG_SYSTEMCMDS_BSONDUMP=y
 CONFIG_SYSTEMCMDS_DMESG=y
 CONFIG_SYSTEMCMDS_DUMPFILE=y
@@ -31,7 +31,6 @@ CONFIG_DRIVERS_PCA9685_PWM_OUT=y
 CONFIG_DRIVERS_POWER_MONITOR_INA226=y
 CONFIG_DRIVERS_PWM_OUT=y
 CONFIG_DRIVERS_RC_INPUT=y
-CONFIG_DRIVERS_ROBOCLAW=y
 CONFIG_DRIVERS_SAFETY_BUTTON=y
 CONFIG_DRIVERS_SMART_BATTERY_BATMON=y
 CONFIG_COMMON_TELEMETRY=y
@@ -77,7 +76,6 @@ CONFIG_MODULES_SIMULATION_SIMULATOR_SIH=y
 CONFIG_MODULES_TEMPERATURE_COMPENSATION=y
 CONFIG_MODULES_VTOL_ATT_CONTROL=y
 CONFIG_SYSTEMCMDS_ACTUATOR_TEST=y
-CONFIG_SYSTEMCMDS_BL_UPDATE=y
 CONFIG_SYSTEMCMDS_BSONDUMP=y
 CONFIG_SYSTEMCMDS_DMESG=y
 CONFIG_SYSTEMCMDS_DUMPFILE=y
@@ -9,4 +9,5 @@ CONFIG_MODULES_LOCAL_POSITION_ESTIMATOR=n
 CONFIG_MODULES_ROVER_POS_CONTROL=n
 CONFIG_BOARD_TESTING=y
 CONFIG_DRIVERS_TEST_PPM=y
+CONFIG_SYSTEMCMDS_BL_UPDATE=y
 CONFIG_SYSTEMCMDS_MICROBENCH=y
@@ -69,7 +69,6 @@ CONFIG_MODULES_SENSORS=y
 CONFIG_MODULES_TEMPERATURE_COMPENSATION=y
 CONFIG_MODULES_VTOL_ATT_CONTROL=y
 CONFIG_SYSTEMCMDS_ACTUATOR_TEST=y
-CONFIG_SYSTEMCMDS_BL_UPDATE=y
 CONFIG_SYSTEMCMDS_BSONDUMP=y
 CONFIG_SYSTEMCMDS_DMESG=y
 CONFIG_SYSTEMCMDS_GPIO=y
@@ -6,6 +6,7 @@ CONFIG_DRIVERS_UAVCAN=n
 CONFIG_MODULES_TEMPERATURE_COMPENSATION=n
 CONFIG_BOARD_TESTING=y
 CONFIG_DRIVERS_TEST_PPM=y
+CONFIG_SYSTEMCMDS_BL_UPDATE=y
 CONFIG_SYSTEMCMDS_MICROBENCH=y
 CONFIG_SYSTEMCMDS_SD_BENCH=y
 CONFIG_SYSTEMCMDS_SD_STRESS=y
@@ -19,6 +19,7 @@ CONFIG_DRIVERS_IMU_ANALOG_DEVICES_ADIS16448=y
 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_DRIVERS_IRLOCK=y
 CONFIG_COMMON_LIGHT=y
 CONFIG_COMMON_MAGNETOMETER=y
@@ -49,7 +50,6 @@ CONFIG_MODULES_FW_PATH_NAVIGATION=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
@@ -66,11 +66,9 @@ CONFIG_MODULES_MICRODDS_CLIENT=y
 CONFIG_MODULES_NAVIGATOR=y
 CONFIG_MODULES_RC_UPDATE=y
 CONFIG_MODULES_SENSORS=y
-CONFIG_MODULES_SIMULATION_SIMULATOR_SIH=y
 CONFIG_MODULES_TEMPERATURE_COMPENSATION=y
 CONFIG_MODULES_VTOL_ATT_CONTROL=y
 CONFIG_SYSTEMCMDS_ACTUATOR_TEST=y
-CONFIG_SYSTEMCMDS_BL_UPDATE=y
 CONFIG_SYSTEMCMDS_BSONDUMP=y
 CONFIG_SYSTEMCMDS_DMESG=y
 CONFIG_SYSTEMCMDS_GPIO=y
@@ -4,12 +4,31 @@
 #------------------------------------------------------------------------------
 board_adc start

-# SPI4
+# Variants
+# 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}
+
+# SPI4 is isolated, SPI1 is body-fixed
+
+# SPI4, isolated
 ms5611 -s -b 4 start
-icm42688p -s -b 4 -R 10 start
-icm20948 -s -b 4 -R 10 -M start

-# SPI1
+icm42688p -s -b 4 -R 10 start -c 15
+if ! icm20948 -s -b 4 -R 10 -M -q start
+then
+	icm42688p -s -b 4 -R 6 start -c 13
+fi
+
+# SPI1, body-fixed
+if icm45686 -s -b 1 -R 3 -q start
+then
+	# if we detected the ICM45686 we also ought to have an AK09918
+	ak09916 start -I -R 13
+else
+	icm20649 -s -b 1 start
+
+fi
+
 ms5611 -s -b 1 start
-icm20649 -s -b 1 start

@@ -187,6 +187,7 @@
 #define STM32_RCC_D1CCIPR_SDMMCSEL   RCC_D1CCIPR_SDMMC_PLL1

 #define STM32_RCC_D2CCIP2R_I2C123SRC RCC_D2CCIP2R_I2C123SEL_HSI  /* I2C123 clock source */
+#define STM32_RCC_D3CCIPR_I2C4SRC    RCC_D3CCIPR_I2C4SEL_HSI
 #define STM32_RCC_D2CCIP1R_SPI123SRC RCC_D2CCIP1R_SPI123SEL_PLL2 /* SPI123 clock source */
 #define STM32_RCC_D2CCIP1R_SPI45SRC  RCC_D2CCIP1R_SPI45SEL_PLL2  /* SPI45 clock source */
 #define STM32_RCC_D2CCIP2R_USBSRC    RCC_D2CCIP2R_USBSEL_PLL3    /* USB 1 and 2 clock source */
@@ -264,3 +265,6 @@

 #define GPIO_I2C2_SCL GPIO_I2C2_SCL_1       /* PB10 */
 #define GPIO_I2C2_SDA GPIO_I2C2_SDA_1       /* PB11 */
+
+#define GPIO_I2C4_SCL GPIO_I2C4_SCL_2        /* PF14 */
+#define GPIO_I2C4_SDA GPIO_I2C4_SDA_2        /* PF15 */
@@ -189,6 +189,7 @@ CONFIG_STM32H7_DMACAPABLE=y
 CONFIG_STM32H7_FLOWCONTROL_BROKEN=y
 CONFIG_STM32H7_I2C1=y
 CONFIG_STM32H7_I2C2=y
+CONFIG_STM32H7_I2C4=y
 CONFIG_STM32H7_I2C_DYNTIMEO=y
 CONFIG_STM32H7_I2C_DYNTIMEO_STARTSTOP=10
 CONFIG_STM32H7_OTGFS=y
@@ -189,6 +189,7 @@ CONFIG_STM32H7_DMACAPABLE=y
 CONFIG_STM32H7_FLOWCONTROL_BROKEN=y
 CONFIG_STM32H7_I2C1=y
 CONFIG_STM32H7_I2C2=y
+CONFIG_STM32H7_I2C4=y
 CONFIG_STM32H7_I2C_DYNTIMEO=y
 CONFIG_STM32H7_I2C_DYNTIMEO_STARTSTOP=10
 CONFIG_STM32H7_OTGFS=y
@@ -208,7 +209,6 @@ CONFIG_STM32H7_SPI2=y
 CONFIG_STM32H7_SPI4=y
 CONFIG_STM32H7_SPI4_DMA=y
 CONFIG_STM32H7_SPI4_DMA_BUFFER=1024
-CONFIG_STM32H7_SPI_DMA=y
 CONFIG_STM32H7_TIM1=y
 CONFIG_STM32H7_TIM3=y
 CONFIG_STM32H7_TIM4=y
@@ -44,6 +44,16 @@
 #include <stdint.h>
 #include <stm32_gpio.h>

+/**
+ * If NuttX is built without support for SMPS it can brick the hardware.
+ * Therefore, we make sure the NuttX headers are correct.
+ */
+#include "hardware/stm32h7x3xx_pwr.h"
+#if STM32_PWR_CR3_SMPSEXTHP != (1 << 3)
+#  error "No SMPS support in NuttX submodule");
+#endif
+
+
 /* PX4IO connection configuration */
 #define BOARD_USES_PX4IO_VERSION       2
 #define PX4IO_SERIAL_DEVICE            "/dev/ttyS3"
@@ -170,6 +180,8 @@
 		PX4_MAKE_GPIO_OUTPUT_CLEAR(GPIO_I2C1_SDA), \
 		PX4_MAKE_GPIO_OUTPUT_CLEAR(GPIO_I2C2_SCL), \
 		PX4_MAKE_GPIO_OUTPUT_CLEAR(GPIO_I2C2_SDA), \
+		PX4_MAKE_GPIO_OUTPUT_CLEAR(GPIO_I2C4_SCL), \
+		PX4_MAKE_GPIO_OUTPUT_CLEAR(GPIO_I2C4_SDA), \
 		PX4_GPIO_PIN_OFF(GPIO_SDMMC1_D0), \
 		PX4_GPIO_PIN_OFF(GPIO_SDMMC1_D1), \
 		PX4_GPIO_PIN_OFF(GPIO_SDMMC1_D2), \
@@ -36,4 +36,5 @@
 constexpr px4_i2c_bus_t px4_i2c_buses[I2C_BUS_MAX_BUS_ITEMS] = {
 	initI2CBusExternal(2),
 	initI2CBusExternal(1),
+	initI2CBusInternal(4),
 };
@@ -38,6 +38,7 @@
 constexpr px4_spi_bus_t px4_spi_buses[SPI_BUS_MAX_BUS_ITEMS] = {
 	initSPIBus(SPI::Bus::SPI1, {
 		initSPIDevice(DRV_IMU_DEVTYPE_ICM20649, SPI::CS{GPIO::PortC, GPIO::Pin2}, SPI::DRDY{GPIO::PortD, GPIO::Pin15}), // MPU_CS, MPU_DRDY
+		initSPIDevice(DRV_IMU_DEVTYPE_ICM45686, SPI::CS{GPIO::PortG, GPIO::Pin1}), // ICM45686_CS
 		initSPIDevice(DRV_BARO_DEVTYPE_MS5611,  SPI::CS{GPIO::PortD, GPIO::Pin7}), // BARO_CS
 	}),

@@ -48,6 +49,7 @@ 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_ICM42688P, SPI::CS{GPIO::PortC, GPIO::Pin13}), // GYRO_EXT_CS
 		initSPIDevice(DRV_BARO_DEVTYPE_MS5611, 	SPI::CS{GPIO::PortC, GPIO::Pin14}), // BARO_EXT_CS
 	}),
 };
@@ -3,9 +3,9 @@ CONFIG_DRIVERS_IMU_ANALOG_DEVICES_ADIS16448=n
 CONFIG_DRIVERS_IRLOCK=n
 CONFIG_DRIVERS_PCA9685_PWM_OUT=n
 CONFIG_DRIVERS_UAVCAN=n
-CONFIG_MODULES_ROVER_POS_CONTROL=n
 CONFIG_MODULES_TEMPERATURE_COMPENSATION=n
 CONFIG_BOARD_TESTING=y
 CONFIG_DRIVERS_TEST_PPM=y
+CONFIG_SYSTEMCMDS_BL_UPDATE=y
 CONFIG_SYSTEMCMDS_MICROBENCH=y
 CONFIG_SYSTEMCMDS_SD_STRESS=y
@@ -28,7 +28,6 @@ CONFIG_DRIVERS_PCA9685_PWM_OUT=y
 CONFIG_DRIVERS_POWER_MONITOR_INA226=y
 CONFIG_DRIVERS_PWM_OUT=y
 CONFIG_DRIVERS_PX4IO=y
-CONFIG_DRIVERS_ROBOCLAW=y
 CONFIG_DRIVERS_SMART_BATTERY_BATMON=y
 CONFIG_COMMON_TELEMETRY=y
 CONFIG_DRIVERS_TONE_ALARM=y
@@ -11,6 +11,14 @@ CONFIG_DRIVERS_LIGHTS_RGBLED_NCP5623C=y
 CONFIG_DRIVERS_MAGNETOMETER_ISENTEK_IST8310=y
 CONFIG_BOARD_UAVCAN_INTERFACES=1
 CONFIG_DRIVERS_UAVCANNODE=y
+CONFIG_UAVCANNODE_BEEP_COMMAND=y
+CONFIG_UAVCANNODE_GNSS_FIX=y
+CONFIG_UAVCANNODE_LIGHTS_COMMAND=y
+CONFIG_UAVCANNODE_MAGNETIC_FIELD_STRENGTH=y
+CONFIG_UAVCANNODE_RTK_DATA=y
+CONFIG_UAVCANNODE_SAFETY_BUTTON=y
+CONFIG_UAVCANNODE_STATIC_PRESSURE=y
+CONFIG_UAVCANNODE_STATIC_TEMPERATURE=y
 CONFIG_MODULES_GYRO_CALIBRATION=y
 CONFIG_MODULES_MAG_BIAS_ESTIMATOR=y
 CONFIG_MODULES_SENSORS=y
@@ -13,6 +13,13 @@ CONFIG_DRIVERS_LIGHTS_RGBLED_NCP5623C=y
 CONFIG_DRIVERS_MAGNETOMETER_BOSCH_BMM150=y
 CONFIG_BOARD_UAVCAN_INTERFACES=1
 CONFIG_DRIVERS_UAVCANNODE=y
+CONFIG_UAVCANNODE_BEEP_COMMAND=y
+CONFIG_UAVCANNODE_GNSS_FIX=y
+CONFIG_UAVCANNODE_LIGHTS_COMMAND=y
+CONFIG_UAVCANNODE_MAGNETIC_FIELD_STRENGTH=y
+CONFIG_UAVCANNODE_SAFETY_BUTTON=y
+CONFIG_UAVCANNODE_STATIC_PRESSURE=y
+CONFIG_UAVCANNODE_STATIC_TEMPERATURE=y
 CONFIG_MODULES_GYRO_CALIBRATION=y
 CONFIG_MODULES_MAG_BIAS_ESTIMATOR=y
 CONFIG_MODULES_SENSORS=y
@@ -27,7 +27,6 @@ CONFIG_DRIVERS_PCA9685_PWM_OUT=y
 CONFIG_DRIVERS_POWER_MONITOR_INA226=y
 CONFIG_DRIVERS_PWM_OUT=y
 CONFIG_DRIVERS_PX4IO=y
-CONFIG_DRIVERS_ROBOCLAW=y
 CONFIG_DRIVERS_SMART_BATTERY_BATMON=y
 CONFIG_COMMON_TELEMETRY=y
 CONFIG_DRIVERS_TONE_ALARM=y
@@ -69,7 +68,6 @@ CONFIG_MODULES_SIMULATION_SIMULATOR_SIH=y
 CONFIG_MODULES_TEMPERATURE_COMPENSATION=y
 CONFIG_MODULES_VTOL_ATT_CONTROL=y
 CONFIG_SYSTEMCMDS_ACTUATOR_TEST=y
-CONFIG_SYSTEMCMDS_BL_UPDATE=y
 CONFIG_SYSTEMCMDS_BSONDUMP=y
 CONFIG_SYSTEMCMDS_DMESG=y
 CONFIG_SYSTEMCMDS_GPIO=y
@@ -22,7 +22,6 @@ CONFIG_COMMON_OPTICAL_FLOW=y
 CONFIG_DRIVERS_POWER_MONITOR_INA226=y
 CONFIG_DRIVERS_PWM_OUT=y
 CONFIG_DRIVERS_RC_INPUT=y
-CONFIG_DRIVERS_ROBOCLAW=y
 CONFIG_DRIVERS_SMART_BATTERY_BATMON=y
 CONFIG_COMMON_TELEMETRY=y
 CONFIG_DRIVERS_TONE_ALARM=y
@@ -66,7 +65,6 @@ CONFIG_MODULES_SIMULATION_SIMULATOR_SIH=y
 CONFIG_MODULES_TEMPERATURE_COMPENSATION=y
 CONFIG_MODULES_VTOL_ATT_CONTROL=y
 CONFIG_SYSTEMCMDS_ACTUATOR_TEST=y
-CONFIG_SYSTEMCMDS_BL_UPDATE=y
 CONFIG_SYSTEMCMDS_BSONDUMP=y
 CONFIG_SYSTEMCMDS_DMESG=y
 CONFIG_SYSTEMCMDS_DUMPFILE=y
@@ -21,7 +21,6 @@ CONFIG_COMMON_OPTICAL_FLOW=y
 CONFIG_DRIVERS_POWER_MONITOR_INA226=y
 CONFIG_DRIVERS_PWM_OUT=y
 CONFIG_DRIVERS_RC_INPUT=y
-CONFIG_DRIVERS_ROBOCLAW=y
 CONFIG_DRIVERS_SMART_BATTERY_BATMON=y
 CONFIG_COMMON_TELEMETRY=y
 CONFIG_DRIVERS_TONE_ALARM=y
@@ -64,7 +63,6 @@ CONFIG_MODULES_SIMULATION_PWM_OUT_SIM=y
 CONFIG_MODULES_TEMPERATURE_COMPENSATION=y
 CONFIG_MODULES_VTOL_ATT_CONTROL=y
 CONFIG_SYSTEMCMDS_ACTUATOR_TEST=y
-CONFIG_SYSTEMCMDS_BL_UPDATE=y
 CONFIG_SYSTEMCMDS_DMESG=y
 CONFIG_SYSTEMCMDS_DUMPFILE=y
 CONFIG_SYSTEMCMDS_GPIO=y
@@ -22,13 +22,10 @@ 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_ACC_COMP 0
-param set-default ATT_W_ACC 0.4000
-param set-default ATT_W_GYRO_BIAS 0.0000
-
+# use EKF2 without mag
 param set-default SYS_HAS_MAG 0
+# and 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
@@ -41,11 +38,5 @@ param set-default SYS_DM_BACKEND 1
 # Ignore that there is no SD card
 param set-default COM_ARM_SDCARD 0

-# Store missions in RAM
-param set-default SYS_DM_BACKEND 1
-
-# Ignore that there is no SD card
-param set-default COM_ARM_SDCARD 0
-
 # Don't try to log onto SD card
 param set-default SDLOG_MODE -1
@@ -22,7 +22,6 @@ CONFIG_COMMON_OPTICAL_FLOW=y
 CONFIG_DRIVERS_POWER_MONITOR_INA226=y
 CONFIG_DRIVERS_PWM_OUT=y
 CONFIG_DRIVERS_RC_INPUT=y
-CONFIG_DRIVERS_ROBOCLAW=y
 CONFIG_DRIVERS_SMART_BATTERY_BATMON=y
 CONFIG_COMMON_TELEMETRY=y
 CONFIG_DRIVERS_TONE_ALARM=y
@@ -66,7 +65,6 @@ CONFIG_MODULES_SIMULATION_SIMULATOR_SIH=y
 CONFIG_MODULES_TEMPERATURE_COMPENSATION=y
 CONFIG_MODULES_VTOL_ATT_CONTROL=y
 CONFIG_SYSTEMCMDS_ACTUATOR_TEST=y
-CONFIG_SYSTEMCMDS_BL_UPDATE=y
 CONFIG_SYSTEMCMDS_DMESG=y
 CONFIG_SYSTEMCMDS_DUMPFILE=y
 CONFIG_SYSTEMCMDS_GPIO=y
@@ -22,13 +22,10 @@ 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_ACC_COMP 0
-param set-default ATT_W_ACC 0.4000
-param set-default ATT_W_GYRO_BIAS 0.0000
-
+# use EKF2 without mag
 param set-default SYS_HAS_MAG 0
+# and 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
@@ -31,7 +31,6 @@ CONFIG_DRIVERS_PWM_INPUT=y
 CONFIG_DRIVERS_PWM_OUT=y
 CONFIG_DRIVERS_PX4IO=y
 CONFIG_DRIVERS_RC_INPUT=y
-CONFIG_DRIVERS_ROBOCLAW=y
 CONFIG_DRIVERS_SAFETY_BUTTON=y
 CONFIG_DRIVERS_SMART_BATTERY_BATMON=y
 CONFIG_COMMON_TELEMETRY=y
@@ -12,6 +12,14 @@ CONFIG_DRIVERS_IMU_INVENSENSE_ICM20602=y
 CONFIG_DRIVERS_MAGNETOMETER_RM3100=y
 CONFIG_BOARD_UAVCAN_INTERFACES=1
 CONFIG_DRIVERS_UAVCANNODE=y
+CONFIG_UAVCANNODE_BEEP_COMMAND=y
+CONFIG_UAVCANNODE_GNSS_FIX=y
+CONFIG_UAVCANNODE_LIGHTS_COMMAND=y
+CONFIG_UAVCANNODE_MAGNETIC_FIELD_STRENGTH=y
+CONFIG_UAVCANNODE_RTK_DATA=y
+CONFIG_UAVCANNODE_SAFETY_BUTTON=y
+CONFIG_UAVCANNODE_STATIC_PRESSURE=y
+CONFIG_UAVCANNODE_STATIC_TEMPERATURE=y
 CONFIG_MODULES_GYRO_CALIBRATION=y
 CONFIG_MODULES_MAG_BIAS_ESTIMATOR=y
 CONFIG_MODULES_SENSORS=y
@@ -30,7 +30,6 @@ CONFIG_DRIVERS_POWER_MONITOR_INA226=y
 CONFIG_DRIVERS_POWER_MONITOR_VOXLPM=y
 CONFIG_DRIVERS_PWM_OUT=y
 CONFIG_DRIVERS_RC_INPUT=y
-CONFIG_DRIVERS_ROBOCLAW=y
 CONFIG_DRIVERS_SAFETY_BUTTON=y
 CONFIG_DRIVERS_SMART_BATTERY_BATMON=y
 CONFIG_COMMON_TELEMETRY=y
@@ -29,7 +29,6 @@ CONFIG_DRIVERS_POWER_MONITOR_INA226=y
 CONFIG_DRIVERS_POWER_MONITOR_VOXLPM=y
 CONFIG_DRIVERS_PWM_OUT=y
 CONFIG_DRIVERS_RC_INPUT=y
-CONFIG_DRIVERS_ROBOCLAW=y
 CONFIG_COMMON_TELEMETRY=y
 CONFIG_DRIVERS_UAVCAN=y
 CONFIG_BOARD_UAVCAN_INTERFACES=1
@@ -71,7 +70,6 @@ CONFIG_MODULES_SIMULATION_SIMULATOR_SIH=y
 CONFIG_MODULES_TEMPERATURE_COMPENSATION=y
 CONFIG_MODULES_VTOL_ATT_CONTROL=y
 CONFIG_SYSTEMCMDS_ACTUATOR_TEST=y
-CONFIG_SYSTEMCMDS_BL_UPDATE=y
 CONFIG_SYSTEMCMDS_BSONDUMP=y
 CONFIG_SYSTEMCMDS_DMESG=y
 CONFIG_SYSTEMCMDS_GPIO=y
@@ -4,6 +4,7 @@ CONFIG_DRIVERS_ACTUATORS_MODAL_IO=y
 CONFIG_DRIVERS_LIGHTS_RGBLED_NCP5623C=y
 CONFIG_DRIVERS_POWER_MONITOR_VOXLPM=y
 CONFIG_DRIVERS_QSHELL_QURT=y
+CONFIG_MODULES_COMMANDER=y
 CONFIG_MODULES_EKF2=y
 CONFIG_MODULES_LAND_DETECTOR=y
 CONFIG_MODULES_MC_ATT_CONTROL=y
@@ -1,6 +1,7 @@
 CONFIG_PLATFORM_POSIX=y
 CONFIG_BOARD_LINUX_TARGET=y
 CONFIG_BOARD_TOOLCHAIN="aarch64-linux-gnu"
+CONFIG_BOARD_ROOTFSDIR="/data/px4"
 CONFIG_DRIVERS_QSHELL_POSIX=y
 CONFIG_MODULES_COMMANDER=y
 CONFIG_MODULES_DATAMAN=y
@@ -17,4 +18,3 @@ CONFIG_SYSTEMCMDS_TOPIC_LISTENER=y
 CONFIG_SYSTEMCMDS_UORB=y
 CONFIG_SYSTEMCMDS_VER=y
 CONFIG_ORB_COMMUNICATOR=y
-CONFIG_BOARD_ROOTFSDIR="/data/px4"
@@ -25,7 +25,6 @@ CONFIG_DRIVERS_PCA9685_PWM_OUT=y
 CONFIG_DRIVERS_POWER_MONITOR_INA226=y
 CONFIG_DRIVERS_PWM_OUT=y
 CONFIG_DRIVERS_RC_INPUT=y
-CONFIG_DRIVERS_ROBOCLAW=y
 CONFIG_DRIVERS_SMART_BATTERY_BATMON=y
 CONFIG_COMMON_TELEMETRY=y
 CONFIG_DRIVERS_TONE_ALARM=y
@@ -72,7 +71,6 @@ CONFIG_MODULES_UUV_ATT_CONTROL=y
 CONFIG_MODULES_UUV_POS_CONTROL=y
 CONFIG_MODULES_VTOL_ATT_CONTROL=y
 CONFIG_SYSTEMCMDS_ACTUATOR_TEST=y
-CONFIG_SYSTEMCMDS_BL_UPDATE=y
 CONFIG_SYSTEMCMDS_BSONDUMP=y
 CONFIG_SYSTEMCMDS_DMESG=y
 CONFIG_SYSTEMCMDS_DUMPFILE=y
@@ -25,7 +25,6 @@ CONFIG_DRIVERS_PCA9685_PWM_OUT=y
 CONFIG_DRIVERS_POWER_MONITOR_INA226=y
 CONFIG_DRIVERS_PWM_OUT=y
 CONFIG_DRIVERS_RC_INPUT=y
-CONFIG_DRIVERS_ROBOCLAW=y
 CONFIG_DRIVERS_SAFETY_BUTTON=y
 CONFIG_DRIVERS_SMART_BATTERY_BATMON=y
 CONFIG_COMMON_TELEMETRY=y
@@ -24,7 +24,6 @@ CONFIG_DRIVERS_PCA9685_PWM_OUT=y
 CONFIG_DRIVERS_POWER_MONITOR_INA226=y
 CONFIG_DRIVERS_PWM_OUT=y
 CONFIG_DRIVERS_RC_INPUT=y
-CONFIG_DRIVERS_ROBOCLAW=y
 CONFIG_DRIVERS_SAFETY_BUTTON=y
 CONFIG_DRIVERS_SMART_BATTERY_BATMON=y
 CONFIG_COMMON_TELEMETRY=y
@@ -25,7 +25,6 @@ CONFIG_DRIVERS_PCA9685_PWM_OUT=y
 CONFIG_DRIVERS_POWER_MONITOR_INA226=y
 CONFIG_DRIVERS_PWM_OUT=y
 CONFIG_DRIVERS_RC_INPUT=y
-CONFIG_DRIVERS_ROBOCLAW=y
 CONFIG_DRIVERS_SMART_BATTERY_BATMON=y
 CONFIG_COMMON_TELEMETRY=y
 CONFIG_DRIVERS_TONE_ALARM=y
@@ -70,7 +69,6 @@ CONFIG_MODULES_UUV_ATT_CONTROL=y
 CONFIG_MODULES_UUV_POS_CONTROL=y
 CONFIG_MODULES_VTOL_ATT_CONTROL=y
 CONFIG_SYSTEMCMDS_ACTUATOR_TEST=y
-CONFIG_SYSTEMCMDS_BL_UPDATE=y
 CONFIG_SYSTEMCMDS_BSONDUMP=y
 CONFIG_SYSTEMCMDS_DMESG=y
 CONFIG_SYSTEMCMDS_DUMPFILE=y
@@ -25,7 +25,6 @@ CONFIG_DRIVERS_PCA9685_PWM_OUT=y
 CONFIG_DRIVERS_POWER_MONITOR_INA226=y
 CONFIG_DRIVERS_PWM_OUT=y
 CONFIG_DRIVERS_RC_INPUT=y
-CONFIG_DRIVERS_ROBOCLAW=y
 CONFIG_DRIVERS_SMART_BATTERY_BATMON=y
 CONFIG_COMMON_TELEMETRY=y
 CONFIG_DRIVERS_TONE_ALARM=y
@@ -71,7 +70,6 @@ CONFIG_MODULES_UUV_ATT_CONTROL=y
 CONFIG_MODULES_UUV_POS_CONTROL=y
 CONFIG_MODULES_VTOL_ATT_CONTROL=y
 CONFIG_SYSTEMCMDS_ACTUATOR_TEST=y
-CONFIG_SYSTEMCMDS_BL_UPDATE=y
 CONFIG_SYSTEMCMDS_BSONDUMP=y
 CONFIG_SYSTEMCMDS_DMESG=y
 CONFIG_SYSTEMCMDS_DUMPFILE=y
@@ -25,7 +25,6 @@ CONFIG_DRIVERS_PCA9685_PWM_OUT=y
 CONFIG_DRIVERS_POWER_MONITOR_INA226=y
 CONFIG_DRIVERS_PWM_OUT=y
 CONFIG_DRIVERS_RC_INPUT=y
-CONFIG_DRIVERS_ROBOCLAW=y
 CONFIG_DRIVERS_SMART_BATTERY_BATMON=y
 CONFIG_COMMON_TELEMETRY=y
 CONFIG_DRIVERS_TONE_ALARM=y
@@ -70,7 +69,6 @@ CONFIG_MODULES_UUV_ATT_CONTROL=y
 CONFIG_MODULES_UUV_POS_CONTROL=y
 CONFIG_MODULES_VTOL_ATT_CONTROL=y
 CONFIG_SYSTEMCMDS_ACTUATOR_TEST=y
-CONFIG_SYSTEMCMDS_BL_UPDATE=y
 CONFIG_SYSTEMCMDS_BSONDUMP=y
 CONFIG_SYSTEMCMDS_DMESG=y
 CONFIG_SYSTEMCMDS_DUMPFILE=y
@@ -26,7 +26,6 @@ CONFIG_DRIVERS_POWER_MONITOR_INA226=y
 CONFIG_DRIVERS_PWM_INPUT=y
 CONFIG_DRIVERS_PWM_OUT=y
 CONFIG_DRIVERS_PX4IO=y
-CONFIG_DRIVERS_ROBOCLAW=y
 CONFIG_DRIVERS_SMART_BATTERY_BATMON=y
 CONFIG_COMMON_TELEMETRY=y
 CONFIG_DRIVERS_TONE_ALARM=y
@@ -27,7 +27,6 @@ CONFIG_DRIVERS_POWER_MONITOR_INA226=y
 CONFIG_DRIVERS_PWM_INPUT=y
 CONFIG_DRIVERS_PWM_OUT=y
 CONFIG_DRIVERS_PX4IO=y
-CONFIG_DRIVERS_ROBOCLAW=y
 CONFIG_DRIVERS_SMART_BATTERY_BATMON=y
 CONFIG_COMMON_TELEMETRY=y
 CONFIG_DRIVERS_TONE_ALARM=y
@@ -27,7 +27,6 @@ CONFIG_DRIVERS_PCA9685_PWM_OUT=y
 CONFIG_DRIVERS_POWER_MONITOR_INA226=y
 CONFIG_DRIVERS_PWM_OUT=y
 CONFIG_DRIVERS_RC_INPUT=y
-CONFIG_DRIVERS_ROBOCLAW=y
 CONFIG_DRIVERS_SAFETY_BUTTON=y
 CONFIG_DRIVERS_SMART_BATTERY_BATMON=y
 CONFIG_COMMON_TELEMETRY=y
@@ -28,7 +28,6 @@ CONFIG_DRIVERS_PCA9685_PWM_OUT=y
 CONFIG_DRIVERS_POWER_MONITOR_INA226=y
 CONFIG_DRIVERS_PWM_OUT=y
 CONFIG_DRIVERS_RC_INPUT=y
-CONFIG_DRIVERS_ROBOCLAW=y
 CONFIG_DRIVERS_SAFETY_BUTTON=y
 CONFIG_DRIVERS_SMART_BATTERY_BATMON=y
 CONFIG_COMMON_TELEMETRY=y
@@ -13,6 +13,21 @@ CONFIG_COMMON_MAGNETOMETER=y
 CONFIG_DRIVERS_PWM_OUT=y
 CONFIG_BOARD_UAVCAN_INTERFACES=1
 CONFIG_DRIVERS_UAVCANNODE=y
+CONFIG_UAVCANNODE_ARMING_STATUS=y
+CONFIG_UAVCANNODE_BEEP_COMMAND=y
+CONFIG_UAVCANNODE_ESC_RAW_COMMAND=y
+CONFIG_UAVCANNODE_ESC_STATUS=y
+CONFIG_UAVCANNODE_FLOW_MEASUREMENT=y
+CONFIG_UAVCANNODE_GNSS_FIX=y
+CONFIG_UAVCANNODE_HYGROMETER_MEASUREMENT=y
+CONFIG_UAVCANNODE_LIGHTS_COMMAND=y
+CONFIG_UAVCANNODE_MAGNETIC_FIELD_STRENGTH=y
+CONFIG_UAVCANNODE_RANGE_SENSOR_MEASUREMENT=y
+CONFIG_UAVCANNODE_RAW_AIR_DATA=y
+CONFIG_UAVCANNODE_RTK_DATA=y
+CONFIG_UAVCANNODE_SERVO_ARRAY_COMMAND=y
+CONFIG_UAVCANNODE_STATIC_PRESSURE=y
+CONFIG_UAVCANNODE_STATIC_TEMPERATURE=y
 CONFIG_MODULES_CONTROL_ALLOCATOR=y
 CONFIG_MODULES_SENSORS=y
 CONFIG_SYSTEMCMDS_ACTUATOR_TEST=y
@@ -23,13 +23,18 @@ 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_FLIGHT_MODE_MANAGER=y
 CONFIG_MODULES_LAND_DETECTOR=y
 CONFIG_MODULES_LOGGER=y
 CONFIG_MODULES_MANUAL_CONTROL=y
 CONFIG_MODULES_MAVLINK=y
 CONFIG_MODULES_MC_ATT_CONTROL=y
-CONFIG_MODULES_MC_HOVER_THRUST_ESTIMATOR=n
 CONFIG_MODULES_MC_POS_CONTROL=y
 CONFIG_MODULES_MC_RATE_CONTROL=y
 CONFIG_MODULES_NAVIGATOR=y
@@ -1,6 +1,5 @@
 CONFIG_MODULES_FLIGHT_MODE_MANAGER=n
 CONFIG_MODULES_MC_ATT_CONTROL=n
-CONFIG_MODULES_MC_HOVER_THRUST_ESTIMATOR=n
 CONFIG_MODULES_MC_POS_CONTROL=n
 CONFIG_MODULES_MC_RATE_CONTROL=n
 CONFIG_SYSTEMCMDS_BL_UPDATE=n
@@ -1,6 +1,5 @@
 CONFIG_MODULES_FLIGHT_MODE_MANAGER=n
 CONFIG_MODULES_MC_ATT_CONTROL=n
-CONFIG_MODULES_MC_HOVER_THRUST_ESTIMATOR=n
 CONFIG_MODULES_MC_POS_CONTROL=n
 CONFIG_MODULES_MC_RATE_CONTROL=n
 CONFIG_DRIVERS_CAMERA_CAPTURE=y
@@ -31,7 +31,6 @@ CONFIG_DRIVERS_PCA9685_PWM_OUT=y
 CONFIG_DRIVERS_PWM_INPUT=y
 CONFIG_DRIVERS_PWM_OUT=y
 CONFIG_DRIVERS_PX4IO=y
-CONFIG_DRIVERS_ROBOCLAW=y
 CONFIG_DRIVERS_SMART_BATTERY_BATMON=y
 CONFIG_COMMON_TELEMETRY=y
 CONFIG_DRIVERS_TONE_ALARM=y
@@ -30,7 +30,6 @@ CONFIG_DRIVERS_POWER_MONITOR_INA226=y
 CONFIG_DRIVERS_PWM_INPUT=y
 CONFIG_DRIVERS_PWM_OUT=y
 CONFIG_DRIVERS_RC_INPUT=y
-CONFIG_DRIVERS_ROBOCLAW=y
 CONFIG_DRIVERS_SAFETY_BUTTON=y
 CONFIG_DRIVERS_SMART_BATTERY_BATMON=y
 CONFIG_COMMON_TELEMETRY=y
@@ -29,7 +29,6 @@ CONFIG_DRIVERS_PCA9685_PWM_OUT=y
 CONFIG_DRIVERS_PWM_INPUT=y
 CONFIG_DRIVERS_PWM_OUT=y
 CONFIG_DRIVERS_PX4IO=y
-CONFIG_DRIVERS_ROBOCLAW=y
 CONFIG_DRIVERS_SMART_BATTERY_BATMON=y
 CONFIG_COMMON_TELEMETRY=y
 CONFIG_DRIVERS_TONE_ALARM=y
@@ -2,6 +2,6 @@
 CONFIG_DRIVERS_HEATER=n
 CONFIG_DRIVERS_UAVCAN=n
 CONFIG_CYPHAL_BMS_SUBSCRIBER=y
+CONFIG_CYPHAL_ESC_CONTROLLER=y
 CONFIG_CYPHAL_UORB_SENSOR_GPS_SUBSCRIBER=y
 CONFIG_DRIVERS_CYPHAL=y
-CONFIG_CYPHAL_ESC_CONTROLLER=y
@@ -14,7 +14,6 @@ CONFIG_DRIVERS_IRLOCK=n
 CONFIG_DRIVERS_PCA9685_PWM_OUT=n
 CONFIG_DRIVERS_POWER_MONITOR_INA226=n
 CONFIG_DRIVERS_PWM_INPUT=n
-CONFIG_DRIVERS_ROBOCLAW=n
 CONFIG_DRIVERS_SMART_BATTERY_BATMON=n
 CONFIG_DRIVERS_UAVCAN=n
 CONFIG_MODULES_ATTITUDE_ESTIMATOR_Q=n
@@ -35,7 +35,6 @@ CONFIG_DRIVERS_PWM_OUT=y
 CONFIG_DRIVERS_PX4IO=y
 CONFIG_COMMON_RC=y
 CONFIG_DRIVERS_RC_INPUT=y
-CONFIG_DRIVERS_ROBOCLAW=y
 CONFIG_DRIVERS_SAFETY_BUTTON=y
 CONFIG_DRIVERS_SMART_BATTERY_BATMON=y
 CONFIG_COMMON_TELEMETRY=y
@@ -9,7 +9,6 @@ CONFIG_DRIVERS_CAMERA_TRIGGER=n
 CONFIG_DRIVERS_IRLOCK=n
 CONFIG_DRIVERS_PCA9685_PWM_OUT=n
 CONFIG_DRIVERS_PWM_INPUT=n
-CONFIG_DRIVERS_ROBOCLAW=n
 CONFIG_DRIVERS_UAVCAN=n
 CONFIG_EXAMPLES_FAKE_GPS=n
 CONFIG_MODULES_AIRSPEED_SELECTOR=n
@@ -17,7 +17,6 @@ CONFIG_DRIVERS_IRLOCK=n
 CONFIG_DRIVERS_PCA9685_PWM_OUT=n
 CONFIG_DRIVERS_POWER_MONITOR_INA226=n
 CONFIG_DRIVERS_PWM_INPUT=n
-CONFIG_DRIVERS_ROBOCLAW=n
 CONFIG_DRIVERS_SMART_BATTERY_BATMON=n
 CONFIG_DRIVERS_UAVCAN=n
 CONFIG_EXAMPLES_FAKE_GPS=n
@@ -11,7 +11,6 @@ CONFIG_DRIVERS_IMU_INVENSENSE_ICM20948=n
 CONFIG_DRIVERS_IRLOCK=n
 CONFIG_DRIVERS_PCA9685_PWM_OUT=n
 CONFIG_DRIVERS_PWM_INPUT=n
-CONFIG_DRIVERS_ROBOCLAW=n
 CONFIG_DRIVERS_SMART_BATTERY_BATMON=n
 CONFIG_EXAMPLES_FAKE_GPS=n
 CONFIG_MODULES_ATTITUDE_ESTIMATOR_Q=n
@@ -39,7 +39,6 @@ CONFIG_DRIVERS_PWM_OUT=y
 CONFIG_DRIVERS_PX4IO=y
 CONFIG_COMMON_RC=y
 CONFIG_DRIVERS_RC_INPUT=y
-CONFIG_DRIVERS_ROBOCLAW=y
 CONFIG_DRIVERS_SAFETY_BUTTON=y
 CONFIG_DRIVERS_SMART_BATTERY_BATMON=y
 CONFIG_COMMON_TELEMETRY=y
@@ -68,7 +68,6 @@ CONFIG_MODULES_SIMULATION_SIMULATOR_SIH=y
 CONFIG_MODULES_TEMPERATURE_COMPENSATION=y
 CONFIG_MODULES_VTOL_ATT_CONTROL=y
 CONFIG_SYSTEMCMDS_ACTUATOR_TEST=y
-CONFIG_SYSTEMCMDS_BL_UPDATE=y
 CONFIG_SYSTEMCMDS_BSONDUMP=y
 CONFIG_SYSTEMCMDS_DMESG=y
 CONFIG_SYSTEMCMDS_HARDFAULT_LOG=y
@@ -141,12 +141,13 @@
 #define GPIO_HW_VER_SENSE      /* PC1 */  GPIO_ADC123_INP11
 #define HW_INFO_INIT_PREFIX    "V6C"

-#define BOARD_NUM_SPI_CFG_HW_VERSIONS 2 // Rev 0, 10 Sensor sets
+#define BOARD_NUM_SPI_CFG_HW_VERSIONS 3 // Rev 0, 10 and Mini Sensor sets
 //                 Base/FMUM
 #define V6C00   HW_VER_REV(0x0,0x0) // FMUV6C,                 Rev 0  I2C4 External but with Internal devices
 #define V6C01   HW_VER_REV(0x0,0x1) // FMUV6C,                 Rev 1  I2C4 Internal I2C2 External
 #define V6C10   HW_VER_REV(0x1,0x0) // NO PX4IO,               Rev 0  I2C4 External but with Internal devices
 #define V6C11   HW_VER_REV(0x1,0x1) // NO PX4IO,               Rev 1  I2C4 Internal I2C2 External
+#define V6C21   HW_VER_REV(0x2,0x1) // FMUV6CMini,             Rev 1  I2C4 Internal I2C2 External


 /* HEATER
@@ -79,6 +79,11 @@ static const px4_hw_mft_item_t hw_mft_list_v0600[] = {
 		.mandatory   = 1,
 		.connection  = px4_hw_con_onboard,
 	},
+	{
+		.present     = 1,
+		.mandatory   = 1,
+		.connection  = px4_hw_con_onboard,
+	},
 };

 static const px4_hw_mft_item_t hw_mft_list_v0610[] = {
@@ -101,6 +106,7 @@ static px4_hw_mft_list_entry_t mft_lists[] = {
 	{V6C01, hw_mft_list_v0600, arraySize(hw_mft_list_v0600)}, // Rev 1
 	{V6C10, hw_mft_list_v0610, arraySize(hw_mft_list_v0610)}, // Rev 0 No PX4IO
 	{V6C11, hw_mft_list_v0610, arraySize(hw_mft_list_v0610)}, // Rev 1 No PX4IO
+	{V6C21, hw_mft_list_v0600, arraySize(hw_mft_list_v0600)}, // Rev 1 MINI
 };

 /************************************************************************************
--- a/Show More
+++ b/Show More