TiltrotorEffectiveness: limit thrust axis tilt to z effectiveness scaling

During transition to FF, only allow update thrust axis up to 45° as with a high tilt angle
the effectiveness of the thrust axis in z is apporaching 0, and by that is increasing
the motor output to max.
Transition to HF: disable thrust axis tilting, and assume motors are vertical. This is to avoid
a thrust spike when the transition is initiated (as then the tilt is fully forward).

Signed-off-by: Silvan Fuhrer <silvan@auterion.com>

.ci/Jenkinsfile-compile

@@ -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",

.github/workflows/stale.yml

@@ -1,20 +0,0 @@
-name: 'Close stale issues and PRs'
-on:
-  schedule:
-    - cron: '30 1 * * *'
-
-jobs:
-  stale:
-    runs-on: ubuntu-latest
-    steps:
-      - uses: actions/stale@v4.1.1
-        with:
-          stale-issue-message: 'This issue is stale because it has been open 30 days with no activity. Remove stale label or comment or this will be closed in 5 days.'
-          stale-pr-message: 'This PR is stale because it has been open 45 days with no activity. Remove stale label or comment or this will be closed in 10 days.'
-          close-issue-message: 'This issue was closed because it has been stalled for 5 days with no activity.'
-          close-pr-message: 'This PR was closed because it has been stalled for 10 days with no activity.'
-          days-before-issue-stale: 30
-          days-before-pr-stale: 45
-          days-before-issue-close: 5
-          days-before-pr-close: 10
-          debug-only: true

.gitmodules

@@ -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

ROMFS/px4fmu_common/init.d-posix/airframes/1032_gazebo-classic_plane_catapult

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

ROMFS/px4fmu_common/init.d/airframes/4019_x500_v2

@@ -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

ROMFS/px4fmu_common/init.d/rcS

@@ -255,6 +255,7 @@ else
 	rgbled start -X -q
 	rgbled_ncp5623c start -X -q
 	rgbled_lp5562 start -X -q
+	rgbled_is31fl3195 start -X -q
 
 	#
 	# Override parameters from user configuration file.

Tools/auterion/remote_update_fmu.sh

@@ -0,0 +1,115 @@
+#!/bin/bash
+# Flash PX4 to a device running AuterionOS in the local network
+if [ "$1" == "-h" ] || [ "$1" == "--help" ] || [ $# -lt 2 ]; then
+	echo "Usage: $0 -f <firmware.px4|.elf> [-c <configuration_dir>] -d <IP/Device> [-u <user>] [-p <ssh_port>] [--revert]"
+	exit 1
+fi
+
+ssh_port=22
+ssh_user=root
+
+while getopts ":f:c:d:p:u:r" opt; do
+	case ${opt} in
+		f )
+			if [ -n "$OPTARG" ]; then
+				firmware_file="$OPTARG"
+			else
+				echo "ERROR: -f requires a non-empty option argument."
+				exit 1
+			fi
+			;;
+		c )
+			if [ -f "$OPTARG/rc.autostart" ]; then
+				config_dir="$OPTARG"
+			else
+				echo "ERROR: -c configuration directory is empty or does not contain a valid rc.autostart"
+				exit 1
+			fi
+			;;
+		d )
+			if [ "$OPTARG" ]; then
+				device="$OPTARG"
+			else
+				echo "ERROR: -d requires a non-empty option argument."
+				exit 1
+			fi
+			;;
+		p )
+			if [[ "$OPTARG" =~ ^[0-9]+$ ]]; then
+				ssh_port="$OPTARG"
+			else
+				echo "ERROR: -p ssh_port must be a number."
+				exit 1
+			fi
+			;;
+		u )
+			if [ "$OPTARG" ]; then
+				ssh_user="$OPTARG"
+			else
+				echo "ERROR: -u requires a non-empty option argument."
+				exit 1
+			fi
+			;;
+		r )
+			revert=true
+			;;
+	esac
+done
+
+if [ -z "$device" ]; then
+	echo "Error: missing device"
+	exit 1
+fi
+
+target_dir=/shared_container_dir/fmu
+target_file_name="update-dev.tar"
+
+if [ "$revert" == true ]; then
+	# revert to the release version which was originally deployed
+	cmd="cp $target_dir/update.tar $target_dir/$target_file_name"
+	ssh -t -p $ssh_port $ssh_user@$device "$cmd"
+else
+	# create custom update-dev.tar
+	tmp_dir="$(mktemp -d)"
+	config_path=""
+	firmware_path=""
+
+	if [ -d "$config_dir" ]; then
+		cp -r "$config_dir" "$tmp_dir/config"
+		config_path=config
+	fi
+
+	if [ -f "$firmware_file" ]; then
+		extension="${firmware_file##*.}"
+		cp "$firmware_file" "$tmp_dir/firmware.$extension"
+		if [ "$extension" == "elf" ]; then
+			# ensure the file is stripped to reduce file size
+			arm-none-eabi-strip "$tmp_dir/firmware.$extension"
+		fi
+		firmware_path="firmware.$extension"
+	fi
+
+	pushd "$tmp_dir" &>/dev/null
+
+	if [ -z $firmware_path ] && [ -z $config_path ]; then
+		exit 1
+	fi
+
+	tar_name="tar"
+
+	if [ -x "$(command -v gtar)" ]; then
+		# check if gnu-tar is installed on macOS and use that instead
+		tar_name="gtar"
+	fi
+
+	$tar_name -C "$tmp_dir" --sort=name --owner=root:0 --group=root:0 --mtime='2019-01-01 00:00:00' -cvf $target_file_name $firmware_path $config_path
+
+	# send it to the target to start flashing
+	scp -P $ssh_port "$target_file_name" $ssh_user@"$device":$target_dir
+	popd &>/dev/null
+	rm -rf "$tmp_dir"
+fi
+
+# grab status output for flashing progress
+cmd="tail --follow=name $target_dir/update_status 2>/dev/null || true"
+ssh -t -p $ssh_port $ssh_user@$device "$cmd"

Tools/auterion/upload_skynode.sh

@@ -0,0 +1,51 @@
+#!/usr/bin/env bash
+
+DIR="$(dirname $(readlink -f $0))"
+DEFAULT_AUTOPILOT_HOST=10.41.1.1
+DEFAULT_AUTOPILOT_PORT=33333
+DEFAULT_AUTOPILOT_USER=auterion
+
+for i in "$@"
+do
+    case $i in
+        --file=*)
+        PX4_BINARY_FILE="${i#*=}"
+        ;;
+        --default-ip=*)
+        DEFAULT_AUTOPILOT_HOST="${i#*=}"
+        ;;
+        --default-port=*)
+        DEFAULT_AUTOPILOT_PORT="${i#*=}"
+        ;;
+        --default-user=*)
+        DEFAULT_AUTOPILOT_USER="${i#*=}"
+        ;;
+        --revert)
+        REVERT_AUTOPILOT_ARGUMENT=-r
+        ;;
+        --wifi)
+        DEFAULT_AUTOPILOT_HOST=10.41.0.1
+        ;;
+        *)
+            # unknown option
+        ;;
+    esac
+done
+
+# allow these to be overridden
+[ -z "$AUTOPILOT_HOST" ] && AUTOPILOT_HOST=$DEFAULT_AUTOPILOT_HOST
+[ -z "$AUTOPILOT_PORT" ] && AUTOPILOT_PORT=$DEFAULT_AUTOPILOT_PORT
+[ -z "$AUTOPILOT_USER" ] && AUTOPILOT_USER=$DEFAULT_AUTOPILOT_USER
+
+ARGUMENTS=()
+ARGUMENTS+=(-d "$AUTOPILOT_HOST")
+ARGUMENTS+=(-p "$AUTOPILOT_PORT")
+ARGUMENTS+=(-u "$AUTOPILOT_USER")
+ARGUMENTS+=(${PX4_BINARY_FILE:+-f "$PX4_BINARY_FILE"})
+ARGUMENTS+=($REVERT_AUTOPILOT_ARGUMENT)
+
+echo "Flashing $AUTOPILOT_HOST ..."
+
+"$DIR"/remote_update_fmu.sh "${ARGUMENTS[@]}"
+
+exit 0

Tools/simulation/sitl_multiple_run.sh

@@ -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
 

boards/ark/cannode/default.px4board

@@ -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

boards/ark/fmu-v6x/default.px4board

@@ -20,6 +20,7 @@ CONFIG_DRIVERS_HEATER=y
 CONFIG_DRIVERS_IMU_ANALOG_DEVICES_ADIS16507=y
 CONFIG_DRIVERS_IMU_INVENSENSE_ICM42688P=y
 CONFIG_DRIVERS_IMU_INVENSENSE_IIM42652=y
+CONFIG_DRIVERS_IMU_INVENSENSE_IIM42653=y
 CONFIG_COMMON_LIGHT=y
 CONFIG_COMMON_MAGNETOMETER=y
 CONFIG_COMMON_OPTICAL_FLOW=y

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

@@ -17,12 +17,21 @@ param set-default SENS_EN_INA228 0
 param set-default SENS_EN_INA226 1
 
 param set-default SENS_EN_THERMAL 1
-param set-default SENS_TEMP_ID 2818058
 param set-default SENS_IMU_TEMP 10.0
 #param set-default SENS_IMU_TEMP_FF 0.0
 #param set-default SENS_IMU_TEMP_I 0.025
 #param set-default SENS_IMU_TEMP_P 1.0
 
+if ver hwtypecmp ARKV6X000000 ARKV6X001000 ARKV6X002000 ARKV6X003000 ARKV6X004000 ARKV6X005000 ARKV6X006000 ARKV6X007000
+then
+	param set-default SENS_TEMP_ID 2818058
+fi
+
+if ver hwtypecmp ARKV6X000001 ARKV6X001001 ARKV6X002001 ARKV6X003001 ARKV6X004001 ARKV6X005001 ARKV6X006001 ARKV6X007001
+then
+	param set-default SENS_TEMP_ID 3014666
+fi
+
 if ver hwtypecmp ARKV6X001000 ARKV6X001001 ARKV6X001002 ARKV6X001003 ARKV6X001004 ARKV6X001005 ARKV6X001006 ARKV6X001007
 then
 	param set-default SYS_USE_IO 0

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

@@ -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

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

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

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

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

boards/ark/fmu-v6x/src/spi.cpp

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

boards/bitcraze/crazyflie/src/board_config.h

@@ -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 */

boards/bitcraze/crazyflie21/src/board_config.h

@@ -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 */

boards/cubepilot/cubeorangeplus/init/rc.board_sensors

@@ -8,16 +8,22 @@ board_adc start
 # 1. Isolated {ICM42688p, ICM20948(with mag)}, body-fixed {ICM20649}
 # 2. Isolated {ICM42688p, ICM42688p}, body-fixed  {ICM20649, ICM45686, AK09918}
 # 3. Isolated {ICM42688p, ICM42688p}, body-fixed  {ICM45686, AK09918}
+# 4. Isolated {ICM45686, ICM45686}, body-fixed  {ICM45686, AK09918}
 
 # SPI4 is isolated, SPI1 is body-fixed
 
 # SPI4, isolated
 ms5611 -s -b 4 start
 
-icm42688p -s -b 4 -R 10 start -c 15
-if ! icm20948 -s -b 4 -R 10 -M -q start
+if icm42688p -s -b 4 -R 10 -q start -c 15
 then
-	icm42688p -s -b 4 -R 6 start -c 13
+	if ! icm20948 -s -b 4 -R 10 -M -q start
+	then
+		icm42688p -s -b 4 -R 6 start -c 13
+	fi
+else
+	icm45686 -s -b 4 -R 10 start -c 15
+	icm45686 -s -b 4 -R 6 start -c 13
 fi
 
 # SPI1, body-fixed

boards/cubepilot/cubeorangeplus/src/spi.cpp

@@ -49,7 +49,9 @@ constexpr px4_spi_bus_t px4_spi_buses[SPI_BUS_MAX_BUS_ITEMS] = {
 	initSPIBus(SPI::Bus::SPI4, {
 		initSPIDevice(DRV_IMU_DEVTYPE_ICM20948, SPI::CS{GPIO::PortE, GPIO::Pin4}),  // MPU_EXT_CS
 		initSPIDevice(DRV_IMU_DEVTYPE_ICM42688P, SPI::CS{GPIO::PortC, GPIO::Pin15}), // ACCEL_EXT_CS
+		initSPIDevice(DRV_IMU_DEVTYPE_ICM45686, SPI::CS{GPIO::PortC, GPIO::Pin15}), // ACCEL_EXT_CS
 		initSPIDevice(DRV_IMU_DEVTYPE_ICM42688P, SPI::CS{GPIO::PortC, GPIO::Pin13}), // GYRO_EXT_CS
+		initSPIDevice(DRV_IMU_DEVTYPE_ICM45686, SPI::CS{GPIO::PortC, GPIO::Pin13}), // GYRO_EXT_CS
 		initSPIDevice(DRV_BARO_DEVTYPE_MS5611, 	SPI::CS{GPIO::PortC, GPIO::Pin14}), // BARO_EXT_CS
 	}),
 };

boards/modalai/fc-v2/default.px4board

@@ -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

boards/modalai/fc-v2/init/rc.board_sensors

@@ -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

boards/modalai/fc-v2/src/CMakeLists.txt

@@ -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()

boards/modalai/fc-v2/src/board_config.h

@@ -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
 

boards/modalai/src/systemcmds/modalai/modalai.c

@@ -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;
-}

boards/modalai/src/systemcmds/modalai/modalai_fc-v1.c

@@ -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

boards/modalai/src/systemcmds/modalai/modalai_fc-v1.h

@@ -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_

boards/modalai/src/systemcmds/modalai/modalai_fc-v2.c

@@ -1,437 +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"
-
-void modalai_print_usage_v2(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");
-	return;
-}
-void modalai_print_usage_con_gpio_test_v2(void)
-{
-	return;
-}
-int modalai_con_gpio_test_v2(uint8_t con, uint8_t pin, bool state)
-{
-	return 0;
-}
-int modalai_led_test_v2(void)
-{
-	PX4_INFO("Running led test");
-
-	stm32_configgpio(GPIO_nLED_RED);
-	stm32_configgpio(GPIO_nLED_GREEN);
-	stm32_configgpio(GPIO_nLED_BLUE);
-
-	int i = 0;
-
-	for (i = 0; i < 3; i++) {
-		usleep(1000 * 100);
-		stm32_gpiowrite(GPIO_nLED_RED, false);
-
-		usleep(1000 * 100);
-		stm32_gpiowrite(GPIO_nLED_RED, true);
-
-		usleep(1000 * 100);
-		stm32_gpiowrite(GPIO_nLED_GREEN, false);
-		usleep(1000 * 100);
-		stm32_gpiowrite(GPIO_nLED_GREEN, true);
-
-		usleep(1000 * 100);
-		stm32_gpiowrite(GPIO_nLED_BLUE, false);
-		usleep(1000 * 100);
-		stm32_gpiowrite(GPIO_nLED_BLUE, true);
-	}
-
-	return 0;
-}
-
-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;
-}
-
-int modalai_buzz_test_v2(eHW_TYPE hw_type)
-{
-	PX4_INFO("test: buzz");
-	usleep(1000 * 100 * 10);
-
-	if (hw_type == eM0079) {
-		PX4_INFO("Using M0079 config");
-
-	} else if (hw_type == eM0087) {
-		PX4_INFO("Using M0087 config");
-
-	} else {
-		return -1;
-
-	}
-
-	if (hw_type == eM0079) {
-		//
-		//
-		//
-		PX4_INFO(">> Testing J1");
-		stm32_configgpio(M0079_J1_PIN_2_OUT); // 2-3
-		stm32_configgpio(M0079_J1_PIN_3_IN);  // 3-2
-		stm32_configgpio(M0079_J1_PIN_4_OUT); // 4-5
-		stm32_configgpio(M0079_J1_PIN_5_IN);  // 5-4
-
-		if (test_pair(M0079_J1_PIN_2_OUT, M0079_J1_PIN_3_IN)) {
-			PX4_INFO("PASS: M0079_J1_PIN_2_OUT M0079_J1_PIN_3_IN");
-
-		} else {
-			PX4_ERR("FAIL: M0079_J1_PIN_2_OUT M0079_J1_PIN_3_IN");
-		}
-
-		if (test_pair(M0079_J1_PIN_4_OUT, M0079_J1_PIN_5_IN)) {
-			PX4_INFO("PASS: M0079_J1_PIN_4_OUT M0079_J1_PIN_5_IN");
-
-		} else {
-			PX4_ERR("FAIL: M0079_J1_PIN_4_OUT M0079_J1_PIN_5_IN");
-		}
-
-		//
-		//
-		//
-		PX4_INFO(">> Testing J5");
-		stm32_configgpio(M0079_J5_PIN_2_OUT); // 2-4
-		stm32_configgpio(M0079_J5_PIN_3_OUT); // 3-5
-		stm32_configgpio(M0079_J5_PIN_4_IN);  // 4-2
-		stm32_configgpio(M0079_J5_PIN_5_IN);  // 5-3
-
-		if (test_pair(M0079_J5_PIN_2_OUT, M0079_J5_PIN_4_IN)) {
-			PX4_INFO("PASS: M0079_J5_PIN_2_OUT M0079_J5_PIN_4_IN");
-
-		} else {
-			PX4_ERR("FAIL: M0079_J5_PIN_2_OUT M0079_J5_PIN_4_IN");
-		}
-
-		if (test_pair(M0079_J5_PIN_3_OUT, M0079_J5_PIN_5_IN)) {
-			PX4_INFO("PASS: M0079_J5_PIN_3_OUT M0079_J5_PIN_5_IN");
-
-		} else {
-			PX4_ERR("FAIL: M0079_J5_PIN_3_OUT M0079_J5_PIN_5_IN");
-		}
-
-		//
-		//
-		//
-		PX4_INFO(">> Testing J7");
-		stm32_configgpio(M0079_J7_PIN_2_OUT); // 2-6
-		stm32_configgpio(M0079_J7_PIN_3_OUT); // 3-7
-		stm32_configgpio(M0079_J7_PIN_4_OUT); // 4-8
-		stm32_configgpio(M0079_J7_PIN_5_OUT); // 5-9
-		stm32_configgpio(M0079_J7_PIN_6_IN); // 6-2
-		stm32_configgpio(M0079_J7_PIN_7_IN); // 7-3
-		stm32_configgpio(M0079_J7_PIN_8_IN); // 8-4
-		stm32_configgpio(M0079_J7_PIN_9_IN); // 9-5
-
-		if (test_pair(M0079_J7_PIN_2_OUT, M0079_J7_PIN_6_IN)) {
-			PX4_INFO("PASS: M0079_J7_PIN_2_OUT M0079_J7_PIN_6_IN");
-
-		} else {
-			PX4_ERR("FAIL: M0079_J7_PIN_2_OUT M0079_J7_PIN_6_IN");
-		}
-
-		if (test_pair(M0079_J7_PIN_3_OUT, M0079_J7_PIN_7_IN)) {
-			PX4_INFO("PASS: M0079_J7_PIN_3_OUT M0079_J7_PIN_7_IN");
-
-		} else {
-			PX4_ERR("FAIL: M0079_J7_PIN_3_OUT M0079_J7_PIN_7_IN");
-		}
-
-		if (test_pair(M0079_J7_PIN_4_OUT, M0079_J7_PIN_8_IN)) {
-			PX4_INFO("PASS: M0079_J7_PIN_4_OUT M0079_J7_PIN_8_IN");
-
-		} else {
-			PX4_ERR("FAIL: M0079_J7_PIN_4_OUT M0079_J7_PIN_8_IN");
-		}
-
-		if (test_pair(M0079_J7_PIN_5_OUT, M0079_J7_PIN_9_IN)) {
-			PX4_INFO("PASS: M0079_J7_PIN_5_OUT M0079_J7_PIN_9_IN");
-
-		} else {
-			PX4_ERR("FAIL: M0079_J7_PIN_5_OUT M0079_J7_PIN_9_IN");
-		}
-
-		//
-		//
-		//
-		PX4_INFO(">> Testing J10");
-		stm32_configgpio(M0079_J10_PIN_2_OUT); // 2-4
-		stm32_configgpio(M0079_J10_PIN_3_OUT); // 3-5
-		stm32_configgpio(M0079_J10_PIN_4_IN);  // 4-2
-		stm32_configgpio(M0079_J10_PIN_5_IN);  // 5-3
-
-		if (test_pair(M0079_J10_PIN_2_OUT, M0079_J10_PIN_4_IN)) {
-			PX4_INFO("PASS: M0079_J10_PIN_2_OUT M0079_J10_PIN_4_IN");
-
-		} else {
-			PX4_ERR("FAIL: M0079_J10_PIN_2_OUT M0079_J10_PIN_4_IN");
-		}
-
-		if (test_pair(M0079_J10_PIN_3_OUT, M0079_J10_PIN_5_IN)) {
-			PX4_INFO("PASS: M0079_J10_PIN_3_OUT M0079_J10_PIN_5_IN");
-
-		} else {
-			PX4_ERR("FAIL: M0079_J10_PIN_3_OUT M0079_J10_PIN_5_IN");
-		}
-
-
-		//
-		//
-		//
-		PX4_INFO(">> Testing J13");
-		stm32_configgpio(M0079_J12_PIN_2_OUT); // 2-3
-		stm32_configgpio(M0079_J12_PIN_3_IN);  // 3-2
-
-		if (test_pair(M0079_J12_PIN_2_OUT, M0079_J12_PIN_3_IN)) {
-			PX4_INFO("PASS: M0079_J12_PIN_2 M0079_J12_PIN_3");
-
-		} else {
-			PX4_ERR("FAIL: M0079_J12_PIN_2 M0079_J12_PIN_3");
-		}
-
-
-	} else if (hw_type == eM0087) {
-		//
-		//
-		//
-		PX4_INFO(">> Testing J1");
-		stm32_configgpio(M0087_J1_PIN_2_IN);    // 2-4
-		stm32_configgpio(M0087_J1_PIN_3_OUT);   // 3-5
-		stm32_configgpio(M0087_J1_PIN_4_OUT);   // 4-2
-		stm32_configgpio(M0087_J1_PIN_5_IN);    // 5-3
-
-		if (test_pair(M0087_J1_PIN_4_OUT, M0087_J1_PIN_2_IN)) {
-			PX4_INFO("PASS: M0087_J1_PIN_4_OUT M0087_J1_PIN_2_IN");
-
-		} else {
-			PX4_ERR("FAIL: M0087_J1_PIN_4_OUT M0087_J1_PIN_2_IN");
-		}
-
-		if (test_pair(M0087_J1_PIN_3_OUT, M0087_J1_PIN_5_IN)) {
-			PX4_INFO("PASS: M0087_J1_PIN_3_OUT M0087_J1_PIN_5_IN");
-
-		} else {
-			PX4_ERR("FAIL: M0087_J1_PIN_3_OUT M0087_J1_PIN_5_IN");
-		}
-
-		//
-		//
-		//
-		PX4_INFO(">> Testing J5");
-		stm32_configgpio(M0087_J5_PIN_2_OUT); // 2-4
-		stm32_configgpio(M0087_J5_PIN_3_OUT); // 3-5
-		stm32_configgpio(M0087_J5_PIN_4_IN);  // 4-2
-		stm32_configgpio(M0087_J5_PIN_5_IN);  // 5-3
-
-		if (test_pair(M0087_J5_PIN_2_OUT, M0087_J5_PIN_4_IN)) {
-			PX4_INFO("PASS: M0087_J5_PIN_2_OUT M0087_J5_PIN_4_IN");
-
-		} else {
-			PX4_ERR("FAIL: M0087_J5_PIN_2_OUT M0087_J5_PIN_4_IN");
-		}
-
-		if (test_pair(M0087_J5_PIN_3_OUT, M0087_J5_PIN_5_IN)) {
-			PX4_INFO("PASS: M0087_J5_PIN_3_OUT M0087_J5_PIN_5_IN");
-
-		} else {
-			PX4_ERR("FAIL: M0087_J5_PIN_3_OUT M0087_J5_PIN_5_IN");
-		}
-
-		//
-		//
-		//
-		PX4_INFO(">> Testing J7");
-		stm32_configgpio(M0087_J7_PIN_2_OUT); // 2-6
-		stm32_configgpio(M0087_J7_PIN_3_OUT); // 3-7
-		stm32_configgpio(M0087_J7_PIN_4_OUT); // 4-8
-		stm32_configgpio(M0087_J7_PIN_5_OUT); // 5-9
-		stm32_configgpio(M0087_J7_PIN_6_IN); // 6-2
-		stm32_configgpio(M0087_J7_PIN_7_IN); // 7-3
-		stm32_configgpio(M0087_J7_PIN_8_IN); // 8-4
-		stm32_configgpio(M0087_J7_PIN_9_IN); // 9-5
-
-		if (test_pair(M0087_J7_PIN_2_OUT, M0087_J7_PIN_6_IN)) {
-			PX4_INFO("PASS: M0087_J7_PIN_2_OUT M0087_J7_PIN_6_IN");
-
-		} else {
-			PX4_ERR("FAIL: M0087_J7_PIN_2_OUT M0087_J7_PIN_6_IN");
-		}
-
-		if (test_pair(M0087_J7_PIN_3_OUT, M0087_J7_PIN_7_IN)) {
-			PX4_INFO("PASS: M0087_J7_PIN_3_OUT M0087_J7_PIN_7_IN");
-
-		} else {
-			PX4_ERR("FAIL: M0087_J7_PIN_3_OUT M0087_J7_PIN_7_IN");
-		}
-
-		if (test_pair(M0087_J7_PIN_4_OUT, M0087_J7_PIN_8_IN)) {
-			PX4_INFO("PASS: M0087_J7_PIN_4_OUT M0087_J7_PIN_8_IN");
-
-		} else {
-			PX4_ERR("FAIL: M0087_J7_PIN_4_OUT M0087_J7_PIN_8_IN");
-		}
-
-		if (test_pair(M0087_J7_PIN_5_OUT, M0087_J7_PIN_9_IN)) {
-			PX4_INFO("PASS: M0087_J7_PIN_5_OUT M0087_J7_PIN_9_IN");
-
-		} else {
-			PX4_ERR("FAIL: M0087_J7_PIN_5_OUT M0087_J7_PIN_9_IN");
-		}
-
-		//
-		//
-		//
-		PX4_INFO(">> Testing J10");
-		stm32_configgpio(M0087_J10_PIN_2_OUT); // 2-4
-		stm32_configgpio(M0087_J10_PIN_3_OUT); // 3-5
-		stm32_configgpio(M0087_J10_PIN_4_IN);  // 4-2
-		stm32_configgpio(M0087_J10_PIN_5_IN);  // 5-3
-
-		if (test_pair(M0087_J10_PIN_2_OUT, M0087_J10_PIN_4_IN)) {
-			PX4_INFO("PASS: M0087_J10_PIN_2_OUT M0087_J10_PIN_4_IN");
-
-		} else {
-			PX4_ERR("FAIL: M0087_J10_PIN_2_OUT M0087_J10_PIN_4_IN");
-		}
-
-		if (test_pair(M0087_J10_PIN_3_OUT, M0087_J10_PIN_5_IN)) {
-			PX4_INFO("PASS: M0087_J10_PIN_3_OUT M0087_J10_PIN_5_IN");
-
-		} else {
-			PX4_ERR("FAIL: M0087_J10_PIN_3_OUT M0087_J10_PIN_5_IN");
-		}
-
-
-		//
-		//
-		//
-		PX4_INFO(">> Testing J12");
-		stm32_configgpio(M0087_J12_PIN_2_OUT); // 2-3
-		stm32_configgpio(M0087_J12_PIN_3_IN);  // 3-2
-
-		if (test_pair(M0087_J12_PIN_2_OUT, M0087_J12_PIN_3_IN)) {
-			PX4_INFO("PASS: M0087_J12_PIN_2_OUT M0087_J12_PIN_3_IN");
-
-		} else {
-			PX4_ERR("FAIL: M0087_J12_PIN_2_OUT M0087_J12_PIN_3_IN");
-		}
-
-		//
-		//
-		//
-		PX4_INFO(">> Testing J14");
-		stm32_configgpio(M0087_J14_PIN_2_OUT);
-		stm32_configgpio(M0087_J14_PIN_3_OUT);
-		stm32_configgpio(M0087_J14_PIN_4_OUT);
-		stm32_configgpio(M0087_J14_PIN_5_OUT);
-		stm32_configgpio(M0087_J14_PIN_6_OUT);
-		stm32_configgpio(M0087_J14_PIN_7_IN);
-		stm32_configgpio(M0087_J14_PIN_8_IN);
-		stm32_configgpio(M0087_J14_PIN_9_IN);
-		stm32_configgpio(M0087_J14_PIN_10_IN);
-		stm32_configgpio(M0087_J14_PIN_11_IN);
-
-		if (test_pair(M0087_J14_PIN_2_OUT, M0087_J14_PIN_7_IN)) {
-			PX4_INFO("PASS: M0087_J14_PIN_2_OUT M0087_J14_PIN_7_IN");
-
-		} else {
-			PX4_ERR("FAIL: M0087_J14_PIN_2_OUT M0087_J14_PIN_7_IN");
-		}
-
-		if (test_pair(M0087_J14_PIN_3_OUT, M0087_J14_PIN_8_IN)) {
-			PX4_INFO("PASS: M0087_J14_PIN_3_OUT M0087_J14_PIN_8_IN");
-
-		} else {
-			PX4_ERR("FAIL: M0087_J14_PIN_3_OUT M0087_J14_PIN_8_IN");
-		}
-
-		if (test_pair(M0087_J14_PIN_4_OUT, M0087_J14_PIN_9_IN)) {
-			PX4_INFO("PASS: M0087_J14_PIN_4_OUT M0087_J14_PIN_9_IN");
-
-		} else {
-			PX4_ERR("FAIL: M0087_J14_PIN_4_OUT M0087_J14_PIN_9_IN");
-		}
-
-		if (test_pair(M0087_J14_PIN_5_OUT, M0087_J14_PIN_10_IN)) {
-			PX4_INFO("PASS: M0087_J14_PIN_5_OUT M0087_J14_PIN_10_IN");
-
-		} else {
-			PX4_ERR("FAIL: M0087_J14_PIN_5_OUT M0087_J14_PIN_10_IN");
-		}
-
-		if (test_pair(M0087_J14_PIN_6_OUT, M0087_J14_PIN_11_IN)) {
-			PX4_INFO("PASS: M0087_J14_PIN_6_OUT M0087_J14_PIN_11_IN");
-
-		} else {
-			PX4_ERR("FAIL: M0087_J14_PIN_6_OUT M0087_J14_PIN_11_IN");
-		}
-
-	}
-
-	return 0;
-}
-
-int modalai_hw_detect_v2(eHW_TYPE hw_type)
-{
-	int result = -1;
-
-	if (hw_type == eM0079) {
-		PX4_INFO("V230 - M0079");
-		result = 0;
-
-	} else if (hw_type == eM0087) {
-		PX4_INFO("V230 - M0087");
-		result = 0;
-
-	} else {
-		PX4_ERR("Unknown hardware");
-	}
-
-	return result;
-}
-
-#endif //CONFIG_ARCH_CHIP_STM32H753II

