gps: update submodule

param translation: fix too early return false (#22729 )
Signed-off-by: Silvan Fuhrer <silvan@auterion.com>
2026-06-14 04:30:04 +08:00 · 2024-02-15 10:09:28 -05:00 · 2024-02-07 12:41:23 +01:00 · 2024-02-02 09:38:48 -05:00 · 2024-02-02 09:38:48 -05:00 · 2024-02-01 21:11:57 -05:00
518 changed files with 12429 additions and 13373 deletions
@@ -40,6 +40,8 @@ pipeline {
                     "ark_can-flow_default",
                     "ark_can-gps_canbootloader",
                     "ark_can-gps_default",
+                     "ark_cannode_canbootloader",
+                     "ark_cannode_default",
                     "ark_can-rtk-gps_canbootloader",
                     "ark_can-rtk-gps_default",
                     "ark_fmu-v6x_bootloader",
@@ -53,8 +55,10 @@ pipeline {
                     "cuav_nora_default",
                     "cuav_x7pro_default",
                     "cubepilot_cubeorange_default",
+                     "cubepilot_cubeorangeplus_default",
                     "cubepilot_cubeyellow_default",
                     "diatone_mamba-f405-mk2_default",
+                     "flywoo_gn-f405_default",
                     "freefly_can-rtk-gps_canbootloader",
                     "freefly_can-rtk-gps_default",
                     "holybro_can-gps-v1_canbootloader",
@@ -72,7 +76,7 @@ pipeline {
                     "matek_h743_default",
                     "modalai_fc-v1_default",
                     "modalai_fc-v2_default",
-                     "modalai_voxl2-io_default",
+                     "mro_ctrl-zero-classic_default",
                     "mro_ctrl-zero-f7_default",
                     "mro_ctrl-zero-f7-oem_default",
                     "mro_ctrl-zero-h7-oem_default",
@@ -85,6 +89,7 @@ pipeline {
                     "nxp_fmuk66-v3_default",
                     "nxp_fmuk66-v3_socketcan",
                     "nxp_fmurt1062-v1_default",
+                     "nxp_mr-canhubk3_default",
                     "nxp_ucans32k146_canbootloader",
                     "nxp_ucans32k146_default",
                     "omnibus_f4sd_default",
@@ -0,0 +1,34 @@
+---
+name: 🐛 Bug report
+about: Create a report to help us improve
+labels: bug-report
+
+---
+
+## Describe the bug
+A clear and concise description of the bug.
+
+## To Reproduce
+Steps to reproduce the behavior:
+1. Drone switched on '...'
+2. Uploaded mission '....' (attach QGC mission file)
+3. Took off '....'
+4. See error
+
+## Expected behavior
+A clear and concise description of what you expected to happen.
+
+## Log Files and Screenshots
+*Always* provide a link to the flight log file:
+- Download the flight log file from the vehicle ([tutorial](https://docs.px4.io/main/en/getting_started/flight_reporting.html)).
+- Upload the log to the [PX4 Flight Review](http://logs.px4.io/)
+- Share the link to the log (Copy and paste the URL of the log)
+
+Add screenshots to help explain your problem.
+
+## Drone (please complete the following information):
+- Describe the type of drone.
+- Photo of the IMU / autopilot setup if possible.
+
+## Additional context
+Add any other context about the problem here.
@@ -0,0 +1,20 @@
+---
+name: 🚀 Feature Request
+about: Suggest an idea for this project
+labels: feature-request
+
+---
+
+For general questions please use [PX4 Discuss](http://discuss.px4.io/) or Discord (you can find an invite link on this project README).
+
+## Describe problem solved by the proposed feature
+A clear and concise description of the problem, if any, this feature will solve. E.g. I'm always frustrated when ...
+
+## Describe your preferred solution
+A clear and concise description of what you want to happen.
+
+## Describe possible alternatives
+A clear and concise description of alternative solutions or features you've considered.
+
+## Additional context
+Add any other context or screenshots for the feature request here.
@@ -0,0 +1,13 @@
+---
+name: ⛔ Support Question
+about: See http://discuss.px4.io/ for questions about using PX4.
+
+---
+
+## Attention! Please read the note below
+
+We use GitHub issues only to discuss PX4 bugs and new features.
+
+**For questions about using PX4 or related components, please use [PX4 Discuss](http://discuss.px4.io/).**
+
+Thanks!
@@ -0,0 +1,11 @@
+---
+name: ⛔ Documentation Issue
+about: See https://github.com/PX4/px4_user_guide for documentation issues
+
+---
+
+## Attention! Please read the note below
+
+**Please submit the documentation issue to the [User Guide](https://github.com/PX4/px4_user_guide) repository.**
+
+Thanks!
@@ -1,94 +0,0 @@
-name: 🐛 Bug report
-description: Create a report to help us improve
-title: "[Bug] "
-labels: ["bug-report"]
-body:
-  - type: textarea
-    attributes:
-      label: Describe the bug
-      description: A clear and concise description of the bug.
-    validations:
-      required: true
-
-  - type: textarea
-    attributes:
-      label: To Reproduce
-      description: |
-          Steps to reproduce the behavior.
-          1. Drone switched on '...'
-          2. Uploaded mission '....' (attach QGC mission file)
-          3. Took off '....'
-          4. See error
-    validations:
-      required: true
-
-  - type: textarea
-    attributes:
-      label: Expected behavior
-      description: A clear and concise description of what you expected to happen.
-    validations:
-      required: true
-
-  - type: textarea
-    attributes:
-      label: Screenshot / Media
-      description: Add screenshot / media if you have them
-
-  - type: textarea
-    attributes:
-      label: Flight Log
-      description: |
-          *Always* provide a link to the flight log file:
-          - Download the flight log file from the vehicle ([tutorial](https://docs.px4.io/main/en/getting_started/flight_reporting.html)).
-          - Upload the log to the [PX4 Flight Review](http://logs.px4.io/)
-          - Share the link to the log (Copy and paste the URL of the log)
-      placeholder: |
-          # PASTE HERE THE LINK TO THE LOG
-    validations:
-      required: true
-
-  - type: markdown
-    attributes:
-      value: |
-        ## Setup
-
-  - type: textarea
-    attributes:
-      label: Software Version
-      description: |
-          Which version of PX4 are you using?
-      placeholder: |
-          # If you don't know the version, paste the output of `ver all` in the MAVLink Shell of QGC
-    validations:
-      required: true
-
-  - type: input
-    attributes:
-      label: Flight controller
-      description: Specify your flight controller model (what type is it, where was it bought from, ...).
-    validations:
-      required: true
-
-  - type: dropdown
-    attributes:
-      label: Vehicle type
-      options:
-        - Multicopter
-        - Helicopter
-        - Fixed Wing
-        - Hybrid VTOL
-        - Airship/Balloon
-        - Rover
-        - Boat
-        - Submarine
-        - Other
-
-  - type: textarea
-    attributes:
-      label: How are the different components wired up (including port information)
-      description: Details about how all is wired.
-
-  - type: textarea
-    attributes:
-      label: Additional context
-      description: Add any other context about the problem here.
@@ -1,8 +0,0 @@
-blank_issues_enabled: false
-contact_links:
-  - name: Support Question
-    url: https://docs.px4.io/main/en/contribute/support.html#forums-and-chat
-    about: For questions about using PX4 or related components, please use the discuss forum and discord server
-  - name: Documentation Issue
-    url: https://github.com/PX4/PX4-user_guide/issues
-    about: If you found an issue in documentation, please submit it directly to the docs repository issues
@@ -1,35 +0,0 @@
-name: 🚀 Feature Request
-description: Suggest an idea for this project
-labels: ["feature-request"]
-body:
-  - type: markdown
-    attributes:
-      value: |
-        ## Please note that feature requests are not 'fire and forget'
-        It is a lot more likely that the feature you would like to have will be implemented if you keep watching your feature request, and provide more details to developers looking into implementing your feature, and help them with testing.
-
-  - type: textarea
-    attributes:
-      label: Describe problem solved by the proposed feature
-      description: A clear and concise description of the problem, if any, this feature will solve. E.g. I'm always frustrated when ...
-    validations:
-      required: true
-
-  - type: textarea
-    attributes:
-      label: Describe your preferred solution
-      description: A clear and concise description of what you want to happen.
-    validations:
-      required: true
-
-  - type: textarea
-    attributes:
-      label: Describe possible alternatives
-      description: A clear and concise description of any alternative solutions or features you've considered.
-    validations:
-      required: true
-
-  - type: textarea
-    attributes:
-      label: Additional context
-      description: Add any other context or screenshots for the feature request here.
@@ -0,0 +1,24 @@
+<svg xmlns="http://www.w3.org/2000/svg" xmlns:xlink="http://www.w3.org/1999/xlink" width="90" height="20">
+  <linearGradient id="smooth" x2="0" y2="100%">
+    <stop offset="0" stop-color="#bbb" stop-opacity=".1"/>
+    <stop offset="1" stop-opacity=".1"/>
+  </linearGradient>
+
+  <mask id="round">
+    <rect width="90" height="20" rx="3" fill="#fff"/>
+  </mask>
+
+  <g mask="url(#round)">
+    <rect width="42" height="20" fill="#555"/>
+    <rect x="42" width="48" height="20" fill="#E01563"/>
+    <rect width="90" height="20" fill="url(#smooth)"/>
+  </g>
+
+  <g fill="#fff" text-anchor="middle" font-family="DejaVu Sans,Verdana,Geneva,sans-serif" font-size="11">
+    <text x="22" y="15" fill="#010101" fill-opacity=".3">slack</text>
+    <text x="22" y="14">slack</text>
+    <text x="65" y="15" fill="#010101" fill-opacity=".3">Join us!</text>
+    <text x="65" y="14">Join us!</text>
+  </g>
+</svg>
+
@@ -0,0 +1,15 @@
+# Number of days of inactivity before an issue becomes stale
+daysUntilStale: 90
+# Number of days of inactivity before a stale issue is closed, or `false` to disable
+daysUntilClose: false
+# Issues with these labels will never be considered stale
+exemptLabels:
+  - pinned
+# Label to use when marking an issue as stale
+staleLabel: stale
+# Comment to post when marking an issue as stale. Set to `false` to disable
+markComment: >
+  This issue has been automatically marked as stale because it has not had
+  recent activity. Thank you for your contributions.
+# Comment to post when closing a stale issue. Set to `false` to disable
+closeComment: false
@@ -11,7 +11,7 @@ on:
 jobs:
  build:
    runs-on: ubuntu-latest
-    container: px4io/px4-dev-armhf:2023-06-26
+    container: px4io/px4-dev-armhf:2021-09-08
    strategy:
      matrix:
        config: [
@@ -24,8 +24,7 @@ jobs:
    - uses: actions/checkout@v1
      with:
        token: ${{secrets.ACCESS_TOKEN}}
-    - name: ownership workaround
-      run: git config --system --add safe.directory '*'
+
    - name: Prepare ccache timestamp
      id: ccache_cache_timestamp
      shell: cmake -P {0}
@@ -1,16 +0,0 @@
-name: 'Handle stale issues and PRs'
-on:
-  schedule:
-    - cron: '30 1 * * *'
-
-jobs:
-  stale:
-    runs-on: ubuntu-latest
-    steps:
-      - uses: actions/stale@v8
-        with:
-          days-before-stale: 30
-          days-before-close: -1
-          stale-issue-label: 'stale'
-          stale-pr-label: 'stale'
-          remove-stale-when-updated: true
@@ -21,7 +21,7 @@
 [submodule "platforms/nuttx/NuttX/nuttx"]
 	path = platforms/nuttx/NuttX/nuttx
 	url = https://github.com/PX4/NuttX.git
-	branch = px4_firmware_nuttx-10.3.0+
+	branch = px4_firmware_nuttx-10.3.0+-v1.14
 [submodule "platforms/nuttx/NuttX/apps"]
 	path = platforms/nuttx/NuttX/apps
 	url = https://github.com/PX4/NuttX-apps.git
@@ -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

@@ -94,6 +94,4 @@ px4_add_romfs_files(

 	17001_flightgear_tf-g1
 	17002_flightgear_tf-g2
-
-	# [22000, 22999] Reserve for custom models
 )
@@ -34,9 +34,7 @@ param set-default CA_ROTOR3_PX -0.25
 param set-default CA_ROTOR3_PY 0.25
 param set-default CA_ROTOR3_KM -0.05

-param set-default PWM_AUX_FUNC1 101
-param set-default PWM_AUX_FUNC2 102
-param set-default PWM_AUX_FUNC3 103
-param set-default PWM_AUX_FUNC4 104
-param set-default PWM_AUX_TIM0 -4
-
+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
@@ -21,11 +21,7 @@ control_allocator start
 fw_rate_control start
 fw_att_control start
 fw_pos_control start
-
-if param greater -s ASPD_SEL_EN 0
-then
-	airspeed_selector start
-fi
+airspeed_selector start

 #
 # Start attitude control auto-tuner
@@ -20,10 +20,7 @@ ekf2 start &
 #
 control_allocator start

-if param greater -s ASPD_SEL_EN 0
-then
-	airspeed_selector start
-fi
+airspeed_selector start

 vtol_att_control start

@@ -266,12 +266,9 @@ else
 		. $FCONFIG
 	fi

-	#
-	# Start IO for PWM output or RC input if enabled
-	#
-	if param compare -s SYS_USE_IO 1
+	if px4io supported
 	then
-		# Check if PX4IO present and update firmware if needed.
+	# Check if PX4IO present and update firmware if needed.
 		if [ -f $IOFW ]
 		then
 			if ! px4io checkcrc ${IOFW}
@@ -293,12 +290,12 @@ else
 					tune_control stop
 				fi
 			fi
-		fi

-		if ! px4io start
-		then
-			echo "PX4IO start failed"
-			set STARTUP_TUNE 2 # tune 2 = ERROR_TUNE
+			if ! px4io start
+			then
+				echo "PX4IO start failed"
+				set STARTUP_TUNE 2 # tune 2 = ERROR_TUNE
+			fi
 		fi
 	fi

@@ -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"
@@ -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
@@ -56,14 +56,11 @@ def perform_imu_checks(
    # perform the vibration check
    imu_status['imu_vibration_check'] = 'Pass'
    for imu_vibr_metric in ['imu_coning', 'imu_hfgyro', 'imu_hfaccel']:
-        mean_metric = '{:s}_mean_warn'.format(imu_vibr_metric)
-        peak_metric = '{:s}_peak_warn'.format(imu_vibr_metric)
-        mean_key = '{:s}_mean'.format(imu_vibr_metric)
-        peak_key = '{:s}_peak'.format(imu_vibr_metric)
-        if mean_key in imu_metrics and peak_key in imu_metrics:
-            if imu_metrics[mean_key] > check_levels[mean_metric] \
-                   or imu_metrics[peak_key] > check_levels[peak_metric]:
-                imu_status['imu_vibration_check'] = 'Warning'
+        mean_metric = '{:s}_mean'.format(imu_vibr_metric)
+        peak_metric = '{:s}_peak'.format(imu_vibr_metric)
+        if imu_metrics[mean_metric] > check_levels['{:s}_warn'.format(mean_metric)] \
+                or imu_metrics[peak_metric] > check_levels['{:s}_warn'.format(peak_metric)]:
+            imu_status['imu_vibration_check'] = 'Warning'

    if imu_status['imu_vibration_check'] == 'Warning':
        print('IMU vibration check warning.')
@@ -104,9 +104,9 @@ def calculate_innov_fail_metrics(

    # calculate innovation check fail metrics
    for signal_id, signal, result in [('posv', 'reject_ver_pos', 'hgt_fail_percentage'),
-                                      ('magx', 'fs_bad_mag_x', 'magx_fail_percentage'),
-                                      ('magy', 'fs_bad_mag_y', 'magy_fail_percentage'),
-                                      ('magz', 'fs_bad_mag_z', 'magz_fail_percentage'),
+                                      ('magx', 'reject_mag_x', 'magx_fail_percentage'),
+                                      ('magy', 'reject_mag_y', 'magy_fail_percentage'),
+                                      ('magz', 'reject_mag_z', 'magz_fail_percentage'),
                                      ('yaw', 'reject_yaw', 'yaw_fail_percentage'),
                                      ('velh', 'reject_hor_vel', 'vel_fail_percentage'),
                                      ('velv', 'reject_ver_vel', 'vel_fail_percentage'),
@@ -222,7 +222,7 @@ def create_pdf_report(ulog: ULog, multi_instance: int, output_plot_filename: str
        data_plot = CheckFlagsPlot(
            status_flags_time, estimator_status_flags, [['reject_hor_vel', 'reject_hor_pos'], ['reject_ver_vel', 'reject_ver_pos',
                                                                               'reject_hagl'],
-                                       ['fs_bad_mag_x', 'fs_bad_mag_y', 'fs_bad_mag_z',
+                                       ['reject_mag_x', 'reject_mag_y', 'reject_mag_z',
                                        'reject_yaw'], ['reject_airspeed'], ['reject_sideslip'],
                                       ['reject_optflow_x',
                                        'reject_optflow_y']], x_label='time (sec)',
@@ -1 +1 @@
-symforce>=0.9.0
+symforce>=0.5.0
@@ -10,7 +10,7 @@ sitl_num=2
 [ -n "$1" ] && sitl_num="$1"

 SCRIPT_DIR="$( cd "$( dirname "${BASH_SOURCE[0]}" )" && pwd )"
-src_path="$SCRIPT_DIR/../../"
+src_path="$SCRIPT_DIR/.."

 build_path=${src_path}/build/px4_sitl_default

@@ -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=2944
 CONFIG_LIBC_FLOATINGPOINT=y
 CONFIG_LIBC_LONG_LONG=y
 CONFIG_LIBC_MAX_EXITFUNS=1
@@ -20,7 +20,6 @@ CONFIG_DRIVERS_IRLOCK=y
 CONFIG_COMMON_LIGHT=y
 CONFIG_COMMON_MAGNETOMETER=y
 CONFIG_COMMON_OPTICAL_FLOW=y
-CONFIG_COMMON_OSD=y
 CONFIG_DRIVERS_PWM_OUT=y
 CONFIG_BOARD_UAVCAN_INTERFACES=1
 CONFIG_DRIVERS_UAVCANNODE=y
@@ -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
@@ -17,17 +17,19 @@ 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 ARKV6X001000 ARKV6X001001 ARKV6X001002 ARKV6X001003 ARKV6X001004 ARKV6X001005 ARKV6X001006 ARKV6X001007
+if ver hwtypecmp ARKV6X000000 ARKV6X001000 ARKV6X002000 ARKV6X003000 ARKV6X004000 ARKV6X005000 ARKV6X006000 ARKV6X007000
 then
-	param set-default SYS_USE_IO 0
-else
-	param set-default SYS_USE_IO 1
+	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

 safety_button start
@@ -47,14 +47,29 @@ then
 	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
@@ -7,7 +7,6 @@
 #
 # CONFIG_DEV_CONSOLE is not set
 # CONFIG_DISABLE_PSEUDOFS_OPERATIONS is not set
-# CONFIG_DISABLE_PTHREAD is not set
 # CONFIG_SPI_EXCHANGE is not set
 # CONFIG_STM32H7_SYSCFG is not set
 CONFIG_ARCH="arm"
@@ -49,14 +48,13 @@ CONFIG_HAVE_CXX=y
 CONFIG_HAVE_CXXINITIALIZE=y
 CONFIG_IDLETHREAD_STACKSIZE=750
 CONFIG_INIT_ENTRYPOINT="bootloader_main"
-CONFIG_INIT_STACKSIZE=3094
+CONFIG_INIT_STACKSIZE=2944
 CONFIG_LIBC_FLOATINGPOINT=y
 CONFIG_LIBC_LONG_LONG=y
 CONFIG_LIBC_STRERROR=y
 CONFIG_MEMSET_64BIT=y
 CONFIG_MEMSET_OPTSPEED=y
 CONFIG_PREALLOC_TIMERS=50
-CONFIG_PTHREAD_MUTEX_ROBUST=y
 CONFIG_PTHREAD_STACK_MIN=512
 CONFIG_RAM_SIZE=245760
 CONFIG_RAM_START=0x20010000
@@ -387,7 +387,7 @@

 #define GPIO_UART7_RX    GPIO_UART7_RX_4    /* PF6  */
 #define GPIO_UART7_TX    GPIO_UART7_TX_3    /* PE8  */
-#define GPIO_UART7_RTS   GPIO_UART7_RTS_2   /* PF8  */
+#define GPIO_UART7_RTS   (GPIO_UART7_RTS_2 | GPIO_PULLDOWN)   /* PF8  */
 #define GPIO_UART7_CTS   (GPIO_UART7_CTS_1 | GPIO_PULLDOWN)   /* PE10 */

 #define GPIO_UART8_RX    GPIO_UART8_RX_1    /* PE0 */
@@ -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=2944
 CONFIG_IOB_NBUFFERS=24
 CONFIG_IOB_THROTTLE=0
 CONFIG_IPCFG_BINARY=y
@@ -291,9 +291,8 @@ CONFIG_UART4_RXBUFSIZE=600
 CONFIG_UART4_TXBUFSIZE=1500
 CONFIG_UART5_IFLOWCONTROL=y
 CONFIG_UART5_OFLOWCONTROL=y
-CONFIG_UART7_RXBUFSIZE=1500
 CONFIG_UART5_RXDMA=y
-CONFIG_UART5_TXBUFSIZE=10000
+CONFIG_UART5_TXBUFSIZE=3000
 CONFIG_UART5_TXDMA=y
 CONFIG_UART7_BAUD=57600
 CONFIG_UART7_IFLOWCONTROL=y
@@ -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

@@ -252,7 +247,6 @@
 /* PWM
 */
 #define DIRECT_PWM_OUTPUT_CHANNELS   8
-#define BOARD_PWM_FREQ	             1024000

 #define GPIO_FMU_CH1                    /* PI0  */ (GPIO_INPUT|GPIO_PULLDOWN|GPIO_PORTI|GPIO_PIN0)
 #define GPIO_FMU_CH2                    /* PH12 */ (GPIO_INPUT|GPIO_PULLDOWN|GPIO_PORTH|GPIO_PIN12)
@@ -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
 };

 /************************************************************************************
@@ -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);
@@ -82,9 +82,7 @@
 #define rDMAR(_tmr)     REG(_tmr, STM32_GTIM_DMAR_OFFSET)
 #define rBDTR(_tmr)     REG(_tmr, STM32_ATIM_BDTR_OFFSET)

-#if !defined(BOARD_PWM_FREQ)
-#define BOARD_PWM_FREQ 1000000
-#endif
+#define BOARD_SPIX_SYNC_PWM_FREQ 1024000

 unsigned
 spix_sync_timer_get_period(unsigned timer)
@@ -129,11 +127,11 @@ static void spix_sync_timer_init_timer(unsigned timer, unsigned rate)
 		 * Otherwise, other frequencies are attainable by adjusting .clock_freq accordingly.
 		 */

-		rPSC(timer) = (spix_sync_timers[timer].clock_freq / BOARD_PWM_FREQ) - 1;
+		rPSC(timer) = (spix_sync_timers[timer].clock_freq / BOARD_SPIX_SYNC_PWM_FREQ) - 1;

 		/* configure the timer to update at the desired rate */

-		rARR(timer) = (BOARD_PWM_FREQ / rate) - 1;
+		rARR(timer) = (BOARD_SPIX_SYNC_PWM_FREQ / rate) - 1;

 		/* generate an update event; reloads the counter and all registers */
 		rEGR(timer) = GTIM_EGR_UG;
@@ -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=2944
 CONFIG_LIBC_FLOATINGPOINT=y
 CONFIG_LIBC_LONG_LONG=y
 CONFIG_LIBC_MAX_EXITFUNS=1
@@ -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=2944
 CONFIG_IOB_NBUFFERS=24
 CONFIG_IOB_THROTTLE=0
 CONFIG_LIBC_LONG_LONG=y
@@ -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=2944
 CONFIG_LIBC_FLOATINGPOINT=y
 CONFIG_LIBC_LONG_LONG=y
 CONFIG_LIBC_MAX_EXITFUNS=1
@@ -140,15 +140,7 @@

 #define PX4_PWM_ALTERNATE_RANGES
 #define PWM_LOWEST_MIN 0
-#define PWM_MOTOR_OFF	0
-#define PWM_SERVO_STOP	0
-#define PWM_DEFAULT_MIN 20
-#define PWM_HIGHEST_MIN 0
 #define PWM_HIGHEST_MAX 255
-#define PWM_DEFAULT_MAX 255
-#define PWM_LOWEST_MAX 255
-#define PWM_DEFAULT_TRIM 1500
-

 /* High-resolution timer */
 #define HRT_TIMER		8	/* use timer8 for the HRT */
@@ -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=2944
 CONFIG_LIBC_FLOATINGPOINT=y
 CONFIG_LIBC_LONG_LONG=y
 CONFIG_LIBC_MAX_EXITFUNS=1
@@ -141,15 +141,7 @@

 #define PX4_PWM_ALTERNATE_RANGES
 #define PWM_LOWEST_MIN 0
-#define PWM_MOTOR_OFF	0
-#define PWM_SERVO_STOP	0
-#define PWM_DEFAULT_MIN 20
-#define PWM_HIGHEST_MIN 0
 #define PWM_HIGHEST_MAX 255
-#define PWM_DEFAULT_MAX 255
-#define PWM_LOWEST_MAX 255
-#define PWM_DEFAULT_TRIM 1500
-

 /* High-resolution timer */
 #define HRT_TIMER		8	/* use timer8 for the HRT */
@@ -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=2944
 CONFIG_LIBC_FLOATINGPOINT=y
 CONFIG_LIBC_LONG_LONG=y
 CONFIG_LIBC_STRERROR=y
@@ -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=2944
 CONFIG_LIBC_FLOATINGPOINT=y
 CONFIG_LIBC_LONG_LONG=y
 CONFIG_LIBC_MAX_EXITFUNS=1
@@ -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=2944
 CONFIG_LIBC_FLOATINGPOINT=y
 CONFIG_LIBC_LONG_LONG=y
 CONFIG_LIBC_STRERROR=y
@@ -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=2944
 CONFIG_LIBC_LONG_LONG=y
 CONFIG_LIBC_MAX_EXITFUNS=1
 CONFIG_LIBC_STRERROR=y
@@ -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=2944
 CONFIG_LIBC_LONG_LONG=y
 CONFIG_LIBC_MAX_EXITFUNS=1
 CONFIG_LIBC_STRERROR=y
@@ -14,6 +14,4 @@ param set-default SENS_EN_THERMAL 0

 param set-default -s SENS_TEMP_ID 2621474

-param set-default SYS_USE_IO 1
-
 set IOFW "/etc/extras/cubepilot_io-v2_default.bin"
@@ -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=2944
 CONFIG_LIBC_FLOATINGPOINT=y
 CONFIG_LIBC_LONG_LONG=y
 CONFIG_LIBC_STRERROR=y
@@ -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=2944
 CONFIG_LIBC_FLOATINGPOINT=y
 CONFIG_LIBC_LONG_LONG=y
 CONFIG_LIBC_MAX_EXITFUNS=1
@@ -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=2944
 CONFIG_LIBC_LONG_LONG=y
 CONFIG_LIBC_MAX_EXITFUNS=1
 CONFIG_LIBC_STRERROR=y
@@ -14,6 +14,4 @@ param set-default SENS_EN_THERMAL 0

 param set-default -s SENS_TEMP_ID 2621474

-param set-default SYS_USE_IO 1
-
 set IOFW "/etc/extras/cubepilot_io-v2_default.bin"
@@ -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
@@ -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=2944
 CONFIG_LIBC_FLOATINGPOINT=y
 CONFIG_LIBC_LONG_LONG=y
 CONFIG_LIBC_STRERROR=y
@@ -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=2944
 CONFIG_LIBC_FLOATINGPOINT=y
 CONFIG_LIBC_LONG_LONG=y
 CONFIG_LIBC_MAX_EXITFUNS=1
@@ -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=2944
 CONFIG_LIBC_LONG_LONG=y
 CONFIG_LIBC_MAX_EXITFUNS=1
 CONFIG_LIBC_STRERROR=y
@@ -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
 	}),
 };
@@ -13,6 +13,4 @@ param set-default BAT2_A_PER_V 17
 # Disable IMU thermal control
 param set-default SENS_EN_THERMAL 0

-param set-default SYS_USE_IO 1
-
 set IOFW "/etc/extras/cubepilot_io-v2_default.bin"
@@ -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=2944
 CONFIG_LIBC_FLOATINGPOINT=y
 CONFIG_LIBC_LONG_LONG=y
 CONFIG_LIBC_MAX_EXITFUNS=1
@@ -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=2944
 CONFIG_LIBC_MAX_EXITFUNS=1
 CONFIG_LIBC_STRERROR=y
 CONFIG_MEMSET_64BIT=y
@@ -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=2944
 CONFIG_LIBC_LONG_LONG=y
 CONFIG_LIBC_MAX_EXITFUNS=1
 CONFIG_LIBC_STRERROR=y
@@ -11,5 +11,3 @@ param set-default BAT2_A_PER_V 36.367515152

 # Enable IMU thermal control
 param set-default SENS_EN_THERMAL 1
-
-param set-default SYS_USE_IO 1
@@ -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=2944
 CONFIG_LIBC_FLOATINGPOINT=y
 CONFIG_LIBC_LONG_LONG=y
 CONFIG_LIBC_STRERROR=y
@@ -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=2944
 CONFIG_LIBC_FLOATINGPOINT=y
 CONFIG_LIBC_LONG_LONG=y
 CONFIG_LIBC_MAX_EXITFUNS=1
@@ -10,10 +10,8 @@ CONFIG_BOARD_SERIAL_RC="/dev/ttyS4"
 CONFIG_DRIVERS_ADC_BOARD_ADC=y
 CONFIG_DRIVERS_BAROMETER_BMP280=y
 CONFIG_DRIVERS_DSHOT=y
-CONFIG_DRIVERS_GPS=y
 CONFIG_DRIVERS_IMU_INVENSENSE_ICM20689=y
 CONFIG_DRIVERS_IMU_INVENSENSE_MPU6000=y
-CONFIG_DRIVERS_OSD_ATXXXX=y
 CONFIG_DRIVERS_PWM_OUT=y
 CONFIG_DRIVERS_RC_INPUT=y
 CONFIG_DRIVERS_TELEMETRY_FRSKY_TELEMETRY=y
@@ -33,6 +31,5 @@ CONFIG_MODULES_MC_RATE_CONTROL=y
 CONFIG_MODULES_NAVIGATOR=y
 CONFIG_MODULES_RC_UPDATE=y
 CONFIG_MODULES_SENSORS=y
+CONFIG_MODULES_UXRCE_DDS_CLIENT=y
 CONFIG_SYSTEMCMDS_PARAM=y
-CONFIG_SYSTEMCMDS_TOP=y
-CONFIG_SYSTEMCMDS_DMESG=y
@@ -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=2944
 CONFIG_LIBC_FLOATINGPOINT=y
 CONFIG_LIBC_LONG_LONG=y
 CONFIG_LIBC_MAX_EXITFUNS=1
@@ -113,7 +113,6 @@
 /* RC Serial port */

 #define RC_SERIAL_PORT                     "/dev/ttyS4"
-#define RC_SERIAL_SINGLEWIRE

 #define GPIO_RSSI_IN                       /* PC5  */ (GPIO_INPUT|GPIO_PULLUP|GPIO_PORTC|GPIO_PIN5)

@@ -130,8 +129,6 @@
 		GPIO_RSSI_IN,                \
 	}

-#define BOARD_ENABLE_CONSOLE_BUFFER
-
 #define BOARD_NUM_IO_TIMERS 4

 __BEGIN_DECLS
@@ -252,5 +252,9 @@ __EXPORT int board_app_initialize(uintptr_t arg)

 #endif

+	/* Configure the HW based on the manifest */
+
+	px4_platform_configure();
+
 	return OK;
 }
@@ -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=2944
 CONFIG_LIBC_FLOATINGPOINT=y
 CONFIG_LIBC_LONG_LONG=y
 CONFIG_LIBC_STRERROR=y
@@ -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=2944
 CONFIG_LIBC_FLOATINGPOINT=y
 CONFIG_LIBC_LONG_LONG=y
 CONFIG_LIBC_MAX_EXITFUNS=1
@@ -47,7 +47,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=2944
 CONFIG_LIBC_FLOATINGPOINT=y
 CONFIG_LIBC_LONG_LONG=y
 CONFIG_LIBC_STRERROR=y
@@ -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=2944
 CONFIG_LIBC_FLOATINGPOINT=y
 CONFIG_LIBC_LONG_LONG=y
 CONFIG_LIBC_MAX_EXITFUNS=1
@@ -47,7 +47,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=2944
 CONFIG_LIBC_FLOATINGPOINT=y
 CONFIG_LIBC_LONG_LONG=y
 CONFIG_LIBC_STRERROR=y
@@ -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=2944
 CONFIG_LIBC_FLOATINGPOINT=y
 CONFIG_LIBC_LONG_LONG=y
 CONFIG_LIBC_MAX_EXITFUNS=1
@@ -9,7 +9,5 @@ param set-default BAT2_V_DIV 18.1
 param set-default BAT1_A_PER_V 36.367515152
 param set-default BAT2_A_PER_V 36.367515152

-param set-default SYS_USE_IO 1
-
 rgbled_pwm start
 safety_button start
@@ -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=2944
 CONFIG_LIBC_FLOATINGPOINT=y
 CONFIG_LIBC_LONG_LONG=y
 CONFIG_LIBC_MAX_EXITFUNS=1
@@ -30,7 +30,7 @@ CONFIG_FDCLONE_STDIO=y
 CONFIG_HAVE_CXX=y
 CONFIG_HAVE_CXXINITIALIZE=y
 CONFIG_IDLETHREAD_STACKSIZE=4096
-CONFIG_INIT_STACKSIZE=3094
+CONFIG_INIT_STACKSIZE=2944
 CONFIG_LIBC_FLOATINGPOINT=y
 CONFIG_LIBC_LONG_LONG=y
 CONFIG_LIBC_STRERROR=y
@@ -104,7 +104,7 @@ CONFIG_I2C=y
 CONFIG_I2C_RESET=y
 CONFIG_IDLETHREAD_STACKSIZE=750
 CONFIG_INIT_ENTRYPOINT="nsh_main"
-CONFIG_INIT_STACKSIZE=3094
+CONFIG_INIT_STACKSIZE=2944
 CONFIG_LIBC_FLOATINGPOINT=y
 CONFIG_LIBC_LONG_LONG=y
 CONFIG_LIBC_MAX_EXITFUNS=1
@@ -47,7 +47,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=2944
 CONFIG_LIBC_FLOATINGPOINT=y
 CONFIG_LIBC_LONG_LONG=y
 CONFIG_LIBC_STRERROR=y
@@ -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=2944
 CONFIG_LIBC_FLOATINGPOINT=y
 CONFIG_LIBC_LONG_LONG=y
 CONFIG_LIBC_MAX_EXITFUNS=1
@@ -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=2944
 CONFIG_LIBC_FLOATINGPOINT=y
 CONFIG_LIBC_LONG_LONG=y
 CONFIG_LIBC_STRERROR=y
@@ -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=2944
 CONFIG_LIBC_FLOATINGPOINT=y
 CONFIG_LIBC_LONG_LONG=y
 CONFIG_LIBC_MAX_EXITFUNS=1
@@ -47,7 +47,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=2944
 CONFIG_LIBC_FLOATINGPOINT=y
 CONFIG_LIBC_LONG_LONG=y
 CONFIG_LIBC_STRERROR=y
@@ -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=2944
 CONFIG_LIBC_FLOATINGPOINT=y
 CONFIG_LIBC_LONG_LONG=y
 CONFIG_LIBC_MAX_EXITFUNS=1
@@ -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=2944
 CONFIG_LIBC_FLOATINGPOINT=y
 CONFIG_LIBC_LONG_LONG=y
 CONFIG_LIBC_MAX_EXITFUNS=1
@@ -14,10 +14,10 @@ CONFIG_COMMON_DIFFERENTIAL_PRESSURE=y
 CONFIG_COMMON_DISTANCE_SENSOR=y
 CONFIG_DRIVERS_DSHOT=y
 CONFIG_DRIVERS_GPS=y
-# CONFIG_DRIVERS_IMU_BOSCH_BMI088=y
-# CONFIG_DRIVERS_IMU_INVENSENSE_ICM20602=y
-# CONFIG_DRIVERS_IMU_INVENSENSE_ICM20649=y
-# CONFIG_DRIVERS_IMU_INVENSENSE_ICM20948=y
+CONFIG_DRIVERS_IMU_BOSCH_BMI088=y
+CONFIG_DRIVERS_IMU_INVENSENSE_ICM20602=y
+CONFIG_DRIVERS_IMU_INVENSENSE_ICM20649=y
+CONFIG_DRIVERS_IMU_INVENSENSE_ICM20948=y
 CONFIG_DRIVERS_IMU_INVENSENSE_ICM42688P=y
 CONFIG_DRIVERS_IRLOCK=y
 CONFIG_COMMON_LIGHT=y
@@ -29,13 +29,11 @@ CONFIG_DRIVERS_POWER_MONITOR_INA226=y
 CONFIG_DRIVERS_POWER_MONITOR_VOXLPM=y
 CONFIG_DRIVERS_PWM_OUT=y
 CONFIG_DRIVERS_RC_INPUT=y
-CONFIG_DRIVERS_RPM=y
 CONFIG_COMMON_TELEMETRY=y
 CONFIG_DRIVERS_UAVCAN=y
 CONFIG_BOARD_UAVCAN_INTERFACES=1
 CONFIG_BOARD_UAVCAN_TIMER_OVERRIDE=2
 CONFIG_MODULES_AIRSPEED_SELECTOR=y
-# CONFIG_MODULES_ATTITUDE_ESTIMATOR_Q=y
 CONFIG_MODULES_CAMERA_FEEDBACK=y
 CONFIG_MODULES_COMMANDER=y
 CONFIG_MODULES_CONTROL_ALLOCATOR=y
@@ -54,7 +52,6 @@ CONFIG_MODULES_GYRO_FFT=y
 CONFIG_MODULES_LAND_DETECTOR=y
 CONFIG_MODULES_LANDING_TARGET_ESTIMATOR=y
 CONFIG_MODULES_LOAD_MON=y
-# CONFIG_MODULES_LOCAL_POSITION_ESTIMATOR=y
 CONFIG_MODULES_LOGGER=y
 CONFIG_MODULES_MAG_BIAS_ESTIMATOR=y
 CONFIG_MODULES_MANUAL_CONTROL=y
@@ -73,7 +70,6 @@ CONFIG_MODULES_TEMPERATURE_COMPENSATION=y
 CONFIG_MODULES_UXRCE_DDS_CLIENT=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
@@ -20,8 +20,8 @@ icm42688p -s -b 1 -R 12 start
 # Internal SPI2 ICM-42688
 icm42688p -s -b 2 -R 12 start

-# Don't start Internal I2C mag
-# bmm150 -I start
+# Internal I2C mag
+bmm150 -I start

 # Internal I2C baro
 icp201xx -I start
@@ -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=2944
 CONFIG_LIBC_FLOATINGPOINT=y
 CONFIG_LIBC_LONG_LONG=y
 CONFIG_LIBC_STRERROR=y
@@ -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=2944
 CONFIG_IOB_NBUFFERS=24
 CONFIG_IOB_NCHAINS=24
 CONFIG_LIBC_FLOATINGPOINT=y
@@ -68,18 +68,4 @@ else()
 			nuttx_drivers # sdio
 			px4_layer
 	)
-
-	set(COMMON_MODALAI_SRC_DIR ${PX4_SOURCE_DIR}/boards/modalai/src)
-	set(MODALAI_SYSTEMCMD_SRC_DIR ${COMMON_MODALAI_SRC_DIR}/systemcmds/modalai)
-
-	px4_add_module(
-		MODULE systemcmds__modalai
-		MAIN modalai
-		COMPILE_FLAGS
-		SRCS
-			${MODALAI_SYSTEMCMD_SRC_DIR}/modalai_fc-v2.c
-			${MODALAI_SYSTEMCMD_SRC_DIR}/modalai_fc-v1.c
-			${MODALAI_SYSTEMCMD_SRC_DIR}/modalai.c
-		DEPENDS
-	)
 endif()
@@ -91,12 +91,6 @@
 #  define GPIO_nLED_GREEN      /* PE4 */  (GPIO_OUTPUT|GPIO_OPENDRAIN|GPIO_SPEED_50MHz|GPIO_OUTPUT_SET|GPIO_PORTE|GPIO_PIN4)
 #  define GPIO_nLED_BLUE       /* PE5 */  (GPIO_OUTPUT|GPIO_OPENDRAIN|GPIO_SPEED_50MHz|GPIO_OUTPUT_SET|GPIO_PORTE|GPIO_PIN5)

-// GPIO_nLED_2_RED/ GPIO_nLED_2_GREEN /GPIO_nLED_2_BLUE are for v1 LED tests
-
-#  define GPIO_nLED_2_RED      /* PI0 */  (GPIO_OUTPUT|GPIO_OPENDRAIN|GPIO_SPEED_50MHz|GPIO_OUTPUT_SET|GPIO_PORTI|GPIO_PIN0)
-#  define GPIO_nLED_2_GREEN    /* PH11 */ (GPIO_OUTPUT|GPIO_OPENDRAIN|GPIO_SPEED_50MHz|GPIO_OUTPUT_SET|GPIO_PORTH|GPIO_PIN11)
-#  define GPIO_nLED_2_BLUE     /* PA2 */  (GPIO_OUTPUT|GPIO_OPENDRAIN|GPIO_SPEED_50MHz|GPIO_OUTPUT_SET|GPIO_PORTA|GPIO_PIN2)
-
 #  define BOARD_HAS_CONTROL_STATUS_LEDS      1
 #  define BOARD_OVERLOAD_LED     LED_RED
 #  define BOARD_ARMED_STATE_LED  LED_BLUE
@@ -214,12 +208,6 @@

 #define CAN1_SILENT                       /* PD15 */  (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_2MHz|GPIO_OUTPUT_CLEAR|GPIO_PORTD|GPIO_PIN15)

-/* For primary/backup signaling with VOXL, 2 pins on J4 are exposed */
-// GPIO_VOXL_STATUS_OUT/ GPIO_VOXL_STATUS_IN are for v1 Spare MSS Communications Interface and J4 tests
-
-#define GPIO_VOXL_STATUS_OUT            /* PE4 */ (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_2MHz|GPIO_OUTPUT_CLEAR|GPIO_PORTE|GPIO_PIN4)
-#define GPIO_VOXL_STATUS_IN             /* PE3 */ (GPIO_INPUT|GPIO_FLOAT|GPIO_PORTE|GPIO_PIN3)
-
 /* Define True logic Power Control in arch agnostic form */

 #define VDD_3V3_SPEKTRUM_POWER_EN(on_true) px4_arch_gpiowrite(GPIO_VDD_3V3_SPEKTRUM_POWER_EN, (on_true))
@@ -350,8 +338,8 @@

 #define BOARD_NUM_IO_TIMERS 5

-// J5 USART5 TELEM2 Port next to PWM connector
-#define MODAL_IO_DEFAULT_PORT                     "/dev/ttyS4"
+// J1 / TELEM1 / USART7
+#define MODAL_IO_DEFAULT_PORT                     "/dev/ttyS6"

 __BEGIN_DECLS

@@ -1,131 +0,0 @@
-#include <px4_platform_common/module.h>
-
-#include "chip.h"
-#include "stm32_gpio.h"
-#include "board_config.h"
-
-#include <nuttx/board.h>
-#include <arch/board/board.h>
-
-// v2
-#ifdef CONFIG_ARCH_CHIP_STM32H753II		// chip on M0087
-#include "modalai_fc-v2.h"
-#define MODALAI_FC_V2 1
-#else
-#include "modalai_fc-v1.h"
-#endif
-
-__EXPORT int modalai_main(int argc, char *argv[]);
-
-int modalai_main(int argc, char *argv[])
-{
-	int hw_rev = board_get_hw_revision();
-	int hw_ver = board_get_hw_version();
-
-	eHW_TYPE hw_type = eHwNone;
-
-#ifdef MODALAI_FC_V2
-
-	if (hw_rev == 0 && hw_ver == 3) {	// (should be hw_rev == 1 && hw_ver == 3) eventually...
-		hw_type = eM0087;
-
-	} else if (hw_rev == 0 && hw_ver == 3) {
-		hw_type = eM0079;
-
-	} else {
-		return -1;
-
-	}
-
-#else
-
-	if (hw_rev == 6 && hw_ver == 0) {
-		hw_type = eM0018;
-
-	} else if (hw_rev == 0 && hw_ver == 1) {
-		hw_type = eM0019;
-
-	} else if (hw_rev == 0 && hw_ver == 2) {
-		hw_type = eM0051;
-
-	} else {
-		return -1;
-
-	}
-
-#endif
-
-	if (argc <= 1) {
-#ifdef MODALAI_FC_V2
-		modalai_print_usage_v2();
-#else
-		modalai_print_usage_v1();
-#endif
-		return 1;
-	}
-
-	if (!strcmp(argv[1], "led")) {
-#ifdef MODALAI_FC_V2
-		return modalai_led_test_v2();
-#else
-		return modalai_led_test_v1();
-#endif
-
-	} else if (!strcmp(argv[1], "con")) {
-		if (argc <= 2) {
-			PRINT_MODULE_USAGE_COMMAND("con");
-			PRINT_MODULE_USAGE_ARG("<1,4,5,6,7,9,10,12,13>", "Connector ID", false);
-			PRINT_MODULE_USAGE_ARG("<uint>", "Pin Number", false);
-			PRINT_MODULE_USAGE_ARG("0 | 1", "<output state> (defaults to 0)", false);
-			return 1;
-		}
-
-		uint8_t con = 0;
-		uint8_t pin = 0;
-		bool state = false;
-
-		if (argc > 2) {
-			con = atoi(argv[2]);
-		}
-
-		if (argc > 3) {
-			pin = atoi(argv[3]);
-		}
-
-		if (argc > 4) {
-			state = atoi(argv[4]);
-		}
-
-#ifdef MODALAI_FC_V2
-		return modalai_con_gpio_test_v2(con, pin, state);
-#else
-		return modalai_con_gpio_test_v1(con, pin, state);
-#endif
-
-	} else if (!strcmp(argv[1], "buzz")) {
-
-#ifdef MODALAI_FC_V2
-		return modalai_buzz_test_v2(hw_type);
-#else
-		return modalai_buzz_test_v1(hw_type);
-#endif
-
-
-
-	} else if (!strcmp(argv[1], "detect")) {
-#ifdef MODALAI_FC_V2
-		modalai_hw_detect_v2(hw_type);
-#else
-		modalai_hw_detect_v1(hw_type);
-#endif
-
-		return 0;
-	}
-
-#ifdef MODALAI_FC_V2
-	modalai_print_usage_v2();
-#else
-	modalai_print_usage_v1();
-#endif
-	return -EINVAL;
-}
@@ -1,913 +0,0 @@
-#include <px4_platform_common/module.h>
-
-
-#include "chip.h"
-#include "stm32_gpio.h"
-#include "board_config.h"
-
-#include <nuttx/board.h>
-#include <arch/board/board.h>
-
-// v1
-#ifndef CONFIG_ARCH_CHIP_STM32H743ZI
-
-#include "modalai_fc-v1.h"
-
-
-
-
-void modalai_print_usage_v1(void)
-{
-	PRINT_MODULE_DESCRIPTION("ModalAI Test utility\n");
-
-	PRINT_MODULE_USAGE_NAME_SIMPLE("modalai", "command");
-
-	PRINT_MODULE_USAGE_COMMAND_DESCR("led", "LED Test");
-	PRINT_MODULE_USAGE_COMMAND_DESCR("con", "Connector Output Test (as GPIO)");
-	PRINT_MODULE_USAGE_COMMAND_DESCR("buzz", "Automated buzz out test");
-	PRINT_MODULE_USAGE_COMMAND_DESCR("detect", "Detect board type");
-}
-
-void modalai_print_usage_con_gpio_test_v1(void)
-{
-	PRINT_MODULE_USAGE_NAME_SIMPLE("modalai con", "command");
-	PRINT_MODULE_USAGE_COMMAND_DESCR("1",  "W<3,6> R<2,6>, <0-1>");
-	PRINT_MODULE_USAGE_COMMAND_DESCR("4",  "W<2-4,6-7> R<8>, <0-1>");
-	PRINT_MODULE_USAGE_COMMAND_DESCR("5",  "W<2-5>,   <0-1>");
-	PRINT_MODULE_USAGE_COMMAND_DESCR("6",  "W<2-5>,   <0-1>");
-	PRINT_MODULE_USAGE_COMMAND_DESCR("7",  "W<2-9>,   <0-1>");
-	PRINT_MODULE_USAGE_COMMAND_DESCR("9",  "R<2>");
-	PRINT_MODULE_USAGE_COMMAND_DESCR("10", "W<2-5>,   <0-1>");
-	PRINT_MODULE_USAGE_COMMAND_DESCR("12", "W<1-3>,   <0-1>");
-	PRINT_MODULE_USAGE_COMMAND_DESCR("13", "W<3-5>,   <0-1>");
-}
-
-int modalai_led_test_v1(void)
-{
-	PX4_INFO("Running led test");
-
-	stm32_configgpio(GPIO_nLED_RED);
-	stm32_configgpio(GPIO_nLED_GREEN);
-	stm32_configgpio(GPIO_nLED_BLUE);
-
-	int i = 0;
-
-	stm32_configgpio(GPIO_nLED_2_RED);
-	stm32_configgpio(GPIO_nLED_2_GREEN);
-	stm32_configgpio(GPIO_nLED_2_BLUE);
-
-	for (i = 0; i < 3; i++) {
-		usleep(1000 * 100);
-		stm32_gpiowrite(GPIO_nLED_RED, false);
-
-		stm32_gpiowrite(GPIO_nLED_2_RED, false);
-		usleep(1000 * 100);
-		stm32_gpiowrite(GPIO_nLED_RED, true);
-		stm32_gpiowrite(GPIO_nLED_2_RED, true);
-
-		usleep(1000 * 100);
-		stm32_gpiowrite(GPIO_nLED_GREEN, false);
-		stm32_gpiowrite(GPIO_nLED_2_GREEN, false);
-		usleep(1000 * 100);
-		stm32_gpiowrite(GPIO_nLED_GREEN, true);
-		stm32_gpiowrite(GPIO_nLED_2_GREEN, true);
-
-		usleep(1000 * 100);
-		stm32_gpiowrite(GPIO_nLED_BLUE, false);
-		stm32_gpiowrite(GPIO_nLED_2_BLUE, false);
-		usleep(1000 * 100);
-		stm32_gpiowrite(GPIO_nLED_BLUE, true);
-		stm32_gpiowrite(GPIO_nLED_2_BLUE, true);
-	}
-
-	return OK;
-}
-
-int modalai_con_gpio_test_v1(uint8_t con, uint8_t pin, bool state)
-{
-	// validate
-	switch (con) {
-	// Primary MSS Communications Interface
-	case 1:
-		switch (pin) {
-		case 2:
-			stm32_configgpio(J1_PIN2_IN);
-			state = stm32_gpioread(J1_PIN2_IN);
-			break;
-
-		case 3:
-			stm32_configgpio(J1_PIN3);
-			stm32_gpiowrite(J1_PIN3, state);
-			break;
-
-		case 4:
-			stm32_configgpio(J1_PIN4);
-			stm32_gpiowrite(J1_PIN4, state);
-			break;
-
-		case 6:
-			stm32_configgpio(J1_PIN6_IN);
-			state = stm32_gpioread(J1_PIN6_IN);
-			break;
-
-		default:
-			modalai_print_usage_con_gpio_test_v1();
-			return -1;
-		}
-
-		break;
-
-	// STM JTAG Programming Header
-	case 2:
-		modalai_print_usage_con_gpio_test_v1();
-		return -1;
-
-	// USB 2.0 Full-Speed Downstream Device Port
-	case 3:
-		modalai_print_usage_con_gpio_test_v1();
-		return -1;
-
-	// Spare MSS Communications Interface
-	case 4:
-		switch (pin) {
-		case 2:
-			stm32_configgpio(J4_PIN2);
-			stm32_gpiowrite(J4_PIN2, state);
-			break;
-
-		case 3:
-			stm32_configgpio(J4_PIN3);
-			stm32_gpiowrite(J4_PIN3, state);
-			break;
-
-		case 4:
-			stm32_configgpio(J4_PIN4);
-			stm32_gpiowrite(J4_PIN4, state);
-			break;
-
-		case 6:
-			stm32_configgpio(J4_PIN6);
-			stm32_gpiowrite(J4_PIN6, state);
-			break;
-
-		case 7:
-			stm32_configgpio(J4_PIN7);
-			stm32_gpiowrite(J4_PIN7, state);
-			break;
-
-		case 8:
-			stm32_configgpio(J4_PIN8);
-			state = stm32_gpioread(J4_PIN8);
-			break;
-
-		default:
-			modalai_print_usage_con_gpio_test_v1();
-			return -1;
-		}
-
-		break;
-
-	// TELEMETRY CONNECTOR
-	case 5:
-		switch (pin) {
-		case 2:
-			stm32_configgpio(J5_PIN2);
-			stm32_gpiowrite(J5_PIN2, state);
-			break;
-
-		case 3:
-			stm32_configgpio(J5_PIN3);
-			stm32_gpiowrite(J5_PIN3, state);
-			break;
-
-		case 4:
-			stm32_configgpio(J5_PIN4);
-			stm32_gpiowrite(J5_PIN4, state);
-			break;
-
-		case 5:
-			stm32_configgpio(J5_PIN5);
-			stm32_gpiowrite(J5_PIN5, state);
-			break;
-
-		default:
-			modalai_print_usage_con_gpio_test_v1();
-			return -1;
-		}
-
-		break;
-
-	// EXPANSION CONNECTOR
-	case 6:
-		switch (pin) {
-		case 2:
-			stm32_configgpio(J6_PIN2);
-			stm32_gpiowrite(J6_PIN2, state);
-			break;
-
-		case 3:
-			stm32_configgpio(J6_PIN3);
-			stm32_gpiowrite(J6_PIN3, state);
-			break;
-
-		case 4:
-			stm32_configgpio(J6_PIN4);
-			stm32_gpiowrite(J6_PIN4, state);
-			break;
-
-		case 5:
-			stm32_configgpio(J6_PIN5);
-			stm32_gpiowrite(J6_PIN5, state);
-			break;
-
-		default:
-			modalai_print_usage_con_gpio_test_v1();
-			return -1;
-		}
-
-		break;
-
-	// PWM Output Connector
-	case 7:
-		switch (pin) {
-		case 2:
-			stm32_configgpio(J7_PIN2);
-			stm32_gpiowrite(J7_PIN2, state);
-			break;
-
-		case 3:
-			stm32_configgpio(J7_PIN3);
-			stm32_gpiowrite(J7_PIN3, state);
-			break;
-
-		case 4:
-			stm32_configgpio(J7_PIN4);
-			stm32_gpiowrite(J7_PIN4, state);
-			break;
-
-		case 5:
-			stm32_configgpio(J7_PIN5);
-			stm32_gpiowrite(J7_PIN5, state);
-			break;
-
-		case 6:
-			stm32_configgpio(J7_PIN6);
-			stm32_gpiowrite(J7_PIN6, state);
-			break;
-
-		case 7:
-			stm32_configgpio(J7_PIN7);
-			stm32_gpiowrite(J7_PIN7, state);
-			break;
-
-		case 8:
-			stm32_configgpio(J7_PIN8);
-			stm32_gpiowrite(J7_PIN8, state);
-			break;
-
-		case 9:
-			stm32_configgpio(J7_PIN9);
-			stm32_gpiowrite(J7_PIN9, state);
-			break;
-
-		default:
-			modalai_print_usage_con_gpio_test_v1();
-			return -1;
-		}
-
-		break;
-
-	// CAN 1 Peripheral Connector
-	case 8:
-		modalai_print_usage_con_gpio_test_v1();
-		return -1;
-
-	// PPM (RC) IN
-	case 9:
-		switch (pin) {
-		case 2:
-			stm32_configgpio(J9_PIN2_IN);
-			state = stm32_gpioread(J9_PIN2_IN);
-			break;
-
-		default:
-			modalai_print_usage_con_gpio_test_v1();
-			return -1;
-		}
-
-		break;
-
-	// GPS CONNECTOR
-	case 10:
-		switch (pin) {
-		case 2:
-			stm32_configgpio(J10_PIN2);
-			stm32_gpiowrite(J10_PIN2, state);
-			break;
-
-		case 3:
-			stm32_configgpio(J10_PIN3);
-			stm32_gpiowrite(J10_PIN3, state);
-			break;
-
-		case 4:
-			stm32_configgpio(J10_PIN4);
-			stm32_gpiowrite(J10_PIN4, state);
-			break;
-
-		case 5:
-			stm32_configgpio(J10_PIN5);
-			stm32_gpiowrite(J10_PIN5, state);
-			break;
-
-		default:
-			modalai_print_usage_con_gpio_test_v1();
-			return -1;
-		}
-
-		break;
-
-	// Micro SD Card Slot
-	case 11:
-		modalai_print_usage_con_gpio_test_v1();
-		return -1;
-
-	// Spektrum RC Input Connector
-	case 12:
-		switch (pin) {
-		case 1:
-			VDD_3V3_SPEKTRUM_POWER_EN(state);
-			break;
-
-		case 2:
-			__asm("nop");
-			stm32_configgpio(J12_PIN2);
-			stm32_gpiowrite(J12_PIN2, state);
-			//state = stm32_gpioread(J12_PIN2);
-			__asm("nop");
-			break;
-
-		case 3:
-			stm32_configgpio(J12_PIN3);
-			stm32_gpiowrite(J12_PIN3, state);
-			break;
-
-		default:
-			modalai_print_usage_con_gpio_test_v1();
-			return -1;
-		}
-
-		break;
-
-	// I2C DISPLAY / SPARE SENSOR CONNECTOR
-	case 13:
-		switch (pin) {
-		case 3:
-			stm32_configgpio(J13_PIN3);
-			stm32_gpiowrite(J13_PIN3, state);
-			break;
-
-		case 4:
-			stm32_configgpio(J13_PIN4);
-			stm32_gpiowrite(J13_PIN4, state);
-			break;
-
-		case 5:
-			stm32_configgpio(J13_PIN5);
-			stm32_gpiowrite(J13_PIN5, state);
-			break;
-
-		default:
-			modalai_print_usage_con_gpio_test_v1();
-			return -1;
-		}
-
-		break;
-	}
-
-	printf("GPIO - Con: %d, Pin: %d, State: %d\n", con, pin, state);
-	return OK;
-}
-
-bool test_pair(uint32_t output_pin, uint32_t input_pin)
-{
-
-	bool state = false;
-
-	stm32_gpiowrite(output_pin, true);
-	usleep(1000 * 10);
-	state = stm32_gpioread(input_pin);
-
-	if (state != true) {
-		return false;
-	}
-
-	usleep(1000 * 10);
-
-	stm32_gpiowrite(output_pin, false);
-	usleep(1000 * 10);
-	state = stm32_gpioread(input_pin);
-
-	if (state != false) {
-		return false;
-	}
-
-	return true;
-}
-
-bool modalai_test_pair(uint32_t output_pin, uint32_t input_pin)
-{
-
-	bool state = false;
-
-	stm32_gpiowrite(output_pin, true);
-	usleep(1000 * 10);
-	state = stm32_gpioread(input_pin);
-
-	if (state != true) {
-		return false;
-	}
-
-	usleep(1000 * 10);
-
-	stm32_gpiowrite(output_pin, false);
-	usleep(1000 * 10);
-	state = stm32_gpioread(input_pin);
-
-	if (state != false) {
-		return false;
-	}
-
-	return true;
-}
-
-int modalai_buzz_test_v1(eHW_TYPE hw_type)
-{
-	PX4_INFO("test: buzz");
-	usleep(1000 * 100 * 10);
-
-	if (hw_type == eM0018) {
-		PX4_INFO("Using Flight Core Config");
-
-	} else if (hw_type == eM0019) {
-		PX4_INFO("Using VOXL-Flight Config");
-
-	} else if (hw_type == eM0051) {
-		PX4_INFO("Using M0051 Config");
-
-	} else {
-		return -1;
-	}
-
-	if (hw_type == eM0018) {
-		PX4_INFO(">> Testing J1");
-		stm32_configgpio(J1_PIN2_IN); // 2 [in] to 4 [out]
-		stm32_configgpio(J1_PIN3); // 3 [out] to 6 [in]
-		stm32_configgpio(J1_PIN4); // 4 [out] to 2 [in]
-		stm32_configgpio(J1_PIN6_IN); // 6 [in] to 3 [out]
-
-		if (test_pair(J1_PIN4, J1_PIN2_IN)) {
-			PX4_INFO("PASS: J1P4-J1P2");
-
-		} else {
-			PX4_ERR("FAIL: J1P4-J1P2 ----------------------------------------");
-		}
-
-		if (test_pair(J1_PIN3, J1_PIN6_IN)) {
-			PX4_INFO("PASS: J1P3-J1P6");
-
-		} else {
-			PX4_ERR("FAIL: J1P3-J1P6 ----------------------------------------");
-		}
-
-	} else if (hw_type == eM0019) {
-		// NA on VOXL-Flight (internally routed)
-	}
-
-	if (hw_type == eM0018) {
-		PX4_INFO(">> Testing J4");
-		stm32_configgpio(J4_PIN2);    // 2 [out] 6 [in]
-		stm32_configgpio(J4_PIN3);    // 3 [out] 7 [in]
-		stm32_configgpio(J4_PIN4);    // 4 [out] 8 [in]
-		stm32_configgpio(J4_PIN6_IN); // 2 [out] 6 [in]
-		stm32_configgpio(J4_PIN7_IN); // 3 [out] 7 [in]
-		stm32_configgpio(J4_PIN8_IN); // 4 [out] 8 [in]
-
-		if (test_pair(J4_PIN2, J4_PIN6_IN)) {
-			PX4_INFO("PASS: J4P2-J4P6");
-
-		} else {
-			PX4_ERR("FAIL: J4P2-J4P6 ----------------------------------------");
-		}
-
-		if (test_pair(J4_PIN3, J4_PIN7_IN)) {
-			PX4_INFO("PASS: J4P3-J4P7");
-
-		} else {
-			PX4_ERR("FAIL: J4P3-J4P7 ----------------------------------------");
-		}
-
-		if (test_pair(J4_PIN4, J4_PIN8_IN)) {
-			PX4_INFO("PASS: J4P4-J4P8");
-
-		} else {
-			PX4_ERR("FAIL: J4P4-J4P8 ----------------------------------------");
-		}
-
-	} else if (hw_type == eM0019) {
-		PX4_INFO(">> Testing J1002");
-		stm32_configgpio(J1002_PIN2);    // 2 [out] 4 [in]
-		stm32_configgpio(J1002_PIN3);    // 3 [out] 6 [in]
-		stm32_configgpio(J1002_PIN4_IN); // 2 [out] 4 [in]
-		stm32_configgpio(J1002_PIN6_IN); // 3 [out] 6 [in]
-
-		if (test_pair(J1002_PIN2, J1002_PIN4_IN)) {
-			PX4_INFO("PASS: J1002P2-J1002P4");
-
-		} else {
-			PX4_ERR("FAIL: J1002P2-J1002P4 ----------------------------------------");
-		}
-
-		if (test_pair(J1002_PIN3, J1002_PIN6_IN)) {
-			PX4_INFO("PASS: J1002P3-J1002P6");
-
-		} else {
-			PX4_ERR("FAIL: J1002P3-J1002P6 ----------------------------------------");
-		}
-
-	}
-
-	if (hw_type == eM0018) {
-		PX4_INFO(">> Testing J5");
-		stm32_configgpio(J5_PIN2);    // 2 [out] 4 [in]
-		stm32_configgpio(J5_PIN3);    // 3 [out] 5 [in]
-		stm32_configgpio(J5_PIN4_IN); // 4 [in] 2 [out]
-		stm32_configgpio(J5_PIN5_IN); // 5 [in] 3 [out]
-
-		if (test_pair(J5_PIN2, J5_PIN4_IN)) {
-			PX4_INFO("PASS: J5P2-J5P4");
-
-		} else {
-			PX4_ERR("FAIL: J5P2-J5P4 ----------------------------------------");
-		}
-
-		if (test_pair(J5_PIN3, J5_PIN5_IN)) {
-			PX4_INFO("PASS: J5P3-J5P5");
-
-		} else {
-			PX4_ERR("FAIL: J5P3-J5P5 ----------------------------------------");
-		}
-
-	} else if (hw_type == eM0019) {
-		PX4_INFO(">> Testing J1010");
-		stm32_configgpio(J1010_PIN2);    // 2 [out] 4 [in]
-		stm32_configgpio(J1010_PIN3);    // 3 [out] 5 [in]
-		stm32_configgpio(J1010_PIN4_IN); // 4 [in] 2 [out]
-		stm32_configgpio(J1010_PIN5_IN); // 5 [in] 3 [out]
-
-		if (test_pair(J1010_PIN2, J1010_PIN4_IN)) {
-			PX4_INFO("PASS: J1010P2-J1010P4");
-
-		} else {
-			PX4_ERR("FAIL: J1010P2-J1010P4 ----------------------------------------");
-		}
-
-		if (test_pair(J1010_PIN3, J1010_PIN5_IN)) {
-			PX4_INFO("PASS: J1010P3-J1010P5");
-
-		} else {
-			PX4_ERR("FAIL: J1010P3-J1010P5 ----------------------------------------");
-		}
-
-	}
-
-	if (hw_type == eM0018) {
-		PX4_INFO(">> Testing J6");
-		stm32_configgpio(J6_PIN2);    // 2 [out] 4 [in]
-		stm32_configgpio(J6_PIN3);    // 3 [out] 5 [in]
-		stm32_configgpio(J6_PIN4_IN); // 4 [in] 2 [out]
-		stm32_configgpio(J6_PIN5_IN); // 5 [in] 3 [out]
-
-		if (test_pair(J6_PIN2, J6_PIN4_IN)) {
-			PX4_INFO("PASS: J6P2-J6P4");
-
-		} else {
-			PX4_ERR("FAIL: J6P2-J6P4 ----------------------------------------");
-		}
-
-		if (test_pair(J6_PIN3, J6_PIN5_IN)) {
-			PX4_INFO("PASS: J6P3-J6P5");
-
-		} else {
-			PX4_ERR("FAIL: J6P3-J6P5 ----------------------------------------");
-		}
-
-	} else if (hw_type == eM0019) {
-		PX4_INFO(">> Testing J1009");
-		stm32_configgpio(J1009_PIN2);    // 2 [out] 4 [in]
-		stm32_configgpio(J1009_PIN3);    // 3 [out] 5 [in]
-		stm32_configgpio(J1009_PIN4_IN); // 4 [in] 2 [out]
-		stm32_configgpio(J1009_PIN5_IN); // 5 [in] 3 [out]
-
-		if (test_pair(J1009_PIN2, J1009_PIN4_IN)) {
-			PX4_INFO("PASS: J1009P2-J1009P4");
-
-		} else {
-			PX4_ERR("FAIL: J1009P2-J1009P4 ----------------------------------------");
-		}
-
-		if (test_pair(J1009_PIN3, J1009_PIN5_IN)) {
-			PX4_INFO("PASS: J1009P3-J1009P5");
-
-		} else {
-			PX4_ERR("FAIL: J1009P3-J1009P5 ----------------------------------------");
-		}
-	}
-
-
-	if (hw_type == eM0018) {
-		PX4_INFO(">> Testing J7");
-		stm32_configgpio(J7_PIN2);    // 2 [out] 6 [in]
-		stm32_configgpio(J7_PIN3);    // 3 [out] 7 [in]
-		stm32_configgpio(J7_PIN4);    // 4 [out] 8 [in]
-		stm32_configgpio(J7_PIN5);    // 5 [out] 9 [in]
-		stm32_configgpio(J7_PIN6_IN); // 6 [in] 2 [out]
-		stm32_configgpio(J7_PIN7_IN); // 7 [in] 3 [out]
-		stm32_configgpio(J7_PIN8_IN); // 8 [in] 4 [out]
-		stm32_configgpio(J7_PIN9_IN); // 9 [in] 5 [out]
-
-		if (test_pair(J7_PIN2, J7_PIN6_IN)) {
-			PX4_INFO("PASS: J7P2-J7P6");
-
-		} else {
-			PX4_ERR("FAIL: J7P2-J7P6 ----------------------------------------");
-		}
-
-		if (test_pair(J7_PIN3, J7_PIN7_IN)) {
-			PX4_INFO("PASS: J7P3-J7P7");
-
-		} else {
-			PX4_ERR("FAIL: J7P3-J7P7 ----------------------------------------");
-		}
-
-		if (test_pair(J7_PIN4, J7_PIN8_IN)) {
-			PX4_INFO("PASS: J7P4-J7P8");
-
-		} else {
-			PX4_ERR("FAIL: J7P4-J7P8 ----------------------------------------");
-		}
-
-		if (test_pair(J7_PIN5, J7_PIN9_IN)) {
-			PX4_INFO("PASS: J7P5-J7P9");
-
-		} else {
-			PX4_ERR("FAIL: J7P5-J7P9 ----------------------------------------");
-		}
-
-	} else if (hw_type == eM0019) {
-		PX4_INFO(">> Testing J1007");
-		stm32_configgpio(J1007_PIN2);    // 2 [out] 6 [in]
-		stm32_configgpio(J1007_PIN3);    // 3 [out] 7 [in]
-		stm32_configgpio(J1007_PIN4);    // 4 [out] 8 [in]
-		stm32_configgpio(J1007_PIN5);    // 5 [out] 9 [in]
-		stm32_configgpio(J1007_PIN6_IN); // 6 [in] 2 [out]
-		stm32_configgpio(J1007_PIN7_IN); // 7 [in] 3 [out]
-		stm32_configgpio(J1007_PIN8_IN); // 8 [in] 4 [out]
-		stm32_configgpio(J1007_PIN9_IN); // 9 [in] 5 [out]
-
-		if (test_pair(J1007_PIN2, J1007_PIN6_IN)) {
-			PX4_INFO("PASS: J1007P2-J1007P6");
-
-		} else {
-			PX4_ERR("FAIL: J1007P2-J1007P6 ----------------------------------------");
-		}
-
-		if (test_pair(J1007_PIN3, J1007_PIN7_IN)) {
-			PX4_INFO("PASS: J1007P3-J1007P7");
-
-		} else {
-			PX4_ERR("FAIL: J1007P3-J1007P7 ----------------------------------------");
-		}
-
-		if (test_pair(J1007_PIN4, J1007_PIN8_IN)) {
-			PX4_INFO("PASS: J1007P4-J1007P8");
-
-		} else {
-			PX4_ERR("FAIL: J1007P4-J1007P8 ----------------------------------------");
-		}
-
-		if (test_pair(J1007_PIN5, J1007_PIN9_IN)) {
-			PX4_INFO("PASS: J1007P5-J1007P9");
-
-		} else {
-			PX4_ERR("FAIL: J1007P5-J1007P9 ----------------------------------------");
-		}
-
-	} else if (hw_type == eM0051) {
-		PX4_INFO(">> Testing M0051 J13");
-		stm32_configgpio(M0051J13_PIN2);    // 2 [out] 6 [in]
-		stm32_configgpio(M0051J13_PIN3);    // 3 [out] 7 [in]
-		stm32_configgpio(M0051J13_PIN4);    // 4 [out] 8 [in]
-		stm32_configgpio(M0051J13_PIN5);    // 5 [out] 9 [in]
-		stm32_configgpio(M0051J13_PIN6_IN); // 6 [in] 2 [out]
-		stm32_configgpio(M0051J13_PIN7_IN); // 7 [in] 3 [out]
-		stm32_configgpio(M0051J13_PIN8_IN); // 8 [in] 4 [out]
-		stm32_configgpio(M0051J13_PIN9_IN); // 9 [in] 5 [out]
-
-		if (test_pair(M0051J13_PIN2, M0051J13_PIN6_IN)) {
-			PX4_INFO("PASS: J13_P2-J13_P6");
-
-		} else {
-			PX4_ERR("FAIL: J13_P2-J13_P6 ----------------------------------------");
-		}
-
-		if (test_pair(M0051J13_PIN3, M0051J13_PIN7_IN)) {
-			PX4_INFO("PASS: JJ13_P3-J13_P7");
-
-		} else {
-			PX4_ERR("FAIL: J13_P3-J13_7P7 ----------------------------------------");
-		}
-
-		if (test_pair(M0051J13_PIN4, M0051J13_PIN8_IN)) {
-			PX4_INFO("PASS: J13_P4-J13_P8");
-
-		} else {
-			PX4_ERR("FAIL: J13_P4-J13_P8 ----------------------------------------");
-		}
-
-		if (test_pair(M0051J13_PIN5, M0051J13_PIN9_IN)) {
-			PX4_INFO("PASS: J13_P5-J13_P9");
-
-		} else {
-			PX4_ERR("FAIL: J13_P5-J13_P9 ----------------------------------------");
-		}
-	}
-
-	if (hw_type == eM0018) {
-		PX4_INFO(">> Testing J10");
-		stm32_configgpio(J10_PIN2);    // 2 [out] 4 [in]
-		stm32_configgpio(J10_PIN3);    // 3 [out] 5 [in]
-		stm32_configgpio(J10_PIN4_IN); // 4 [in] 2 [out]
-		stm32_configgpio(J10_PIN5_IN); // 5 [in] 3 [out]
-
-		if (test_pair(J10_PIN2, J10_PIN4_IN)) {
-			PX4_INFO("PASS: J10P2-J10P4");
-
-		} else {
-			PX4_ERR("FAIL: J10P2-J10P4 --------------------------------------");
-		}
-
-		if (test_pair(J10_PIN3, J10_PIN5_IN)) {
-			PX4_INFO("PASS: J10P3-J10P5");
-
-		} else {
-			PX4_ERR("FAIL: J10P3-J10P5 --------------------------------------");
-		}
-
-	} else if (hw_type == eM0019) {
-		PX4_INFO(">> Testing J1012");
-		stm32_configgpio(J1012_PIN2);    // 2 [out] 4 [in]
-		stm32_configgpio(J1012_PIN3);    // 3 [out] 5 [in]
-		stm32_configgpio(J1012_PIN4_IN); // 4 [in] 2 [out]
-		stm32_configgpio(J1012_PIN5_IN); // 5 [in] 3 [out]
-
-		if (test_pair(J1012_PIN2, J1012_PIN4_IN)) {
-			PX4_INFO("PASS: J1012P2-J1120P4");
-
-		} else {
-			PX4_ERR("FAIL: J1012P2-J1012P4 --------------------------------------");
-		}
-
-		if (test_pair(J1012_PIN3, J1012_PIN5_IN)) {
-			PX4_INFO("PASS: J1012P3-J1012P5");
-
-		} else {
-			PX4_ERR("FAIL: J1012P3-J1012P5 --------------------------------------");
-		}
-
-	} else if (hw_type == eM0051) {
-		PX4_INFO(">> Testing M0051 J15");
-		stm32_configgpio(M0051J15_PIN2);    // 2 [out] 4 [in]
-		stm32_configgpio(M0051J15_PIN3);    // 3 [out] 5 [in]
-		stm32_configgpio(M0051J15_PIN4_IN); // 4 [in] 2 [out]
-		stm32_configgpio(M0051J15_PIN5_IN); // 5 [in] 3 [out]
-
-		if (test_pair(M0051J15_PIN2, M0051J15_PIN4_IN)) {
-			PX4_INFO("PASS: J15_P2-J15_P4");
-
-		} else {
-			PX4_ERR("FAIL: J15_P2-JJ15_P4 --------------------------------------");
-		}
-
-		if (test_pair(M0051J15_PIN3, M0051J15_PIN5_IN)) {
-			PX4_INFO("PASS: J15_P3-J15_P5");
-
-		} else {
-			PX4_ERR("FAIL: J15_P3-J15_P5 --------------------------------------");
-		}
-
-	}
-
-	if (hw_type == eM0018) {
-		PX4_INFO(">> Testing J9/J12/J13");
-		stm32_configgpio(J9_PIN2_IN);  // J9-2 [in] J13-5 [out]
-
-		stm32_configgpio(J12_PIN2_IN); // J12-2 [in] J13-3 [out]
-		stm32_configgpio(J12_PIN3_IN); // J12-3 [in] J13-4 [out]
-
-		stm32_configgpio(J13_PIN3);    // J13-3 [out] J12-2 [in]
-		stm32_configgpio(J13_PIN4);    // J13-4 [out] J12-3 [in]
-		stm32_configgpio(J13_PIN5);    // J13-5 [out] J9-2 [in]
-
-		if (test_pair(J13_PIN3, J12_PIN2_IN)) {
-			PX4_INFO("PASS: J13P3-J12P2");
-
-		} else {
-			PX4_ERR("FAIL: J13P3-J12P2 --------------------------------------");
-		}
-
-		if (test_pair(J13_PIN4, J12_PIN3_IN)) {
-			PX4_INFO("PASS: J13P4-J12P3");
-
-		} else {
-			PX4_ERR("FAIL: J13P4-J12P3 --------------------------------------");
-		}
-
-		if (test_pair(J13_PIN5, J9_PIN2_IN)) {
-			PX4_INFO("PASS: J13P5-J9P2");
-
-		} else {
-			PX4_ERR("FAIL: J13P5-J9P2 --------------------------------------");
-		}
-
-	} else if (hw_type == eM0019) {
-		PX4_INFO(">> Testing J1003/J1004/J1011");
-		stm32_configgpio(J1003_PIN2_IN);  // J1003-2 [in] J13-5 [out]
-
-		stm32_configgpio(J1004_PIN2_IN); // J1004-2 [in] J13-3 [out]
-		stm32_configgpio(J1004_PIN3_IN); // J1004-3 [in] J13-4 [out]
-
-		stm32_configgpio(J1011_PIN3);    // J1011-3 [out] J12-2 [in]
-		stm32_configgpio(J1011_PIN4);    // J1011-4 [out] J12-3 [in]
-		stm32_configgpio(J1011_PIN5);    // J1011-5 [out] J9-2 [in]
-
-		if (test_pair(J1011_PIN3, J1004_PIN2_IN)) {
-			PX4_INFO("PASS: J1011P3-J1004P2");
-
-		} else {
-			PX4_ERR("FAIL: J1011P3-J1004P2 --------------------------------------");
-		}
-
-		if (test_pair(J1011_PIN4, J1004_PIN3_IN)) {
-			PX4_INFO("PASS: J1011P4-J1004P3");
-
-		} else {
-			PX4_ERR("FAIL: J1011P4-J1004P3 --------------------------------------");
-		}
-
-		if (test_pair(J1011_PIN5, J1003_PIN2_IN)) {
-			PX4_INFO("PASS: J1011P5-J1011P5");
-
-		} else {
-			PX4_ERR("FAIL: J1011P5-J1011P5 --------------------------------------");
-		}
-
-	} else if (hw_type == eM0051) {
-		PX4_INFO(">> Testing M0051 J14");
-		stm32_configgpio(M0051J14_PIN2);  // J14-2 [out] J14-3 [in]
-		stm32_configgpio(M0051J14_PIN3_IN); // J14-3 [in] J14-2 [out]
-
-		if (test_pair(M0051J14_PIN2, M0051J14_PIN3_IN)) {
-			PX4_INFO("PASS: J14_P2-J14_P3");
-
-		} else {
-			PX4_ERR("FAIL: J14_P2-J14_P3 --------------------------------------");
-		}
-
-	}
-
-	return 0;
-}
-
-int modalai_hw_detect_v1(eHW_TYPE hw_type)
-{
-	int result = 0;
-
-	if (hw_type == eM0018) {
-		PX4_INFO("V106 - Flight Core");
-
-	} else if (hw_type == eM0019) {
-		PX4_INFO("V110 - VOXL-Flight");
-
-	} else if (hw_type == eM0051) {
-		PX4_INFO("V120 - M0051");
-
-	} else {
-		PX4_ERR("Unknown hardware");
-		result = -1;
-	}
-
-	return result;
-}
-
-#endif //CONFIG_ARCH_CHIP_STM32H743ZI
@@ -1,218 +0,0 @@
-#ifndef MODALAI_FC_V1_H_
-#define MODALAI_FC_V1_H_
-
-typedef enum {
-	eHwUnknown = -1,
-	eHwNone = 0,
-	eM0018,           // Flight Core
-	eM0019,           // VOXL Flight
-	eM0051
-} eHW_TYPE;
-
-#define _MK_GPIO_INPUT(def)  (((def) & (GPIO_PORT_MASK | GPIO_PIN_MASK)) | (GPIO_INPUT|GPIO_PULLUP))
-#define _MK_GPIO_OUTPUT(def) (((def) & (GPIO_PORT_MASK | GPIO_PIN_MASK)) | (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_2MHz|GPIO_OUTPUT_CLEAR))
-
-
-//
-// Flight Core - J1 - Primary MSS Communications Interface
-// VOXL Flight - NA
-//
-#define J1_PIN2_IN  _MK_GPIO_INPUT(GPIO_UART5_RX)
-#define J1_PIN3     _MK_GPIO_OUTPUT(GPIO_UART5_TX)
-#define J1_PIN4     _MK_GPIO_OUTPUT(GPIO_UART5_RTS)
-#define J1_PIN6_IN  _MK_GPIO_INPUT(GPIO_UART5_CTS)
-
-//
-// STM JTAG Programming Header
-// Flight Core - J2
-// VOXL Flight - J1001
-//
-
-//
-// USB 2.0 Full-Speed Downstream Device Port
-// Flight Core - J
-// VOXL Flight - J1006
-//
-
-//
-// Spare MSS Comms
-// Flight Core - J4
-// VOXL Flight - J1002
-//
-#define J4_PIN2       _MK_GPIO_OUTPUT(GPIO_USART2_RX)
-#define J1002_PIN2    J4_PIN2
-#define J4_PIN3       _MK_GPIO_OUTPUT(GPIO_USART2_TX)
-#define J1002_PIN3    J4_PIN3
-#define J4_PIN4       _MK_GPIO_OUTPUT(GPIO_USART2_RTS)
-#define J1002_PIN4    J4_PIN4
-#define J4_PIN4_IN    _MK_GPIO_INPUT(GPIO_USART2_RTS)
-#define J1002_PIN4_IN J4_PIN4_IN
-#define J4_PIN6       _MK_GPIO_OUTPUT(GPIO_USART2_CTS)
-#define J4_PIN6_IN    _MK_GPIO_INPUT(GPIO_USART2_CTS)
-#define J1002_PIN6_IN J4_PIN6_IN
-#define J4_PIN7       _MK_GPIO_OUTPUT(GPIO_VOXL_STATUS_OUT)
-#define J4_PIN7_IN    _MK_GPIO_INPUT(GPIO_VOXL_STATUS_OUT)
-#define J4_PIN8       _MK_GPIO_OUTPUT(GPIO_VOXL_STATUS_IN)
-#define J4_PIN8_IN    _MK_GPIO_INPUT(GPIO_VOXL_STATUS_IN)
-
-//
-// TELEMETRY CONNECTOR
-// Flight Core - J5
-// VOXL Flight - J1010
-//
-#define J5_PIN2       _MK_GPIO_OUTPUT(GPIO_UART7_TX)
-#define J1010_PIN2    J5_PIN2
-#define J5_PIN3       _MK_GPIO_OUTPUT(GPIO_UART7_RX)
-#define J1010_PIN3    J5_PIN3
-#define J5_PIN4       _MK_GPIO_OUTPUT(GPIO_UART7_CTS)
-#define J1010_PIN4    J5_PIN4
-#define J5_PIN4_IN    _MK_GPIO_INPUT(GPIO_UART7_CTS)
-#define J1010_PIN4_IN J5_PIN4_IN
-#define J5_PIN5       _MK_GPIO_OUTPUT(GPIO_UART7_RTS)
-#define J1010_PIN5    J5_PIN5
-#define J5_PIN5_IN    _MK_GPIO_INPUT(GPIO_UART7_RTS)
-#define J1010_PIN5_IN J5_PIN5_IN
-
-//
-// EXPANSION CONNECTOR
-// Flight Core - J6
-// VOXL Flight - J1009
-//
-#define J6_PIN2        _MK_GPIO_OUTPUT(GPIO_UART4_TX_5)
-#define J1009_PIN2     J6_PIN2
-#define J6_PIN3        _MK_GPIO_OUTPUT(GPIO_UART4_RX_5)
-#define J1009_PIN3     J6_PIN3
-#define J6_PIN4        _MK_GPIO_OUTPUT(GPIO_I2C3_SCL_2)
-#define J1009_PIN4     J6_PIN4
-#define J6_PIN4_IN     _MK_GPIO_INPUT(GPIO_I2C3_SCL_2)
-#define J1009_PIN4_IN  J6_PIN4_IN
-#define J6_PIN5        _MK_GPIO_OUTPUT(GPIO_I2C3_SDA_2)
-#define J1009_PIN5     J6_PIN5
-#define J6_PIN5_IN     _MK_GPIO_INPUT(GPIO_I2C3_SDA_2)
-#define J1009_PIN5_IN  J6_PIN5_IN
-
-//
-// Flight Core - J7 - PWM Output Connector
-// VOXL Flight - J1007
-// M0051       - J13
-//
-#define J7_PIN2          _MK_GPIO_OUTPUT(GPIO_TIM1_CH4OUT_2)
-#define J1007_PIN2       J7_PIN2
-#define M0051J13_PIN2    J7_PIN2
-#define J7_PIN3          _MK_GPIO_OUTPUT(GPIO_TIM1_CH3OUT_1)
-#define J1007_PIN3       J7_PIN3
-#define M0051J13_PIN3    J7_PIN3
-#define J7_PIN4          _MK_GPIO_OUTPUT(GPIO_TIM1_CH2OUT_2)
-#define J1007_PIN4       J7_PIN4
-#define M0051J13_PIN4    J7_PIN4
-#define J7_PIN5          _MK_GPIO_OUTPUT(GPIO_TIM1_CH1OUT_1)
-#define J1007_PIN5       J7_PIN5
-#define M0051J13_PIN5    J7_PIN5
-#define J7_PIN6          _MK_GPIO_OUTPUT(GPIO_TIM4_CH2OUT_2)
-#define J1007_PIN6       J7_PIN6
-#define M0051J13_PIN6    J7_PIN6
-#define J7_PIN6_IN       _MK_GPIO_INPUT(GPIO_TIM4_CH2OUT_2)
-#define J1007_PIN6_IN    J7_PIN6_IN
-#define M0051J13_PIN6_IN J7_PIN6_IN
-#define J7_PIN7          _MK_GPIO_OUTPUT(GPIO_TIM4_CH3OUT_2)
-#define J1007_PIN7       J7_PIN7
-#define M0051J13_PIN7    J7_PIN7
-#define J7_PIN7_IN       _MK_GPIO_INPUT(GPIO_TIM4_CH3OUT_2)
-#define J1007_PIN7_IN    J7_PIN7_IN
-#define M0051J13_PIN7_IN J7_PIN7_IN
-#define J7_PIN8          _MK_GPIO_OUTPUT(GPIO_TIM4_CH1OUT_2)
-#define J1007_PIN8       J7_PIN8
-#define M0051J13_PIN8    J7_PIN8
-#define J7_PIN8_IN       _MK_GPIO_INPUT(GPIO_TIM4_CH1OUT_2)
-#define J1007_PIN8_IN    J7_PIN8_IN
-#define M0051J13_PIN8_IN J7_PIN8_IN
-#define J7_PIN9          _MK_GPIO_OUTPUT(GPIO_TIM4_CH4OUT_2)
-#define J1007_PIN9       J7_PIN9
-#define M0051J13_PIN9    J7_PIN9
-#define J7_PIN9_IN       _MK_GPIO_INPUT(GPIO_TIM4_CH4OUT_2)
-#define J1007_PIN9_IN    J7_PIN9_IN
-#define M0051J13_PIN9_IN J7_PIN9_IN
-
-//
-// CAN 1 Peripheral Connector
-// Flight Core - J8
-// VOXL Flight - J1008
-//
-//#define J8_PIN2     _MK_GPIO_OUTPUT()
-//#define J8_PIN3     _MK_GPIO_OUTPUT()
-
-// PPM (RC) IN
-// Flight Core - J9
-// VOXL Flight - J1003
-//
-#define J9_PIN2_IN    _MK_GPIO_INPUT(GPIO_TIM8_CH1IN_2)
-#define J1003_PIN2_IN J9_PIN2_IN
-
-//
-// GPS CONNECTOR
-// Flight Core - J10
-// VOXL Flight - J1012
-// M0051       - J15
-//
-#define J10_PIN2         _MK_GPIO_OUTPUT(GPIO_USART1_TX_3)
-#define J1012_PIN2       J10_PIN2
-#define M0051J15_PIN2    J10_PIN2
-#define J10_PIN3         _MK_GPIO_OUTPUT(GPIO_USART1_RX_3)
-#define J1012_PIN3       J10_PIN3
-#define M0051J15_PIN3    J10_PIN3
-#define J10_PIN4         _MK_GPIO_OUTPUT(GPIO_I2C1_SCL_2)
-#define J1012_PIN4       J10_PIN4
-#define M0051J15_PIN4    J10_PIN4
-#define J10_PIN4_IN      _MK_GPIO_INPUT(GPIO_I2C1_SCL_2)
-#define J1012_PIN4_IN    J10_PIN4_IN
-#define M0051J15_PIN4_IN J10_PIN4_IN
-#define J10_PIN5         _MK_GPIO_OUTPUT(GPIO_I2C1_SDA_1)
-#define J1012_PIN5       J10_PIN5
-#define M0051J15_PIN5    J10_PIN5
-#define J10_PIN5_IN      _MK_GPIO_INPUT(GPIO_I2C1_SDA_1)
-#define J1012_PIN5_IN    J10_PIN5_IN
-#define M0051J15_PIN5_IN J10_PIN5_IN
-
-//
-// Spektrum RC Input Connector
-// Flight Core - J12
-// VOXL Flight - J1004
-// M0051       - J14
-//
-#define J12_PIN1      GPIO_VDD_3V3_SPEKTRUM_POWER_EN
-#define J1004_PIN1       J12_PIN1
-#define M0051J14_PIN1    J12_PIN1
-#define J12_PIN2         _MK_GPIO_OUTPUT(GPIO_USART6_TX_1)
-#define J1004_PIN2       J12_PIN2
-#define M0051J14_PIN2    J12_PIN2
-#define J12_PIN2_IN      _MK_GPIO_INPUT(GPIO_USART6_TX_1)
-#define J1004_PIN2_IN    J12_PIN2_IN
-#define M0051J14_PIN2_IN J12_PIN2_IN
-#define J12_PIN3         _MK_GPIO_OUTPUT(GPIO_USART6_RX_1)
-#define J1004_PIN3       J12_PIN3
-#define M0051J14_PIN3    J12_PIN3
-#define J12_PIN3_IN      _MK_GPIO_INPUT(GPIO_USART6_RX_1)
-#define J1004_PIN3_IN    J12_PIN3_IN
-#define M0051J14_PIN3_IN J12_PIN3_IN
-
-//
-// I2C Display / Spare Sensor Connector
-// Flight Core - J13
-// VOXL Flight - J1011
-//
-#define J13_PIN3   _MK_GPIO_OUTPUT(GPIO_I2C2_SDA_2)
-#define J1011_PIN3 J13_PIN3
-#define J13_PIN4   _MK_GPIO_OUTPUT(GPIO_I2C2_SCL_2)
-#define J1011_PIN4 J13_PIN4
-#define J13_PIN5   _MK_GPIO_OUTPUT(GPIO_PF3_EVENTOUT)
-#define J1011_PIN5 J13_PIN5
-
-
-void 	modalai_print_usage_v1(void);
-void 	modalai_print_usage_con_gpio_test_v1(void);
-int  	modalai_con_gpio_test_v1(uint8_t con, uint8_t pin, bool state);
-int  	modalai_led_test_v1(void);
-int  	modalai_buzz_test_v1(eHW_TYPE type);
-int	modalai_hw_detect_v1(eHW_TYPE type);
-
-#endif //MODALAI_FC_V1_H_
--- a/Show More
+++ b/Show More