boards/modalai/src/systemcmds/modalai/modalai_fc-v2.h

@@ -1,183 +0,0 @@
-#ifndef MODALAI_FC_V2_H_
-#define MODALAI_FC_V2_H_
-
-typedef enum {
-	eHwUnknown = -1,
-	eHwNone = 0,
-	eM0079,		//FCv2
-	eM0087		//FCv2 Pro
-} 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))
-
-/*	M0079 Pins	*/
-
-//
-// TELEM1
-//   M0079- J1
-//             PF6   PIN2 - out
-//             PE8   PIN3 - in
-//	       PF8   PIN4 - out
-//	       PE10  PIN4 - in
-//
-#define M0079_J1_PIN_2_OUT             _MK_GPIO_OUTPUT(GPIO_PORTF|GPIO_PIN6)
-#define M0079_J1_PIN_3_IN              _MK_GPIO_INPUT(GPIO_PORTE|GPIO_PIN8)
-#define M0079_J1_PIN_4_OUT             _MK_GPIO_OUTPUT(GPIO_PORTF|GPIO_PIN8)
-#define M0079_J1_PIN_5_IN              _MK_GPIO_INPUT(GPIO_PORTE|GPIO_PIN10)
-
-//
-// TELEM2
-//   M0079- J5
-//             PC12 PIN2 - out
-//             PD2  PIN3 - out
-//	       PC9  PIN4 - in
-//	       PC8  PIN4 - in
-//
-#define M0079_J5_PIN_2_OUT             _MK_GPIO_OUTPUT(GPIO_PORTC|GPIO_PIN12)
-#define M0079_J5_PIN_3_OUT             _MK_GPIO_OUTPUT(GPIO_PORTD|GPIO_PIN2)
-#define M0079_J5_PIN_4_IN              _MK_GPIO_INPUT(GPIO_PORTC|GPIO_PIN9)
-#define M0079_J5_PIN_5_IN              _MK_GPIO_INPUT(GPIO_PORTC|GPIO_PIN8)
-
-//
-// PWM Output
-//   M0079- J7
-//             PI0  PIN2 - out
-//             PH12 PIN3 - out
-//             PH11 PIN4 - out
-//             PH10 PIN5 - out
-//
-//             PD13 PIN6 - in
-//             PD14 PIN7 - in
-//             PH6  PIN8 - in
-//             PH9  PIN9 - in
-//
-#define M0079_J7_PIN_2_OUT             _MK_GPIO_OUTPUT(GPIO_PORTI|GPIO_PIN0)
-#define M0079_J7_PIN_3_OUT             _MK_GPIO_OUTPUT(GPIO_PORTH|GPIO_PIN12)
-#define M0079_J7_PIN_4_OUT             _MK_GPIO_OUTPUT(GPIO_PORTH|GPIO_PIN11)
-#define M0079_J7_PIN_5_OUT             _MK_GPIO_OUTPUT(GPIO_PORTH|GPIO_PIN10)
-#define M0079_J7_PIN_6_IN              _MK_GPIO_INPUT(GPIO_PORTD|GPIO_PIN13)
-#define M0079_J7_PIN_7_IN              _MK_GPIO_INPUT(GPIO_PORTD|GPIO_PIN14)
-#define M0079_J7_PIN_8_IN              _MK_GPIO_INPUT(GPIO_PORTH|GPIO_PIN6)
-#define M0079_J7_PIN_9_IN              _MK_GPIO_INPUT(GPIO_PORTH|GPIO_PIN9)
-
-//
-// GPS/Mag
-//   M0079- J10
-//             PB6 PIN2 - out
-//             PB7 PIN3 - out
-//	       PB8 PIN4 - in
-//	       PB9 PIN4 - in
-//
-#define M0079_J10_PIN_2_OUT            _MK_GPIO_OUTPUT(GPIO_PORTB|GPIO_PIN6)
-#define M0079_J10_PIN_3_OUT            _MK_GPIO_OUTPUT(GPIO_PORTB|GPIO_PIN7)
-#define M0079_J10_PIN_4_IN             _MK_GPIO_INPUT(GPIO_PORTB|GPIO_PIN8)
-#define M0079_J10_PIN_5_IN             _MK_GPIO_INPUT(GPIO_PORTB|GPIO_PIN9)
-
-//
-// Spektrum RC Input Connector
-//   M0079- J12
-//             PC6 PIN2 - out
-//             PC7 PIN3 - in
-//
-#define M0079_J12_PIN_2_OUT             _MK_GPIO_OUTPUT(GPIO_PORTC|GPIO_PIN6)
-#define M0079_J12_PIN_3_IN              _MK_GPIO_INPUT(GPIO_PORTC|GPIO_PIN7)
-
-
-/*	M0087 Pins 	*/
-
-//
-// TELEM1
-//   M0087- J1
-//             PF6   PIN2 - in
-//             PE8   PIN3 - out
-//	       PF8   PIN4 - out
-//	       PE10  PIN5 - in
-//
-#define M0087_J1_PIN_2_IN             	_MK_GPIO_INPUT(GPIO_PORTF|GPIO_PIN6)
-#define M0087_J1_PIN_3_OUT              _MK_GPIO_OUTPUT(GPIO_PORTE|GPIO_PIN8)
-#define M0087_J1_PIN_4_OUT             	_MK_GPIO_OUTPUT(GPIO_PORTF|GPIO_PIN8)
-#define M0087_J1_PIN_5_IN              	_MK_GPIO_INPUT(GPIO_PORTE|GPIO_PIN10)
-
-//
-// TELEM2
-//   M0087- J5
-//             PC12 PIN2 - out
-//             PD2  PIN3 - out
-//	       PC9  PIN4 - in
-//	       PC8  PIN5 - in
-//
-#define M0087_J5_PIN_2_OUT             _MK_GPIO_OUTPUT(GPIO_PORTC|GPIO_PIN12)
-#define M0087_J5_PIN_3_OUT             _MK_GPIO_OUTPUT(GPIO_PORTD|GPIO_PIN2)
-#define M0087_J5_PIN_4_IN              _MK_GPIO_INPUT(GPIO_PORTC|GPIO_PIN9)
-#define M0087_J5_PIN_5_IN              _MK_GPIO_INPUT(GPIO_PORTC|GPIO_PIN8)
-
-//
-// PWM Output
-//   M0087- J7
-//             PI0  PIN2 - out
-//             PH12 PIN3 - out
-//             PH11 PIN4 - out
-//             PH10 PIN5 - out
-//
-//             PD13 PIN6 - in
-//             PD14 PIN7 - in
-//             PH6  PIN8 - in
-//             PH9  PIN9 - in
-//
-#define M0087_J7_PIN_2_OUT             _MK_GPIO_OUTPUT(GPIO_PORTI|GPIO_PIN0)
-#define M0087_J7_PIN_3_OUT             _MK_GPIO_OUTPUT(GPIO_PORTH|GPIO_PIN12)
-#define M0087_J7_PIN_4_OUT             _MK_GPIO_OUTPUT(GPIO_PORTH|GPIO_PIN11)
-#define M0087_J7_PIN_5_OUT             _MK_GPIO_OUTPUT(GPIO_PORTH|GPIO_PIN10)
-#define M0087_J7_PIN_6_IN              _MK_GPIO_INPUT(GPIO_PORTD|GPIO_PIN13)
-#define M0087_J7_PIN_7_IN              _MK_GPIO_INPUT(GPIO_PORTD|GPIO_PIN14)
-#define M0087_J7_PIN_8_IN              _MK_GPIO_INPUT(GPIO_PORTH|GPIO_PIN6)
-#define M0087_J7_PIN_9_IN              _MK_GPIO_INPUT(GPIO_PORTH|GPIO_PIN9)
-
-//
-// GPS/Mag
-//   M0087- J10
-//             PB6 PIN2 - out
-//             PB7 PIN3 - out
-//	       PB8 PIN4 - in
-//	       PB9 PIN5 - in
-//
-#define M0087_J10_PIN_2_OUT            _MK_GPIO_OUTPUT(GPIO_PORTB|GPIO_PIN6)
-#define M0087_J10_PIN_3_OUT            _MK_GPIO_OUTPUT(GPIO_PORTB|GPIO_PIN7)
-#define M0087_J10_PIN_4_IN             _MK_GPIO_INPUT(GPIO_PORTB|GPIO_PIN8)
-#define M0087_J10_PIN_5_IN             _MK_GPIO_INPUT(GPIO_PORTB|GPIO_PIN9)
-
-//
-// Spektrum RC Input Connector
-//   M0087- J12
-//             PC6 PIN2 - out
-//             PC7 PIN3 - in
-//
-#define M0087_J12_PIN_2_OUT             _MK_GPIO_OUTPUT(GPIO_PORTC|GPIO_PIN6)
-#define M0087_J12_PIN_3_IN              _MK_GPIO_INPUT(GPIO_PORTC|GPIO_PIN7)
-
-//
-// AVIATOR SPI/I2C/ADC EXPANSION CONNECTOR
-//   M0087- J14
-//             PC6 PIN2 - out
-//             PC7 PIN3 - in
-//
-#define  M0087_J14_PIN_2_OUT				_MK_GPIO_OUTPUT(GPIO_PORTA|GPIO_PIN6)
-#define  M0087_J14_PIN_3_OUT				_MK_GPIO_OUTPUT(GPIO_PORTG|GPIO_PIN14)
-#define  M0087_J14_PIN_4_OUT				_MK_GPIO_OUTPUT(GPIO_PORTB|GPIO_PIN3)
-#define  M0087_J14_PIN_5_OUT				_MK_GPIO_OUTPUT(GPIO_PORTI|GPIO_PIN10)
-#define  M0087_J14_PIN_6_OUT				_MK_GPIO_OUTPUT(GPIO_PORTF|GPIO_PIN0)
-#define  M0087_J14_PIN_7_IN				_MK_GPIO_INPUT(GPIO_PORTF|GPIO_PIN1)
-#define  M0087_J14_PIN_8_IN				_MK_GPIO_INPUT(GPIO_PORTF|GPIO_PIN12)
-#define  M0087_J14_PIN_9_IN				_MK_GPIO_INPUT(GPIO_PORTB|GPIO_PIN0)
-#define  M0087_J14_PIN_10_IN				_MK_GPIO_INPUT(GPIO_PORTA|GPIO_PIN0)
-#define  M0087_J14_PIN_11_IN				_MK_GPIO_INPUT(GPIO_PORTA|GPIO_PIN4)
-
-void	modalai_print_usage_v2(void);
-void	modalai_print_usage_con_gpio_test_v2(void);
-int 	modalai_con_gpio_test_v2(uint8_t con, uint8_t pin, bool state);
-int 	modalai_led_test_v2(void);
-int 	modalai_buzz_test_v2(eHW_TYPE type);
-int 	modalai_hw_detect_v2(eHW_TYPE type);
-
-#endif //MODALAI_FC_V2_H_

boards/modalai/voxl2-io/default.px4board

@@ -0,0 +1,5 @@
+CONFIG_BOARD_TOOLCHAIN="arm-none-eabi"
+CONFIG_BOARD_ARCHITECTURE="cortex-m3"
+CONFIG_BOARD_ROMFSROOT=""
+CONFIG_BOARD_CONSTRAINED_FLASH=y
+CONFIG_MODULES_PX4IOFIRMWARE=y

boards/modalai/voxl2-io/firmware.prototype

@@ -0,0 +1,13 @@
+{
+    "board_id": 41777,
+    "magic": "PX4FWv2",
+    "description": "Firmware for the voxl2-io board",
+    "image": "",
+    "build_time": 0,
+    "summary": "voxl2io",
+    "version": "2.0",
+    "image_size": 0,
+    "image_maxsize": 61440,
+    "git_identity": "",
+    "board_revision": 0
+}

boards/modalai/voxl2-io/nuttx-config/include/board.h

@@ -0,0 +1,141 @@
+/************************************************************************************
+ * nuttx-configs/px4io/include/board.h
+ * include/arch/board/board.h
+ *
+ *   Copyright (C) 2009 Gregory Nutt. All rights reserved.
+ *   Author: Gregory Nutt <gnutt@nuttx.org>
+ *   Copyright (C) 2012 PX4 Development Team. All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright
+ *    notice, this list of conditions and the following disclaimer.
+ * 2. Redistributions in binary form must reproduce the above copyright
+ *    notice, this list of conditions and the following disclaimer in
+ *    the documentation and/or other materials provided with the
+ *    distribution.
+ * 3. Neither the name NuttX nor the names of its contributors may be
+ *    used to endorse or promote products derived from this software
+ *    without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
+ * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
+ * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *
+ ************************************************************************************/
+
+#ifndef __ARCH_BOARD_BOARD_H
+#define __ARCH_BOARD_BOARD_H
+
+/************************************************************************************
+ * Included Files
+ ************************************************************************************/
+
+#include <nuttx/config.h>
+#ifndef __ASSEMBLY__
+# include <stdint.h>
+#endif
+#include <stm32.h>
+
+/************************************************************************************
+ * Definitions
+ ************************************************************************************/
+
+/* Clocking *************************************************************************/
+
+/* On-board crystal frequency is 24MHz (HSE) */
+
+#define STM32_BOARD_XTAL        16000000ul
+
+/* Use the HSE output as the system clock */
+
+#define STM32_SYSCLK_SW         RCC_CFGR_SW_HSE
+#define STM32_SYSCLK_SWS        RCC_CFGR_SWS_HSE
+#define STM32_SYSCLK_FREQUENCY  STM32_BOARD_XTAL
+
+/* AHB clock (HCLK) is SYSCLK (24MHz) */
+
+#define STM32_RCC_CFGR_HPRE     RCC_CFGR_HPRE_SYSCLK
+#define STM32_HCLK_FREQUENCY    STM32_SYSCLK_FREQUENCY
+#define STM32_BOARD_HCLK        STM32_HCLK_FREQUENCY    /* same as above, to satisfy compiler */
+
+/* APB2 clock (PCLK2) is HCLK (24MHz) */
+
+#define STM32_RCC_CFGR_PPRE2    RCC_CFGR_PPRE2_HCLK
+#define STM32_PCLK2_FREQUENCY   STM32_HCLK_FREQUENCY
+#define STM32_APB2_CLKIN        (STM32_PCLK2_FREQUENCY)   /* Timers 2-4 */
+
+/* APB2 timer 1 will receive PCLK2. */
+
+#define STM32_APB2_TIM1_CLKIN   (STM32_PCLK2_FREQUENCY)
+#define STM32_APB2_TIM15_CLKIN  (STM32_PCLK2_FREQUENCY)
+#define STM32_APB2_TIM16_CLKIN  (STM32_PCLK2_FREQUENCY)
+#define STM32_APB2_TIM17_CLKIN  (STM32_PCLK2_FREQUENCY)
+
+/* APB1 clock (PCLK1) is HCLK (24MHz) */
+
+#define STM32_RCC_CFGR_PPRE1    RCC_CFGR_PPRE1_HCLK
+#define STM32_PCLK1_FREQUENCY   (STM32_HCLK_FREQUENCY)
+
+/* All timers run off PCLK */
+
+
+#define STM32_APB1_TIM2_CLKIN   (STM32_PCLK1_FREQUENCY)
+#define STM32_APB1_TIM3_CLKIN   (STM32_PCLK1_FREQUENCY)
+#define STM32_APB1_TIM4_CLKIN   (STM32_PCLK1_FREQUENCY)
+#define STM32_APB1_TIM5_CLKIN   (STM32_PCLK1_FREQUENCY)
+#define STM32_APB1_TIM6_CLKIN   (STM32_PCLK1_FREQUENCY)
+#define STM32_APB1_TIM7_CLKIN   (STM32_PCLK1_FREQUENCY)
+#define STM32_APB1_TIM12_CLKIN  (STM32_PCLK1_FREQUENCY)
+#define STM32_APB1_TIM13_CLKIN  (STM32_PCLK1_FREQUENCY)
+#define STM32_APB1_TIM14_CLKIN  (STM32_PCLK1_FREQUENCY)
+
+/* Timer Frequencies, if APBx is set to 1, frequency is same to APBx
+ * otherwise frequency is 2xAPBx.
+ * Note: TIM1, 15-17 are on APB2, others on APB1
+ */
+
+#define BOARD_TIM1_FREQUENCY    STM32_APB2_TIM1_CLKIN
+#define BOARD_TIM2_FREQUENCY    STM32_APB1_TIM2_CLKIN
+#define BOARD_TIM3_FREQUENCY    STM32_APB1_TIM3_CLKIN
+#define BOARD_TIM4_FREQUENCY    STM32_APB1_TIM4_CLKIN
+#define BOARD_TIM5_FREQUENCY    STM32_APB1_TIM5_CLKIN
+#define BOARD_TIM6_FREQUENCY    STM32_APB1_TIM6_CLKIN
+#define BOARD_TIM7_FREQUENCY    STM32_APB1_TIM7_CLKIN
+#define BOARD_TIM12_FREQUENCY   STM32_APB1_TIM12_CLKIN
+#define BOARD_TIM13_FREQUENCY   STM32_APB1_TIM13_CLKIN
+#define BOARD_TIM14_FREQUENCY   STM32_APB1_TIM14_CLKIN
+#define BOARD_TIM15_FREQUENCY   STM32_APB2_TIM15_CLKIN
+#define BOARD_TIM16_FREQUENCY   STM32_APB2_TIM16_CLKIN
+#define BOARD_TIM17_FREQUENCY   STM32_APB2_TIM17_CLKIN
+
+
+/*
+ * Some of the USART pins are not available; override the GPIO
+ * definitions with an invalid pin configuration.
+ */
+#undef GPIO_USART2_CTS
+#define GPIO_USART2_CTS	0xffffffff
+#undef GPIO_USART2_RTS
+#define GPIO_USART2_RTS	0xffffffff
+#undef GPIO_USART2_CK
+#define GPIO_USART2_CK 	0xffffffff
+#undef GPIO_USART3_CK
+#define GPIO_USART3_CK 	0xffffffff
+#undef GPIO_USART3_CTS
+#define GPIO_USART3_CTS	0xffffffff
+#undef GPIO_USART3_RTS
+#define GPIO_USART3_RTS	0xffffffff
+
+#endif  /* __ARCH_BOARD_BOARD_H */

boards/modalai/voxl2-io/nuttx-config/nsh/defconfig

@@ -0,0 +1,61 @@
+#
+# This file is autogenerated: PLEASE DO NOT EDIT IT.
+#
+# You can use "make menuconfig" to make any modifications to the installed .config file.
+# You can then do "make savedefconfig" to generate a new defconfig file that includes your
+# modifications.
+#
+# CONFIG_DEV_NULL is not set
+CONFIG_ARCH="arm"
+CONFIG_ARCH_BOARD_CUSTOM=y
+CONFIG_ARCH_BOARD_CUSTOM_DIR="../../../../boards/modalai/voxl2-io/nuttx-config"
+CONFIG_ARCH_BOARD_CUSTOM_DIR_RELPATH=y
+CONFIG_ARCH_BOARD_CUSTOM_NAME="px4"
+CONFIG_ARCH_CHIP="stm32"
+CONFIG_ARCH_CHIP_STM32=y
+CONFIG_ARCH_CHIP_STM32F100C8=y
+CONFIG_ARMV7M_USEBASEPRI=y
+CONFIG_ARM_MPU_EARLY_RESET=y
+CONFIG_BOARD_LOOPSPERMSEC=2000
+CONFIG_DEBUG_FULLOPT=y
+CONFIG_DEBUG_SYMBOLS=y
+CONFIG_DEFAULT_SMALL=y
+CONFIG_DISABLE_MOUNTPOINT=y
+CONFIG_FDCLONE_DISABLE=y
+CONFIG_FDCLONE_STDIO=y
+CONFIG_HAVE_CXX=y
+CONFIG_HAVE_CXXINITIALIZE=y
+CONFIG_IDLETHREAD_STACKSIZE=316
+CONFIG_INIT_ENTRYPOINT="user_start"
+CONFIG_INIT_STACKSIZE=1100
+CONFIG_MM_FILL_ALLOCATIONS=y
+CONFIG_MM_SMALL=y
+CONFIG_NAME_MAX=12
+CONFIG_PREALLOC_TIMERS=0
+CONFIG_RAM_SIZE=8192
+CONFIG_RAM_START=0x20000000
+CONFIG_RAW_BINARY=y
+CONFIG_SERIAL_TERMIOS=y
+CONFIG_STACK_COLORATION=y
+CONFIG_START_DAY=30
+CONFIG_START_MONTH=11
+CONFIG_STDIO_DISABLE_BUFFERING=y
+CONFIG_STM32_ADC1=y
+CONFIG_STM32_DISABLE_IDLE_SLEEP_DURING_DEBUG=y
+CONFIG_STM32_DMA1=y
+CONFIG_STM32_JTAG_SW_ENABLE=y
+CONFIG_STM32_USART1=y
+CONFIG_STM32_USART2=y
+CONFIG_STM32_USART3=y
+CONFIG_STM32_USART_SINGLEWIRE=y
+CONFIG_TASK_NAME_SIZE=0
+CONFIG_USART1_RXBUFSIZE=64
+CONFIG_USART1_RXDMA=y
+CONFIG_USART1_SERIAL_CONSOLE=y
+CONFIG_USART1_TXBUFSIZE=32
+CONFIG_USART2_RXBUFSIZE=64
+CONFIG_USART2_TXBUFSIZE=64
+CONFIG_USART3_RXBUFSIZE=64
+CONFIG_USART3_RXDMA=y
+CONFIG_USART3_TXBUFSIZE=64
+CONFIG_USEC_PER_TICK=1000

boards/modalai/voxl2-io/nuttx-config/scripts/script.ld

@@ -0,0 +1,131 @@
+/****************************************************************************
+ * configs/px4io-v2/scripts/ld.script
+ *
+ *   Copyright (C) 2009, 2011 Gregory Nutt. All rights reserved.
+ *   Author: Gregory Nutt <spudmonkey@racsa.co.cr>
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright
+ *    notice, this list of conditions and the following disclaimer.
+ * 2. Redistributions in binary form must reproduce the above copyright
+ *    notice, this list of conditions and the following disclaimer in
+ *    the documentation and/or other materials provided with the
+ *    distribution.
+ * 3. Neither the name NuttX nor the names of its contributors may be
+ *    used to endorse or promote products derived from this software
+ *    without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
+ * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
+ * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *
+ ****************************************************************************/
+
+/* The STM32F100C8 has 64Kb of FLASH beginning at address 0x0800:0000 and
+ * 8Kb of SRAM beginning at address 0x2000:0000.  When booting from FLASH,
+ * FLASH memory is aliased to address 0x0000:0000 where the code expects to
+ * begin execution by jumping to the entry point in the 0x0800:0000 address
+ * range.
+ */
+
+MEMORY
+{
+    flash (rx) : ORIGIN = 0x08001000, LENGTH = 60K
+    sram (rwx) : ORIGIN = 0x20000000, LENGTH = 8K
+}
+
+OUTPUT_ARCH(arm)
+ENTRY(__start)		/* treat __start as the anchor for dead code stripping */
+EXTERN(_vectors)	/* force the vectors to be included in the output */
+
+/*
+ * Ensure that abort() is present in the final object.  The exception handling
+ * code pulled in by libgcc.a requires it (and that code cannot be easily avoided).
+ */
+EXTERN(abort)
+
+SECTIONS
+{
+	.text : {
+		_stext = ABSOLUTE(.);
+		*(.vectors)
+		*(.text .text.*)
+		*(.fixup)
+		*(.gnu.warning)
+		*(.rodata .rodata.*)
+		*(.gnu.linkonce.t.*)
+		*(.glue_7)
+		*(.glue_7t)
+		*(.got)
+		*(.gcc_except_table)
+		*(.gnu.linkonce.r.*)
+		_etext = ABSOLUTE(.);
+	} > flash
+
+	/*
+	 * Init functions (static constructors and the like)
+	 */
+        .init_section : {
+                _sinit = ABSOLUTE(.);
+                KEEP(*(.init_array .init_array.*))
+                _einit = ABSOLUTE(.);
+        } > flash
+
+	.ARM.extab : {
+		*(.ARM.extab*)
+	} > flash
+
+	__exidx_start = ABSOLUTE(.);
+	.ARM.exidx : {
+		*(.ARM.exidx*)
+	} > flash
+	__exidx_end = ABSOLUTE(.);
+
+	_eronly = ABSOLUTE(.);
+
+	/* The STM32F100CB has 8Kb of SRAM beginning at the following address */
+
+	.data : {
+		_sdata = ABSOLUTE(.);
+		*(.data .data.*)
+		*(.gnu.linkonce.d.*)
+		CONSTRUCTORS
+		_edata = ABSOLUTE(.);
+		. = ALIGN(4);
+	} > sram AT > flash
+
+	.bss : {
+		_sbss = ABSOLUTE(.);
+		*(.bss .bss.*)
+		*(.gnu.linkonce.b.*)
+		*(COMMON)
+		. = ALIGN(4);
+		_ebss = ABSOLUTE(.);
+	} > sram
+
+	/* Stabs debugging sections. */
+	.stab 0 : { *(.stab) }
+	.stabstr 0 : { *(.stabstr) }
+	.stab.excl 0 : { *(.stab.excl) }
+	.stab.exclstr 0 : { *(.stab.exclstr) }
+	.stab.index 0 : { *(.stab.index) }
+	.stab.indexstr 0 : { *(.stab.indexstr) }
+	.comment 0 : { *(.comment) }
+	.debug_abbrev 0 : { *(.debug_abbrev) }
+	.debug_info 0 : { *(.debug_info) }
+	.debug_line 0 : { *(.debug_line) }
+	.debug_pubnames 0 : { *(.debug_pubnames) }
+	.debug_aranges 0 : { *(.debug_aranges) }
+}

boards/modalai/voxl2-io/src/CMakeLists.txt

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

boards/modalai/voxl2-io/src/board_config.h

@@ -0,0 +1,157 @@
+/****************************************************************************
+ *
+ *   Copyright (c) 2013, 2014 PX4 Development Team. All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright
+ *    notice, this list of conditions and the following disclaimer.
+ * 2. Redistributions in binary form must reproduce the above copyright
+ *    notice, this list of conditions and the following disclaimer in
+ *    the documentation and/or other materials provided with the
+ *    distribution.
+ * 3. Neither the name PX4 nor the names of its contributors may be
+ *    used to endorse or promote products derived from this software
+ *    without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
+ * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
+ * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *
+ ****************************************************************************/
+
+/**
+ * @file board_config.h
+ *
+ * PX4IOV2 internal definitions
+ */
+
+#pragma once
+
+/******************************************************************************
+ * Included Files
+ ******************************************************************************/
+
+#include <px4_platform_common/px4_config.h>
+#include <nuttx/compiler.h>
+#include <stdint.h>
+
+/******************************************************************************
+ * Definitions
+ ******************************************************************************/
+/* Configuration **************************************************************/
+
+/******************************************************************************
+ * Serial
+ ******************************************************************************/
+#define PX4FMU_SERIAL_BASE	STM32_USART2_BASE
+#define PX4FMU_SERIAL_VECTOR	STM32_IRQ_USART2
+#define PX4FMU_SERIAL_TX_GPIO	GPIO_USART2_TX
+#define PX4FMU_SERIAL_RX_GPIO	GPIO_USART2_RX
+#define PX4FMU_SERIAL_TX_DMA	DMACHAN_USART2_TX
+#define PX4FMU_SERIAL_RX_DMA	DMACHAN_USART2_RX
+#define PX4FMU_SERIAL_CLOCK	STM32_PCLK1_FREQUENCY
+#define PX4FMU_SERIAL_BITRATE	921600
+
+/******************************************************************************
+ * GPIOS
+ ******************************************************************************/
+
+/* LEDS  **********************************************************************/
+
+#define GPIO_LED_BLUE (GPIO_OUTPUT|GPIO_CNF_OUTOD|GPIO_MODE_50MHz|GPIO_OUTPUT_SET|GPIO_PORTB|GPIO_PIN14)
+#define GPIO_LED_AMBER (GPIO_OUTPUT|GPIO_CNF_OUTOD|GPIO_MODE_50MHz|GPIO_OUTPUT_SET|GPIO_PORTB|GPIO_PIN15)
+#define GPIO_LED_SAFETY (GPIO_OUTPUT|GPIO_CNF_OUTOD|GPIO_MODE_50MHz|GPIO_OUTPUT_SET|GPIO_PORTB|GPIO_PIN13)
+
+#define LED_BLUE(on_true)   stm32_gpiowrite(GPIO_LED_BLUE, !(on_true))
+#define LED_AMBER(on_true)  stm32_gpiowrite(GPIO_LED_AMBER, !(on_true))
+#define LED_SAFETY(on_true) stm32_gpiowrite(GPIO_LED_SAFETY, !(on_true))
+
+//#define GPIO_LED4 (GPIO_OUTPUT|GPIO_CNF_OUTOD|GPIO_MODE_50MHz|GPIO_OUTPUT_SET|GPIO_PORTA|GPIO_PIN11)
+
+//#define GPIO_HEATER_OFF 0 // (GPIO_OUTPUT|GPIO_CNF_OUTPP|GPIO_MODE_50MHz|GPIO_OUTPUT_SET|GPIO_PORTB|GPIO_PIN14)
+
+#define GPIO_PC14 (GPIO_INPUT|GPIO_CNF_INFLOAT|GPIO_MODE_INPUT|GPIO_PORTC|GPIO_PIN14)
+#define GPIO_PC15 (GPIO_INPUT|GPIO_CNF_INFLOAT|GPIO_MODE_INPUT|GPIO_PORTC|GPIO_PIN15)
+
+#define GPIO_SENSE_PC14_DN (GPIO_INPUT|GPIO_CNF_INPULLDWN|GPIO_MODE_INPUT|GPIO_PORTC|GPIO_PIN14)
+#define GPIO_SENSE_PC15_UP (GPIO_INPUT|GPIO_CNF_INPULLUP|GPIO_MODE_INPUT|GPIO_PORTC|GPIO_PIN15)
+# define SENSE_PH1 0b10 /* Floating pulled as set */
+# define SENSE_PH2 0b01 /* Driven as tied */
+
+#define GPIO_USART1_RX_SPEKTRUM		(GPIO_OUTPUT|GPIO_CNF_OUTPP|GPIO_MODE_50MHz|GPIO_OUTPUT_SET|GPIO_PORTA|GPIO_PIN10)
+
+/* Safety switch button *******************************************************/
+
+/* CONNECTED TO TP8 - pulled down via SW */
+
+#define GPIO_BTN_SAFETY (GPIO_INPUT|GPIO_CNF_INPULLDWN|GPIO_MODE_INPUT|GPIO_PORTB|GPIO_PIN5)
+
+/* Power switch controls ******************************************************/
+
+#define GPIO_SPEKTRUM_PWR_EN         (GPIO_OUTPUT|GPIO_CNF_OUTPP|GPIO_MODE_50MHz|GPIO_OUTPUT_SET|GPIO_PORTC|GPIO_PIN13)
+#define SPEKTRUM_POWER(_on_true)     px4_arch_gpiowrite(GPIO_SPEKTRUM_PWR_EN, (_on_true))
+
+#define SPEKTRUM_OUT(_one_true)      px4_arch_gpiowrite(GPIO_USART1_RX_SPEKTRUM, (_one_true))
+#define SPEKTRUM_RX_AS_UART()        px4_arch_configgpio(GPIO_USART1_RX)
+#define SPEKTRUM_RX_AS_GPIO_OUTPUT() px4_arch_configgpio(GPIO_USART1_RX_SPEKTRUM)
+
+#define GPIO_SERVO_FAULT_DETECT 0 // (GPIO_INPUT|GPIO_CNF_INPULLUP|GPIO_MODE_INPUT|GPIO_PORTA|GPIO_PIN15)
+
+/* Analog inputs **************************************************************/
+
+/* PWM pins  **************************************************************/
+
+#define GPIO_PWM1 (GPIO_OUTPUT|GPIO_CNF_OUTPP|GPIO_MODE_50MHz|GPIO_OUTPUT_CLEAR|GPIO_PORTA|GPIO_PIN0)
+#define GPIO_PWM2 (GPIO_OUTPUT|GPIO_CNF_OUTPP|GPIO_MODE_50MHz|GPIO_OUTPUT_CLEAR|GPIO_PORTA|GPIO_PIN1)
+#define GPIO_PWM3 (GPIO_OUTPUT|GPIO_CNF_OUTPP|GPIO_MODE_50MHz|GPIO_OUTPUT_CLEAR|GPIO_PORTB|GPIO_PIN8)
+#define GPIO_PWM4 (GPIO_OUTPUT|GPIO_CNF_OUTPP|GPIO_MODE_50MHz|GPIO_OUTPUT_CLEAR|GPIO_PORTB|GPIO_PIN9)
+#define GPIO_PWM5 (GPIO_OUTPUT|GPIO_CNF_OUTPP|GPIO_MODE_50MHz|GPIO_OUTPUT_CLEAR|GPIO_PORTA|GPIO_PIN6)
+#define GPIO_PWM6 (GPIO_OUTPUT|GPIO_CNF_OUTPP|GPIO_MODE_50MHz|GPIO_OUTPUT_CLEAR|GPIO_PORTA|GPIO_PIN7)
+#define GPIO_PWM7 (GPIO_OUTPUT|GPIO_CNF_OUTPP|GPIO_MODE_50MHz|GPIO_OUTPUT_CLEAR|GPIO_PORTB|GPIO_PIN0)
+#define GPIO_PWM8 (GPIO_OUTPUT|GPIO_CNF_OUTPP|GPIO_MODE_50MHz|GPIO_OUTPUT_CLEAR|GPIO_PORTB|GPIO_PIN1)
+
+#define DIRECT_PWM_OUTPUT_CHANNELS 8
+#define BOARD_HAS_NO_CAPTURE
+
+/* SBUS pins  *************************************************************/
+
+/* XXX these should be UART pins */
+#define GPIO_SBUS_INPUT   (GPIO_INPUT|GPIO_CNF_INFLOAT|GPIO_MODE_INPUT|GPIO_PORTB|GPIO_PIN11)
+#define GPIO_SBUS_OUTPUT  (GPIO_OUTPUT|GPIO_CNF_OUTPP|GPIO_MODE_50MHz|GPIO_OUTPUT_CLEAR|GPIO_PORTB|GPIO_PIN10)
+#define GPIO_SBUS_OENABLE 0 // (GPIO_OUTPUT|GPIO_CNF_OUTPP|GPIO_MODE_50MHz|GPIO_OUTPUT_SET|GPIO_PORTB|GPIO_PIN4)
+
+/*
+ * High-resolution timer
+ */
+#define HRT_TIMER		1	/* use timer1 for the HRT */
+#define HRT_TIMER_CHANNEL	2	/* use capture/compare channel 2 */
+#define HRT_PPM_CHANNEL		1	/* use capture/compare channel 1 PA8 */
+#define GPIO_PPM_IN		(GPIO_ALT|GPIO_CNF_INPULLUP|GPIO_PORTA|GPIO_PIN8)
+
+/* LED definitions ******************************************************************/
+/* PX4 has two LEDs that we will encode as: */
+
+#define LED_STARTED       0  /* LED? */
+#define LED_HEAPALLOCATE  1  /* LED? */
+#define LED_IRQSENABLED   2  /* LED? + LED? */
+#define LED_STACKCREATED  3  /* LED? */
+#define LED_INIRQ         4  /* LED? + LED? */
+#define LED_SIGNAL        5  /* LED? + LED? */
+#define LED_ASSERTION     6  /* LED? + LED? + LED? */
+#define LED_PANIC         7  /* N/C  + N/C  + N/C + LED? */
+
+#define BOARD_NUM_IO_TIMERS 3
+
+#include <px4_platform_common/board_common.h>

boards/modalai/voxl2-io/src/init.c

@@ -0,0 +1,133 @@
+/****************************************************************************
+ *
+ *   Copyright (C) 2012 PX4 Development Team. All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright
+ *    notice, this list of conditions and the following disclaimer.
+ * 2. Redistributions in binary form must reproduce the above copyright
+ *    notice, this list of conditions and the following disclaimer in
+ *    the documentation and/or other materials provided with the
+ *    distribution.
+ * 3. Neither the name PX4 nor the names of its contributors may be
+ *    used to endorse or promote products derived from this software
+ *    without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
+ * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
+ * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *
+ ****************************************************************************/
+
+/**
+ * @file init.c
+ *
+ * PX4FMU-specific early startup code.  This file implements the
+ * stm32_boardinitialize() function that is called during cpu startup.
+ *
+ * Code here is run before the rcS script is invoked; it should start required
+ * subsystems and perform board-specific initialization.
+ */
+
+/****************************************************************************
+ * Included Files
+ ****************************************************************************/
+
+#include <px4_platform_common/px4_config.h>
+
+#include <stdbool.h>
+#include <stdio.h>
+#include <debug.h>
+#include <errno.h>
+#include <syslog.h>
+
+#include <nuttx/board.h>
+
+#include <stm32.h>
+#include "board_config.h"
+
+#include <arch/board/board.h>
+
+/****************************************************************************
+ * Pre-Processor Definitions
+ ****************************************************************************/
+
+/****************************************************************************
+ * Protected Functions
+ ****************************************************************************/
+
+/****************************************************************************
+ * Public Functions
+ ****************************************************************************/
+
+/************************************************************************************
+ * Name: stm32_boardinitialize
+ *
+ * Description:
+ *   All STM32 architectures must provide the following entry point.  This entry point
+ *   is called early in the intitialization -- after all memory has been configured
+ *   and mapped but before any devices have been initialized.
+ *
+ ************************************************************************************/
+
+__EXPORT void stm32_boardinitialize(void)
+{
+	/* configure GPIOs */
+
+	/* Set up for sensing HW */
+	stm32_configgpio(GPIO_SENSE_PC14_DN);
+	stm32_configgpio(GPIO_SENSE_PC15_UP);
+
+	/* LEDS - default to off */
+	stm32_configgpio(GPIO_LED_BLUE);
+	stm32_configgpio(GPIO_LED_AMBER);
+	stm32_configgpio(GPIO_LED_SAFETY);
+
+	stm32_configgpio(GPIO_PC14);
+	stm32_configgpio(GPIO_PC15);
+
+
+	stm32_configgpio(GPIO_BTN_SAFETY);
+
+	/* spektrum power enable is active high - enable it by default */
+	stm32_configgpio(GPIO_SPEKTRUM_PWR_EN);
+
+	stm32_configgpio(GPIO_SBUS_INPUT); /* xxx alternate function */
+	stm32_configgpio(GPIO_SBUS_OUTPUT);
+
+	stm32_gpiowrite(GPIO_PWM1, true);
+	stm32_configgpio(GPIO_PWM1);
+
+	stm32_gpiowrite(GPIO_PWM2, true);
+	stm32_configgpio(GPIO_PWM2);
+
+	stm32_gpiowrite(GPIO_PWM3, true);
+	stm32_configgpio(GPIO_PWM3);
+
+	stm32_gpiowrite(GPIO_PWM4, true);
+	stm32_configgpio(GPIO_PWM4);
+
+	stm32_gpiowrite(GPIO_PWM5, true);
+	stm32_configgpio(GPIO_PWM5);
+
+	stm32_gpiowrite(GPIO_PWM6, true);
+	stm32_configgpio(GPIO_PWM6);
+
+	stm32_gpiowrite(GPIO_PWM7, true);
+	stm32_configgpio(GPIO_PWM7);
+
+	stm32_gpiowrite(GPIO_PWM8, true);
+	stm32_configgpio(GPIO_PWM8);
+}

boards/modalai/voxl2-io/src/timer_config.cpp

@@ -1,6 +1,6 @@
 /****************************************************************************
  *
- *   Copyright (c) 2023 PX4 Development Team. All rights reserved.
+ *  Copyright (C) 2012 PX4 Development Team. All rights reserved.
  *
  * Redistribution and use in source and binary forms, with or without
  * modification, are permitted provided that the following conditions
@@ -31,42 +31,24 @@
  *
  ****************************************************************************/
 
-#pragma once
+#include <px4_arch/io_timer_hw_description.h>
 
-#include <px4_platform_common/atomic_bitset.h>
-#include "atomic_transaction.h"
-#include "param.h"
-#include <stdlib.h>
-
-class ParamLayer
-{
-public:
-	static constexpr param_t PARAM_COUNT = sizeof(px4::parameters) / sizeof(param_info_s);
-
-	ParamLayer(ParamLayer *parent = nullptr) : _parent(parent) {}
-
-	ParamLayer(const ParamLayer &) = delete;
-	ParamLayer &operator=(const ParamLayer &) = delete;
-	ParamLayer(ParamLayer &&) = delete;
-	ParamLayer &operator=(ParamLayer &&) = delete;
-
-
-	virtual bool store(param_t param, param_value_u value) = 0;
-
-	virtual bool contains(param_t param) const = 0;
-
-	virtual px4::AtomicBitset<PARAM_COUNT> containedAsBitset() const = 0;
-
-	virtual param_value_u get(param_t param) const = 0;
-
-	virtual void reset(param_t param) = 0;
-
-	virtual void refresh(param_t param) = 0;
-
-	virtual int size() const = 0;
-
-	virtual int byteSize() const = 0;
-
-protected:
-	ParamLayer *const _parent;
+constexpr io_timers_t io_timers[MAX_IO_TIMERS] = {
+	initIOTimer(Timer::Timer2),
+	initIOTimer(Timer::Timer3),
+	initIOTimer(Timer::Timer4),
 };
+
+constexpr timer_io_channels_t timer_io_channels[MAX_TIMER_IO_CHANNELS] = {
+	initIOTimerChannel(io_timers, {Timer::Timer2, Timer::Channel1}, {GPIO::PortA, GPIO::Pin0}),
+	initIOTimerChannel(io_timers, {Timer::Timer2, Timer::Channel2}, {GPIO::PortA, GPIO::Pin1}),
+	initIOTimerChannel(io_timers, {Timer::Timer4, Timer::Channel3}, {GPIO::PortB, GPIO::Pin8}),
+	initIOTimerChannel(io_timers, {Timer::Timer4, Timer::Channel4}, {GPIO::PortB, GPIO::Pin9}),
+	initIOTimerChannel(io_timers, {Timer::Timer3, Timer::Channel1}, {GPIO::PortA, GPIO::Pin6}),
+	initIOTimerChannel(io_timers, {Timer::Timer3, Timer::Channel2}, {GPIO::PortA, GPIO::Pin7}),
+	initIOTimerChannel(io_timers, {Timer::Timer3, Timer::Channel3}, {GPIO::PortB, GPIO::Pin0}),
+	initIOTimerChannel(io_timers, {Timer::Timer3, Timer::Channel4}, {GPIO::PortB, GPIO::Pin1}),
+};
+
+constexpr io_timers_channel_mapping_t io_timers_channel_mapping =
+	initIOTimerChannelMapping(io_timers, timer_io_channels);

boards/nxp/fmuk66-e/default.px4board

@@ -19,6 +19,7 @@ CONFIG_DRIVERS_IMU_BOSCH_BMI088=y
 CONFIG_DRIVERS_IMU_INVENSENSE_ICM20948=y
 CONFIG_DRIVERS_IMU_INVENSENSE_ICM42688P=y
 CONFIG_DRIVERS_IRLOCK=y
+CONFIG_DRIVERS_UWB_UWB_SR150=y
 CONFIG_COMMON_LIGHT=y
 CONFIG_DRIVERS_LIGHTS_RGBLED_PWM=y
 CONFIG_COMMON_MAGNETOMETER=y

boards/nxp/fmurt1062-v1/default.px4board

@@ -22,7 +22,7 @@ CONFIG_DRIVERS_PWM_OUT=y
 CONFIG_DRIVERS_RC_INPUT=y
 CONFIG_DRIVERS_SAFETY_BUTTON=y
 CONFIG_DRIVERS_TONE_ALARM=y
-CONFIG_COMMON_UWB=y
+CONFIG_DRIVERS_UWB_UWB_SR150=y
 CONFIG_MODULES_BATTERY_STATUS=y
 CONFIG_MODULES_CAMERA_FEEDBACK=y
 CONFIG_MODULES_COMMANDER=y

boards/nxp/mr-canhubk3/fmu.px4board

@@ -1,20 +1,25 @@
 # CONFIG_BOARD_ROMFSROOT is not set
 CONFIG_BOARD_SERIAL_GPS1="/dev/ttyS1"
+CONFIG_BOARD_SERIAL_GPS2="/dev/ttyS4"
 CONFIG_BOARD_SERIAL_RC="/dev/ttyS5"
 CONFIG_BOARD_SERIAL_TEL1="/dev/ttyS2"
+CONFIG_BOARD_SERIAL_TEL4="/dev/ttyS3"
 CONFIG_BOARD_UAVCAN_INTERFACES=1
 CONFIG_COMMON_LIGHT=y
 CONFIG_CYPHAL_BMS_SUBSCRIBER=y
+CONFIG_DRIVERS_ADC_BOARD_ADC=y
 CONFIG_DRIVERS_CYPHAL=y
 CONFIG_DRIVERS_GPS=y
 CONFIG_DRIVERS_IRLOCK=y
 CONFIG_DRIVERS_MAGNETOMETER_ISENTEK_IST8310=y
 CONFIG_DRIVERS_MAGNETOMETER_LIS3MDL=y
 CONFIG_DRIVERS_RC_INPUT=y
+CONFIG_DRIVERS_SAFETY_BUTTON=y
 CONFIG_DRIVERS_UAVCAN=y
 CONFIG_EXAMPLES_FAKE_GPS=y
 CONFIG_MODULES_AIRSPEED_SELECTOR=y
 CONFIG_MODULES_ATTITUDE_ESTIMATOR_Q=y
+CONFIG_MODULES_BATTERY_STATUS=y
 CONFIG_MODULES_CAMERA_FEEDBACK=y
 CONFIG_MODULES_COMMANDER=y
 CONFIG_MODULES_ESC_BATTERY=y

boards/nxp/mr-canhubk3/init/rc.board_defaults

@@ -17,6 +17,17 @@ param set-default MAV_1_UDP_PRT 14550
 param set-default SENS_EXT_I2C_PRB 0
 param set-default CYPHAL_ENABLE 0
 
+if ver hwtypecmp MR-CANHUBK3-ADAP
+then
+	param set-default GPS_1_CONFIG 202
+	param set-default RC_PORT_CONFIG 104
+	param set-default SENS_INT_BARO_EN 0
+	param set-default SYS_HAS_BARO 0
+	# MR-CANHUBK3-ADAP voltage divider
+	param set-default BAT1_V_DIV 13.158
+	safety_button start
+fi
+
 if param greater -s UAVCAN_ENABLE 0
 then
 	ifup can0

boards/nxp/mr-canhubk3/init/rc.board_sensors

@@ -2,22 +2,31 @@
 #
 # NXP MR-CANHUBK3 specific board sensors init
 #------------------------------------------------------------------------------
+if ver hwtypecmp MR-CANHUBK3-ADAP
+then
+	icm42688p -c 2 -b 3 -R 0 -S -f 15000 start
+	# Internal magnetometer on I2c on ADAP
+	bmm150 -X -a 18 start
+	ist8310 -X -b 1 -R 10 start
+	# ADC for voltage input sensing
+	board_adc start
 
-#board_adc start FIXME no ADC drivers
+	# External SPI bus ICM20649
+	icm20649 -b 4 -S -R 10 start
 
-#FMUv5Xbase board orientation
+	# External SPI bus ICM42688p
+	icm42688p -c 1 -b 3 -R 10 -S -f 15000 start
+else
+	bmm150 -X start
+	# External compass on GPS1/I2C1 (the 3rd external bus): standard Holybro Pixhawk 4 or CUAV V5 GPS/compass puck (with lights, safety button, and buzzer)
+	ist8310 -X -b 2 -R 10 start
 
-# Internal SPI bus ICM20649
-icm20649 -s -R 6 start
+	# External SPI bus ICM20649
+	icm20649 -b 4 -S -R 6 start
 
-# Internal SPI bus ICM42688p
-icm42688p -R 6 -s start
-
-# Internal magnetometer on I2c
-bmm150 -I start
-
-# External compass on GPS1/I2C1 (the 3rd external bus): standard Holybro Pixhawk 4 or CUAV V5 GPS/compass puck (with lights, safety button, and buzzer)
-ist8310 -X -b 2 -R 10 start
+	# External SPI bus ICM42688p
+	icm42688p -c 1 -b 3 -R 6 -S -f 15000 start
+fi
 
 # External compass on GPS1/I2C1 (the 3rd external bus): Drotek RTK GPS with LIS3MDL Compass
 lis3mdl -X -b 2 -R 2 start
@@ -25,5 +34,5 @@ lis3mdl -X -b 2 -R 2 start
 # Disable startup of internal baros if param is set to false
 if param compare SENS_INT_BARO_EN 1
 then
-	bmp388 -I -a 0x77 start
+	bmp388 -X -a 0x77 start
 fi

boards/nxp/mr-canhubk3/nuttx-config/include/board.h

@@ -152,6 +152,18 @@
 #define PIN_LPUART1_RX    (PIN_LPUART1_RX_3 | PIN_INPUT_PULLUP)   /* PTC6 */
 #define PIN_LPUART1_TX    PIN_LPUART1_TX_3   /* PTC7 */
 
+
+/* LPUART4 /dev/ttyS3  P8B 3X7 Pin 3 Single wire RC UART */
+
+#define PIN_LPUART4_RX    PIN_LPUART4_TX_3   /* Dummy since it's Single Wire TX-only */
+#define PIN_LPUART4_TX    PIN_LPUART4_TX_3   /* PTE11 */
+
+
+/* LPUART7 /dev/ttyS4  P8B 3X7 Pin 3 and Pin 8 */
+
+#define PIN_LPUART7_RX    (PIN_LPUART7_RX_3 | PIN_INPUT_PULLUP)   /* PTE0 */
+#define PIN_LPUART7_TX    PIN_LPUART7_TX_3   /* PTE1 */
+
 /* LPUART9   P24 UART connector */
 
 #define PIN_LPUART9_RX    (PIN_LPUART9_RX_1 | PIN_INPUT_PULLUP)   /* PTB2 */
@@ -209,7 +221,8 @@
 #define PIN_LPSPI4_PCS0   PIN_LPSPI4_PCS0_1  /* PTB8  */
 #define PIN_LPSPI4_PCS3   PIN_LPSPI4_PCS3_1  /* PTA16 */
 
-#define PIN_LPSPI4_PCS    (PIN_PTA16 | GPIO_LOWDRIVE | GPIO_OUTPUT_ONE)  /* PTA16 */
+#define PIN_LPSPI4_CS_P26 (PIN_PTA16 | GPIO_LOWDRIVE | GPIO_OUTPUT_ONE)  /* PTA16 */
+#define PIN_LPSPI4_CS_P8B (PIN_PTB8 | GPIO_LOWDRIVE | GPIO_OUTPUT_ONE)   /* PTB8 */
 
 /* LPSPI5  P26 external IMU connector */
 

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

@@ -245,6 +245,8 @@ CONFIG_S32K3XX_LPUART13=y
 CONFIG_S32K3XX_LPUART14=y
 CONFIG_S32K3XX_LPUART1=y
 CONFIG_S32K3XX_LPUART2=y
+CONFIG_S32K3XX_LPUART4=y
+CONFIG_S32K3XX_LPUART7=y
 CONFIG_S32K3XX_LPUART9=y
 CONFIG_S32K3XX_LPUART_INVERT=y
 CONFIG_S32K3XX_LPUART_SINGLEWIRE=y

boards/nxp/mr-canhubk3/src/CMakeLists.txt

@@ -38,6 +38,7 @@ if("${PX4_BOARD_LABEL}" STREQUAL  "canbootloader")
 		clockconfig.c
 		periphclocks.c
 		timer_config.cpp
+		hw_rev_ver_canhubk3.c
 	)
 
 	target_link_libraries(drivers_board
@@ -65,6 +66,8 @@ else()
 		spi.cpp
 		timer_config.cpp
 		s32k3xx_userleds.c
+		hw_rev_ver_canhubk3.c
+		manifest.c
 	)
 
 	target_link_libraries(drivers_board

boards/nxp/mr-canhubk3/src/board_config.h

@@ -88,7 +88,7 @@ __BEGIN_DECLS
 #define DIRECT_PWM_OUTPUT_CHANNELS  8
 
 #define RC_SERIAL_PORT          "/dev/ttyS5"
-#define RC_SERIAL_SINGLEWIRE
+#define RC_SERIAL_SINGLEWIRE_FORCE
 #define RC_SERIAL_INVERT_RX_ONLY
 
 #define BOARD_ENABLE_CONSOLE_BUFFER
@@ -110,6 +110,40 @@ __BEGIN_DECLS
 /* Reboot and ulog we store on a wear-level filesystem */
 #define HARDFAULT_REBOOT_PATH "/mnt/progmem/reboot"
 
+/* To detect MR-CANHUBK3-ADAP board */
+#define BOARD_HAS_HW_VERSIONING  1
+#define CANHUBK3_ADAP_DETECT     (PIN_PTA12 | GPIO_INPUT | GPIO_PULLUP)
+
+
+/*
+ * ADC channels
+ *
+ * These are the channel numbers of the ADCs of the microcontroller that can be used by the Px4
+ * Firmware in the adc driver. ADC1 has 32 channels, with some a/b selection overlap
+ * in the AD4-AD7 range on the same ADC.
+ *
+ * Only ADC1 is used
+ *         Bits 31:0 are ADC1 channels 31:0
+ */
+
+#define ADC1_CH(c)    (((c) & 0x1f))	/* Define ADC number Channel number */
+
+/* ADC defines to be used in sensors.cpp to read from a particular channel */
+
+#define ADC_BATTERY_VOLTAGE_CHANNEL ADC1_CH(0)     /* BAT_VSENS        85   PTB4  ADC1_SE10 */
+#define ADC_BATTERY_CURRENT_CHANNEL ADC1_CH(1)     /* Non-existant but needed for compilation */
+
+
+/* Mask use to initialize the ADC driver */
+
+#define ADC_CHANNELS ((1 << ADC_BATTERY_VOLTAGE_CHANNEL))
+
+/* Safety Switch
+ * TBD
+ */
+#define GPIO_LED_SAFETY         (PIN_PTE26 | GPIO_OUTPUT | GPIO_OUTPUT_ZERO)
+#define GPIO_BTN_SAFETY         (PIN_PTA11 | GPIO_INPUT | GPIO_PULLDOWN)
+
 /****************************************************************************
  * Public Data
  ****************************************************************************/

boards/nxp/mr-canhubk3/src/hw_rev_ver_canhubk3.c

@@ -0,0 +1,142 @@
+/****************************************************************************
+ *
+ *   Copyright (C) 2023 PX4 Development Team. All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright
+ *    notice, this list of conditions and the following disclaimer.
+ * 2. Redistributions in binary form must reproduce the above copyright
+ *    notice, this list of conditions and the following disclaimer in
+ *    the documentation and/or other materials provided with the
+ *    distribution.
+ * 3. Neither the name PX4 nor the names of its contributors may be
+ *    used to endorse or promote products derived from this software
+ *    without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
+ * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
+ * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *
+ ****************************************************************************/
+
+/**
+ * @file hw_rev_ver_canhubk3.c
+ * CANHUBK3 Hardware Revision and Version ID API
+ */
+#include <drivers/drv_adc.h>
+#include <px4_arch/adc.h>
+#include <px4_platform_common/micro_hal.h>
+#include <px4_platform_common/px4_config.h>
+#include <px4_platform/board_determine_hw_info.h>
+#include <stdio.h>
+#include <board_config.h>
+
+#include <systemlib/px4_macros.h>
+
+#if defined(BOARD_HAS_HW_VERSIONING)
+
+
+#define HW_INFO_SIZE HW_INFO_VER_DIGITS + HW_INFO_REV_DIGITS
+
+
+/****************************************************************************
+ * Private Data
+ ****************************************************************************/
+static int is_adap_connected = 0;
+
+/****************************************************************************
+ * Public Functions
+ ****************************************************************************/
+/************************************************************************************
+ * Name: board_get_hw_type
+ *
+ * Description:
+ *   Optional returns a 0 terminated string defining the HW type.
+ *
+ * Input Parameters:
+ *   None
+ *
+ * Returned Value:
+ *   a 0 terminated string defining the HW type. This my be a 0 length string ""
+ *
+ ************************************************************************************/
+
+__EXPORT const char *board_get_hw_type_name()
+{
+	if (is_adap_connected) {
+		return (const char *)"MR-CANHUBK3-ADAP";
+
+	} else {
+		return (const char *)"MR-CANHUBK344";
+	}
+}
+
+/************************************************************************************
+ * Name: board_get_hw_version
+ *
+ * Description:
+ *   Optional returns a integer HW version
+ *
+ * Input Parameters:
+ *   None
+ *
+ * Returned Value:
+ *   An integer value of this boards hardware version.
+ *   A value of -1 is the default for boards not supporting the BOARD_HAS_VERSIONING API.
+ *   A value of 0 is the default for boards supporting the API but not having version.
+ *
+ ************************************************************************************/
+
+__EXPORT int board_get_hw_version()
+{
+	return  0;
+}
+
+/************************************************************************************
+ * Name: board_get_hw_revision
+ *
+ * Description:
+ *   Optional returns a integer HW revision
+ *
+ * Input Parameters:
+ *   None
+ *
+ * Returned Value:
+ *   An integer value of this boards hardware revision.
+ *   A value of -1 is the default for boards not supporting the BOARD_HAS_VERSIONING API.
+ *   A value of 0 is the default for boards supporting the API but not having revision.
+ *
+ ************************************************************************************/
+
+__EXPORT int board_get_hw_revision()
+{
+	return  0;
+}
+
+/************************************************************************************
+  * Name: board_determine_hw_info
+ *
+ * Description:
+ *	Uses GPIO to detect MR-CANHUBK3-ADAP
+ *
+ ************************************************************************************/
+
+int board_determine_hw_info()
+{
+	s32k3xx_pinconfig(CANHUBK3_ADAP_DETECT);
+	is_adap_connected = !s32k3xx_gpioread(CANHUBK3_ADAP_DETECT);
+	return 0;
+}
+#endif

boards/nxp/mr-canhubk3/src/i2c.cpp

@@ -34,6 +34,6 @@
 #include <px4_arch/i2c_hw_description.h>
 
 constexpr px4_i2c_bus_t px4_i2c_buses[I2C_BUS_MAX_BUS_ITEMS] = {
-	initI2CBusInternal(PX4_BUS_NUMBER_TO_PX4(0)),
-	initI2CBusExternal(PX4_BUS_NUMBER_TO_PX4(1)),
+	initI2CBusExternal(PX4_BUS_NUMBER_TO_PX4(0)),
+	initI2CBusInternal(PX4_BUS_NUMBER_TO_PX4(1)),
 };

boards/nxp/mr-canhubk3/src/init.c

@@ -44,6 +44,7 @@
 #include "board_config.h"
 
 #include <px4_platform_common/init.h>
+#include <px4_platform/board_determine_hw_info.h>
 
 #if defined(CONFIG_S32K3XX_LPSPI1) && defined(CONFIG_MMCSD)
 #include <nuttx/mmcsd.h>
@@ -96,6 +97,8 @@ __EXPORT int board_app_initialize(uintptr_t arg)
 
 	int rv;
 
+	board_determine_hw_info();
+
 
 #if defined(CONFIG_S32K3XX_LPSPI1) && defined(CONFIG_MMCSD)
 	/* LPSPI1 *****************************************************************/

boards/nxp/mr-canhubk3/src/manifest.c

@@ -0,0 +1,79 @@
+/****************************************************************************
+ *
+ *   Copyright (c) 2023 PX4 Development Team. All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright
+ *    notice, this list of conditions and the following disclaimer.
+ * 2. Redistributions in binary form must reproduce the above copyright
+ *    notice, this list of conditions and the following disclaimer in
+ *    the documentation and/or other materials provided with the
+ *    distribution.
+ * 3. Neither the name PX4 nor the names of its contributors may be
+ *    used to endorse or promote products derived from this software
+ *    without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
+ * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
+ * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *
+ ****************************************************************************/
+
+/**
+ * @file manifest.c
+ *
+ * This module supplies the interface to the manifest of hardware that is
+ * optional and dependent on the HW REV and HW VER IDs
+ *
+ * The manifest allows the system to know whether a hardware option
+ * say for example the PX4IO is an no-pop option vs it is broken.
+ *
+ */
+
+/****************************************************************************
+ * Included Files
+ ****************************************************************************/
+
+#include <px4_platform_common/px4_config.h>
+#include <stdbool.h>
+#include <syslog.h>
+
+#include "systemlib/px4_macros.h"
+#include "px4_log.h"
+
+/****************************************************************************
+ * Pre-Processor Definitions
+ ****************************************************************************/
+
+
+/************************************************************************************
+ * Name: board_query_manifest
+ *
+ * Description:
+ *   Optional returns manifest item.
+ *
+ * Input Parameters:
+ *   manifest_id - the ID for the manifest item to retrieve
+ *
+ * Returned Value:
+ *   0 - item is not in manifest => assume legacy operations
+ *   pointer to a manifest item
+ *
+ ************************************************************************************/
+
+__EXPORT px4_hw_mft_item board_query_manifest(px4_hw_mft_item_id_t id)
+{
+	return px4_hw_mft_unsupported;
+}

boards/nxp/mr-canhubk3/src/s32k3xx_bringup.c

@@ -107,6 +107,9 @@ int s32k3xx_bringup(void)
 	s32k3xx_spidev_initialize();
 #endif
 
+	s32k3xx_pinconfig(GPIO_LED_SAFETY);
+	s32k3xx_pinconfig(GPIO_BTN_SAFETY);
+
 #ifdef CONFIG_INPUT_BUTTONS
 	/* Register the BUTTON driver */
 

boards/nxp/mr-canhubk3/src/s32k3xx_periphclocks.c

@@ -168,6 +168,22 @@ const struct peripheral_clock_config_s g_peripheral_clockconfig0[] = {
 		.clkgate = true,
 #else
 		.clkgate = false,
+#endif
+	},
+	{
+		.clkname = LPUART4_CLK,
+#ifdef CONFIG_S32K3XX_LPUART4
+		.clkgate = true,
+#else
+		.clkgate = false,
+#endif
+	},
+	{
+		.clkname = LPUART7_CLK,
+#ifdef CONFIG_S32K3XX_LPUART7
+		.clkgate = true,
+#else
+		.clkgate = false,
 #endif
 	},
 	{
@@ -258,6 +274,10 @@ const struct peripheral_clock_config_s g_peripheral_clockconfig0[] = {
 		.clkname = EMIOS0_CLK,
 		.clkgate = true,
 	},
+	{
+		.clkname = ADC2_CLK,
+		.clkgate = true,
+	}
 };
 
 unsigned int const num_of_peripheral_clocks_0 =

boards/nxp/mr-canhubk3/src/spi.cpp

@@ -70,11 +70,12 @@ constexpr px4_spi_bus_t px4_spi_buses[SPI_BUS_MAX_BUS_ITEMS] = {
 	initSPIBus(SPI::Bus::SPI3, { // SPI3 is ignored only used for FS26 by a NuttX driver
 		initSPIDevice(SPIDEV_FLASH(0), SPI::CS{GPIO::PortD, GPIO::Pin17})
 	}),
-	initSPIBus(SPI::Bus::SPI4, {
-		initSPIDevice(DRV_IMU_DEVTYPE_ICM42688P, SPI::CS{GPIO::PortA, GPIO::Pin16}, SPI::DRDY{PIN_WKPU20})
+	initSPIBusExternal(SPI::Bus::SPI4, {
+		initSPIConfigExternal(SPI::CS{GPIO::PortA, GPIO::Pin16}, SPI::DRDY{PIN_WKPU20}),
+		initSPIConfigExternal(SPI::CS{GPIO::PortB, GPIO::Pin8}, SPI::DRDY{PIN_WKPU56})
 	}),
-	initSPIBus(SPI::Bus::SPI5, {
-		initSPIDevice(DRV_IMU_DEVTYPE_ICM20649, SPI::CS{GPIO::PortA, GPIO::Pin14}, SPI::DRDY{PIN_WKPU4})
+	initSPIBusExternal(SPI::Bus::SPI5, {
+		initSPIConfigExternal(SPI::CS{GPIO::PortA, GPIO::Pin14}, SPI::DRDY{PIN_WKPU4})
 	}),
 };
 
@@ -337,7 +338,14 @@ void s32k3xx_lpspi4select(FAR struct spi_dev_s *dev, uint32_t devid,
 	spiinfo("devid: %" PRId32 ", CS: %s\n", devid,
 		selected ? "assert" : "de-assert");
 
-	s32k3xx_gpiowrite(PIN_LPSPI4_PCS, !selected);
+	devid = ((devid) & 0xF);
+
+	if (devid == 0) {
+		s32k3xx_gpiowrite(PIN_LPSPI4_CS_P26, !selected);
+
+	} else if (devid == 1) {
+		s32k3xx_gpiowrite(PIN_LPSPI4_CS_P8B, !selected);
+	}
 }
 
 uint8_t s32k3xx_lpspi4status(FAR struct spi_dev_s *dev, uint32_t devid)

boards/nxp/ucans32k146/default.px4board

@@ -13,6 +13,7 @@ CONFIG_COMMON_MAGNETOMETER=y
 CONFIG_DRIVERS_PWM_OUT=y
 CONFIG_BOARD_UAVCAN_INTERFACES=1
 CONFIG_DRIVERS_UAVCANNODE=y
+CONFIG_DRIVERS_UWB_UWB_SR150=y
 CONFIG_UAVCANNODE_ARMING_STATUS=y
 CONFIG_UAVCANNODE_BEEP_COMMAND=y
 CONFIG_UAVCANNODE_ESC_RAW_COMMAND=y

boards/px4/fmu-v5/test.px4board

@@ -4,11 +4,13 @@ CONFIG_DRIVERS_IRLOCK=n
 CONFIG_DRIVERS_PCA9685_PWM_OUT=n
 CONFIG_EXAMPLES_FAKE_GPS=n
 CONFIG_MODULES_ATTITUDE_ESTIMATOR_Q=n
-CONFIG_MODULES_FW_AUTOTUNE_ATTITUDE_CONTROL=n
-CONFIG_MODULES_MC_AUTOTUNE_ATTITUDE_CONTROL=n
 CONFIG_MODULES_LOCAL_POSITION_ESTIMATOR=n
 CONFIG_MODULES_ROVER_POS_CONTROL=n
 CONFIG_MODULES_SIMULATION_SIMULATOR_SIH=n
 CONFIG_BOARD_TESTING=y
 CONFIG_DRIVERS_TEST_PPM=y
 CONFIG_SYSTEMCMDS_MICROBENCH=y
+CONFIG_MODULES_FW_ATT_CONTROL=n
+CONFIG_MODULES_FW_AUTOTUNE_ATTITUDE_CONTROL=n
+CONFIG_MODULES_FW_POS_CONTROL=n
+CONFIG_MODULES_FW_RATE_CONTROL=n

boards/px4/fmu-v5x/cmake/upload.cmake

@@ -0,0 +1,49 @@
+############################################################################
+#
+#   Copyright (c) 2020 PX4 Development Team. All rights reserved.
+#
+# Redistribution and use in source and binary forms, with or without
+# modification, are permitted provided that the following conditions
+# are met:
+#
+# 1. Redistributions of source code must retain the above copyright
+#    notice, this list of conditions and the following disclaimer.
+# 2. Redistributions in binary form must reproduce the above copyright
+#    notice, this list of conditions and the following disclaimer in
+#    the documentation and/or other materials provided with the
+#    distribution.
+# 3. Neither the name PX4 nor the names of its contributors may be
+#    used to endorse or promote products derived from this software
+#    without specific prior written permission.
+#
+# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+# "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+# LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+# FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+# COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+# INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+# BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
+# OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
+# AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+# LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+# ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+# POSSIBILITY OF SUCH DAMAGE.
+#
+############################################################################
+
+
+set(PX4_FW_NAME ${PX4_BINARY_DIR}/${PX4_BOARD_VENDOR}_${PX4_BOARD_MODEL}_${PX4_BOARD_LABEL}.px4)
+
+add_custom_target(upload_skynode_usb
+	COMMAND ${PX4_SOURCE_DIR}/Tools/auterion/upload_skynode.sh --file=${PX4_FW_NAME}
+	DEPENDS ${PX4_FW_NAME}
+	COMMENT "Uploading PX4"
+	USES_TERMINAL
+)
+
+add_custom_target(upload_skynode_wifi
+	COMMAND ${PX4_SOURCE_DIR}/Tools/auterion/upload_skynode.sh --file=${PX4_FW_NAME} --wifi
+	DEPENDS ${PX4_FW_NAME}
+	COMMENT "Uploading PX4"
+	USES_TERMINAL
+)

boards/px4/fmu-v5x/test.px4board

@@ -7,7 +7,6 @@ CONFIG_DRIVERS_IRLOCK=n
 CONFIG_MODULES_FW_AUTOTUNE_ATTITUDE_CONTROL=n
 CONFIG_MODULES_MC_AUTOTUNE_ATTITUDE_CONTROL=n
 CONFIG_MODULES_ROVER_POS_CONTROL=n
-CONFIG_MODULES_SIMULATION_SIMULATOR_SIH=n
 CONFIG_BOARD_TESTING=y
 CONFIG_DRIVERS_DISTANCE_SENSOR_LIGHTWARE_LASER_SERIAL=y
 CONFIG_DRIVERS_TEST_PPM=y

boards/px4/fmu-v6x/cmake/upload.cmake

@@ -0,0 +1,49 @@
+############################################################################
+#
+#   Copyright (c) 2023 PX4 Development Team. All rights reserved.
+#
+# Redistribution and use in source and binary forms, with or without
+# modification, are permitted provided that the following conditions
+# are met:
+#
+# 1. Redistributions of source code must retain the above copyright
+#    notice, this list of conditions and the following disclaimer.
+# 2. Redistributions in binary form must reproduce the above copyright
+#    notice, this list of conditions and the following disclaimer in
+#    the documentation and/or other materials provided with the
+#    distribution.
+# 3. Neither the name PX4 nor the names of its contributors may be
+#    used to endorse or promote products derived from this software
+#    without specific prior written permission.
+#
+# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+# "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+# LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+# FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+# COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+# INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+# BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
+# OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
+# AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+# LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+# ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+# POSSIBILITY OF SUCH DAMAGE.
+#
+############################################################################
+
+
+set(PX4_FW_NAME ${PX4_BINARY_DIR}/${PX4_BOARD_VENDOR}_${PX4_BOARD_MODEL}_${PX4_BOARD_LABEL}.px4)
+
+add_custom_target(upload_skynode_usb
+	COMMAND ${PX4_SOURCE_DIR}/Tools/auterion/upload_skynode.sh --file=${PX4_FW_NAME}
+	DEPENDS ${PX4_FW_NAME}
+	COMMENT "Uploading PX4"
+	USES_TERMINAL
+)
+
+add_custom_target(upload_skynode_wifi
+	COMMAND ${PX4_SOURCE_DIR}/Tools/auterion/upload_skynode.sh --file=${PX4_FW_NAME} --wifi
+	DEPENDS ${PX4_FW_NAME}
+	COMMENT "Uploading PX4"
+	USES_TERMINAL
+)

boards/px4/fmu-v6x/default.px4board

@@ -7,6 +7,7 @@ CONFIG_BOARD_SERIAL_TEL1="/dev/ttyS6"
 CONFIG_BOARD_SERIAL_TEL2="/dev/ttyS4"
 CONFIG_BOARD_SERIAL_TEL3="/dev/ttyS1"
 CONFIG_BOARD_SERIAL_EXT2="/dev/ttyS3"
+CONFIG_DRIVERS_ADC_ADS1115=y
 CONFIG_DRIVERS_ADC_BOARD_ADC=y
 CONFIG_DRIVERS_BAROMETER_BMP388=y
 CONFIG_DRIVERS_BAROMETER_INVENSENSE_ICP201XX=y
@@ -41,6 +42,7 @@ CONFIG_DRIVERS_TONE_ALARM=y
 CONFIG_DRIVERS_UAVCAN=y
 CONFIG_BOARD_UAVCAN_TIMER_OVERRIDE=2
 CONFIG_MODULES_AIRSPEED_SELECTOR=y
+CONFIG_MODULES_BATTERY_STATUS=y
 CONFIG_MODULES_CAMERA_FEEDBACK=y
 CONFIG_MODULES_COMMANDER=y
 CONFIG_MODULES_CONTROL_ALLOCATOR=y

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

@@ -102,13 +102,8 @@ if ver hwtypecmp V6X002001
 then
 	rm3100 -I -b 4 start
 else
-	if ver hwtypecmp V6X009010 V6X010010
-	then
-		# Internal magnetometer on I2C
-		bmm150 -I -R 0 start
-	else
-		bmm150 -I -R 6 start
-	fi
+	# Internal magnetometer on I2C
+	bmm150 -I -R 0 start
 fi
 
 # External compass on GPS1/I2C1 (the 3rd external bus): standard Holybro Pixhawk 4 or CUAV V5 GPS/compass puck (with lights, safety button, and buzzer)

boards/px4/fmu-v6x/nuttx-config/nsh/defconfig

@@ -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

boards/px4/raspberrypi/cmake/upload.cmake

@@ -38,7 +38,7 @@ else()
 endif()
 
 add_custom_target(upload
-	COMMAND rsync -arh --progress
+	COMMAND rsync -arh --progress -e "ssh -o StrictHostKeyChecking=no"
 			${CMAKE_RUNTIME_OUTPUT_DIRECTORY} ${PX4_SOURCE_DIR}/posix-configs/rpi/*.config ${PX4_BINARY_DIR}/etc # source
 			pi@${AUTOPILOT_HOST}:/home/pi/px4 # destination
 	DEPENDS px4

boards/px4/raspberrypi/default.px4board

@@ -4,15 +4,15 @@ CONFIG_BOARD_TOOLCHAIN="arm-linux-gnueabihf"
 CONFIG_BOARD_ARCHITECTURE="cortex-a53"
 CONFIG_BOARD_TESTING=y
 CONFIG_DRIVERS_ADC_ADS1115=y
-CONFIG_DRIVERS_BAROMETER_MS5611=y
+CONFIG_COMMON_BAROMETERS=y
 CONFIG_DRIVERS_BATT_SMBUS=y
 CONFIG_DRIVERS_CAMERA_TRIGGER=y
 CONFIG_COMMON_DIFFERENTIAL_PRESSURE=y
 CONFIG_COMMON_DISTANCE_SENSOR=y
 CONFIG_DRIVERS_GPS=y
+CONFIG_COMMON_IMU=y
 CONFIG_DRIVERS_IMU_INVENSENSE_ICM20948=y
-CONFIG_DRIVERS_IMU_INVENSENSE_MPU9250=y
-CONFIG_DRIVERS_MAGNETOMETER_HMC5883=y
+CONFIG_COMMON_MAGNETOMETER=y
 CONFIG_DRIVERS_PCA9685_PWM_OUT=y
 CONFIG_DRIVERS_RC_INPUT=y
 CONFIG_DRIVERS_RPI_RC_IN=y

msg/CMakeLists.txt

@@ -174,6 +174,7 @@ set(msg_files
 	SensorSelection.msg
 	SensorsStatus.msg
 	SensorsStatusImu.msg
+	SensorUwb.msg
 	SystemPower.msg
 	TakeoffStatus.msg
 	TaskStackInfo.msg
@@ -190,8 +191,6 @@ set(msg_files
 	UavcanParameterValue.msg
 	UlogStream.msg
 	UlogStreamAck.msg
-	UwbDistance.msg
-	UwbGrid.msg
 	VehicleAcceleration.msg
 	VehicleAirData.msg
 	VehicleAngularAccelerationSetpoint.msg

msg/GeofenceResult.msg

@@ -1,14 +1,14 @@
-uint64 timestamp                # time since system start (microseconds)
-uint8 GF_ACTION_NONE      = 0   # no action on geofence violation
-uint8 GF_ACTION_WARN      = 1   # critical mavlink message
-uint8 GF_ACTION_LOITER    = 2   # switch to AUTO|LOITER
-uint8 GF_ACTION_RTL       = 3   # switch to AUTO|RTL
-uint8 GF_ACTION_TERMINATE = 4   # flight termination
-uint8 GF_ACTION_LAND      = 5   # switch to AUTO|LAND
+uint64 timestamp                        # time since system start (microseconds)
+uint8 GF_ACTION_NONE = 0                # no action on geofence violation
+uint8 GF_ACTION_WARN = 1                # critical mavlink message
+uint8 GF_ACTION_LOITER = 2              # switch to AUTO|LOITER
+uint8 GF_ACTION_RTL = 3                 # switch to AUTO|RTL
+uint8 GF_ACTION_TERMINATE = 4           # flight termination
+uint8 GF_ACTION_LAND = 5                # switch to AUTO|LAND
 
-uint8 geofence_violation_reason # one of geofence_violation_reason_t::*
+uint8 geofence_violation_reason         # one of geofence_violation_reason_t::*
 
-bool primary_geofence_breached  # true if the primary geofence is breached
-uint8 primary_geofence_action   # action to take when the primary geofence is breached
+bool primary_geofence_breached          # true if the primary geofence is breached
+uint8 primary_geofence_action           # action to take when the primary geofence is breached
 
-bool home_required              # true if the geofence requires a valid home position
+bool home_required                      # true if the geofence requires a valid home position

msg/SensorCorrection.msg

@@ -4,15 +4,6 @@
 
 uint64 timestamp		# time since system start (microseconds)
 
-# Corrections for acceleromter acceleration outputs where corrected_accel = raw_accel * accel_scale + accel_offset
-# Note the corrections are in the sensor frame and must be applied before the sensor data is rotated into body frame
-uint32[4] accel_device_ids
-float32[4] accel_temperature
-float32[3] accel_offset_0	# accelerometer 0 offsets in the FRD board frame XYZ-axis in m/s^s
-float32[3] accel_offset_1	# accelerometer 1 offsets in the FRD board frame XYZ-axis in m/s^s
-float32[3] accel_offset_2	# accelerometer 2 offsets in the FRD board frame XYZ-axis in m/s^s
-float32[3] accel_offset_3	# accelerometer 3 offsets in the FRD board frame XYZ-axis in m/s^s
-
 # Corrections for gyro angular rate outputs where corrected_rate = raw_rate * gyro_scale + gyro_offset
 # Note the corrections are in the sensor frame and must be applied before the sensor data is rotated into body frame
 uint32[4] gyro_device_ids
@@ -22,14 +13,14 @@ float32[3] gyro_offset_1	# gyro 1 XYZ offsets in the sensor frame in rad/s
 float32[3] gyro_offset_2	# gyro 2 XYZ offsets in the sensor frame in rad/s
 float32[3] gyro_offset_3	# gyro 3 XYZ offsets in the sensor frame in rad/s
 
-# Corrections for magnetometer measurement outputs where corrected_mag = raw_mag * mag_scale + mag_offset
+# Corrections for acceleromter acceleration outputs where corrected_accel = raw_accel * accel_scale + accel_offset
 # Note the corrections are in the sensor frame and must be applied before the sensor data is rotated into body frame
-uint32[4] mag_device_ids
-float32[4] mag_temperature
-float32[3] mag_offset_0	# magnetometer 0 offsets in the FRD board frame XYZ-axis in m/s^s
-float32[3] mag_offset_1	# magnetometer 1 offsets in the FRD board frame XYZ-axis in m/s^s
-float32[3] mag_offset_2	# magnetometer 2 offsets in the FRD board frame XYZ-axis in m/s^s
-float32[3] mag_offset_3	# magnetometer 3 offsets in the FRD board frame XYZ-axis in m/s^s
+uint32[4] accel_device_ids
+float32[4] accel_temperature
+float32[3] accel_offset_0	# accelerometer 0 offsets in the FRD board frame XYZ-axis in m/s^s
+float32[3] accel_offset_1	# accelerometer 1 offsets in the FRD board frame XYZ-axis in m/s^s
+float32[3] accel_offset_2	# accelerometer 2 offsets in the FRD board frame XYZ-axis in m/s^s
+float32[3] accel_offset_3	# accelerometer 3 offsets in the FRD board frame XYZ-axis in m/s^s
 
 # Corrections for barometric pressure outputs where corrected_pressure = raw_pressure * pressure_scale + pressure_offset
 # Note the corrections are in the sensor frame and must be applied before the sensor data is rotated into body frame

msg/SensorUwb.msg

@@ -0,0 +1,34 @@
+# UWB distance contains the distance information measured by an ultra-wideband positioning system,
+# such as Pozyx or NXP Rddrone.
+
+uint64 	timestamp		# time since system start (microseconds)
+
+uint32 	sessionid		# UWB SessionID
+uint32 	time_offset		# Time between Ranging Rounds in ms
+uint32 	counter			# Number of Ranges since last Start of Ranging
+uint16 	mac			# MAC adress of Initiator (controller)
+
+uint16 	mac_dest		# MAC adress of Responder (Controlee)
+uint16 	status			# status feedback #
+uint8 	nlos			# None line of site condition y/n
+float32 distance		# distance in m to the UWB receiver
+
+
+#Angle of arrival, Angle in Degree -60..+60; FOV in both axis is 120 degrees
+float32 aoa_azimuth_dev	# Angle of arrival of first incomming RX msg
+float32 aoa_elevation_dev	# Angle of arrival of first incomming RX msg
+float32 aoa_azimuth_resp	# Angle of arrival of first incomming RX msg at the responder
+float32 aoa_elevation_resp	# Angle of arrival of first incomming RX msg at the responder
+
+# Figure of merit for the angle measurements
+uint8 aoa_azimuth_fom		# AOA Azimuth FOM
+uint8 aoa_elevation_fom		# AOA Elevation FOM
+uint8 aoa_dest_azimuth_fom	# AOA Azimuth FOM
+uint8 aoa_dest_elevation_fom	# AOA Elevation FOM
+
+# Initiator physical configuration
+uint8 orientation		# Direction the sensor faces from MAV_SENSOR_ORIENTATION enum
+				# Standard configuration is Antennas facing down and azimuth aligened in forward direction
+float32 offset_x		# UWB initiator offset in X axis (NED drone frame)
+float32 offset_y		# UWB initiator offset in Y axis (NED drone frame)
+float32 offset_z		# UWB initiator offset in Z axis (NED drone frame)

msg/UwbDistance.msg

@@ -1,15 +0,0 @@
-# UWB distance contains the distance information measured by an ultra-wideband positioning system,
-# such as Pozyx or NXP Rddrone.
-
-uint64 	timestamp		# time since system start (microseconds)
-uint32 	time_uwb_ms		# Time of UWB device in ms
-uint32 	counter			# Number of Ranges since last Start of Ranging
-uint32 	sessionid		# UWB SessionID
-uint32 	time_offset		# Time between Ranging Rounds in ms
-uint16 	status			# status feedback #
-
-uint16[12] anchor_distance	# distance in cm to each UWB Anchor (UWB Device which takes part in Ranging)
-bool[12] nlos			# Visual line of sight yes/no
-float32[12] aoafirst		# Angle of arrival of first incoming RX msg
-
-float32[3] position		# Position of the Landing point in relation to the Drone (x,y,z in Meters NED)

msg/UwbGrid.msg

@@ -1,25 +0,0 @@
-# UWB report message contains the grid information measured by an ultra-wideband positioning system,
-# such as Pozyx or NXP Rddrone.
-
-uint64 	timestamp	# time since system start (microseconds)
-uint16 	initator_time	# time to synchronize clocks (microseconds)
-uint8 	num_anchors	# Number of anchors
-
-float64[4] target_gps	# GPS position of target of the UWB system (Lat / Lon / Alt / Yaw Offset to true North)
-
-# Grid position information
-# Position in x,y,z in  (x,y,z in centimeters NED)
-# staring with POI and Anchor 0 up to Anchor 11
-int16[3] target_pos 	# Point of Interest, mostly landing Target x,y,z
-int16[3] anchor_pos_0
-int16[3] anchor_pos_1
-int16[3] anchor_pos_2
-int16[3] anchor_pos_3
-int16[3] anchor_pos_4
-int16[3] anchor_pos_5
-int16[3] anchor_pos_6
-int16[3] anchor_pos_7
-int16[3] anchor_pos_8
-int16[3] anchor_pos_9
-int16[3] anchor_pos_10
-int16[3] anchor_pos_11

platforms/common/include/px4_platform_common/board_common.h

@@ -440,6 +440,8 @@ __BEGIN_DECLS
 
 #if defined(RC_SERIAL_SINGLEWIRE)
 static inline bool board_rc_singlewire(const char *device) { return strcmp(device, RC_SERIAL_PORT) == 0; }
+#elif defined(RC_SERIAL_SINGLEWIRE_FORCE)
+static inline bool board_rc_singlewire(const char *device) { return true; }
 #else
 static inline bool board_rc_singlewire(const char *device) { return false; }
 #endif

platforms/common/px4_log.cpp

@@ -103,7 +103,7 @@ __EXPORT void px4_log_modulename(int level, const char *module_name, const char
 #if defined(PX4_LOG_COLORIZED_OUTPUT)
 
 		if (use_color) {
-			pos += snprintf(buf + pos, math::max(max_length - pos, (ssize_t)0), "%s", __px4_log_level_color[level]);
+			pos += sprintf(buf + pos, "%s", __px4_log_level_color[level]);
 		}
 
 #endif // PX4_LOG_COLORIZED_OUTPUT
@@ -138,12 +138,12 @@ __EXPORT void px4_log_modulename(int level, const char *module_name, const char
 		if (use_color) {
 			// alway reset color
 			const ssize_t sz = math::min(pos, max_length - (ssize_t)strlen(PX4_ANSI_COLOR_RESET) - (ssize_t)1);
-			pos += snprintf(buf + sz, math::max(max_length - sz, (ssize_t)0), "%s\n", PX4_ANSI_COLOR_RESET);
+			pos += sprintf(buf + sz, "%s\n", PX4_ANSI_COLOR_RESET);
 
 		} else
 #endif // PX4_LOG_COLORIZED_OUTPUT
 		{
-			pos += snprintf(buf + math::min(pos, max_length - (ssize_t)1), 2, "\n");
+			pos += sprintf(buf + math::min(pos, max_length - (ssize_t)1), "\n");
 		}
 
 		// ensure NULL termination (buffer is max_length + 1)
@@ -162,7 +162,7 @@ __EXPORT void px4_log_modulename(int level, const char *module_name, const char
 			va_start(argptr, fmt);
 			pos += vsnprintf(buf + pos, math::max(max_length - pos, (ssize_t)0), fmt, argptr);
 			va_end(argptr);
-			pos += snprintf(buf + math::min(pos, max_length - (ssize_t)1), 2, "\n");
+			pos += sprintf(buf + math::min(pos, max_length - (ssize_t)1), "\n");
 			buf[max_length] = 0; // ensure NULL termination
 		}
 
@@ -220,7 +220,7 @@ __EXPORT void px4_log_raw(int level, const char *fmt, ...)
 #if defined(PX4_LOG_COLORIZED_OUTPUT)
 
 		if (use_color) {
-			pos += snprintf(buf + pos, math::max(max_length - pos, (ssize_t)0), "%s", __px4_log_level_color[level]);
+			pos += sprintf(buf + pos, "%s", __px4_log_level_color[level]);
 		}
 
 #endif // PX4_LOG_COLORIZED_OUTPUT
@@ -235,7 +235,7 @@ __EXPORT void px4_log_raw(int level, const char *fmt, ...)
 		if (use_color) {
 			// alway reset color
 			const ssize_t sz = math::min(pos, max_length - (ssize_t)strlen(PX4_ANSI_COLOR_RESET));
-			pos += snprintf(buf + sz, math::max(max_length - sz, (ssize_t)0), "%s", PX4_ANSI_COLOR_RESET);
+			pos += sprintf(buf + sz, "%s", PX4_ANSI_COLOR_RESET);
 		}
 
 #endif // PX4_LOG_COLORIZED_OUTPUT

platforms/common/px4_work_queue/WorkQueueManager.cpp

@@ -61,13 +61,12 @@ static BlockingList<WorkQueue *> *_wq_manager_wqs_list{nullptr};
 static BlockingQueue<const wq_config_t *, 1> *_wq_manager_create_queue{nullptr};
 
 static px4::atomic_bool _wq_manager_should_exit{true};
-static px4::atomic_bool _wq_manager_running{false};
 
 
 static WorkQueue *
 FindWorkQueueByName(const char *name)
 {
-	if (!_wq_manager_running.load()) {
+	if (_wq_manager_wqs_list == nullptr) {
 		PX4_ERR("not running");
 		return nullptr;
 	}
@@ -87,7 +86,7 @@ FindWorkQueueByName(const char *name)
 WorkQueue *
 WorkQueueFindOrCreate(const wq_config_t &new_wq)
 {
-	if (!_wq_manager_running.load()) {
+	if (_wq_manager_create_queue == nullptr) {
 		PX4_ERR("not running");
 		return nullptr;
 	}
@@ -259,7 +258,6 @@ WorkQueueManagerRun(int, char **)
 {
 	_wq_manager_wqs_list = new BlockingList<WorkQueue *>();
 	_wq_manager_create_queue = new BlockingQueue<const wq_config_t *, 1>();
-	_wq_manager_running.store(true);
 
 	while (!_wq_manager_should_exit.load()) {
 		// create new work queues as needed
@@ -363,15 +361,13 @@ WorkQueueManagerRun(int, char **)
 		}
 	}
 
-	_wq_manager_running.store(false);
-
 	return 0;
 }
 
 int
 WorkQueueManagerStart()
 {
-	if (_wq_manager_should_exit.load() && !_wq_manager_running.load()) {
+	if (_wq_manager_should_exit.load() && (_wq_manager_create_queue == nullptr)) {
 
 		_wq_manager_should_exit.store(false);
 
@@ -388,18 +384,6 @@ WorkQueueManagerStart()
 			return -errno;
 		}
 
-		// Wait until initialized
-		int max_tries = 1000;
-
-		while (!_wq_manager_running.load() && --max_tries > 0) {
-			px4_usleep(1000);
-		}
-
-		if (max_tries <= 0) {
-			PX4_ERR("failed to wait for task to start");
-			return PX4_ERROR;
-		}
-
 	} else {
 		PX4_WARN("already running");
 		return PX4_ERROR;
@@ -414,7 +398,7 @@ WorkQueueManagerStop()
 	if (!_wq_manager_should_exit.load()) {
 
 		// error can't shutdown until all WorkItems are removed/stopped
-		if (_wq_manager_running.load() && (_wq_manager_wqs_list->size() > 0)) {
+		if ((_wq_manager_wqs_list != nullptr) && (_wq_manager_wqs_list->size() > 0)) {
 			PX4_ERR("can't shutdown with active WQs");
 			WorkQueueManagerStatus();
 			return PX4_ERROR;
@@ -438,7 +422,6 @@ WorkQueueManagerStop()
 			}
 
 			delete _wq_manager_wqs_list;
-			_wq_manager_wqs_list = nullptr;
 		}
 
 		_wq_manager_should_exit.store(true);
@@ -450,7 +433,6 @@ WorkQueueManagerStop()
 			px4_usleep(10000);
 
 			delete _wq_manager_create_queue;
-			_wq_manager_create_queue = nullptr;
 		}
 
 	} else {
@@ -464,7 +446,7 @@ WorkQueueManagerStop()
 int
 WorkQueueManagerStatus()
 {
-	if (!_wq_manager_should_exit.load() && _wq_manager_running.load()) {
+	if (!_wq_manager_should_exit.load() && (_wq_manager_wqs_list != nullptr)) {
 
 		const size_t num_wqs = _wq_manager_wqs_list->size();
 		PX4_INFO_RAW("\nWork Queue: %-2zu threads                          RATE        INTERVAL\n", num_wqs);

platforms/common/uORB/uORB_tests/uORBTest_UnitTest.cpp

@@ -111,7 +111,7 @@ int uORBTest::UnitTest::pubsublatency_main()
 
 	if (pubsubtest_print && timings) {
 		char fname[32] {};
-		snprintf(fname, sizeof(fname), PX4_STORAGEDIR"/uorb_timings%u.txt", timingsgroup);
+		sprintf(fname, PX4_STORAGEDIR"/uorb_timings%u.txt", timingsgroup);
 		FILE *f = fopen(fname, "w");
 
 		if (f == nullptr) {

platforms/nuttx/src/px4/common/px4_init.cpp

@@ -127,6 +127,8 @@ int px4_platform_init()
 	hrt_ioctl_init();
 #endif
 
+	param_init();
+
 	/* configure CPU load estimation */
 #ifdef CONFIG_SCHED_INSTRUMENTATION
 	cpuload_initialize_once();
@@ -178,9 +180,8 @@ int px4_platform_init()
 
 #endif // CONFIG_FS_BINFS
 
-	px4::WorkQueueManagerStart();
 
-	param_init();
+	px4::WorkQueueManagerStart();
 
 	uorb_start();
 

platforms/nuttx/src/px4/nxp/s32k3xx/adc/adc.cpp

@@ -38,144 +38,76 @@
 
 #include <nuttx/analog/adc.h>
 
-#include <hardware/s32k1xx_sim.h>
-
-//todo S32K add ADC fior now steal the kinetis one
-#include <kinetis.h>
-#include <hardware/kinetis_adc.h>
-
-
-#define _REG(_addr)	(*(volatile uint32_t *)(_addr))
-
-/* ADC register accessors */
-
-#define REG(a, _reg)	_REG(KINETIS_ADC##a##_BASE + (_reg))
-
-#define rSC1A(adc)  REG(adc, KINETIS_ADC_SC1A_OFFSET) /* ADC status and control registers 1 */
-#define rSC1B(adc)  REG(adc, KINETIS_ADC_SC1B_OFFSET) /* ADC status and control registers 1 */
-#define rCFG1(adc)  REG(adc, KINETIS_ADC_CFG1_OFFSET) /* ADC configuration register 1 */
-#define rCFG2(adc)  REG(adc, KINETIS_ADC_CFG2_OFFSET) /* Configuration register 2 */
-#define rRA(adc)    REG(adc, KINETIS_ADC_RA_OFFSET)   /* ADC data result register */
-#define rRB(adc)    REG(adc, KINETIS_ADC_RB_OFFSET)   /* ADC data result register */
-#define rCV1(adc)   REG(adc, KINETIS_ADC_CV1_OFFSET)  /* Compare value registers */
-#define rCV2(adc)   REG(adc, KINETIS_ADC_CV2_OFFSET)  /* Compare value registers */
-#define rSC2(adc)   REG(adc, KINETIS_ADC_SC2_OFFSET)  /* Status and control register 2 */
-#define rSC3(adc)   REG(adc, KINETIS_ADC_SC3_OFFSET)  /* Status and control register 3 */
-#define rOFS(adc)   REG(adc, KINETIS_ADC_OFS_OFFSET)  /* ADC offset correction register */
-#define rPG(adc)    REG(adc, KINETIS_ADC_PG_OFFSET)   /* ADC plus-side gain register */
-#define rMG(adc)    REG(adc, KINETIS_ADC_MG_OFFSET)   /* ADC minus-side gain register */
-#define rCLPD(adc)  REG(adc, KINETIS_ADC_CLPD_OFFSET) /* ADC plus-side general calibration value register */
-#define rCLPS(adc)  REG(adc, KINETIS_ADC_CLPS_OFFSET) /* ADC plus-side general calibration value register */
-#define rCLP4(adc)  REG(adc, KINETIS_ADC_CLP4_OFFSET) /* ADC plus-side general calibration value register */
-#define rCLP3(adc)  REG(adc, KINETIS_ADC_CLP3_OFFSET) /* ADC plus-side general calibration value register */
-#define rCLP2(adc)  REG(adc, KINETIS_ADC_CLP2_OFFSET) /* ADC plus-side general calibration value register */
-#define rCLP1(adc)  REG(adc, KINETIS_ADC_CLP1_OFFSET) /* ADC plus-side general calibration value register */
-#define rCLP0(adc)  REG(adc, KINETIS_ADC_CLP0_OFFSET) /* ADC plus-side general calibration value register */
-#define rCLMD(adc)  REG(adc, KINETIS_ADC_CLMD_OFFSET) /* ADC minus-side general calibration value register */
-#define rCLMS(adc)  REG(adc, KINETIS_ADC_CLMS_OFFSET) /* ADC minus-side general calibration value register */
-#define rCLM4(adc)  REG(adc, KINETIS_ADC_CLM4_OFFSET) /* ADC minus-side general calibration value register */
-#define rCLM3(adc)  REG(adc, KINETIS_ADC_CLM3_OFFSET) /* ADC minus-side general calibration value register */
-#define rCLM2(adc)  REG(adc, KINETIS_ADC_CLM2_OFFSET) /* ADC minus-side general calibration value register */
-#define rCLM1(adc)  REG(adc, KINETIS_ADC_CLM1_OFFSET) /* ADC minus-side general calibration value register */
-#define rCLM0(adc)  REG(adc, KINETIS_ADC_CLM0_OFFSET) /* ADC minus-side general calibration value register */
+#include <hardware/s32k3xx_adc.h>
+#include <hardware/s32k344_pinmux.h>
 
 int px4_arch_adc_init(uint32_t base_address)
 {
-	/* Input is Buss Clock 56 Mhz We will use /8 for 7 Mhz */
+	uint32_t regval;
 
-	irqstate_t flags = px4_enter_critical_section();
+	/* Configure and perform calibration */
+	putreg32(ADC_MCR_ADCLKSEL_DIV4, S32K3XX_ADC2_MCR);
 
-	_REG(KINETIS_SIM_SCGC3) |= SIM_SCGC3_ADC1;
-	rCFG1(1) = ADC_CFG1_ADICLK_BUSCLK | ADC_CFG1_MODE_1213BIT | ADC_CFG1_ADIV_DIV8;
-	rCFG2(1) = 0;
-	rSC2(1) = ADC_SC2_REFSEL_DEFAULT;
+	regval = getreg32(S32K3XX_ADC2_AMSIO);
+	regval |= ADC_AMSIO_HSEN_MASK;
+	putreg32(regval, S32K3XX_ADC2_AMSIO);
 
-	px4_leave_critical_section(flags);
+	regval = getreg32(S32K3XX_ADC2_CAL2);
+	regval &= ~ADC_CAL2_ENX;
+	putreg32(regval, S32K3XX_ADC2_CAL2);
 
-	/* Clear the CALF and begin the calibration */
+	regval = getreg32(S32K3XX_ADC2_CALBISTREG);
+	regval &= ~(ADC_CALBISTREG_TEST_EN | ADC_CALBISTREG_AVG_EN | ADC_CALBISTREG_NR_SMPL_MASK |
+		    ADC_CALBISTREG_CALSTFUL | ADC_CALBISTREG_TSAMP_MASK | ADC_CALBISTREG_RESN_MASK);
+	regval |= ADC_CALBISTREG_TEST_EN | ADC_CALBISTREG_AVG_EN | ADC_CALBISTREG_NR_SMPL_4SMPL |
+		  ADC_CALBISTREG_CALSTFUL | ADC_CALBISTREG_RESN_14BIT;
+	putreg32(regval, S32K3XX_ADC2_CALBISTREG);
 
-	rSC3(1) = ADC_SC3_CAL | ADC_SC3_CALF;
+	while (getreg32(S32K3XX_ADC2_CALBISTREG) & ADC_CALBISTREG_C_T_BUSY) {};
 
-	while ((rSC1A(1) & ADC_SC1_COCO) == 0) {
-		usleep(100);
+	putreg32(ADC_MCR_PWDN, S32K3XX_ADC2_MCR);
 
-		if (rSC3(1) & ADC_SC3_CALF) {
-			return -1;
-		}
-	}
+	putreg32(22, S32K3XX_ADC2_CTR0);
 
-	/* dummy read to clear COCO of calibration */
+	putreg32(22, S32K3XX_ADC2_CTR1);
 
-	int32_t r = rRA(1);
+	putreg32(0, S32K3XX_ADC2_DMAE);
 
-	/* Check the state of CALF at the end of calibration */
+	putreg32(ADC_MCR_ADCLKSEL_DIV4 | ADC_MCR_AVGS_32CONV | ADC_MCR_AVGEN | ADC_MCR_BCTU_MODE | ADC_MCR_MODE,
+		 S32K3XX_ADC2_MCR);
 
-	if (rSC3(1) & ADC_SC3_CALF) {
-		return -1;
-	}
+	putreg32(0x10, S32K3XX_ADC2_NCMR0);
 
-	/* Calculate the calibration values for single ended positive */
+	putreg32(0x10, S32K3XX_ADC2_NCMR1);
 
-	r = rCLP0(1) + rCLP1(1)  + rCLP2(1)  + rCLP3(1)  + rCLP4(1)  + rCLPS(1) ;
-	r = 0x8000U | (r >> 1U);
-	rPG(1) = r;
+	regval = getreg32(S32K3XX_ADC2_MCR);
 
-	/* Calculate the calibration values for double ended Negitive */
+	regval |= ADC_MCR_NSTART;
 
-	r = rCLM0(1) + rCLM1(1)  + rCLM2(1)  + rCLM3(1)  + rCLM4(1)  + rCLMS(1) ;
-	r = 0x8000U | (r >> 1U);
-	rMG(1) = r;
-
-	/* kick off a sample and wait for it to complete */
-	hrt_abstime now = hrt_absolute_time();
-
-	rSC1A(1) =  ADC_SC1_ADCH(ADC_SC1_ADCH_TEMP);
-
-	while (!(rSC1A(1) & ADC_SC1_COCO)) {
-
-		/* don't wait for more than 500us, since that means something broke - should reset here if we see this */
-		if ((hrt_absolute_time() - now) > 500) {
-			return -1;
-		}
-	}
+	putreg32(regval, S32K3XX_ADC2_MCR);
 
 	return 0;
 }
 
 void px4_arch_adc_uninit(uint32_t base_address)
 {
-	irqstate_t flags = px4_enter_critical_section();
-	_REG(KINETIS_SIM_SCGC3) &= ~SIM_SCGC3_ADC1;
-	px4_leave_critical_section(flags);
 }
 
 uint32_t px4_arch_adc_sample(uint32_t base_address, unsigned channel)
 {
-	irqstate_t flags = px4_enter_critical_section();
+	uint32_t result = 0;
 
-	/* clear any previous COCC */
-	rRA(1);
+	if (channel == 0) {
+		result = getreg32(S32K3XX_ADC2_PCDR4);
 
-	/* run a single conversion right now - should take about 35 cycles (5 microseconds) max */
-	rSC1A(1) = ADC_SC1_ADCH(channel);
+		if ((result & ADC_PCDR_VALID) == ADC_PCDR_VALID) {
+			result = result & 0xFFFF;
 
-	/* wait for the conversion to complete */
-	const hrt_abstime now = hrt_absolute_time();
-
-	while (!(rSC1A(1) & ADC_SC1_COCO)) {
-
-		/* don't wait for more than 10us, since that means something broke - should reset here if we see this */
-		if ((hrt_absolute_time() - now) > 10) {
-			px4_leave_critical_section(flags);
-			return 0xffff;
+		} else {
+			result = 0;
 		}
 	}
 
-	/* read the result and clear EOC */
-	uint32_t result = rRA(1);
-
-	px4_leave_critical_section(flags);
-
 	return result;
 }
 
@@ -186,10 +118,10 @@ float px4_arch_adc_reference_v()
 
 uint32_t px4_arch_adc_temp_sensor_mask()
 {
-	return 1 << (ADC_SC1_ADCH_TEMP >> ADC_SC1_ADCH_SHIFT);
+	return 0; // No temp sensor
 }
 
 uint32_t px4_arch_adc_dn_fullcount()
 {
-	return 1 << 12; // 12 bit ADC
+	return 1 << 15; // 15 bit conversion data
 }

platforms/posix/src/px4/common/px4_init.cpp

@@ -44,10 +44,10 @@ int px4_platform_init(void)
 {
 	hrt_init();
 
-	px4::WorkQueueManagerStart();
-
 	param_init();
 
+	px4::WorkQueueManagerStart();
+
 	uorb_start();
 
 	px4_log_initialize();

src/drivers/actuators/modal_io/modal_io.cpp

@@ -48,7 +48,6 @@ const char *_device;
 
 ModalIo::ModalIo() :
 	OutputModuleInterface(MODULE_NAME, px4::serial_port_to_wq(MODAL_IO_DEFAULT_PORT)),
-	_mixing_output{"MODAL_IO", MODAL_IO_OUTPUT_CHANNELS, *this, MixingOutput::SchedulingPolicy::Auto, false, false},
 	_cycle_perf(perf_alloc(PC_ELAPSED, MODULE_NAME": cycle")),
 	_output_update_perf(perf_alloc(PC_INTERVAL, MODULE_NAME": output update interval"))
 {

src/drivers/actuators/modal_io/modal_io.hpp

@@ -174,7 +174,7 @@ private:
 	} led_rsc_t;
 
 	ch_assign_t		_output_map[MODAL_IO_OUTPUT_CHANNELS] {{1, 1}, {2, 1}, {3, 1}, {4, 1}};
-	MixingOutput 		_mixing_output;
+	MixingOutput _mixing_output{"MODAL_IO", MODAL_IO_OUTPUT_CHANNELS, *this, MixingOutput::SchedulingPolicy::Auto, false, false};
 
 	perf_counter_t		_cycle_perf;
 	perf_counter_t		_output_update_perf;

src/drivers/barometer/invensense/icp201xx/ICP201XX.cpp

@@ -250,7 +250,7 @@ ICP201XX::RunImpl()
 	case STATE::CONFIG: {
 			if (configure()) {
 				_state = STATE::WAIT_READ;
-				ScheduleDelayed(10_ms);
+				ScheduleDelayed(30_ms);
 
 			} else {
 				if (hrt_elapsed_time(&_reset_timestamp) > 1000_ms) {

src/drivers/drv_pwm_output.h

@@ -61,41 +61,11 @@ __BEGIN_DECLS
  */
 #define PWM_LOWEST_MIN 90
 
-/**
- * Default value for a shutdown motor
- */
-#define PWM_MOTOR_OFF	900
-
-/**
- * Default minimum PWM in us
- */
-#define PWM_DEFAULT_MIN 1000
-
-/**
- * Highest PWM allowed as the minimum PWM
- */
-#define PWM_HIGHEST_MIN 1600
-
 /**
  * Highest maximum PWM in us
  */
 #define PWM_HIGHEST_MAX 2500
 
-/**
- * Default maximum PWM in us
- */
-#define PWM_DEFAULT_MAX 2000
-
-/**
- * Default trim PWM in us
- */
-#define PWM_DEFAULT_TRIM 0
-
-/**
- * Lowest PWM allowed as the maximum PWM
- */
-#define PWM_LOWEST_MAX 200
-
 #endif // not PX4_PWM_ALTERNATE_RANGES
 
 /**

src/drivers/drv_sensor.h

@@ -82,6 +82,7 @@
 #define DRV_IMU_DEVTYPE_IIM42652 0x2B
 #define DRV_IMU_DEVTYPE_IAM20680HP 0x2C
 #define DRV_IMU_DEVTYPE_ICM42686P 0x2D
+#define DRV_IMU_DEVTYPE_IIM42653 0x2E
 
 #define DRV_RNG_DEVTYPE_MB12XX   0x31
 #define DRV_RNG_DEVTYPE_LL40LS   0x32

src/drivers/dshot/DShot.h

@@ -143,7 +143,7 @@ private:
 
 	void handle_vehicle_commands();
 
-	MixingOutput _mixing_output {PARAM_PREFIX, DIRECT_PWM_OUTPUT_CHANNELS, *this, MixingOutput::SchedulingPolicy::Auto, false, false};
+	MixingOutput _mixing_output{PARAM_PREFIX, DIRECT_PWM_OUTPUT_CHANNELS, *this, MixingOutput::SchedulingPolicy::Auto, false, false};
 	uint32_t _reversible_outputs{};
 
 	Telemetry *_telemetry{nullptr};

src/drivers/gps/gps.cpp

@@ -60,6 +60,7 @@
 #include <uORB/Publication.hpp>
 #include <uORB/PublicationMulti.hpp>
 #include <uORB/Subscription.hpp>
+#include <uORB/SubscriptionMultiArray.hpp>
 #include <uORB/topics/gps_dump.h>
 #include <uORB/topics/gps_inject_data.h>
 #include <uORB/topics/sensor_gps.h>
@@ -203,7 +204,7 @@ private:
 
 	const Instance 			_instance;
 
-	uORB::Subscription		     _orb_inject_data_sub{ORB_ID(gps_inject_data)};
+	uORB::SubscriptionMultiArray<gps_inject_data_s, gps_inject_data_s::MAX_INSTANCES> _orb_inject_data_sub{ORB_ID::gps_inject_data};
 	uORB::Publication<gps_inject_data_s> _gps_inject_data_pub{ORB_ID(gps_inject_data)};
 	uORB::Publication<gps_dump_s>	     _dump_communication_pub{ORB_ID(gps_dump)};
 	gps_dump_s			     *_dump_to_device{nullptr};
@@ -530,13 +531,15 @@ void GPS::handleInjectDataTopic()
 	// If there has not been a valid RTCM message for a while, try to switch to a different RTCM link
 	if ((hrt_absolute_time() - _last_rtcm_injection_time) > 5_s) {
 
-		for (uint8_t i = 0; i < gps_inject_data_s::MAX_INSTANCES; i++) {
-			if (_orb_inject_data_sub.ChangeInstance(i)) {
-				if (_orb_inject_data_sub.copy(&msg)) {
+		for (int instance = 0; instance < _orb_inject_data_sub.size(); instance++) {
+			const bool exists = _orb_inject_data_sub[instance].advertised();
+
+			if (exists) {
+				if (_orb_inject_data_sub[instance].copy(&msg)) {
 					if ((hrt_absolute_time() - msg.timestamp) < 5_s) {
 						// Remember that we already did a copy on this instance.
 						already_copied = true;
-						_selected_rtcm_instance = i;
+						_selected_rtcm_instance = instance;
 						break;
 					}
 				}
@@ -544,9 +547,6 @@ void GPS::handleInjectDataTopic()
 		}
 	}
 
-	// Reset instance in case we didn't actually want to switch
-	_orb_inject_data_sub.ChangeInstance(_selected_rtcm_instance);
-
 	bool updated = already_copied;
 
 	// Limit maximum number of GPS injections to 8 since usually
@@ -574,7 +574,7 @@ void GPS::handleInjectDataTopic()
 			}
 		}
 
-		updated = _orb_inject_data_sub.update(&msg);
+		updated = _orb_inject_data_sub[_selected_rtcm_instance].update(&msg);
 
 	} while (updated && num_injections < max_num_injections);
 }

src/drivers/imu/adis16497/ADIS16497.cpp

@@ -342,9 +342,9 @@ ADIS16497::read_reg16(uint8_t reg)
 
 	cmd[0] = ((reg | DIR_READ) << 8) & 0xff00;
 	transferhword(cmd, nullptr, 1);
-	up_udelay(T_STALL);
+	px4_udelay(T_STALL);
 	transferhword(nullptr, cmd, 1);
-	up_udelay(T_STALL);
+	px4_udelay(T_STALL);
 
 	return cmd[0];
 }
@@ -367,9 +367,9 @@ ADIS16497::write_reg16(uint8_t reg, uint16_t value)
 	cmd[1] = (((reg + 0x1) | DIR_WRITE) << 8) | ((0xff00 & value) >> 8);
 
 	transferhword(cmd, nullptr, 1);
-	up_udelay(T_STALL);
+	px4_udelay(T_STALL);
 	transferhword(cmd + 1, nullptr, 1);
-	up_udelay(T_STALL);
+	px4_udelay(T_STALL);
 }
 
 void

src/drivers/imu/bosch/bmi055/BMI055_Gyroscope.cpp

@@ -33,8 +33,6 @@
 
 #include "BMI055_Gyroscope.hpp"
 
-#include <px4_platform/board_dma_alloc.h>
-
 using namespace time_literals;
 
 namespace Bosch::BMI055::Gyroscope

src/drivers/imu/bosch/bmi088/BMI088_Gyroscope.cpp

@@ -33,8 +33,6 @@
 
 #include "BMI088_Gyroscope.hpp"
 
-#include <px4_platform/board_dma_alloc.h>
-
 using namespace time_literals;
 
 namespace Bosch::BMI088::Gyroscope

src/drivers/imu/invensense/iim42653/CMakeLists.txt

@@ -0,0 +1,48 @@
+############################################################################
+#
+#   Copyright (c) 2022 PX4 Development Team. All rights reserved.
+#
+# Redistribution and use in source and binary forms, with or without
+# modification, are permitted provided that the following conditions
+# are met:
+#
+# 1. Redistributions of source code must retain the above copyright
+#    notice, this list of conditions and the following disclaimer.
+# 2. Redistributions in binary form must reproduce the above copyright
+#    notice, this list of conditions and the following disclaimer in
+#    the documentation and/or other materials provided with the
+#    distribution.
+# 3. Neither the name PX4 nor the names of its contributors may be
+#    used to endorse or promote products derived from this software
+#    without specific prior written permission.
+#
+# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+# "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+# LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+# FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+# COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+# INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+# BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
+# OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
+# AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+# LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+# ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+# POSSIBILITY OF SUCH DAMAGE.
+#
+############################################################################
+px4_add_module(
+	MODULE drivers__imu__invensense__iim42653
+	MAIN iim42653
+	COMPILE_FLAGS
+		${MAX_CUSTOM_OPT_LEVEL}
+		#-DDEBUG_BUILD
+	SRCS
+		iim42653_main.cpp
+		IIM42653.cpp
+		IIM42653.hpp
+		InvenSense_IIM42653_registers.hpp
+	DEPENDS
+		px4_work_queue
+		drivers_accelerometer
+		drivers_gyroscope
+	)

src/drivers/imu/invensense/iim42653/IIM42653.cpp

@@ -0,0 +1,861 @@
+/****************************************************************************
+ *
+ *   Copyright (c) 2023 PX4 Development Team. All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright
+ *    notice, this list of conditions and the following disclaimer.
+ * 2. Redistributions in binary form must reproduce the above copyright
+ *    notice, this list of conditions and the following disclaimer in
+ *    the documentation and/or other materials provided with the
+ *    distribution.
+ * 3. Neither the name PX4 nor the names of its contributors may be
+ *    used to endorse or promote products derived from this software
+ *    without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
+ * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
+ * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *
+ ****************************************************************************/
+
+#include "IIM42653.hpp"
+
+using namespace time_literals;
+
+static constexpr int16_t combine(uint8_t msb, uint8_t lsb)
+{
+	return (msb << 8u) | lsb;
+}
+
+static constexpr uint16_t combine_uint(uint8_t msb, uint8_t lsb)
+{
+	return (msb << 8u) | lsb;
+}
+
+IIM42653::IIM42653(const I2CSPIDriverConfig &config) :
+	SPI(config),
+	I2CSPIDriver(config),
+	_drdy_gpio(config.drdy_gpio),
+	_px4_accel(get_device_id(), config.rotation),
+	_px4_gyro(get_device_id(), config.rotation)
+{
+	if (config.drdy_gpio != 0) {
+		_drdy_missed_perf = perf_alloc(PC_COUNT, MODULE_NAME": DRDY missed");
+	}
+
+	if (config.custom1 != 0) {
+		_enable_clock_input = true;
+		_input_clock_freq = config.custom1;
+		ConfigureCLKIN();
+
+	} else {
+		_enable_clock_input = false;
+	}
+
+	ConfigureSampleRate(_px4_gyro.get_max_rate_hz());
+}
+
+IIM42653::~IIM42653()
+{
+	perf_free(_bad_register_perf);
+	perf_free(_bad_transfer_perf);
+	perf_free(_fifo_empty_perf);
+	perf_free(_fifo_overflow_perf);
+	perf_free(_fifo_reset_perf);
+	perf_free(_drdy_missed_perf);
+}
+
+int IIM42653::init()
+{
+	int ret = SPI::init();
+
+	if (ret != PX4_OK) {
+		DEVICE_DEBUG("SPI::init failed (%i)", ret);
+		return ret;
+	}
+
+	return Reset() ? 0 : -1;
+}
+
+bool IIM42653::Reset()
+{
+	_state = STATE::RESET;
+	DataReadyInterruptDisable();
+	ScheduleClear();
+	ScheduleNow();
+	return true;
+}
+
+void IIM42653::exit_and_cleanup()
+{
+	DataReadyInterruptDisable();
+	I2CSPIDriverBase::exit_and_cleanup();
+}
+
+void IIM42653::print_status()
+{
+	I2CSPIDriverBase::print_status();
+
+	PX4_INFO("FIFO empty interval: %d us (%.1f Hz)", _fifo_empty_interval_us, 1e6 / _fifo_empty_interval_us);
+	PX4_INFO("Clock input: %s", _enable_clock_input ? "enabled" : "disabled");
+
+	perf_print_counter(_bad_register_perf);
+	perf_print_counter(_bad_transfer_perf);
+	perf_print_counter(_fifo_empty_perf);
+	perf_print_counter(_fifo_overflow_perf);
+	perf_print_counter(_fifo_reset_perf);
+	perf_print_counter(_drdy_missed_perf);
+}
+
+int IIM42653::probe()
+{
+	for (int i = 0; i < 3; i++) {
+		uint8_t whoami = RegisterRead(Register::BANK_0::WHO_AM_I);
+
+		if (whoami == WHOAMI) {
+			return PX4_OK;
+
+		} else {
+			DEVICE_DEBUG("unexpected WHO_AM_I 0x%02x", whoami);
+
+			uint8_t reg_bank_sel = RegisterRead(Register::BANK_0::REG_BANK_SEL);
+			int bank = reg_bank_sel >> 4;
+
+			if (bank >= 1 && bank <= 3) {
+				DEVICE_DEBUG("incorrect register bank for WHO_AM_I REG_BANK_SEL:0x%02x, bank:%d", reg_bank_sel, bank);
+				// force bank selection and retry
+				SelectRegisterBank(REG_BANK_SEL_BIT::BANK_SEL_0, true);
+			}
+		}
+	}
+
+	return PX4_ERROR;
+}
+
+void IIM42653::RunImpl()
+{
+	const hrt_abstime now = hrt_absolute_time();
+
+	switch (_state) {
+	case STATE::RESET:
+		// DEVICE_CONFIG: Software reset configuration
+		RegisterWrite(Register::BANK_0::DEVICE_CONFIG, DEVICE_CONFIG_BIT::SOFT_RESET_CONFIG);
+		_reset_timestamp = now;
+		_failure_count = 0;
+		_state = STATE::WAIT_FOR_RESET;
+		ScheduleDelayed(1_ms); // wait 1 ms for soft reset to be effective
+		break;
+
+	case STATE::WAIT_FOR_RESET:
+		if ((RegisterRead(Register::BANK_0::WHO_AM_I) == WHOAMI)
+		    && (RegisterRead(Register::BANK_0::DEVICE_CONFIG) == 0x00)
+		    && (RegisterRead(Register::BANK_0::INT_STATUS) & INT_STATUS_BIT::RESET_DONE_INT)) {
+
+			// Wakeup accel and gyro and schedule remaining configuration
+			RegisterWrite(Register::BANK_0::PWR_MGMT0, PWR_MGMT0_BIT::GYRO_MODE_LOW_NOISE | PWR_MGMT0_BIT::ACCEL_MODE_LOW_NOISE);
+			_state = STATE::CONFIGURE;
+			ScheduleDelayed(30_ms); // 30 ms gyro startup time, 10 ms accel from sleep to valid data
+
+		} else {
+			// RESET not complete
+			if (hrt_elapsed_time(&_reset_timestamp) > 1000_ms) {
+				PX4_DEBUG("Reset failed, retrying");
+				_state = STATE::RESET;
+				ScheduleDelayed(100_ms);
+
+			} else {
+				PX4_DEBUG("Reset not complete, check again in 10 ms");
+				ScheduleDelayed(10_ms);
+			}
+		}
+
+		break;
+
+	case STATE::CONFIGURE:
+		if (Configure()) {
+			// if configure succeeded then reset the FIFO
+			_state = STATE::FIFO_RESET;
+			ScheduleDelayed(1_ms);
+
+		} else {
+			// CONFIGURE not complete
+			if (hrt_elapsed_time(&_reset_timestamp) > 1000_ms) {
+				PX4_DEBUG("Configure failed, resetting");
+				_state = STATE::RESET;
+
+			} else {
+				PX4_DEBUG("Configure failed, retrying");
+			}
+
+			ScheduleDelayed(100_ms);
+		}
+
+		break;
+
+	case STATE::FIFO_RESET:
+
+		_state = STATE::FIFO_READ;
+		FIFOReset();
+
+		if (DataReadyInterruptConfigure()) {
+			_data_ready_interrupt_enabled = true;
+
+			// backup schedule as a watchdog timeout
+			ScheduleDelayed(100_ms);
+
+		} else {
+			_data_ready_interrupt_enabled = false;
+			ScheduleOnInterval(_fifo_empty_interval_us, _fifo_empty_interval_us);
+		}
+
+		break;
+
+	case STATE::FIFO_READ: {
+			hrt_abstime timestamp_sample = now;
+			uint8_t samples = 0;
+
+			if (_data_ready_interrupt_enabled) {
+				// scheduled from interrupt if _drdy_timestamp_sample was set as expected
+				const hrt_abstime drdy_timestamp_sample = _drdy_timestamp_sample.fetch_and(0);
+
+				if ((now - drdy_timestamp_sample) < _fifo_empty_interval_us) {
+					timestamp_sample = drdy_timestamp_sample;
+					samples = _fifo_gyro_samples;
+
+				} else {
+					perf_count(_drdy_missed_perf);
+				}
+
+				// push backup schedule back
+				ScheduleDelayed(_fifo_empty_interval_us * 2);
+			}
+
+			if (samples == 0) {
+				// check current FIFO count
+				const uint16_t fifo_count = FIFOReadCount();
+
+				if (fifo_count >= FIFO::SIZE) {
+					FIFOReset();
+					perf_count(_fifo_overflow_perf);
+
+				} else if (fifo_count == 0) {
+					perf_count(_fifo_empty_perf);
+
+				} else {
+					// FIFO count (size in bytes)
+					samples = (fifo_count / sizeof(FIFO::DATA));
+
+					// tolerate minor jitter, leave sample to next iteration if behind by only 1
+					if (samples == _fifo_gyro_samples + 1) {
+						timestamp_sample -= static_cast<int>(FIFO_SAMPLE_DT);
+						samples--;
+					}
+
+					if (samples > FIFO_MAX_SAMPLES) {
+						// not technically an overflow, but more samples than we expected or can publish
+						FIFOReset();
+						perf_count(_fifo_overflow_perf);
+						samples = 0;
+					}
+				}
+			}
+
+			bool success = false;
+
+			if (samples >= 1) {
+				if (FIFORead(timestamp_sample, samples)) {
+					success = true;
+
+					if (_failure_count > 0) {
+						_failure_count--;
+					}
+				}
+			}
+
+			if (!success) {
+				_failure_count++;
+
+				// full reset if things are failing consistently
+				if (_failure_count > 10) {
+					Reset();
+					return;
+				}
+			}
+
+			if (!success || hrt_elapsed_time(&_last_config_check_timestamp) > 100_ms) {
+				// check configuration registers periodically or immediately following any failure
+				if (RegisterCheck(_register_bank0_cfg[_checked_register_bank0])
+				    && RegisterCheck(_register_bank1_cfg[_checked_register_bank1])
+				    && RegisterCheck(_register_bank2_cfg[_checked_register_bank2])
+				   ) {
+					_last_config_check_timestamp = now;
+					_checked_register_bank0 = (_checked_register_bank0 + 1) % size_register_bank0_cfg;
+					_checked_register_bank1 = (_checked_register_bank1 + 1) % size_register_bank1_cfg;
+					_checked_register_bank2 = (_checked_register_bank2 + 1) % size_register_bank2_cfg;
+
+				} else {
+					// register check failed, force reset
+					perf_count(_bad_register_perf);
+					Reset();
+				}
+			}
+		}
+
+		break;
+	}
+}
+
+void IIM42653::ConfigureSampleRate(int sample_rate)
+{
+	// round down to nearest FIFO sample dt
+	const float min_interval = FIFO_SAMPLE_DT;
+	_fifo_empty_interval_us = math::max(roundf((1e6f / (float)sample_rate) / min_interval) * min_interval, min_interval);
+
+	_fifo_gyro_samples = roundf(math::min((float)_fifo_empty_interval_us / (1e6f / GYRO_RATE), (float)FIFO_MAX_SAMPLES));
+
+	// recompute FIFO empty interval (us) with actual gyro sample limit
+	_fifo_empty_interval_us = _fifo_gyro_samples * (1e6f / GYRO_RATE);
+
+	ConfigureFIFOWatermark(_fifo_gyro_samples);
+}
+
+void IIM42653::ConfigureFIFOWatermark(uint8_t samples)
+{
+	// FIFO watermark threshold in number of bytes
+	const uint16_t fifo_watermark_threshold = samples * sizeof(FIFO::DATA);
+
+	for (auto &r : _register_bank0_cfg) {
+		if (r.reg == Register::BANK_0::FIFO_CONFIG2) {
+			// FIFO_WM[7:0]  FIFO_CONFIG2
+			r.set_bits = fifo_watermark_threshold & 0xFF;
+
+		} else if (r.reg == Register::BANK_0::FIFO_CONFIG3) {
+			// FIFO_WM[11:8] FIFO_CONFIG3
+			r.set_bits = (fifo_watermark_threshold >> 8) & 0x0F;
+		}
+	}
+}
+
+void IIM42653::ConfigureCLKIN()
+{
+	for (auto &r0 : _register_bank0_cfg) {
+		if (r0.reg == Register::BANK_0::INTF_CONFIG1) {
+			r0.set_bits = INTF_CONFIG1_BIT::RTC_MODE;
+			r0.set_bits = INTF_CONFIG1_BIT::CLKSEL;
+			r0.clear_bits = INTF_CONFIG1_BIT::CLKSEL_CLEAR;
+		}
+	}
+
+	for (auto &r1 : _register_bank1_cfg) {
+		if (r1.reg == Register::BANK_1::INTF_CONFIG5) {
+			r1.set_bits = INTF_CONFIG5_BIT::PIN9_FUNCTION_CLKIN_SET;
+			r1.clear_bits = INTF_CONFIG5_BIT::PIN9_FUNCTION_CLKIN_CLEAR;
+		}
+	}
+}
+
+void IIM42653::SelectRegisterBank(enum REG_BANK_SEL_BIT bank, bool force)
+{
+	if (bank != _last_register_bank || force) {
+		// select BANK_0
+		uint8_t cmd_bank_sel[2] {};
+		cmd_bank_sel[0] = static_cast<uint8_t>(Register::BANK_0::REG_BANK_SEL);
+		cmd_bank_sel[1] = bank;
+		transfer(cmd_bank_sel, cmd_bank_sel, sizeof(cmd_bank_sel));
+
+		_last_register_bank = bank;
+	}
+}
+
+bool IIM42653::Configure()
+{
+	// first set and clear all configured register bits
+	for (const auto &reg_cfg : _register_bank0_cfg) {
+		RegisterSetAndClearBits(reg_cfg.reg, reg_cfg.set_bits, reg_cfg.clear_bits);
+	}
+
+	for (const auto &reg_cfg : _register_bank1_cfg) {
+		RegisterSetAndClearBits(reg_cfg.reg, reg_cfg.set_bits, reg_cfg.clear_bits);
+	}
+
+	for (const auto &reg_cfg : _register_bank2_cfg) {
+		RegisterSetAndClearBits(reg_cfg.reg, reg_cfg.set_bits, reg_cfg.clear_bits);
+	}
+
+	// now check that all are configured
+	bool success = true;
+
+	for (const auto &reg_cfg : _register_bank0_cfg) {
+		if (!RegisterCheck(reg_cfg)) {
+			success = false;
+		}
+	}
+
+	for (const auto &reg_cfg : _register_bank1_cfg) {
+		if (!RegisterCheck(reg_cfg)) {
+			success = false;
+		}
+	}
+
+	for (const auto &reg_cfg : _register_bank2_cfg) {
+		if (!RegisterCheck(reg_cfg)) {
+			success = false;
+		}
+	}
+
+	// 20-bits data format used
+	//  the only FSR settings that are operational are ±2000dps for gyroscope and ±16g for accelerometer
+	_px4_accel.set_range(16.f * CONSTANTS_ONE_G);
+	_px4_gyro.set_range(math::radians(2000.f));
+
+	return success;
+}
+
+int IIM42653::DataReadyInterruptCallback(int irq, void *context, void *arg)
+{
+	static_cast<IIM42653 *>(arg)->DataReady();
+	return 0;
+}
+
+void IIM42653::DataReady()
+{
+	_drdy_timestamp_sample.store(hrt_absolute_time());
+	ScheduleNow();
+}
+
+bool IIM42653::DataReadyInterruptConfigure()
+{
+	if (_drdy_gpio == 0) {
+		return false;
+	}
+
+	// Setup data ready on falling edge
+	return px4_arch_gpiosetevent(_drdy_gpio, false, true, true, &DataReadyInterruptCallback, this) == 0;
+}
+
+bool IIM42653::DataReadyInterruptDisable()
+{
+	if (_drdy_gpio == 0) {
+		return false;
+	}
+
+	return px4_arch_gpiosetevent(_drdy_gpio, false, false, false, nullptr, nullptr) == 0;
+}
+
+template <typename T>
+bool IIM42653::RegisterCheck(const T &reg_cfg)
+{
+	bool success = true;
+
+	const uint8_t reg_value = RegisterRead(reg_cfg.reg);
+
+	if (reg_cfg.set_bits && ((reg_value & reg_cfg.set_bits) != reg_cfg.set_bits)) {
+		PX4_DEBUG("0x%02hhX: 0x%02hhX (0x%02hhX not set)", (uint8_t)reg_cfg.reg, reg_value, reg_cfg.set_bits);
+		success = false;
+	}
+
+	if (reg_cfg.clear_bits && ((reg_value & reg_cfg.clear_bits) != 0)) {
+		PX4_DEBUG("0x%02hhX: 0x%02hhX (0x%02hhX not cleared)", (uint8_t)reg_cfg.reg, reg_value, reg_cfg.clear_bits);
+		success = false;
+	}
+
+	return success;
+}
+
+template <typename T>
+uint8_t IIM42653::RegisterRead(T reg)
+{
+	uint8_t cmd[2] {};
+	cmd[0] = static_cast<uint8_t>(reg) | DIR_READ;
+	SelectRegisterBank(reg);
+	transfer(cmd, cmd, sizeof(cmd));
+	return cmd[1];
+}
+
+template <typename T>
+void IIM42653::RegisterWrite(T reg, uint8_t value)
+{
+	uint8_t cmd[2] { (uint8_t)reg, value };
+	SelectRegisterBank(reg);
+	transfer(cmd, cmd, sizeof(cmd));
+}
+
+template <typename T>
+void IIM42653::RegisterSetAndClearBits(T reg, uint8_t setbits, uint8_t clearbits)
+{
+	const uint8_t orig_val = RegisterRead(reg);
+
+	uint8_t val = (orig_val & ~clearbits) | setbits;
+
+	if (orig_val != val) {
+		RegisterWrite(reg, val);
+	}
+}
+
+uint16_t IIM42653::FIFOReadCount()
+{
+	// read FIFO count
+	uint8_t fifo_count_buf[3] {};
+	fifo_count_buf[0] = static_cast<uint8_t>(Register::BANK_0::FIFO_COUNTH) | DIR_READ;
+	SelectRegisterBank(REG_BANK_SEL_BIT::BANK_SEL_0);
+
+	if (transfer(fifo_count_buf, fifo_count_buf, sizeof(fifo_count_buf)) != PX4_OK) {
+		perf_count(_bad_transfer_perf);
+		return 0;
+	}
+
+	return combine(fifo_count_buf[1], fifo_count_buf[2]);
+}
+
+bool IIM42653::FIFORead(const hrt_abstime &timestamp_sample, uint8_t samples)
+{
+	FIFOTransferBuffer buffer{};
+	const size_t transfer_size = math::min(samples * sizeof(FIFO::DATA) + 4, FIFO::SIZE);
+	SelectRegisterBank(REG_BANK_SEL_BIT::BANK_SEL_0);
+
+	if (transfer((uint8_t *)&buffer, (uint8_t *)&buffer, transfer_size) != PX4_OK) {
+		perf_count(_bad_transfer_perf);
+		return false;
+	}
+
+	if (buffer.INT_STATUS & INT_STATUS_BIT::FIFO_FULL_INT) {
+		perf_count(_fifo_overflow_perf);
+		FIFOReset();
+		return false;
+	}
+
+	const uint16_t fifo_count_bytes = combine(buffer.FIFO_COUNTH, buffer.FIFO_COUNTL);
+
+	if (fifo_count_bytes >= FIFO::SIZE) {
+		perf_count(_fifo_overflow_perf);
+		FIFOReset();
+		return false;
+	}
+
+	const uint8_t fifo_count_samples = fifo_count_bytes / sizeof(FIFO::DATA);
+
+	if (fifo_count_samples == 0) {
+		perf_count(_fifo_empty_perf);
+		return false;
+	}
+
+	// check FIFO header in every sample
+	uint8_t valid_samples = 0;
+
+	for (int i = 0; i < math::min(samples, fifo_count_samples); i++) {
+		bool valid = true;
+
+		// With FIFO_ACCEL_EN and FIFO_GYRO_EN header should be 8’b_0110_10xx
+		const uint8_t FIFO_HEADER = buffer.f[i].FIFO_Header;
+
+		if (FIFO_HEADER & FIFO::FIFO_HEADER_BIT::HEADER_MSG) {
+			// FIFO sample empty if HEADER_MSG set
+			valid = false;
+
+		} else if (!(FIFO_HEADER & FIFO::FIFO_HEADER_BIT::HEADER_ACCEL)) {
+			// accel bit not set
+			valid = false;
+
+		} else if (!(FIFO_HEADER & FIFO::FIFO_HEADER_BIT::HEADER_GYRO)) {
+			// gyro bit not set
+			valid = false;
+
+		} else if (!(FIFO_HEADER & FIFO::FIFO_HEADER_BIT::HEADER_20)) {
+			// Packet does not contain a new and valid extended 20-bit data
+			valid = false;
+
+		} else if ((FIFO_HEADER & FIFO::FIFO_HEADER_BIT::HEADER_TIMESTAMP_FSYNC) != Bit3) {
+			// Packet does not contain ODR timestamp
+			valid = false;
+
+		} else if (FIFO_HEADER & FIFO::FIFO_HEADER_BIT::HEADER_ODR_ACCEL) {
+			// accel ODR changed
+			valid = false;
+
+		} else if (FIFO_HEADER & FIFO::FIFO_HEADER_BIT::HEADER_ODR_GYRO) {
+			// gyro ODR changed
+			valid = false;
+		}
+
+		if (valid) {
+			valid_samples++;
+
+		} else {
+			perf_count(_bad_transfer_perf);
+			break;
+		}
+	}
+
+	if (valid_samples > 0) {
+		if (ProcessTemperature(buffer.f, valid_samples)) {
+			ProcessGyro(timestamp_sample, buffer.f, valid_samples);
+			ProcessAccel(timestamp_sample, buffer.f, valid_samples);
+			return true;
+		}
+	}
+
+	return false;
+}
+
+void IIM42653::FIFOReset()
+{
+	perf_count(_fifo_reset_perf);
+
+	// SIGNAL_PATH_RESET: FIFO flush
+	RegisterSetBits(Register::BANK_0::SIGNAL_PATH_RESET, SIGNAL_PATH_RESET_BIT::FIFO_FLUSH);
+
+	// reset while FIFO is disabled
+	_drdy_timestamp_sample.store(0);
+}
+
+static constexpr int32_t reassemble_20bit(const uint32_t a, const uint32_t b, const uint32_t c)
+{
+	// 0xXXXAABBC
+	uint32_t high   = ((a << 12) & 0x000FF000);
+	uint32_t low    = ((b << 4)  & 0x00000FF0);
+	uint32_t lowest = (c         & 0x0000000F);
+
+	uint32_t x = high | low | lowest;
+
+	if (a & Bit7) {
+		// sign extend
+		x |= 0xFFF00000u;
+	}
+
+	return static_cast<int32_t>(x);
+}
+
+void IIM42653::ProcessAccel(const hrt_abstime &timestamp_sample, const FIFO::DATA fifo[], const uint8_t samples)
+{
+	sensor_accel_fifo_s accel{};
+	accel.timestamp_sample = timestamp_sample;
+	accel.samples = 0;
+
+	// 18-bits of accelerometer data
+	bool scale_20bit = false;
+
+	// first pass
+	for (int i = 0; i < samples; i++) {
+
+		uint16_t timestamp_fifo = combine_uint(fifo[i].TimeStamp_h, fifo[i].TimeStamp_l);
+
+		if (_enable_clock_input) {
+			accel.dt = (float)timestamp_fifo * ((1.f / _input_clock_freq) * 1e6f);
+
+		} else {
+			accel.dt = (float)timestamp_fifo * FIFO_TIMESTAMP_SCALING;
+		}
+
+		// 20 bit hires mode
+		// Sign extension + Accel [19:12] + Accel [11:4] + Accel [3:2] (20 bit extension byte)
+		// Accel data is 18 bit ()
+		int32_t accel_x = reassemble_20bit(fifo[i].ACCEL_DATA_X1, fifo[i].ACCEL_DATA_X0,
+						   fifo[i].Ext_Accel_X_Gyro_X & 0xF0 >> 4);
+		int32_t accel_y = reassemble_20bit(fifo[i].ACCEL_DATA_Y1, fifo[i].ACCEL_DATA_Y0,
+						   fifo[i].Ext_Accel_Y_Gyro_Y & 0xF0 >> 4);
+		int32_t accel_z = reassemble_20bit(fifo[i].ACCEL_DATA_Z1, fifo[i].ACCEL_DATA_Z0,
+						   fifo[i].Ext_Accel_Z_Gyro_Z & 0xF0 >> 4);
+
+		// sample invalid if -524288
+		if (accel_x != -524288 && accel_y != -524288 && accel_z != -524288) {
+			// check if any values are going to exceed int16 limits
+			static constexpr int16_t max_accel = INT16_MAX;
+			static constexpr int16_t min_accel = INT16_MIN;
+
+			if (accel_x >= max_accel || accel_x <= min_accel) {
+				scale_20bit = true;
+			}
+
+			if (accel_y >= max_accel || accel_y <= min_accel) {
+				scale_20bit = true;
+			}
+
+			if (accel_z >= max_accel || accel_z <= min_accel) {
+				scale_20bit = true;
+			}
+
+			// shift by 2 (2 least significant bits are always 0)
+			accel.x[accel.samples] = accel_x / 4;
+			accel.y[accel.samples] = accel_y / 4;
+			accel.z[accel.samples] = accel_z / 4;
+			accel.samples++;
+		}
+	}
+
+	if (!scale_20bit) {
+		// if highres enabled accel data is always 4096 LSB/g
+		_px4_accel.set_scale(CONSTANTS_ONE_G / 4096.f);
+
+	} else {
+		// 20 bit data scaled to 16 bit (2^4)
+		for (int i = 0; i < samples; i++) {
+			// 20 bit hires mode
+			// Sign extension + Accel [19:12] + Accel [11:4] + Accel [3:2] (20 bit extension byte)
+			// Accel data is 18 bit ()
+			int16_t accel_x = combine(fifo[i].ACCEL_DATA_X1, fifo[i].ACCEL_DATA_X0);
+			int16_t accel_y = combine(fifo[i].ACCEL_DATA_Y1, fifo[i].ACCEL_DATA_Y0);
+			int16_t accel_z = combine(fifo[i].ACCEL_DATA_Z1, fifo[i].ACCEL_DATA_Z0);
+
+			accel.x[i] = accel_x;
+			accel.y[i] = accel_y;
+			accel.z[i] = accel_z;
+		}
+
+		_px4_accel.set_scale(CONSTANTS_ONE_G / 1024.f);
+	}
+
+	// correct frame for publication
+	for (int i = 0; i < accel.samples; i++) {
+		// sensor's frame is +x forward, +y left, +z up
+		//  flip y & z to publish right handed with z down (x forward, y right, z down)
+		accel.x[i] = accel.x[i];
+		accel.y[i] = (accel.y[i] == INT16_MIN) ? INT16_MAX : -accel.y[i];
+		accel.z[i] = (accel.z[i] == INT16_MIN) ? INT16_MAX : -accel.z[i];
+	}
+
+	_px4_accel.set_error_count(perf_event_count(_bad_register_perf) + perf_event_count(_bad_transfer_perf) +
+				   perf_event_count(_fifo_empty_perf) + perf_event_count(_fifo_overflow_perf));
+
+	if (accel.samples > 0) {
+		_px4_accel.updateFIFO(accel);
+	}
+}
+
+void IIM42653::ProcessGyro(const hrt_abstime &timestamp_sample, const FIFO::DATA fifo[], const uint8_t samples)
+{
+	sensor_gyro_fifo_s gyro{};
+	gyro.timestamp_sample = timestamp_sample;
+	gyro.samples = 0;
+
+	// 20-bits of gyroscope data
+	bool scale_20bit = false;
+
+	// first pass
+	for (int i = 0; i < samples; i++) {
+
+		uint16_t timestamp_fifo = combine_uint(fifo[i].TimeStamp_h, fifo[i].TimeStamp_l);
+
+		if (_enable_clock_input) {
+			gyro.dt = (float)timestamp_fifo * ((1.f / _input_clock_freq) * 1e6f);
+
+		} else {
+			gyro.dt = (float)timestamp_fifo * FIFO_TIMESTAMP_SCALING;
+		}
+
+		// 20 bit hires mode
+		// Gyro [19:12] + Gyro [11:4] + Gyro [3:0] (bottom 4 bits of 20 bit extension byte)
+		int32_t gyro_x = reassemble_20bit(fifo[i].GYRO_DATA_X1, fifo[i].GYRO_DATA_X0, fifo[i].Ext_Accel_X_Gyro_X & 0x0F);
+		int32_t gyro_y = reassemble_20bit(fifo[i].GYRO_DATA_Y1, fifo[i].GYRO_DATA_Y0, fifo[i].Ext_Accel_Y_Gyro_Y & 0x0F);
+		int32_t gyro_z = reassemble_20bit(fifo[i].GYRO_DATA_Z1, fifo[i].GYRO_DATA_Z0, fifo[i].Ext_Accel_Z_Gyro_Z & 0x0F);
+
+		// check if any values are going to exceed int16 limits
+		static constexpr int16_t max_gyro = INT16_MAX;
+		static constexpr int16_t min_gyro = INT16_MIN;
+
+		if (gyro_x >= max_gyro || gyro_x <= min_gyro) {
+			scale_20bit = true;
+		}
+
+		if (gyro_y >= max_gyro || gyro_y <= min_gyro) {
+			scale_20bit = true;
+		}
+
+		if (gyro_z >= max_gyro || gyro_z <= min_gyro) {
+			scale_20bit = true;
+		}
+
+		gyro.x[gyro.samples] = gyro_x / 2;
+		gyro.y[gyro.samples] = gyro_y / 2;
+		gyro.z[gyro.samples] = gyro_z / 2;
+		gyro.samples++;
+	}
+
+	if (!scale_20bit) {
+		// if highres enabled gyro data is always 65.5 LSB/dps
+		_px4_gyro.set_scale(math::radians(1.f / 65.5f));
+
+	} else {
+		// 20 bit data scaled to 16 bit (2^4)
+		for (int i = 0; i < samples; i++) {
+			gyro.x[i] = combine(fifo[i].GYRO_DATA_X1, fifo[i].GYRO_DATA_X0);
+			gyro.y[i] = combine(fifo[i].GYRO_DATA_Y1, fifo[i].GYRO_DATA_Y0);
+			gyro.z[i] = combine(fifo[i].GYRO_DATA_Z1, fifo[i].GYRO_DATA_Z0);
+		}
+
+		_px4_gyro.set_scale(math::radians(4000.f / 32768.f));
+	}
+
+	// correct frame for publication
+	for (int i = 0; i < gyro.samples; i++) {
+		// sensor's frame is +x forward, +y left, +z up
+		//  flip y & z to publish right handed with z down (x forward, y right, z down)
+		gyro.x[i] = gyro.x[i];
+		gyro.y[i] = (gyro.y[i] == INT16_MIN) ? INT16_MAX : -gyro.y[i];
+		gyro.z[i] = (gyro.z[i] == INT16_MIN) ? INT16_MAX : -gyro.z[i];
+	}
+
+	_px4_gyro.set_error_count(perf_event_count(_bad_register_perf) + perf_event_count(_bad_transfer_perf) +
+				  perf_event_count(_fifo_empty_perf) + perf_event_count(_fifo_overflow_perf));
+
+	if (gyro.samples > 0) {
+		_px4_gyro.updateFIFO(gyro);
+	}
+}
+
+bool IIM42653::ProcessTemperature(const FIFO::DATA fifo[], const uint8_t samples)
+{
+	int16_t temperature[FIFO_MAX_SAMPLES];
+	float temperature_sum{0};
+
+	int valid_samples = 0;
+
+	for (int i = 0; i < samples; i++) {
+		const int16_t t = combine(fifo[i].TEMP_DATA1, fifo[i].TEMP_DATA0);
+
+		// sample invalid if -32768
+		if (t != -32768) {
+			temperature_sum += t;
+			temperature[valid_samples] = t;
+			valid_samples++;
+		}
+	}
+
+	if (valid_samples > 0) {
+		const float temperature_avg = temperature_sum / valid_samples;
+
+		for (int i = 0; i < valid_samples; i++) {
+			// temperature changing wildly is an indication of a transfer error
+			if (fabsf(temperature[i] - temperature_avg) > 1000) {
+				perf_count(_bad_transfer_perf);
+				return false;
+			}
+		}
+
+		// use average temperature reading
+		const float TEMP_degC = (temperature_avg / TEMPERATURE_SENSITIVITY) + TEMPERATURE_OFFSET;
+
+		if (PX4_ISFINITE(TEMP_degC)) {
+			_px4_accel.set_temperature(TEMP_degC);
+			_px4_gyro.set_temperature(TEMP_degC);
+			return true;
+
+		} else {
+			perf_count(_bad_transfer_perf);
+		}
+	}
+
+	return false;
+}

src/drivers/imu/invensense/iim42653/IIM42653.hpp

@@ -0,0 +1,227 @@
+/****************************************************************************
+ *
+ *   Copyright (c) 2023 PX4 Development Team. All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright
+ *    notice, this list of conditions and the following disclaimer.
+ * 2. Redistributions in binary form must reproduce the above copyright
+ *    notice, this list of conditions and the following disclaimer in
+ *    the documentation and/or other materials provided with the
+ *    distribution.
+ * 3. Neither the name PX4 nor the names of its contributors may be
+ *    used to endorse or promote products derived from this software
+ *    without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
+ * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
+ * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *
+ ****************************************************************************/
+
+/**
+ * @file IIM42653.hpp
+ *
+ * Driver for the Invensense IIM42653 connected via SPI.
+ *
+ */
+
+#pragma once
+
+#include "InvenSense_IIM42653_registers.hpp"
+
+#include <drivers/drv_hrt.h>
+#include <lib/drivers/accelerometer/PX4Accelerometer.hpp>
+#include <lib/drivers/device/spi.h>
+#include <lib/drivers/gyroscope/PX4Gyroscope.hpp>
+#include <lib/geo/geo.h>
+#include <lib/perf/perf_counter.h>
+#include <px4_platform_common/atomic.h>
+#include <px4_platform_common/i2c_spi_buses.h>
+
+using namespace InvenSense_IIM42653;
+
+class IIM42653 : public device::SPI, public I2CSPIDriver<IIM42653>
+{
+public:
+	IIM42653(const I2CSPIDriverConfig &config);
+	~IIM42653() override;
+
+	static void print_usage();
+
+	void RunImpl();
+
+	int init() override;
+	void print_status() override;
+
+private:
+	void exit_and_cleanup() override;
+
+	// Sensor Configuration
+	static constexpr float FIFO_SAMPLE_DT{1e6f / 8000.f};     // 8000 Hz accel & gyro ODR configured
+	static constexpr float GYRO_RATE{1e6f / FIFO_SAMPLE_DT};
+	static constexpr float ACCEL_RATE{1e6f / FIFO_SAMPLE_DT};
+
+	static constexpr float FIFO_TIMESTAMP_SCALING{16.f *(32.f / 30.f)}; // Used when not using clock input
+
+	// maximum FIFO samples per transfer is limited to the size of sensor_accel_fifo/sensor_gyro_fifo
+	static constexpr int32_t FIFO_MAX_SAMPLES{math::min(FIFO::SIZE / sizeof(FIFO::DATA), sizeof(sensor_gyro_fifo_s::x) / sizeof(sensor_gyro_fifo_s::x[0]), sizeof(sensor_accel_fifo_s::x) / sizeof(sensor_accel_fifo_s::x[0]) * (int)(GYRO_RATE / ACCEL_RATE))};
+
+	// Transfer data
+	struct FIFOTransferBuffer {
+		uint8_t cmd{static_cast<uint8_t>(Register::BANK_0::INT_STATUS) | DIR_READ};
+		uint8_t INT_STATUS{0};
+		uint8_t FIFO_COUNTH{0};
+		uint8_t FIFO_COUNTL{0};
+		FIFO::DATA f[FIFO_MAX_SAMPLES] {};
+	};
+	// ensure no struct padding
+	static_assert(sizeof(FIFOTransferBuffer) == (4 + FIFO_MAX_SAMPLES *sizeof(FIFO::DATA)));
+
+	struct register_bank0_config_t {
+		Register::BANK_0 reg;
+		uint8_t set_bits{0};
+		uint8_t clear_bits{0};
+	};
+
+	struct register_bank1_config_t {
+		Register::BANK_1 reg;
+		uint8_t set_bits{0};
+		uint8_t clear_bits{0};
+	};
+
+	struct register_bank2_config_t {
+		Register::BANK_2 reg;
+		uint8_t set_bits{0};
+		uint8_t clear_bits{0};
+	};
+
+	int probe() override;
+
+	bool Reset();
+
+	bool Configure();
+	void ConfigureSampleRate(int sample_rate);
+	void ConfigureFIFOWatermark(uint8_t samples);
+	void ConfigureCLKIN();
+
+	void SelectRegisterBank(enum REG_BANK_SEL_BIT bank, bool force = false);
+	void SelectRegisterBank(Register::BANK_0 reg) { SelectRegisterBank(REG_BANK_SEL_BIT::BANK_SEL_0); }
+	void SelectRegisterBank(Register::BANK_1 reg) { SelectRegisterBank(REG_BANK_SEL_BIT::BANK_SEL_1); }
+	void SelectRegisterBank(Register::BANK_2 reg) { SelectRegisterBank(REG_BANK_SEL_BIT::BANK_SEL_2); }
+
+	static int DataReadyInterruptCallback(int irq, void *context, void *arg);
+	void DataReady();
+	bool DataReadyInterruptConfigure();
+	bool DataReadyInterruptDisable();
+
+	template <typename T> bool RegisterCheck(const T &reg_cfg);
+	template <typename T> uint8_t RegisterRead(T reg);
+	template <typename T> void RegisterWrite(T reg, uint8_t value);
+	template <typename T> void RegisterSetAndClearBits(T reg, uint8_t setbits, uint8_t clearbits);
+	template <typename T> void RegisterSetBits(T reg, uint8_t setbits) { RegisterSetAndClearBits(reg, setbits, 0); }
+	template <typename T> void RegisterClearBits(T reg, uint8_t clearbits) { RegisterSetAndClearBits(reg, 0, clearbits); }
+
+	uint16_t FIFOReadCount();
+	bool FIFORead(const hrt_abstime &timestamp_sample, uint8_t samples);
+	void FIFOReset();
+
+	void ProcessAccel(const hrt_abstime &timestamp_sample, const FIFO::DATA fifo[], const uint8_t samples);
+	void ProcessGyro(const hrt_abstime &timestamp_sample, const FIFO::DATA fifo[], const uint8_t samples);
+	bool ProcessTemperature(const FIFO::DATA fifo[], const uint8_t samples);
+
+	const spi_drdy_gpio_t _drdy_gpio;
+
+	PX4Accelerometer _px4_accel;
+	PX4Gyroscope _px4_gyro;
+
+	perf_counter_t _bad_register_perf{perf_alloc(PC_COUNT, MODULE_NAME": bad register")};
+	perf_counter_t _bad_transfer_perf{perf_alloc(PC_COUNT, MODULE_NAME": bad transfer")};
+	perf_counter_t _fifo_empty_perf{perf_alloc(PC_COUNT, MODULE_NAME": FIFO empty")};
+	perf_counter_t _fifo_overflow_perf{perf_alloc(PC_COUNT, MODULE_NAME": FIFO overflow")};
+	perf_counter_t _fifo_reset_perf{perf_alloc(PC_COUNT, MODULE_NAME": FIFO reset")};
+	perf_counter_t _drdy_missed_perf{nullptr};
+
+	hrt_abstime _reset_timestamp{0};
+	hrt_abstime _last_config_check_timestamp{0};
+	hrt_abstime _temperature_update_timestamp{0};
+	int _failure_count{0};
+
+	bool _enable_clock_input{false};
+	float _input_clock_freq{0.f};
+
+	enum REG_BANK_SEL_BIT _last_register_bank {REG_BANK_SEL_BIT::BANK_SEL_0};
+
+	px4::atomic<hrt_abstime> _drdy_timestamp_sample{0};
+	bool _data_ready_interrupt_enabled{false};
+
+	enum class STATE : uint8_t {
+		RESET,
+		WAIT_FOR_RESET,
+		CONFIGURE,
+		FIFO_RESET,
+		FIFO_READ,
+	} _state{STATE::RESET};
+
+	uint16_t _fifo_empty_interval_us{1250}; // default 1250 us / 800 Hz transfer interval
+	int32_t _fifo_gyro_samples{static_cast<int32_t>(_fifo_empty_interval_us / (1000000 / GYRO_RATE))};
+
+	uint8_t _checked_register_bank0{0};
+	static constexpr uint8_t size_register_bank0_cfg{16};
+	register_bank0_config_t _register_bank0_cfg[size_register_bank0_cfg] {
+		// Register                              | Set bits, Clear bits
+		{ Register::BANK_0::INT_CONFIG,           INT_CONFIG_BIT::INT1_MODE | INT_CONFIG_BIT::INT1_DRIVE_CIRCUIT, INT_CONFIG_BIT::INT1_POLARITY },
+		{ Register::BANK_0::FIFO_CONFIG,          FIFO_CONFIG_BIT::FIFO_MODE_STOP_ON_FULL, 0 },
+		{ Register::BANK_0::INTF_CONFIG1,         0, 0}, // RTC_MODE[2] set at runtime
+		{ Register::BANK_0::PWR_MGMT0,            PWR_MGMT0_BIT::GYRO_MODE_LOW_NOISE | PWR_MGMT0_BIT::ACCEL_MODE_LOW_NOISE, 0 },
+		{ Register::BANK_0::GYRO_CONFIG0,         GYRO_CONFIG0_BIT::GYRO_FS_SEL_4000_DPS | GYRO_CONFIG0_BIT::GYRO_ODR_8KHZ_SET, GYRO_CONFIG0_BIT::GYRO_ODR_8KHZ_CLEAR },
+		{ Register::BANK_0::ACCEL_CONFIG0,        ACCEL_CONFIG0_BIT::ACCEL_FS_SEL_32G | ACCEL_CONFIG0_BIT::ACCEL_ODR_8KHZ_SET, ACCEL_CONFIG0_BIT::ACCEL_ODR_8KHZ_CLEAR },
+		{ Register::BANK_0::GYRO_CONFIG1,         0, GYRO_CONFIG1_BIT::GYRO_UI_FILT_ORD },
+		{ Register::BANK_0::GYRO_ACCEL_CONFIG0,   0, GYRO_ACCEL_CONFIG0_BIT::ACCEL_UI_FILT_BW | GYRO_ACCEL_CONFIG0_BIT::GYRO_UI_FILT_BW },
+		{ Register::BANK_0::ACCEL_CONFIG1,        0, ACCEL_CONFIG1_BIT::ACCEL_UI_FILT_ORD },
+		{ Register::BANK_0::TMST_CONFIG,          TMST_CONFIG_BIT::TMST_EN | TMST_CONFIG_BIT::TMST_DELTA_EN | TMST_CONFIG_BIT::TMST_TO_REGS_EN | TMST_CONFIG_BIT::TMST_RES, TMST_CONFIG_BIT::TMST_FSYNC_EN },
+		{ Register::BANK_0::FIFO_CONFIG1,         FIFO_CONFIG1_BIT::FIFO_WM_GT_TH | FIFO_CONFIG1_BIT::FIFO_HIRES_EN | FIFO_CONFIG1_BIT::FIFO_TEMP_EN | FIFO_CONFIG1_BIT::FIFO_GYRO_EN | FIFO_CONFIG1_BIT::FIFO_ACCEL_EN, FIFO_CONFIG1_BIT::FIFO_TMST_FSYNC_EN },
+		{ Register::BANK_0::FIFO_CONFIG2,         0, 0 }, // FIFO_WM[7:0] set at runtime
+		{ Register::BANK_0::FIFO_CONFIG3,         0, 0 }, // FIFO_WM[11:8] set at runtime
+		{ Register::BANK_0::INT_CONFIG0,          INT_CONFIG0_BIT::CLEAR_ON_FIFO_READ, 0 },
+		{ Register::BANK_0::INT_CONFIG1,          0, INT_CONFIG1_BIT::INT_ASYNC_RESET },
+		{ Register::BANK_0::INT_SOURCE0,          INT_SOURCE0_BIT::FIFO_THS_INT1_EN, 0 },
+	};
+
+	uint8_t _checked_register_bank1{0};
+	static constexpr uint8_t size_register_bank1_cfg{5};
+	register_bank1_config_t _register_bank1_cfg[size_register_bank1_cfg] {
+		// Register                              | Set bits, Clear bits
+		{ Register::BANK_1::GYRO_CONFIG_STATIC2,  0, GYRO_CONFIG_STATIC2_BIT::GYRO_NF_DIS | GYRO_CONFIG_STATIC2_BIT::GYRO_AAF_DIS },
+		{ Register::BANK_1::GYRO_CONFIG_STATIC3,  GYRO_CONFIG_STATIC3_BIT::GYRO_AAF_DELT_585HZ_SET, GYRO_CONFIG_STATIC3_BIT::GYRO_AAF_DELT_585HZ_CLEAR},
+		{ Register::BANK_1::GYRO_CONFIG_STATIC4,  GYRO_CONFIG_STATIC4_BIT::GYRO_AAF_DELTSQR_LSB_585HZ_SET, GYRO_CONFIG_STATIC4_BIT::GYRO_AAF_DELTSQR_LSB_585HZ_CLEAR},
+		{ Register::BANK_1::GYRO_CONFIG_STATIC5,  GYRO_CONFIG_STATIC5_BIT::GYRO_AAF_BITSHIFT_585HZ_SET | GYRO_CONFIG_STATIC5_BIT::GYRO_AAF_DELTSQR_MSB_585HZ_SET, GYRO_CONFIG_STATIC5_BIT::GYRO_AAF_BITSHIFT_585HZ_CLEAR | GYRO_CONFIG_STATIC5_BIT::GYRO_AAF_DELTSQR_MSB_585HZ_CLEAR},
+		{ Register::BANK_1::INTF_CONFIG5,         0, 0 },
+	};
+
+	uint8_t _checked_register_bank2{0};
+	static constexpr uint8_t size_register_bank2_cfg{8};
+	register_bank2_config_t _register_bank2_cfg[size_register_bank2_cfg] {
+		// Register                              | Set bits, Clear bits
+		{ Register::BANK_2::ACCEL_CONFIG_STATIC2, ACCEL_CONFIG_STATIC2_BIT::ACCEL_AAF_DELT_585HZ_SET, ACCEL_CONFIG_STATIC2_BIT::ACCEL_AAF_DELT_585HZ_CLEAR | ACCEL_CONFIG_STATIC2_BIT::ACCEL_AAF_DIS },
+		{ Register::BANK_2::ACCEL_CONFIG_STATIC3, ACCEL_CONFIG_STATIC3_BIT::ACCEL_AAF_DELTSQR_LSB_585HZ_SET, ACCEL_CONFIG_STATIC3_BIT::ACCEL_AAF_DELTSQR_LSB_585HZ_CLEAR },
+		{ Register::BANK_2::ACCEL_CONFIG_STATIC4, ACCEL_CONFIG_STATIC4_BIT::ACCEL_AAF_BITSHIFT_585HZ_SET | ACCEL_CONFIG_STATIC4_BIT::ACCEL_AAF_DELTSQR_MSB_SET, ACCEL_CONFIG_STATIC4_BIT::ACCEL_AAF_BITSHIFT_585HZ_CLEAR | ACCEL_CONFIG_STATIC4_BIT::ACCEL_AAF_DELTSQR_MSB_CLEAR },
+		{ Register::BANK_2::AUX1_CONFIG1,         0, AUX1_CONFIG1_BIT::AUX1_ACCEL_LP_CLK_SEL | AUX1_CONFIG1_BIT::GYRO_AUX1_EN | AUX1_CONFIG1_BIT::ACCEL_AUX1_EN},
+		{ Register::BANK_2::AUX1_CONFIG2,         AUX1_CONFIG2_BIT::GYRO_AUX1_HPF_DIS, 0},
+		{ Register::BANK_2::AUX1_SPI_REG1,        AUX1_SPI_REG1_BIT::AUX1_SPI_REG1_SET, AUX1_SPI_REG1_BIT::AUX1_SPI_REG1_CLEAR },
+		{ Register::BANK_2::AUX1_SPI_REG2,        AUX1_SPI_REG2_BIT::AUX1_SPI_REG2_SET, AUX1_SPI_REG2_BIT::AUX1_SPI_REG2_CLEAR },
+		{ Register::BANK_2::AUX1_SPI_REG3,        AUX1_SPI_REG3_BIT::AUX1_SPI_REG3_SET, AUX1_SPI_REG3_BIT::AUX1_SPI_REG3_CLEAR },
+	};
+};

src/drivers/imu/invensense/iim42653/InvenSense_IIM42653_registers.hpp

@@ -0,0 +1,453 @@
+/****************************************************************************
+ *
+ *   Copyright (c) 2023 PX4 Development Team. All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright
+ *    notice, this list of conditions and the following disclaimer.
+ * 2. Redistributions in binary form must reproduce the above copyright
+ *    notice, this list of conditions and the following disclaimer in
+ *    the documentation and/or other materials provided with the
+ *    distribution.
+ * 3. Neither the name PX4 nor the names of its contributors may be
+ *    used to endorse or promote products derived from this software
+ *    without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
+ * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
+ * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *
+ ****************************************************************************/
+
+/**
+ * @file InvenSense_IIM42653_registers.hpp
+ *
+ * Invensense IIM-42653 registers.
+ *
+ */
+
+#pragma once
+
+#include <cstdint>
+#include <cstddef>
+
+namespace InvenSense_IIM42653
+{
+// TODO: move to a central header
+static constexpr uint8_t Bit0 = (1 << 0);
+static constexpr uint8_t Bit1 = (1 << 1);
+static constexpr uint8_t Bit2 = (1 << 2);
+static constexpr uint8_t Bit3 = (1 << 3);
+static constexpr uint8_t Bit4 = (1 << 4);
+static constexpr uint8_t Bit5 = (1 << 5);
+static constexpr uint8_t Bit6 = (1 << 6);
+static constexpr uint8_t Bit7 = (1 << 7);
+
+static constexpr uint32_t SPI_SPEED = 24 * 1000 * 1000; // 24 MHz SPI
+static constexpr uint8_t DIR_READ = 0x80;
+
+static constexpr uint8_t WHOAMI = 0x56;
+
+static constexpr float TEMPERATURE_SENSITIVITY = 132.48f; // LSB/C
+static constexpr float TEMPERATURE_OFFSET = 25.f; // C
+
+namespace Register
+{
+
+enum class BANK_0 : uint8_t {
+	DEVICE_CONFIG      = 0x11,
+
+	INT_CONFIG         = 0x14,
+
+	FIFO_CONFIG        = 0x16,
+
+	TEMP_DATA1         = 0x1D,
+	TEMP_DATA0         = 0x1E,
+
+	INT_STATUS         = 0x2D,
+	FIFO_COUNTH        = 0x2E,
+	FIFO_COUNTL        = 0x2F,
+	FIFO_DATA          = 0x30,
+
+	SIGNAL_PATH_RESET  = 0x4B,
+	INTF_CONFIG0       = 0x4C,
+	INTF_CONFIG1       = 0x4D,
+	PWR_MGMT0          = 0x4E,
+	GYRO_CONFIG0       = 0x4F,
+	ACCEL_CONFIG0      = 0x50,
+	GYRO_CONFIG1       = 0x51,
+	GYRO_ACCEL_CONFIG0 = 0x52,
+	ACCEL_CONFIG1      = 0x53,
+	TMST_CONFIG        = 0x54,
+
+	FIFO_CONFIG1       = 0x5F,
+	FIFO_CONFIG2       = 0x60,
+	FIFO_CONFIG3       = 0x61,
+
+	INT_CONFIG0        = 0x63,
+	INT_CONFIG1        = 0x64,
+
+	INT_SOURCE0        = 0x65,
+
+	SELF_TEST_CONFIG   = 0x70,
+
+	WHO_AM_I           = 0x75,
+	REG_BANK_SEL       = 0x76,
+};
+
+enum class BANK_1 : uint8_t {
+	GYRO_CONFIG_STATIC2  = 0x0B,
+	GYRO_CONFIG_STATIC3  = 0x0C,
+	GYRO_CONFIG_STATIC4  = 0x0D,
+	GYRO_CONFIG_STATIC5  = 0x0E,
+
+	INTF_CONFIG5         = 0x7B,
+};
+
+enum class BANK_2 : uint8_t {
+	ACCEL_CONFIG_STATIC2 = 0x03,
+	ACCEL_CONFIG_STATIC3 = 0x04,
+	ACCEL_CONFIG_STATIC4 = 0x05,
+	AUX1_CONFIG1         = 0x44,
+	AUX1_CONFIG2         = 0x45,
+	AUX1_CONFIG3         = 0x46,
+	AUX1_SPI_REG1        = 0x71,
+	AUX1_SPI_REG2        = 0x72,
+	AUX1_SPI_REG3        = 0x73,
+};
+
+};
+
+//---------------- BANK0 Register bits
+
+// DEVICE_CONFIG
+enum DEVICE_CONFIG_BIT : uint8_t {
+	SOFT_RESET_CONFIG = Bit0, //
+};
+
+// INT_CONFIG
+enum INT_CONFIG_BIT : uint8_t {
+	INT1_MODE           = Bit2,
+	INT1_DRIVE_CIRCUIT  = Bit1,
+	INT1_POLARITY       = Bit0,
+};
+
+// FIFO_CONFIG
+enum FIFO_CONFIG_BIT : uint8_t {
+	// 7:6 FIFO_MODE
+	FIFO_MODE_STOP_ON_FULL = Bit7 | Bit6, // 11: STOP-on-FULL Mode
+};
+
+// INT_STATUS
+enum INT_STATUS_BIT : uint8_t {
+	RESET_DONE_INT = Bit4,
+	DATA_RDY_INT   = Bit3,
+	FIFO_THS_INT   = Bit2,
+	FIFO_FULL_INT  = Bit1,
+};
+
+// SIGNAL_PATH_RESET
+enum SIGNAL_PATH_RESET_BIT : uint8_t {
+	ABORT_AND_RESET = Bit3,
+	FIFO_FLUSH      = Bit1,
+};
+
+enum INTF_CONFIG1_BIT : uint8_t {
+	RTC_MODE = Bit2, // 0: No input RTC clock is required, 1: RTC clock input is required
+	CLKSEL = Bit0,
+	CLKSEL_CLEAR = Bit1,
+};
+
+// PWR_MGMT0
+enum PWR_MGMT0_BIT : uint8_t {
+	GYRO_MODE_LOW_NOISE  = Bit3 | Bit2, // 11: Places gyroscope in Low Noise (LN) Mode
+	ACCEL_MODE_LOW_NOISE = Bit1 | Bit0, // 11: Places accelerometer in Low Noise (LN) Mode
+};
+
+// GYRO_CONFIG0
+enum GYRO_CONFIG0_BIT : uint8_t {
+	// 7:5 GYRO_FS_SEL
+	GYRO_FS_SEL_4000_DPS = 0, // 0b000 = ±4000dps (default)
+	GYRO_FS_SEL_2000_DPS = Bit5,
+	GYRO_FS_SEL_1000_DPS = Bit6,
+	GYRO_FS_SEL_500_DPS  = Bit6 | Bit5,
+	GYRO_FS_SEL_250_DPS  = Bit7,
+
+
+	// 3:0 GYRO_ODR
+	//  0001: 32kHz
+	GYRO_ODR_32KHZ_SET   = Bit0,
+	GYRO_ODR_32KHZ_CLEAR = Bit3 | Bit2 | Bit0,
+	//  0010: 16kHz
+	GYRO_ODR_16KHZ_SET   = Bit1,
+	GYRO_ODR_16KHZ_CLEAR = Bit3 | Bit2 | Bit0,
+	//  0011: 8kHz
+	GYRO_ODR_8KHZ_SET    = Bit1 | Bit0,
+	GYRO_ODR_8KHZ_CLEAR  = Bit3 | Bit2,
+	//  0110: 1kHz (default)
+	GYRO_ODR_1KHZ_SET    = Bit2 | Bit1,
+	GYRO_ODR_1KHZ_CLEAR  = Bit3 | Bit0,
+};
+
+// ACCEL_CONFIG0
+enum ACCEL_CONFIG0_BIT : uint8_t {
+	// 7:5 ACCEL_FS_SEL
+	ACCEL_FS_SEL_32G = 0, // 000: ±32g (default)
+	ACCEL_FS_SEL_16G = Bit5,
+	ACCEL_FS_SEL_8G  = Bit6,
+	ACCEL_FS_SEL_4G  = Bit6 | Bit5,
+
+
+	// 3:0 ACCEL_ODR
+	//  0001: 32kHz
+	ACCEL_ODR_32KHZ_SET   = Bit0,
+	ACCEL_ODR_32KHZ_CLEAR = Bit3 | Bit2 | Bit0,
+	//  0010: 16kHz
+	ACCEL_ODR_16KHZ_SET   = Bit1,
+	ACCEL_ODR_16KHZ_CLEAR = Bit3 | Bit2 | Bit0,
+	//  0011: 8kHz
+	ACCEL_ODR_8KHZ_SET    = Bit1 | Bit0,
+	ACCEL_ODR_8KHZ_CLEAR  = Bit3 | Bit2,
+	//  0110: 1kHz (default)
+	ACCEL_ODR_1KHZ_SET    = Bit2 | Bit1,
+	ACCEL_ODR_1KHZ_CLEAR  = Bit3 | Bit0,
+};
+
+// GYRO_CONFIG1
+enum GYRO_CONFIG1_BIT : uint8_t {
+	GYRO_UI_FILT_ORD = Bit3 | Bit2, // 00: 1st Order
+};
+
+// GYRO_ACCEL_CONFIG0
+enum GYRO_ACCEL_CONFIG0_BIT : uint8_t {
+	// 7:4 ACCEL_UI_FILT_BW
+	ACCEL_UI_FILT_BW = Bit7 | Bit6 | Bit5 | Bit4, // 0: BW=ODR/2
+
+	// 3:0 GYRO_UI_FILT_BW
+	GYRO_UI_FILT_BW  = Bit3 | Bit2 | Bit1 | Bit0, // 0: BW=ODR/2
+};
+
+// ACCEL_CONFIG1
+enum ACCEL_CONFIG1_BIT : uint8_t {
+	ACCEL_UI_FILT_ORD = Bit4 | Bit3, // 00: 1st Order
+};
+
+// TMST_CONFIG
+enum TMST_CONFIG_BIT : uint8_t {
+	TMST_TO_REGS_EN = Bit4, // 1: TMST_VALUE[19:0] read returns timestamp value
+	TMST_RES	= Bit3, // 0: 1us resolution, 1: 16us resolution
+	TMST_DELTA_EN   = Bit2, // 1: Time Stamp delta enable
+	TMST_FSYNC_EN   = Bit1, // 1: The contents of the Timestamp feature of FSYNC is enabled
+	TMST_EN		= Bit0, // 1: Time Stamp register enable (default)
+};
+
+// FIFO_CONFIG1
+enum FIFO_CONFIG1_BIT : uint8_t {
+	FIFO_RESUME_PARTIAL_RD = Bit6,
+	FIFO_WM_GT_TH          = Bit5,
+	FIFO_HIRES_EN          = Bit4,
+	FIFO_TMST_FSYNC_EN     = Bit3,
+	FIFO_TEMP_EN           = Bit2,
+	FIFO_GYRO_EN           = Bit1,
+	FIFO_ACCEL_EN          = Bit0,
+};
+
+// INT_CONFIG0
+enum INT_CONFIG0_BIT : uint8_t {
+	// 3:2 FIFO_THS_INT_CLEAR
+	CLEAR_ON_FIFO_READ = Bit3,
+};
+
+// INT_CONFIG1
+enum INT_CONFIG1_BIT : uint8_t {
+	INT_ASYNC_RESET  = Bit4,
+};
+
+// INT_SOURCE0
+enum INT_SOURCE0_BIT : uint8_t {
+	UI_FSYNC_INT1_EN   = Bit6,
+	PLL_RDY_INT1_EN    = Bit5,
+	RESET_DONE_INT1_EN = Bit4,
+	UI_DRDY_INT1_EN    = Bit3,
+	FIFO_THS_INT1_EN   = Bit2, // FIFO threshold interrupt routed to INT1
+	FIFO_FULL_INT1_EN  = Bit1,
+	UI_AGC_RDY_INT1_EN = Bit0,
+};
+
+// REG_BANK_SEL
+enum REG_BANK_SEL_BIT : uint8_t {
+	// 2:0 BANK_SEL
+	BANK_SEL_0 = 0,           // 000: Bank 0 (default)
+	BANK_SEL_1 = Bit0,        // 001: Bank 1
+	BANK_SEL_2 = Bit1,        // 010: Bank 2
+	BANK_SEL_3 = Bit1 | Bit0, // 011: Bank 3
+	BANK_SEL_4 = Bit2,        // 100: Bank 4
+};
+
+
+//---------------- BANK1 Register bits
+
+// GYRO_CONFIG_STATIC2
+enum GYRO_CONFIG_STATIC2_BIT : uint8_t {
+	GYRO_AAF_DIS = Bit1, // 1: Disable gyroscope anti-aliasing filter
+	GYRO_NF_DIS  = Bit0, // 1: Disable Notch Filter
+};
+
+// GYRO_CONFIG_STATIC3
+enum GYRO_CONFIG_STATIC3_BIT : uint8_t {
+	// 5:0 GYRO_AAF_DELT
+	//  585 Hz = 13 (0b00'1101)
+	GYRO_AAF_DELT_585HZ_SET   = Bit3 | Bit2 | Bit0,
+	GYRO_AAF_DELT_585HZ_CLEAR = Bit5 | Bit4 | Bit1,
+};
+
+// GYRO_CONFIG_STATIC4
+enum GYRO_CONFIG_STATIC4_BIT : uint8_t {
+	// 7:0 GYRO_AAF_DELTSQR
+	//  585 Hz = 170 (0b1010'1010)
+	GYRO_AAF_DELTSQR_LSB_585HZ_SET   = Bit7 | Bit5 | Bit3 | Bit1,
+	GYRO_AAF_DELTSQR_LSB_585HZ_CLEAR = Bit6 | Bit4 | Bit2 | Bit0,
+};
+
+// GYRO_CONFIG_STATIC5
+enum GYRO_CONFIG_STATIC5_BIT : uint8_t {
+	// 7:4 GYRO_AAF_BITSHIFT
+	//  585 Hz = 8 (0b1000)
+	GYRO_AAF_BITSHIFT_585HZ_SET      = Bit7,
+	GYRO_AAF_BITSHIFT_585HZ_CLEAR    = Bit6 | Bit5 | Bit4,
+
+	// 3:0 GYRO_AAF_DELTSQR[11:8]
+	//  585 Hz = 170 (0b0000'1010'1010)
+	GYRO_AAF_DELTSQR_MSB_585HZ_SET   = 0,
+	GYRO_AAF_DELTSQR_MSB_585HZ_CLEAR = Bit3 | Bit2 | Bit1 | Bit0,
+};
+
+// INTF_CONFIG5
+enum INTF_CONFIG5_BIT : uint8_t {
+	// 2:1 PIN9_FUNCTION
+	PIN9_FUNCTION_CLKIN_SET   = Bit2, // 0b10: CLKIN
+	PIN9_FUNCTION_CLKIN_CLEAR = Bit1,
+
+	PIN9_FUNCTION_RESET_SET   = 0,
+	PIN9_FUNCTION_RESET_CLEAR = Bit2 | Bit1,
+};
+
+//---------------- BANK2 Register bits
+
+// ACCEL_CONFIG_STATIC2
+enum ACCEL_CONFIG_STATIC2_BIT : uint8_t {
+	// 6:1 ACCEL_AAF_DELT
+	//  585 Hz = 13 (0b00'1101)
+	ACCEL_AAF_DELT_585HZ_SET   = Bit4 | Bit3 | Bit1,
+	ACCEL_AAF_DELT_585HZ_CLEAR = Bit6 | Bit5 | Bit2,
+
+	// 0 ACCEL_AAF_DIS
+	ACCEL_AAF_DIS              = Bit0, // 0: Enable accelerometer anti-aliasing filter (default)
+};
+
+// ACCEL_CONFIG_STATIC3
+enum ACCEL_CONFIG_STATIC3_BIT : uint8_t {
+	// 7:0 ACCEL_AAF_DELTSQR[7:0]
+	//  585 Hz = 170 (0b0000'1010'1010)
+	ACCEL_AAF_DELTSQR_LSB_585HZ_SET   = Bit7 | Bit5 | Bit3 | Bit1,
+	ACCEL_AAF_DELTSQR_LSB_585HZ_CLEAR = Bit6 | Bit4 | Bit2 | Bit0,
+};
+
+// ACCEL_CONFIG_STATIC4
+enum ACCEL_CONFIG_STATIC4_BIT : uint8_t {
+	// 7:4 ACCEL_AAF_BITSHIFT
+	//  585 Hz = 8 (0b1000)
+	ACCEL_AAF_BITSHIFT_585HZ_SET   = Bit7,
+	ACCEL_AAF_BITSHIFT_585HZ_CLEAR = Bit6 | Bit5 | Bit4,
+
+	// 3:0 ACCEL_AAF_DELTSQR[11:8]
+	//  585 Hz = 170 (0b0000'1010'1010)
+	ACCEL_AAF_DELTSQR_MSB_SET      = 0,
+	ACCEL_AAF_DELTSQR_MSB_CLEAR    = Bit3 | Bit2 | Bit1 | Bit0,
+};
+
+// AUX1_CONFIG1
+enum AUX1_CONFIG1_BIT : uint8_t {
+	AUX1_ACCEL_LP_CLK_SEL = Bit5,
+	GYRO_AUX1_EN = Bit1,
+	ACCEL_AUX1_EN = Bit0,
+};
+
+// AUX1_CONFIG2
+enum AUX1_CONFIG2_BIT : uint8_t {
+	GYRO_AUX1_HPF_DIS = Bit6,
+};
+
+// AUX1_SPI_REG1
+enum AUX1_SPI_REG1_BIT : uint8_t {
+	AUX1_SPI_REG1_CLEAR = Bit1,
+	AUX1_SPI_REG1_SET = Bit0,
+};
+
+// AUX1_SPI_REG2
+enum AUX1_SPI_REG2_BIT : uint8_t {
+	AUX1_SPI_REG2_CLEAR = Bit1,
+	AUX1_SPI_REG2_SET = Bit0,
+};
+
+// AUX1_SPI_REG3
+enum AUX1_SPI_REG3_BIT : uint8_t {
+	AUX1_SPI_REG3_CLEAR = Bit1,
+	AUX1_SPI_REG3_SET = Bit0,
+};
+
+namespace FIFO
+{
+static constexpr size_t SIZE = 2048;
+
+// FIFO_DATA layout when FIFO_CONFIG1 has FIFO_GYRO_EN and FIFO_ACCEL_EN set
+
+// Packet 4
+struct DATA {
+	uint8_t FIFO_Header;
+	uint8_t ACCEL_DATA_X1; // Accel X [19:12]
+	uint8_t ACCEL_DATA_X0; // Accel X [11:4]
+	uint8_t ACCEL_DATA_Y1; // Accel Y [19:12]
+	uint8_t ACCEL_DATA_Y0; // Accel Y [11:4]
+	uint8_t ACCEL_DATA_Z1; // Accel Z [19:12]
+	uint8_t ACCEL_DATA_Z0; // Accel Z [11:4]
+	uint8_t GYRO_DATA_X1;  // Gyro X [19:12]
+	uint8_t GYRO_DATA_X0;  // Gyro X [11:4]
+	uint8_t GYRO_DATA_Y1;  // Gyro Y [19:12]
+	uint8_t GYRO_DATA_Y0;  // Gyro Y [11:4]
+	uint8_t GYRO_DATA_Z1;  // Gyro Z [19:12]
+	uint8_t GYRO_DATA_Z0;  // Gyro Z [11:4]
+	uint8_t TEMP_DATA1;    // Temperature[15:8]
+	uint8_t TEMP_DATA0;    // Temperature[7:0]
+	uint8_t TimeStamp_h;   // TimeStamp[15:8]
+	uint8_t TimeStamp_l;   // TimeStamp[7:0]
+	uint8_t Ext_Accel_X_Gyro_X; // Accel X [3:0] Gyro X [3:0]
+	uint8_t Ext_Accel_Y_Gyro_Y; // Accel Y [3:0] Gyro Y [3:0]
+	uint8_t Ext_Accel_Z_Gyro_Z; // Accel Z [3:0] Gyro Z [3:0]
+};
+
+// With FIFO_ACCEL_EN and FIFO_GYRO_EN header should be 8’b_0110_10xx
+enum FIFO_HEADER_BIT : uint8_t {
+	HEADER_MSG             = Bit7, // 1: FIFO is empty
+	HEADER_ACCEL           = Bit6, // 1: Packet is sized so that accel data have location in the packet, FIFO_ACCEL_EN must be 1
+	HEADER_GYRO            = Bit5, // 1: Packet is sized so that gyro data have location in the packet, FIFO_GYRO_EN must be1
+	HEADER_20              = Bit4, // 1: Packet has a new and valid sample of extended 20-bit data for gyro and/or accel
+	HEADER_TIMESTAMP_FSYNC = Bit3 | Bit2, // 10: Packet contains ODR Timestamp
+	HEADER_ODR_ACCEL       = Bit1, // 1: The ODR for accel is different for this accel data packet compared to the previous accel packet
+	HEADER_ODR_GYRO        = Bit0, // 1: The ODR for gyro is different for this gyro data packet compared to the previous gyro packet
+};
+
+}
+} // namespace InvenSense_IIM42653

src/drivers/imu/invensense/iim42653/Kconfig

@@ -0,0 +1,5 @@
+menuconfig DRIVERS_IMU_INVENSENSE_IIM42653
+	bool "iim42653"
+	default n
+	---help---
+		Enable support for iim42653

src/drivers/imu/invensense/iim42653/iim42653_main.cpp

@@ -31,33 +31,62 @@
  *
  ****************************************************************************/
 
-#pragma once
+#include "IIM42653.hpp"
 
-#include <px4_platform_common/px4_work_queue/ScheduledWorkItem.hpp>
-#include <px4_platform_common/atomic.h>
-#include <drivers/drv_hrt.h>
+#include <px4_platform_common/getopt.h>
+#include <px4_platform_common/module.h>
 
-class ParamAutosave : public px4::ScheduledWorkItem
+void IIM42653::print_usage()
 {
-public:
+	PRINT_MODULE_USAGE_NAME("iim42653", "driver");
+	PRINT_MODULE_USAGE_SUBCATEGORY("imu");
+	PRINT_MODULE_USAGE_COMMAND("start");
+	PRINT_MODULE_USAGE_PARAMS_I2C_SPI_DRIVER(false, true);
+	PRINT_MODULE_USAGE_PARAM_INT('R', 0, 0, 35, "Rotation", true);
+	PRINT_MODULE_USAGE_PARAM_INT('C', 0, 0, 35000, "Input clock frequency (Hz)", true);
+	PRINT_MODULE_USAGE_DEFAULT_COMMANDS();
+}
 
-	ParamAutosave();
+extern "C" int iim42653_main(int argc, char *argv[])
+{
+	int ch;
+	using ThisDriver = IIM42653;
+	BusCLIArguments cli{false, true};
+	cli.default_spi_frequency = SPI_SPEED;
 
-	/**
-	 * Automatically save the parameters after a timeout and at limited rate.
-	 */
-	void request();
+	while ((ch = cli.getOpt(argc, argv, "C:R:")) != EOF) {
+		switch (ch) {
+		case 'C':
+			cli.custom1 = atoi(cli.optArg());
+			break;
 
-	void enable(bool enable);
+		case 'R':
+			cli.rotation = (enum Rotation)atoi(cli.optArg());
+			break;
+		}
+	}
 
-	bool enabled() const { return !_disabled; }
+	const char *verb = cli.optArg();
 
-	hrt_abstime lastAutosave() const { return _last_timestamp; }
+	if (!verb) {
+		ThisDriver::print_usage();
+		return -1;
+	}
 
-	void Run() override;
+	BusInstanceIterator iterator(MODULE_NAME, cli, DRV_IMU_DEVTYPE_IIM42653);
 
-private:
-	hrt_abstime _last_timestamp{0};
-	px4::atomic_bool _scheduled{false};
-	bool _disabled{false};
-};
+	if (!strcmp(verb, "start")) {
+		return ThisDriver::module_start(cli, iterator);
+	}
+
+	if (!strcmp(verb, "stop")) {
+		return ThisDriver::module_stop(iterator);
+	}
+
+	if (!strcmp(verb, "status")) {
+		return ThisDriver::module_status(iterator);
+	}
+
+	ThisDriver::print_usage();
+	return -1;
+}

src/drivers/linux_pwm_out/module.yaml

@@ -4,7 +4,7 @@ actuator_output:
     - param_prefix: PWM_MAIN
       channel_label: 'Channel'
       standard_params:
-        disarmed: { min: 800, max: 2200, default: 900 }
+        disarmed: { min: 800, max: 2200, default: 1000 }
         min: { min: 800, max: 1400, default: 1000 }
         max: { min: 1600, max: 2200, default: 2000 }
         failsafe: { min: 800, max: 2200 }

src/drivers/magnetometer/lis2mdl/lis2mdl_i2c.cpp

@@ -40,7 +40,6 @@
 #include <px4_platform_common/px4_config.h>
 
 #include <assert.h>
-#include <debug.h>
 #include <errno.h>
 #include <stdint.h>
 #include <stdbool.h>

src/drivers/magnetometer/lis2mdl/lis2mdl_spi.cpp

@@ -40,7 +40,6 @@
 #include <px4_platform_common/px4_config.h>
 
 #include <assert.h>
-#include <debug.h>
 #include <errno.h>
 #include <stdint.h>
 #include <stdbool.h>

src/drivers/magnetometer/lis3mdl/lis3mdl_i2c.cpp

@@ -40,7 +40,6 @@
 #include <px4_platform_common/px4_config.h>
 
 #include <assert.h>
-#include <debug.h>
 #include <errno.h>
 #include <stdint.h>
 #include <stdbool.h>

src/drivers/magnetometer/lis3mdl/lis3mdl_spi.cpp

@@ -40,7 +40,6 @@
 #include <px4_platform_common/px4_config.h>
 
 #include <assert.h>
-#include <debug.h>
 #include <errno.h>
 #include <stdint.h>
 #include <stdbool.h>