[DO NOT MERGE] try changing UAVCAN default timer to 6

2026-06-20 15:50:35 +08:00 · 2022-05-06 10:29:14 -04:00
312 changed files with 4781 additions and 5029 deletions
@@ -61,20 +61,18 @@ pipeline {
                     "holybro_kakutef7_default",
                     "holybro_kakuteh7_default",
                     "holybro_pix32v5_default",
+                     "matek_h743-slim",
                     "matek_gnss-m9n-f4_canbootloader",
                     "matek_gnss-m9n-f4_default",
-                     "matek_h743-mini_default",
-                     "matek_h743-slim_default",
-                     "matek_h743_default",
                     "modalai_fc-v1_default",
                     "modalai_fc-v1_rtps",
                     "modalai_fc-v2_default",
                     "mro_ctrl-zero-f7_default",
                     "mro_ctrl-zero-f7-oem_default",
-                     "mro_ctrl-zero-h7-oem_default",
-                     "mro_ctrl-zero-h7-oem_rtps",
                     "mro_ctrl-zero-h7_default",
                     "mro_ctrl-zero-h7_rtps",
+                     "mro_ctrl-zero-h7-oem_default",
+                     "mro_ctrl-zero-h7-oem_rtps",
                     "mro_pixracerpro_default",
                     "mro_pixracerpro_rtps",
                     "mro_x21-777_default",
@@ -89,26 +87,26 @@ pipeline {
                     "nxp_ucans32k146_canbootloader",
                     "nxp_ucans32k146_default",
                     "omnibus_f4sd_default",
+                     "raspberrypi_pico_default",
                     "px4_fmu-v2_default",
                     "px4_fmu-v2_fixedwing",
-                     "px4_fmu-v2_lto",
                     "px4_fmu-v2_multicopter",
                     "px4_fmu-v2_rover",
+                     "px4_fmu-v2_lto",
                     "px4_fmu-v3_default",
                     "px4_fmu-v4_default",
                     "px4_fmu-v4pro_default",
-                     "px4_fmu-v5_cyphal",
                     "px4_fmu-v5_debug",
                     "px4_fmu-v5_default",
-                     "px4_fmu-v5_lto",
                     "px4_fmu-v5_rtps",
                     "px4_fmu-v5_stackcheck",
                     "px4_fmu-v5_uavcanv0periph",
+                     "px4_fmu-v5_uavcanv1",
+                     "px4_fmu-v5_lto",
                     "px4_fmu-v5x_default",
                     "px4_fmu-v6u_default",
                     "px4_fmu-v6x_default",
                     "px4_io-v2_default",
-                     "raspberrypi_pico_default",
                     "sky-drones_smartap-airlink_default",
                     "spracing_h7extreme_default",
                     "uvify_core_default"
@@ -37,10 +37,8 @@ jobs:
          holybro_kakutef7,
          holybro_kakuteh7,
          holybro_pix32v5,
-          matek_gnss-m9n-f4,
-          matek_h743,
-          matek_h743-mini,
          matek_h743-slim,
+          matek_gnss-m9n-f4,
          modalai_fc-v1,
          modalai_fc-v2,
          mro_ctrl-zero-f7,
@@ -55,6 +53,7 @@ jobs:
          nxp_fmurt1062-v1,
          nxp_ucans32k146,
          omnibus_f4sd,
+          raspberrypi_pico,
          px4_fmu-v2,
          px4_fmu-v3,
          px4_fmu-v4,
@@ -63,7 +62,6 @@ jobs:
          px4_fmu-v5x,
          px4_fmu-v6u,
          px4_fmu-v6x,
-          raspberrypi_pico,
          sky-drones_smartap-airlink,
          spracing_h7extreme,
          uvify_core
@@ -36,14 +36,14 @@
 [submodule "Tools/jsbsim_bridge"]
 	path = Tools/jsbsim_bridge
 	url = https://github.com/PX4/px4-jsbsim-bridge.git
-[submodule "src/drivers/cyphal/libcanard"]
-	path = src/drivers/cyphal/libcanard
-	url = https://github.com/opencyphal/libcanard.git
-[submodule "src/drivers/cyphal/public_regulated_data_types"]
-	path = src/drivers/cyphal/public_regulated_data_types
-	url = https://github.com/opencyphal/public_regulated_data_types.git
-[submodule "src/drivers/cyphal/legacy_data_types"]
-	path = src/drivers/cyphal/legacy_data_types
+[submodule "src/drivers/uavcan_v1/libcanard"]
+	path = src/drivers/uavcan_v1/libcanard
+	url = https://github.com/UAVCAN/libcanard.git
+[submodule "src/drivers/uavcan_v1/public_regulated_data_types"]
+	path = src/drivers/uavcan_v1/public_regulated_data_types
+	url = https://github.com/UAVCAN/public_regulated_data_types.git
+[submodule "src/drivers/uavcan_v1/legacy_data_types"]
+	path = src/drivers/uavcan_v1/legacy_data_types
 	url = https://github.com/PX4/public_regulated_data_types.git
 	branch = legacy
 [submodule "src/lib/crypto/monocypher"]
@@ -9,5 +9,3 @@ launch.json
 ipch/

 browse.vc.db*
-
-*.log
@@ -330,7 +330,7 @@ px4io_update: px4_io-v2_default cubepilot_io-v2_default
 	cp build/cubepilot_io-v2_default/cubepilot_io-v2_default.bin boards/cubepilot/cubeyellow/extras/cubepilot_io-v2_default.bin
 	git status

-bootloaders_update: cuav_nora_bootloader cuav_x7pro_bootloader cubepilot_cubeorange_bootloader holybro_durandal-v1_bootloader matek_h743_bootloader matek_h743-mini_bootloader matek_h743-slim_bootloader modalai_fc-v2_bootloader mro_ctrl-zero-classic_bootloader mro_ctrl-zero-h7_bootloader mro_ctrl-zero-h7-oem_bootloader mro_pixracerpro_bootloader px4_fmu-v6u_bootloader px4_fmu-v6x_bootloader
+bootloaders_update: cuav_nora_bootloader cuav_x7pro_bootloader cubepilot_cubeorange_bootloader holybro_durandal-v1_bootloader matek_h743-slim_bootloader modalai_fc-v2_bootloader mro_ctrl-zero-classic_bootloader mro_ctrl-zero-h7_bootloader mro_ctrl-zero-h7-oem_bootloader mro_pixracerpro_bootloader px4_fmu-v6u_bootloader px4_fmu-v6x_bootloader
 	git status

 .PHONY: coverity_scan
@@ -7,11 +7,6 @@

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

-# TODO: Enable motor failure detection when the
-# VTOL no longer reports 0A for all ESCs in SITL
-param set-default FD_ACT_EN 0
-param set-default FD_ACT_MOT_TOUT 500
-
 # param set-default SYS_CTRL_ALLOC 1
 param set-default CA_AIRFRAME 2

@@ -9,7 +9,7 @@

 param set-default MAV_TYPE 21

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

 param set-default CA_ROTOR_COUNT 4
@@ -27,7 +27,9 @@ param set-default CA_ROTOR3_PY 0.1875
 param set-default CA_ROTOR3_KM -0.05

 param set-default CA_ROTOR0_TILT 1
+param set-default CA_ROTOR1_TILT 2
 param set-default CA_ROTOR2_TILT 3
+param set-default CA_ROTOR3_TILT 4
 param set-default CA_SV_CS0_TRQ_R -0.5
 param set-default CA_SV_CS0_TYPE 1
 param set-default CA_SV_CS1_TRQ_R 0.5
@@ -1,4 +1,6 @@

+mixer append /dev/pwm_output0 etc/mixers/mount_legs.aux.mix
+
 mavlink start -x -u 14558 -r 4000 -f -m onboard -o 14530 -p

 # shellcheck disable=SC2154
@@ -0,0 +1,40 @@
+#!/bin/sh
+#
+# @name Steadidrone QU4D
+#
+# @type Quadrotor Wide
+# @class Copter
+#
+# @output MAIN1 motor 1
+# @output MAIN2 motor 2
+# @output MAIN3 motor 3
+# @output MAIN4 motor 4
+# @output MAIN5 feed-through of RC AUX1 channel
+# @output MAIN6 feed-through of RC AUX2 channel
+#
+# @output AUX1 feed-through of RC AUX1 channel
+# @output AUX2 feed-through of RC AUX2 channel
+# @output AUX3 feed-through of RC AUX3 channel
+# @output AUX4 feed-through of RC FLAPS channel
+#
+# @maintainer Lorenz Meier <lorenz@px4.io>
+#
+# @board px4_fmu-v2 exclude
+# @board bitcraze_crazyflie exclude
+#
+
+. ${R}etc/init.d/rc.mc_defaults
+
+param set-default BAT1_N_CELLS 4
+
+param set-default MC_ROLL_P 7
+param set-default MC_ROLLRATE_P 0.13
+param set-default MC_ROLLRATE_I 0.05
+param set-default MC_ROLLRATE_D 0.004
+param set-default MC_PITCH_P 7
+param set-default MC_PITCHRATE_P 0.19
+param set-default MC_PITCHRATE_I 0.05
+param set-default MC_PITCHRATE_D 0.004
+param set-default MC_YAW_P 4
+
+set MIXER quad_w
@@ -0,0 +1,48 @@
+#!/bin/sh
+#
+# @name Steadidrone MAVRIK
+#
+# @type Octo Coax Wide
+# @class Copter
+#
+# @output MAIN1 motor 1
+# @output MAIN2 motor 2
+# @output MAIN3 motor 3
+# @output MAIN4 motor 4
+# @output MAIN5 motor 5
+# @output MAIN6 motor 6
+# @output MAIN7 motor 7
+# @output MAIN8 motor 8
+#
+# @maintainer Simon Wilks <simon@uaventure.com>
+#
+# @board px4_fmu-v2 exclude
+# @board bitcraze_crazyflie exclude
+#
+
+. ${R}etc/init.d/rc.mc_defaults
+
+
+param set-default MC_PITCH_P 4
+param set-default MC_PITCHRATE_P 0.24
+param set-default MC_PITCHRATE_I 0.09
+param set-default MC_PITCHRATE_D 0.013
+param set-default MC_PITCHRATE_MAX 180
+
+param set-default MC_ROLL_P 4
+param set-default MC_ROLLRATE_P 0.16
+param set-default MC_ROLLRATE_I 0.07
+param set-default MC_ROLLRATE_D 0.009
+param set-default MC_ROLLRATE_MAX 180
+
+param set-default MC_YAW_P 3
+
+param set-default MPC_HOLD_MAX_XY 0.25
+param set-default MPC_THR_MIN 0.15
+param set-default MPC_Z_VEL_MAX_DN 2
+
+param set-default BAT1_N_CELLS 4
+
+set MIXER octo_cox_w
+
+set PWM_OUT 12345678
@@ -0,0 +1,37 @@
+#!/bin/sh
+#
+# @name CruiseAder Claire
+#
+# @type VTOL Tiltrotor
+# @class VTOL
+#
+# @maintainer Samay Siga <samay_s@icloud.com>
+#
+# @board px4_fmu-v2 exclude
+# @board bitcraze_crazyflie exclude
+#
+
+. ${R}etc/init.d/rc.vtol_defaults
+
+param set-default MAV_TYPE 21
+
+param set-default PWM_AUX_DISARM 1000
+param set-default PWM_AUX_MAX 2000
+param set-default PWM_AUX_MIN 1000
+param set-default PWM_AUX_RATE 50
+
+param set-default PWM_MAIN_MAX 2000
+
+param set-default VT_MOT_ID 1234
+param set-default VT_FW_MOT_OFFID 13
+param set-default VT_IDLE_PWM_MC 1080
+param set-default VT_TILT_FW 0.9
+param set-default VT_TILT_MC 0.08
+param set-default VT_TILT_TRANS 0.5
+param set-default VT_ELEV_MC_LOCK 0
+param set-default VT_TYPE 1
+
+set MIXER claire
+set MIXER_AUX claire
+
+set PWM_OUT 1234
@@ -131,6 +131,7 @@ param set-default VT_F_TRANS_DUR 1
 param set-default VT_IDLE_PWM_MC 1025
 param set-default VT_B_REV_OUT 0.5
 param set-default VT_B_TRANS_THR 0.7
+param set-default VT_FW_PERM_STAB 1
 param set-default VT_TRANS_TIMEOUT 22
 param set-default VT_F_TRANS_RAMP 4

@@ -0,0 +1,45 @@
+#!/bin/sh
+#
+# @name IO Camflyer
+#
+# @type Flying Wing
+# @class Plane
+#
+# @output MAIN1 left aileron
+# @output MAIN2 right aileron
+# @output MAIN4 throttle
+#
+# @output AUX1 feed-through of RC AUX1 channel
+# @output AUX2 feed-through of RC AUX2 channel
+# @output AUX3 feed-through of RC AUX3 channel
+#
+# @maintainer Simon Wilks <simon@uaventure.com>
+#
+# @board px4_fmu-v2 exclude
+# @board bitcraze_crazyflie exclude
+#
+
+. ${R}etc/init.d/rc.fw_defaults
+
+param set-default FW_AIRSPD_MAX 15
+param set-default FW_AIRSPD_TRIM 13
+param set-default FW_R_TC 0.3
+param set-default FW_P_TC 0.3
+param set-default FW_L1_DAMPING 0.74
+param set-default FW_L1_PERIOD 16
+param set-default FW_LND_ANG 15
+param set-default FW_LND_FLALT 5
+param set-default FW_LND_HVIRT 13
+param set-default FW_LND_TLALT 5
+param set-default FW_THR_LND_MAX 0
+param set-default FW_PR_FF 0.35
+param set-default FW_RR_FF 0.6
+param set-default FW_RR_P 0.04
+
+param set-default PWM_MAIN_DISARM 1000
+
+
+set MIXER fw_generic_wing
+
+# Provide ESC a constant 1000 us pulse while disarmed
+set PWM_OUT 4
@@ -0,0 +1,29 @@
+#!/bin/sh
+#
+# @name FX-79 Buffalo Flying Wing
+#
+# @type Flying Wing
+# @class Plane
+#
+# @output MAIN1 right aileron
+# @output MAIN2 left aileron
+# @output MAIN4 throttle
+#
+# @output AUX1 feed-through of RC AUX1 channel
+# @output AUX2 feed-through of RC AUX2 channel
+# @output AUX3 feed-through of RC AUX3 channel
+#
+# @maintainer Simon Wilks <simon@uaventure.com>
+#
+# @board px4_fmu-v2 exclude
+# @board bitcraze_crazyflie exclude
+#
+
+. ${R}etc/init.d/rc.fw_defaults
+
+param set-default FW_AIRSPD_MAX 30
+param set-default FW_AIRSPD_MIN 13
+
+param set-default NAV_LOITER_RAD 150
+
+set MIXER fw_generic_wing
@@ -0,0 +1,23 @@
+#!/bin/sh
+#
+# @name Viper
+#
+# @type Flying Wing
+# @class Plane
+# @output MAIN1 left aileron
+# @output MAIN2 right aileron
+# @output MAIN4 throttle
+#
+# @output AUX1 feed-through of RC AUX1 channel
+# @output AUX2 feed-through of RC AUX2 channel
+# @output AUX3 feed-through of RC AUX3 channel
+#
+# @maintainer Simon Wilks <simon@uaventure.com>
+#
+# @board px4_fmu-v2 exclude
+# @board bitcraze_crazyflie exclude
+#
+
+. ${R}etc/init.d/rc.fw_defaults
+
+set MIXER Viper
@@ -0,0 +1,53 @@
+#!/bin/sh
+#
+# @name Modified Parrot Disco
+#
+# @url
+#
+# @type Flying Wing
+# @class Plane
+#
+# @output MAIN1 left aileron
+# @output MAIN2 right aileron
+# @output MAIN4 throttle
+#
+# @output AUX1 feed-through of RC AUX1 channel
+# @output AUX2 feed-through of RC AUX2 channel
+# @output AUX3 feed-through of RC AUX3 channel
+#
+# @maintainer Jan Liphardt <JTLiphardt@gmail.com>
+#
+# @board px4_fmu-v2 exclude
+# @board bitcraze_crazyflie exclude
+#
+
+. ${R}etc/init.d/rc.fw_defaults
+
+####################################
+# Airspeed
+####################################
+param set-default FW_AIRSPD_MAX  27 # = 52 knots
+
+####################################
+# Pitch
+####################################
+
+# Pitch rate feed forward (def = 0.5 %/rad/sec)
+param set-default FW_PR_FF 0.35
+
+####################################
+# Roll
+####################################
+
+# Basic limits (def = 50 deg)
+param set-default FW_R_LIM 40
+
+# Roll rate upper limit (def = 70 deg/s)
+param set-default FW_R_RMAX 50
+
+param set-default PWM_MAIN_DISARM 1000
+
+set MIXER fw_generic_wing.main.mix
+
+# Provide ESC a constant 1000 us pulse
+set PWM_OUT 4
@@ -0,0 +1,21 @@
+#!/bin/sh
+#
+# @name DJI F330 w/ DJI ESCs
+#
+# @type Quadrotor x
+# @class Copter
+#
+# @maintainer Lorenz Meier <lorenz@px4.io>
+# @board px4_fmu-v2 exclude
+#
+
+. ${R}etc/init.d/rc.mc_defaults
+
+param set-default MC_ROLL_P 7
+param set-default MC_ROLLRATE_I 0.05
+param set-default MC_PITCH_P 7
+param set-default MC_PITCHRATE_I 0.05
+
+
+# DJI ESCs do not support calibration and need a higher min
+param set-default PWM_MAIN_MIN 1230
@@ -0,0 +1,20 @@
+#!/bin/sh
+#
+# @name DJI F450 w/ DJI ESCs
+#
+# @type Quadrotor x
+# @class Copter
+#
+# @maintainer Lorenz Meier <lorenz@px4.io>
+#
+
+. ${R}etc/init.d/rc.mc_defaults
+
+param set-default MC_ROLL_P 7
+param set-default MC_ROLLRATE_I 0.05
+param set-default MC_PITCH_P 7
+param set-default MC_PITCHRATE_I 0.05
+param set-default MC_YAWRATE_P 0.3
+
+# DJI ESCs do not support calibration and need a higher min
+param set-default PWM_MAIN_MIN 1230
@@ -0,0 +1,27 @@
+#!/bin/sh
+#
+# @name DJI Matrice 100
+#
+# @type Quadrotor x
+# @class Copter
+#
+# @maintainer James Goppert <james.goppert@gmail.com>
+#
+# @board px4_fmu-v2 exclude
+# @board bitcraze_crazyflie exclude
+#
+
+. ${R}etc/init.d/rc.mc_defaults
+
+param set-default BAT1_N_CELLS 6
+
+param set-default MC_ROLLRATE_P 0.05
+param set-default MC_ROLLRATE_I 0.05
+param set-default MC_ROLLRATE_D 0.001
+param set-default MC_PITCHRATE_P 0.05
+param set-default MC_PITCHRATE_I 0.05
+param set-default MC_PITCHRATE_D 0.001
+
+param set-default MC_YAWRATE_I 0
+
+param set-default PWM_MAIN_MIN 1200
@@ -52,16 +52,22 @@ px4_add_romfs_files(

 	# [3000, 3999] Flying wing"
 	3000_generic_wing
+	3030_io_camflyer
 	3031_phantom
 	3032_skywalker_x5
 	3033_wingwing
+	3034_fx79
+	3035_viper
 	3036_pigeon
+	3037_parrot_disco_mod
 	3100_tbs_caipirinha

 	# [4000, 4999] Quadrotor x"
 	4001_quad_x
 	4003_qavr5
 	4009_qav250
+	4010_dji_f330
+	4011_dji_f450
 	4014_s500
 	4015_holybro_s500
 	4016_holybro_px4vision
@@ -75,6 +81,7 @@ px4_add_romfs_files(
 	4051_s250aq
 	4052_holybro_qav250
 	4053_holybro_kopis2
+	4060_dji_matrice_100
 	4061_atl_mantis_edu
 	4071_ifo
 	4072_draco
@@ -105,6 +112,7 @@ px4_add_romfs_files(
 	# [10000, 10999] Quadrotor Wide arm / H frame"
 	10015_tbs_discovery
 	10016_3dr_iris
+	10017_steadidrone_qu4d
 	10018_tbs_endurance

 	# [11000, 11999] Hexa Cox
@@ -112,6 +120,7 @@ px4_add_romfs_files(

 	# [12000, 12999] Octo Cox
 	12001_octo_cox
+	12002_steadidrone_mavrik

 	# [13000, 13999] VTOL
 	13000_generic_vtol_standard
@@ -124,6 +133,7 @@ px4_add_romfs_files(
 	13007_vtol_AAVVT_quad
 	13008_QuadRanger
 	13009_vtol_spt_ranger
+	13010_claire
 	13012_convergence
 	13013_deltaquad
 	13014_vtol_babyshark
@@ -298,9 +298,9 @@ else
 			tune_control play error
 		fi
 	else
-		if param greater -s CYPHAL_ENABLE 0
+		if param greater -s UAVCAN_V1_ENABLE 0
 		then
-			cyphal start
+			uavcan_v1 start
 		fi
 	fi

@@ -40,6 +40,8 @@ px4_add_romfs_files(
 	babyshark.main.mix
 	blade130.main.mix
 	CCPM.main.mix
+	claire.aux.mix
+	claire.main.mix
 	cloudship.main.mix
 	coax.main.mix
 	delta.main.mix
@@ -81,6 +83,7 @@ px4_add_romfs_files(
 	tri_y_yaw-.main.mix
 	uuv_x.main.mix
 	vectored6dof.main.mix
+	Viper.main.mix
 	vtol_AAERT.aux.mix
 	vtol_AAVVT.aux.mix
 	vtol_TTTTAAER.aux.mix
@@ -0,0 +1,66 @@
+Viper Delta-wing mixer
+=================================
+# @board px4_fmu-v2 exclude
+
+Designed for Viper.
+
+TODO (sjwilks): Add mixers for flaps.
+
+This file defines mixers suitable for controlling a delta wing aircraft using
+PX4FMU. The configuration assumes the elevon servos are connected to PX4FMU
+servo outputs 0 and 1 and the motor speed control to output 3. Output 2 is
+assumed to be unused.
+
+Inputs to the mixer come from channel group 0 (vehicle attitude), channels 0
+(roll), 1 (pitch) and 3 (thrust).
+
+See the README for more information on the scaler format.
+
+Elevon mixers
+-------------
+Three scalers total (output, roll, pitch).
+
+On the assumption that the two elevon servos are physically reversed, the pitch
+input is inverted between the two servos.
+
+The scaling factor for roll inputs is adjusted to implement differential travel
+for the elevons.
+
+M: 2
+S: 0 0   7500   7500      0 -10000  10000
+S: 0 1   8000   8000      0 -10000  10000
+
+M: 2
+S: 0 0   7500   7500      0 -10000  10000
+S: 0 1  -8000  -8000      0 -10000  10000
+
+Output 2
+--------
+This mixer is empty.
+
+Z:
+
+Motor speed mixer
+-----------------
+Two scalers total (output, thrust).
+
+This mixer generates a full-range output (-1 to 1) from an input in the (0 - 1)
+range.  Inputs below zero are treated as zero.
+
+M: 1
+S: 0 3      0  20000 -10000 -10000  10000
+
+Inputs to the mixer come from channel group 2 (payload), channels 0
+(bay servo 1), 1 (bay servo 2) and 3 (drop release).
+-----------------------------------------------------
+
+M: 1
+S: 2 0  10000  10000      0 -10000  10000
+
+M: 1
+S: 2 1  10000  10000      0 -10000  10000
+
+M: 1
+S: 2 2  -8000  -8000      0 -10000  10000
+
+
@@ -0,0 +1,28 @@
+# mixer for the CruiseAder Claire tilt mechansim servo and elevons
+
+=======================================================================
+
+
+Tilt mechanism servo mixer
+
+---------------------------
+
+M: 1
+
+
+S: 1 8  10000  10000      0 -10000  10000
+
+
+
+Elevon mixers
+
+-------------
+
+M: 2
+
+S: 1 0   7500   7500      0 -10000  10000
+S: 1 1   7500   7500      0 -10000  10000
+
+M: 2
+S: 1 0    7500    7500    0 -10000  10000
+S: 1 1   -7500   -7500    0 -10000  10000
@@ -0,0 +1,8 @@
+# CruiseAder Claire Main Multirotor mixer for PX4FMU
+# @board px4_fmu-v2 exclude
+
+#
+
+#===========================
+
+R: 4x
@@ -13,7 +13,8 @@ exec find boards msg src platforms test \
    -path platforms/qurt/dspal -prune -o \
    -path src/drivers/uavcan/libuavcan -prune -o \
    -path src/drivers/uavcan/uavcan_drivers/kinetis/driver/include/uavcan_kinetis -prune -o \
-    -path src/drivers/cyphal/libcanard -prune -o \
+    -path src/drivers/uavcan_v1/libcanard -prune -o \
+    -path src/drivers/uavcannode_gps_demo/libcanard -prune -o \
    -path src/lib/crypto/monocypher -prune -o \
    -path src/lib/events/libevents -prune -o \
    -path src/lib/parameters/uthash -prune -o \
@@ -7,9 +7,6 @@ EXTERNAL_METADATA,CONFIG_BOARD_EXTERNAL_METADATA=y
 BUILD_BOOTLOADER,CONFIG_BOARD_BUILD_BOOTLOADER=y
 UAVCAN_INTERFACES 2,CONFIG_BOARD_UAVCAN_INTERFACES=2
 UAVCAN_INTERFACES 1,CONFIG_BOARD_UAVCAN_INTERFACES=1
-UAVCAN_TIMER_OVERRIDE 2,CONFIG_BOARD_UAVCAN_TIMER_OVERRIDE=2
-UAVCAN_TIMER_OVERRIDE 1,CONFIG_BOARD_UAVCAN_TIMER_OVERRIDE=1
-UAVCAN_TIMER_OVERRIDE 1,CONFIG_BOARD_UAVCAN_TIMER_OVERRIDE=0
 TESTING,CONFIG_BOARD_TESTING=y
 ETHERNET,CONFIG_BOARD_ETHERNET=y
 adc/ads1115,CONFIG_DRIVERS_ADC_ADS1115=y
@@ -200,12 +200,13 @@ if [[ $INSTALL_SIM == "true" ]]; then

 	if [[ "${UBUNTU_RELEASE}" == "18.04" ]]; then
 		java_version=11
+		gazebo_version=9
 	elif [[ "${UBUNTU_RELEASE}" == "20.04" ]]; then
 		java_version=13
-	elif [[ "${UBUNTU_RELEASE}" == "22.04" ]]; then
-		java_version=11
+		gazebo_version=11
 	else
 		java_version=14
+		gazebo_version=11
 	fi
 	# Java (jmavsim or fastrtps)
 	sudo DEBIAN_FRONTEND=noninteractive apt-get -y --quiet --no-install-recommends install \
@@ -219,30 +220,20 @@ if [[ $INSTALL_SIM == "true" ]]; then
 	sudo update-alternatives --set java $(update-alternatives --list java | grep "java-$java_version")

 	# Gazebo
-	if [[ "${UBUNTU_RELEASE}" == "18.04" ]]; then
-		gazebo_version=9
-		gazebo_packages="gazebo$gazebo_version libgazebo$gazebo_version-dev"
-	elif [[ "${UBUNTU_RELEASE}" == "22.04" ]]; then
-		gazebo_packages="gazebo libgazebo-dev"
-	else
-		# default and Ubuntu 20.04
-		gazebo_version=11
-		gazebo_packages="gazebo$gazebo_version libgazebo$gazebo_version-dev"
-	fi
-
 	sudo sh -c 'echo "deb http://packages.osrfoundation.org/gazebo/ubuntu-stable `lsb_release -cs` main" > /etc/apt/sources.list.d/gazebo-stable.list'
 	wget http://packages.osrfoundation.org/gazebo.key -O - | sudo apt-key add -
 	# Update list, since new gazebo-stable.list has been added
 	sudo apt-get update -y --quiet
 	sudo DEBIAN_FRONTEND=noninteractive apt-get -y --quiet --no-install-recommends install \
 		dmidecode \
-		$gazebo_packages \
+		gazebo$gazebo_version \
 		gstreamer1.0-plugins-bad \
 		gstreamer1.0-plugins-base \
 		gstreamer1.0-plugins-good \
 		gstreamer1.0-plugins-ugly \
 		gstreamer1.0-libav \
 		libeigen3-dev \
+		libgazebo$gazebo_version-dev \
 		libgstreamer-plugins-base1.0-dev \
 		libimage-exiftool-perl \
 		libopencv-dev \
@@ -40,7 +40,6 @@ CONFIG_DRIVERS_SMART_BATTERY_BATMON=y
 CONFIG_COMMON_TELEMETRY=y
 CONFIG_DRIVERS_TONE_ALARM=y
 CONFIG_DRIVERS_UAVCAN=y
-CONFIG_BOARD_UAVCAN_TIMER_OVERRIDE=2
 CONFIG_MODULES_AIRSPEED_SELECTOR=y
 CONFIG_MODULES_ATTITUDE_ESTIMATOR_Q=y
 CONFIG_MODULES_BATTERY_STATUS=y
@@ -41,7 +41,6 @@ CONFIG_DRIVERS_SMART_BATTERY_BATMON=y
 CONFIG_COMMON_TELEMETRY=y
 CONFIG_DRIVERS_TONE_ALARM=y
 CONFIG_DRIVERS_UAVCAN=y
-CONFIG_BOARD_UAVCAN_TIMER_OVERRIDE=2
 CONFIG_MODULES_AIRSPEED_SELECTOR=y
 CONFIG_MODULES_ATTITUDE_ESTIMATOR_Q=y
 CONFIG_MODULES_BATTERY_STATUS=y
@@ -4,7 +4,6 @@ CONFIG_DRIVERS_IRLOCK=n
 CONFIG_DRIVERS_PCA9685=n
 CONFIG_DRIVERS_PCA9685_PWM_OUT=n
 CONFIG_DRIVERS_RPM=n
-CONFIG_DRIVERS_UAVCAN=n
 CONFIG_MODULES_ATTITUDE_ESTIMATOR_Q=n
 CONFIG_MODULES_LOCAL_POSITION_ESTIMATOR=n
 CONFIG_MODULES_ROVER_POS_CONTROL=n
@@ -4,7 +4,6 @@ CONFIG_DRIVERS_IRLOCK=n
 CONFIG_DRIVERS_PCA9685=n
 CONFIG_DRIVERS_PCA9685_PWM_OUT=n
 CONFIG_DRIVERS_RPM=n
-CONFIG_DRIVERS_UAVCAN=n
 CONFIG_EXAMPLES_FAKE_GPS=n
 CONFIG_MODULES_ATTITUDE_ESTIMATOR_Q=n
 CONFIG_MODULES_LOCAL_POSITION_ESTIMATOR=n
@@ -31,6 +31,7 @@ CONFIG_MODULES_MC_RATE_CONTROL=y
 CONFIG_MODULES_NAVIGATOR=y
 CONFIG_MODULES_RC_UPDATE=y
 CONFIG_MODULES_SENSORS=y
+CONFIG_SYSTEMCMDS_HARDFAULT_LOG=y
 CONFIG_SYSTEMCMDS_MIXER=y
 CONFIG_SYSTEMCMDS_PARAM=y
 CONFIG_SYSTEMCMDS_PWM=y
@@ -6,38 +6,32 @@ CONFIG_BOARD_SERIAL_TEL2="/dev/ttyS3"
 CONFIG_BOARD_SERIAL_TEL3="/dev/ttyS5"
 CONFIG_BOARD_SERIAL_TEL4="/dev/ttyS6"
 CONFIG_DRIVERS_ADC_BOARD_ADC=y
-CONFIG_DRIVERS_BAROMETER_DPS310=y
-CONFIG_DRIVERS_CAMERA_CAPTURE=n
-CONFIG_DRIVERS_CAMERA_TRIGGER=n
+CONFIG_COMMON_BAROMETERS=y
+CONFIG_DRIVERS_CAMERA_CAPTURE=y
+CONFIG_DRIVERS_CAMERA_TRIGGER=y
+CONFIG_COMMON_DISTANCE_SENSOR=y
 CONFIG_DRIVERS_DSHOT=y
 CONFIG_DRIVERS_GPS=y
 CONFIG_DRIVERS_IMU_INVENSENSE_MPU6000=y
 CONFIG_DRIVERS_IMU_INVENSENSE_ICM20602=y
 CONFIG_DRIVERS_IMU_INVENSENSE_ICM42605=y
-CONFIG_DRIVERS_MAGNETOMETER_QMC5883L=y
-CONFIG_COMMON_DISTANCE_SENSOR=n
-CONFIG_COMMON_LIGHT=n
-CONFIG_COMMON_MAGNETOMETER=n
+CONFIG_COMMON_LIGHT=y
+CONFIG_COMMON_MAGNETOMETER=y
 CONFIG_COMMON_OPTICAL_FLOW=y
 CONFIG_DRIVERS_OSD=y
 CONFIG_DRIVERS_PWM_OUT=y
-CONFIG_DRIVERS_PWM_OUT_SIM=n
 CONFIG_DRIVERS_RC_INPUT=y
-CONFIG_DRIVERS_RPM=n
+CONFIG_DRIVERS_RPM=y
 CONFIG_COMMON_TELEMETRY=y
 CONFIG_DRIVERS_TONE_ALARM=y
 CONFIG_DRIVERS_UAVCAN=y
-CONFIG_BOARD_UAVCAN_INTERFACES=1
 CONFIG_MODULES_ATTITUDE_ESTIMATOR_Q=y
 CONFIG_MODULES_BATTERY_STATUS=y
 CONFIG_MODULES_COMMANDER=y
-CONFIG_MODULES_CONTROL_ALLOCATOR=y
 CONFIG_MODULES_DATAMAN=y
 CONFIG_MODULES_EKF2=y
 CONFIG_MODULES_EVENTS=y
 CONFIG_MODULES_FLIGHT_MODE_MANAGER=y
-CONFIG_MODULES_FW_ATT_CONTROL=y
-CONFIG_MODULES_FW_POS_CONTROL_L1=y
 CONFIG_MODULES_GYRO_CALIBRATION=y
 CONFIG_MODULES_GYRO_FFT=y
 CONFIG_MODULES_LAND_DETECTOR=y
@@ -49,42 +43,42 @@ CONFIG_MODULES_MAG_BIAS_ESTIMATOR=y
 CONFIG_MODULES_MANUAL_CONTROL=y
 CONFIG_MODULES_MAVLINK=y
 CONFIG_MODULES_MC_ATT_CONTROL=y
-CONFIG_MODULES_MC_AUTOTUNE_ATTITUDE_CONTROL=n
+CONFIG_MODULES_MC_AUTOTUNE_ATTITUDE_CONTROL=y
 CONFIG_MODULES_MC_HOVER_THRUST_ESTIMATOR=y
 CONFIG_MODULES_MC_POS_CONTROL=y
 CONFIG_MODULES_MC_RATE_CONTROL=y
+CONFIG_MODULES_CONTROL_ALLOCATOR=y
 CONFIG_MODULES_NAVIGATOR=y
 CONFIG_MODULES_RC_UPDATE=y
 CONFIG_MODULES_SENSORS=y
-CONFIG_MODULES_TEMPERATURE_COMPENSATION=n
-CONFIG_MODULES_VTOL_ATT_CONTROL=y
-CONFIG_MODULES_GIMBAL=n
-CONFIG_SYSTEMCMDS_BL_UPDATE=n
-CONFIG_SYSTEMCMDS_ACTUATOR_TEST=y
+CONFIG_MODULES_TEMPERATURE_COMPENSATION=y
+CONFIG_MODULES_VMOUNT=y
+CONFIG_SYSTEMCMDS_BL_UPDATE=y
 CONFIG_SYSTEMCMDS_DMESG=y
-CONFIG_SYSTEMCMDS_DUMPFILE=n
+CONFIG_SYSTEMCMDS_DUMPFILE=y
+CONFIG_SYSTEMCMDS_ESC_CALIB=y
 CONFIG_SYSTEMCMDS_GPIO=y
 CONFIG_SYSTEMCMDS_HARDFAULT_LOG=y
 CONFIG_SYSTEMCMDS_I2CDETECT=y
-CONFIG_SYSTEMCMDS_LED_CONTROL=n
-CONFIG_SYSTEMCMDS_MFT=n
+CONFIG_SYSTEMCMDS_LED_CONTROL=y
+CONFIG_SYSTEMCMDS_MFT=y
 CONFIG_SYSTEMCMDS_MIXER=y
-CONFIG_SYSTEMCMDS_MOTOR_TEST=n
-CONFIG_SYSTEMCMDS_MTD=n
+CONFIG_SYSTEMCMDS_MOTOR_TEST=y
+CONFIG_SYSTEMCMDS_MTD=y
 CONFIG_SYSTEMCMDS_NSHTERM=y
 CONFIG_SYSTEMCMDS_PARAM=y
 CONFIG_SYSTEMCMDS_PERF=y
 CONFIG_SYSTEMCMDS_PWM=y
 CONFIG_SYSTEMCMDS_REBOOT=y
-CONFIG_SYSTEMCMDS_SD_BENCH=n
-CONFIG_SYSTEMCMDS_SD_STRESS=n
-CONFIG_SYSTEMCMDS_SERIAL_TEST=n
+CONFIG_SYSTEMCMDS_SD_BENCH=y
+CONFIG_SYSTEMCMDS_SD_STRESS=y
+CONFIG_SYSTEMCMDS_SERIAL_TEST=y
 CONFIG_SYSTEMCMDS_SYSTEM_TIME=y
 CONFIG_SYSTEMCMDS_TOP=y
 CONFIG_SYSTEMCMDS_TOPIC_LISTENER=y
-CONFIG_SYSTEMCMDS_TUNE_CONTROL=n
+CONFIG_SYSTEMCMDS_TUNE_CONTROL=y
 CONFIG_SYSTEMCMDS_UORB=y
 CONFIG_SYSTEMCMDS_USB_CONNECTED=y
 CONFIG_SYSTEMCMDS_VER=y
 CONFIG_SYSTEMCMDS_WORK_QUEUE=y
-CONFIG_EXAMPLES_FAKE_GPS=n
+CONFIG_EXAMPLES_FAKE_GPS=y
@@ -11,7 +11,7 @@
 # CONFIG_STM32H7_SYSCFG is not set
 CONFIG_ARCH="arm"
 CONFIG_ARCH_BOARD_CUSTOM=y
-CONFIG_ARCH_BOARD_CUSTOM_DIR="../../../../boards/matek/h743-mini/nuttx-config"
+CONFIG_ARCH_BOARD_CUSTOM_DIR="../../../../boards/matek/h743-slim/nuttx-config"
 CONFIG_ARCH_BOARD_CUSTOM_DIR_RELPATH=y
 CONFIG_ARCH_BOARD_CUSTOM_NAME="px4"
 CONFIG_ARCH_CHIP="stm32h7"
@@ -30,7 +30,7 @@ CONFIG_BOARD_RESET_ON_ASSERT=2
 CONFIG_C99_BOOL8=y
 CONFIG_CDCACM=y
 CONFIG_CDCACM_PRODUCTID=0x004b
-CONFIG_CDCACM_PRODUCTSTR="Matek H743-mini"
+CONFIG_CDCACM_PRODUCTSTR="Matek H743-slim"
 CONFIG_CDCACM_RXBUFSIZE=600
 CONFIG_CDCACM_TXBUFSIZE=12000
 CONFIG_CDCACM_VENDORID=0x3162
@@ -51,6 +51,7 @@ CONFIG_LIBC_FLOATINGPOINT=y
 CONFIG_LIBC_LONG_LONG=y
 CONFIG_LIBC_STRERROR=y
 CONFIG_LIB_BOARDCTL=y
+CONFIG_FS_PROCFS_MAX_TASKS=8
 CONFIG_MEMSET_64BIT=y
 CONFIG_MEMSET_OPTSPEED=y
 CONFIG_PREALLOC_TIMERS=50
@@ -284,17 +284,17 @@

 #define STM32_SDMMC_INIT_CLKDIV     (300 << STM32_SDMMC_CLKCR_CLKDIV_SHIFT)

-/* 20 MHz Max for now - more reliable on some boards than 25 MHz
- * 20 MHz = PLL1Q/(2*div), div =  PLL1Q/(2*freq), div = 6 = 240 / 40
+/* 25 MHz Max for now, 25 mHZ = PLL1Q/(2*div), div =  PLL1Q/(2*freq)
+ * div = 4.8 = 240 / 50, So round up to 5 for default speed 24 MB/s
 */

 #if defined(CONFIG_STM32H7_SDMMC_XDMA) || defined(CONFIG_STM32H7_SDMMC_IDMA)
-#  define STM32_SDMMC_MMCXFR_CLKDIV   (6 << STM32_SDMMC_CLKCR_CLKDIV_SHIFT)
+#  define STM32_SDMMC_MMCXFR_CLKDIV   (5 << STM32_SDMMC_CLKCR_CLKDIV_SHIFT)
 #else
 #  define STM32_SDMMC_MMCXFR_CLKDIV   (100 << STM32_SDMMC_CLKCR_CLKDIV_SHIFT)
 #endif
 #if defined(CONFIG_STM32H7_SDMMC_XDMA) || defined(CONFIG_STM32H7_SDMMC_IDMA)
-#  define STM32_SDMMC_SDXFR_CLKDIV    (6 << STM32_SDMMC_CLKCR_CLKDIV_SHIFT)
+#  define STM32_SDMMC_SDXFR_CLKDIV    (5 << STM32_SDMMC_CLKCR_CLKDIV_SHIFT)
 #else
 #  define STM32_SDMMC_SDXFR_CLKDIV    (100 << STM32_SDMMC_CLKCR_CLKDIV_SHIFT)
 #endif
@@ -45,11 +45,11 @@ CONFIG_BUILTIN=y
 CONFIG_C99_BOOL8=y
 CONFIG_CDCACM=y
 CONFIG_CDCACM_PRODUCTID=0x0036
-CONFIG_CDCACM_PRODUCTSTR="MatekH743-mini"
+CONFIG_CDCACM_PRODUCTSTR="MatekH743-slim"
 CONFIG_CDCACM_RXBUFSIZE=600
 CONFIG_CDCACM_TXBUFSIZE=12000
 CONFIG_CDCACM_VENDORID=0x1B8C
-CONFIG_CDCACM_VENDORSTR="Matek"
+CONFIG_CDCACM_VENDORSTR="PX4"
 CONFIG_CLOCK_MONOTONIC=y
 CONFIG_DEBUG_FULLOPT=y
 CONFIG_DEBUG_HARDFAULT_ALERT=y
@@ -86,6 +86,7 @@ CONFIG_LIBC_LONG_LONG=y
 CONFIG_LIBC_STRERROR=y
 CONFIG_MEMSET_64BIT=y
 CONFIG_MEMSET_OPTSPEED=y
+CONFIG_MKFATFS_BUFFER_ALIGMENT=32
 CONFIG_MMCSD=y
 CONFIG_MMCSD_SDIO=y
 CONFIG_MMCSD_SDIOWAIT_WRCOMPLETE=y
@@ -120,8 +120,8 @@

 /* PWM
 */
-#define DIRECT_PWM_OUTPUT_CHANNELS   12
-#define DIRECT_INPUT_TIMER_CHANNELS  12
+#define DIRECT_PWM_OUTPUT_CHANNELS   10
+#define DIRECT_INPUT_TIMER_CHANNELS  10

 #define BOARD_HAS_PWM  DIRECT_PWM_OUTPUT_CHANNELS

@@ -37,7 +37,7 @@ constexpr io_timers_t io_timers[MAX_IO_TIMERS] = {
 	initIOTimer(Timer::Timer3, DMA{DMA::Index1, DMA::Stream2, DMA::Channel5}),
 	initIOTimer(Timer::Timer5, DMA{DMA::Index1, DMA::Stream0, DMA::Channel6}),
 	initIOTimer(Timer::Timer4, DMA{DMA::Index1, DMA::Stream6, DMA::Channel2}),
-	initIOTimer(Timer::Timer15),
+	// initIOTimer(Timer::Timer15),
 };

 constexpr timer_io_channels_t timer_io_channels[MAX_TIMER_IO_CHANNELS] = {
@@ -51,8 +51,8 @@ constexpr timer_io_channels_t timer_io_channels[MAX_TIMER_IO_CHANNELS] = {
 	initIOTimerChannel(io_timers, {Timer::Timer4, Timer::Channel2}, {GPIO::PortD, GPIO::Pin13}),
 	initIOTimerChannel(io_timers, {Timer::Timer4, Timer::Channel3}, {GPIO::PortD, GPIO::Pin14}),
 	initIOTimerChannel(io_timers, {Timer::Timer4, Timer::Channel4}, {GPIO::PortD, GPIO::Pin15}),
-	initIOTimerChannel(io_timers, {Timer::Timer15, Timer::Channel1}, {GPIO::PortE, GPIO::Pin5}),
-	initIOTimerChannel(io_timers, {Timer::Timer15, Timer::Channel2}, {GPIO::PortE, GPIO::Pin6}),
+	// initIOTimerChannel(io_timers, {Timer::Timer15, Timer::Channel1}, {GPIO::PortE, GPIO::Pin5}),
+	// initIOTimerChannel(io_timers, {Timer::Timer15, Timer::Channel2}, {GPIO::PortE, GPIO::Pin6}),
 };

 constexpr io_timers_channel_mapping_t io_timers_channel_mapping =
@@ -7,22 +7,17 @@ CONFIG_BOARD_SERIAL_TEL3="/dev/ttyS5"
 CONFIG_BOARD_SERIAL_TEL4="/dev/ttyS6"
 CONFIG_DRIVERS_ADC_BOARD_ADC=y
 CONFIG_DRIVERS_BAROMETER_DPS310=y
-CONFIG_DRIVERS_CAMERA_CAPTURE=n
-CONFIG_DRIVERS_CAMERA_TRIGGER=n
 CONFIG_COMMON_DISTANCE_SENSOR=y
 CONFIG_DRIVERS_DSHOT=y
 CONFIG_DRIVERS_GPS=y
 CONFIG_DRIVERS_IMU_INVENSENSE_MPU6000=y
 CONFIG_DRIVERS_IMU_INVENSENSE_ICM20602=y
 CONFIG_DRIVERS_IMU_INVENSENSE_ICM42605=y
-CONFIG_COMMON_LIGHT=n
 CONFIG_COMMON_MAGNETOMETER=y
 CONFIG_COMMON_OPTICAL_FLOW=y
 CONFIG_DRIVERS_OSD=y
 CONFIG_DRIVERS_PWM_OUT=y
-CONFIG_DRIVERS_PWM_OUT_SIM=n
 CONFIG_DRIVERS_RC_INPUT=y
-CONFIG_DRIVERS_RPM=n
 CONFIG_COMMON_TELEMETRY=y
 CONFIG_DRIVERS_TONE_ALARM=y
 CONFIG_DRIVERS_UAVCAN=y
@@ -48,42 +43,32 @@ CONFIG_MODULES_MAG_BIAS_ESTIMATOR=y
 CONFIG_MODULES_MANUAL_CONTROL=y
 CONFIG_MODULES_MAVLINK=y
 CONFIG_MODULES_MC_ATT_CONTROL=y
-CONFIG_MODULES_MC_AUTOTUNE_ATTITUDE_CONTROL=n
 CONFIG_MODULES_MC_HOVER_THRUST_ESTIMATOR=y
 CONFIG_MODULES_MC_POS_CONTROL=y
 CONFIG_MODULES_MC_RATE_CONTROL=y
 CONFIG_MODULES_NAVIGATOR=y
 CONFIG_MODULES_RC_UPDATE=y
 CONFIG_MODULES_SENSORS=y
-CONFIG_MODULES_TEMPERATURE_COMPENSATION=n
+CONFIG_MODULES_TEMPERATURE_COMPENSATION=y
 CONFIG_MODULES_VTOL_ATT_CONTROL=y
-CONFIG_MODULES_GIMBAL=n
-CONFIG_SYSTEMCMDS_BL_UPDATE=n
 CONFIG_SYSTEMCMDS_ACTUATOR_TEST=y
 CONFIG_SYSTEMCMDS_DMESG=y
-CONFIG_SYSTEMCMDS_DUMPFILE=n
+CONFIG_SYSTEMCMDS_ESC_CALIB=y
 CONFIG_SYSTEMCMDS_GPIO=y
 CONFIG_SYSTEMCMDS_HARDFAULT_LOG=y
 CONFIG_SYSTEMCMDS_I2CDETECT=y
-CONFIG_SYSTEMCMDS_LED_CONTROL=n
-CONFIG_SYSTEMCMDS_MFT=n
 CONFIG_SYSTEMCMDS_MIXER=y
-CONFIG_SYSTEMCMDS_MOTOR_TEST=n
-CONFIG_SYSTEMCMDS_MTD=n
 CONFIG_SYSTEMCMDS_NSHTERM=y
 CONFIG_SYSTEMCMDS_PARAM=y
 CONFIG_SYSTEMCMDS_PERF=y
 CONFIG_SYSTEMCMDS_PWM=y
 CONFIG_SYSTEMCMDS_REBOOT=y
-CONFIG_SYSTEMCMDS_SD_BENCH=n
-CONFIG_SYSTEMCMDS_SD_STRESS=n
-CONFIG_SYSTEMCMDS_SERIAL_TEST=n
+CONFIG_SYSTEMCMDS_SD_BENCH=y
+CONFIG_SYSTEMCMDS_SERIAL_TEST=y
 CONFIG_SYSTEMCMDS_SYSTEM_TIME=y
 CONFIG_SYSTEMCMDS_TOP=y
 CONFIG_SYSTEMCMDS_TOPIC_LISTENER=y
-CONFIG_SYSTEMCMDS_TUNE_CONTROL=n
 CONFIG_SYSTEMCMDS_UORB=y
 CONFIG_SYSTEMCMDS_USB_CONNECTED=y
 CONFIG_SYSTEMCMDS_VER=y
 CONFIG_SYSTEMCMDS_WORK_QUEUE=y
-CONFIG_EXAMPLES_FAKE_GPS=n
@@ -11,7 +11,7 @@
 # CONFIG_STM32H7_SYSCFG is not set
 CONFIG_ARCH="arm"
 CONFIG_ARCH_BOARD_CUSTOM=y
-CONFIG_ARCH_BOARD_CUSTOM_DIR="../../../../boards/matek/h743/nuttx-config"
+CONFIG_ARCH_BOARD_CUSTOM_DIR="../../../../boards/matek/h743-slim/nuttx-config"
 CONFIG_ARCH_BOARD_CUSTOM_DIR_RELPATH=y
 CONFIG_ARCH_BOARD_CUSTOM_NAME="px4"
 CONFIG_ARCH_CHIP="stm32h7"
@@ -51,6 +51,7 @@ CONFIG_LIBC_FLOATINGPOINT=y
 CONFIG_LIBC_LONG_LONG=y
 CONFIG_LIBC_STRERROR=y
 CONFIG_LIB_BOARDCTL=y
+CONFIG_FS_PROCFS_MAX_TASKS=8
 CONFIG_MEMSET_64BIT=y
 CONFIG_MEMSET_OPTSPEED=y
 CONFIG_PREALLOC_TIMERS=50
@@ -284,17 +284,17 @@

 #define STM32_SDMMC_INIT_CLKDIV     (300 << STM32_SDMMC_CLKCR_CLKDIV_SHIFT)

-/* 20 MHz Max for now - more reliable on some boards than 25 MHz
- * 20 MHz = PLL1Q/(2*div), div =  PLL1Q/(2*freq), div = 6 = 240 / 40
+/* 25 MHz Max for now, 25 mHZ = PLL1Q/(2*div), div =  PLL1Q/(2*freq)
+ * div = 4.8 = 240 / 50, So round up to 5 for default speed 24 MB/s
 */

 #if defined(CONFIG_STM32H7_SDMMC_XDMA) || defined(CONFIG_STM32H7_SDMMC_IDMA)
-#  define STM32_SDMMC_MMCXFR_CLKDIV   (6 << STM32_SDMMC_CLKCR_CLKDIV_SHIFT)
+#  define STM32_SDMMC_MMCXFR_CLKDIV   (5 << STM32_SDMMC_CLKCR_CLKDIV_SHIFT)
 #else
 #  define STM32_SDMMC_MMCXFR_CLKDIV   (100 << STM32_SDMMC_CLKCR_CLKDIV_SHIFT)
 #endif
 #if defined(CONFIG_STM32H7_SDMMC_XDMA) || defined(CONFIG_STM32H7_SDMMC_IDMA)
-#  define STM32_SDMMC_SDXFR_CLKDIV    (6 << STM32_SDMMC_CLKCR_CLKDIV_SHIFT)
+#  define STM32_SDMMC_SDXFR_CLKDIV    (5 << STM32_SDMMC_CLKCR_CLKDIV_SHIFT)
 #else
 #  define STM32_SDMMC_SDXFR_CLKDIV    (100 << STM32_SDMMC_CLKCR_CLKDIV_SHIFT)
 #endif
@@ -50,7 +50,7 @@ CONFIG_CDCACM_PRODUCTSTR="MatekH743"
 CONFIG_CDCACM_RXBUFSIZE=600
 CONFIG_CDCACM_TXBUFSIZE=12000
 CONFIG_CDCACM_VENDORID=0x1B8C
-CONFIG_CDCACM_VENDORSTR="Matek"
+CONFIG_CDCACM_VENDORSTR="PX4"
 CONFIG_CLOCK_MONOTONIC=y
 CONFIG_DEBUG_FULLOPT=y
 CONFIG_DEBUG_HARDFAULT_ALERT=y
@@ -120,8 +120,8 @@

 /* PWM
 */
-#define DIRECT_PWM_OUTPUT_CHANNELS   12
-#define DIRECT_INPUT_TIMER_CHANNELS  12
+#define DIRECT_PWM_OUTPUT_CHANNELS   10
+#define DIRECT_INPUT_TIMER_CHANNELS  10

 #define BOARD_HAS_PWM  DIRECT_PWM_OUTPUT_CHANNELS

@@ -37,7 +37,7 @@ constexpr io_timers_t io_timers[MAX_IO_TIMERS] = {
 	initIOTimer(Timer::Timer3, DMA{DMA::Index1, DMA::Stream2, DMA::Channel5}),
 	initIOTimer(Timer::Timer5, DMA{DMA::Index1, DMA::Stream0, DMA::Channel6}),
 	initIOTimer(Timer::Timer4, DMA{DMA::Index1, DMA::Stream6, DMA::Channel2}),
-	initIOTimer(Timer::Timer15),
+	// initIOTimer(Timer::Timer15),
 };

 constexpr timer_io_channels_t timer_io_channels[MAX_TIMER_IO_CHANNELS] = {
@@ -51,8 +51,8 @@ constexpr timer_io_channels_t timer_io_channels[MAX_TIMER_IO_CHANNELS] = {
 	initIOTimerChannel(io_timers, {Timer::Timer4, Timer::Channel2}, {GPIO::PortD, GPIO::Pin13}),
 	initIOTimerChannel(io_timers, {Timer::Timer4, Timer::Channel3}, {GPIO::PortD, GPIO::Pin14}),
 	initIOTimerChannel(io_timers, {Timer::Timer4, Timer::Channel4}, {GPIO::PortD, GPIO::Pin15}),
-	initIOTimerChannel(io_timers, {Timer::Timer15, Timer::Channel1}, {GPIO::PortE, GPIO::Pin5}),
-	initIOTimerChannel(io_timers, {Timer::Timer15, Timer::Channel2}, {GPIO::PortE, GPIO::Pin6}),
+	// initIOTimerChannel(io_timers, {Timer::Timer15, Timer::Channel1}, {GPIO::PortE, GPIO::Pin5}),
+	// initIOTimerChannel(io_timers, {Timer::Timer15, Timer::Channel2}, {GPIO::PortE, GPIO::Pin6}),
 };

 constexpr io_timers_channel_mapping_t io_timers_channel_mapping =
@@ -47,7 +47,7 @@

 constexpr io_timers_t io_timers[MAX_IO_TIMERS] = {
 	initIOTimer(Timer::Timer1, DMA{DMA::Index2, DMA::Stream5, DMA::Channel6}),
-	initIOTimer(Timer::Timer4, DMA{DMA::Index1, DMA::Stream6, DMA::Channel2}),
+	initIOTimer(Timer::Timer4),
 };


@@ -35,7 +35,6 @@ 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
@@ -1,4 +1,4 @@
 CONFIG_DRIVERS_UAVCAN=n
-CONFIG_DRIVERS_CYPHAL=y
-CONFIG_CYPHAL_BMS_SUBSCRIBER=y
-CONFIG_CYPHAL_UORB_SENSOR_GPS_SUBSCRIBER=y
+CONFIG_DRIVERS_UAVCAN_V1=y
+CONFIG_UAVCAN_V1_BMS_SUBSCRIBER=y
+CONFIG_UAVCAN_V1_UORB_SENSOR_GPS_SUBSCRIBER=y
@@ -1,9 +1,9 @@
 CONFIG_DRIVERS_UAVCAN=n
-CONFIG_DRIVERS_CYPHAL=y
-CONFIG_CYPHAL_BMS_SUBSCRIBER=y
-CONFIG_CYPHAL_ESC_SUBSCRIBER=y
-CONFIG_CYPHAL_GNSS_SUBSCRIBER_0=y
-CONFIG_CYPHAL_GNSS_SUBSCRIBER_1=y
-CONFIG_CYPHAL_READINESS_PUBLISHER=y
-CONFIG_CYPHAL_UORB_ACTUATOR_OUTPUTS_PUBLISHER=y
-CONFIG_CYPHAL_UORB_SENSOR_GPS_SUBSCRIBER=y
+CONFIG_DRIVERS_UAVCAN_V1=y
+CONFIG_UAVCAN_V1_BMS_SUBSCRIBER=y
+CONFIG_UAVCAN_V1_ESC_SUBSCRIBER=y
+CONFIG_UAVCAN_V1_GNSS_SUBSCRIBER_0=y
+CONFIG_UAVCAN_V1_GNSS_SUBSCRIBER_1=y
+CONFIG_UAVCAN_V1_READINESS_PUBLISHER=y
+CONFIG_UAVCAN_V1_UORB_ACTUATOR_OUTPUTS_PUBLISHER=y
+CONFIG_UAVCAN_V1_UORB_SENSOR_GPS_SUBSCRIBER=y
@@ -7,10 +7,10 @@ CONFIG_DRIVERS_BOOTLOADERS=y
 CONFIG_DRIVERS_GPS=y
 CONFIG_DRIVERS_LIGHTS_RGBLED_PWM=y
 CONFIG_DRIVERS_PWM_OUT=y
-CONFIG_DRIVERS_CYPHAL=y
-CONFIG_CYPHAL_CLIENT=y
-CONFIG_CYPHAL_APP_DESCRIPTOR=y
-CONFIG_CYPHAL_UORB_SENSOR_GPS_PUBLISHER=y
+CONFIG_DRIVERS_UAVCAN_V1=y
+CONFIG_UAVCAN_V1_CLIENT=y
+CONFIG_UAVCAN_V1_APP_DESCRIPTOR=y
+CONFIG_UAVCAN_V1_UORB_SENSOR_GPS_PUBLISHER=y
 CONFIG_MODULES_CONTROL_ALLOCATOR=y
 CONFIG_SYSTEMCMDS_I2CDETECT=y
 CONFIG_SYSTEMCMDS_LED_CONTROL=y
@@ -6,4 +6,4 @@
 pwm_out mode_pwm1 start

 ifup can0
-cyphal start
+uavcan_v1 start
@@ -159,12 +159,10 @@ int s32k1xx_bringup(void)
 	if (s32k1xx_gpioread(BOARD_REVISION_DETECT_PIN)) {
 		/* STB high -> active CAN phy */
 		s32k1xx_pinconfig(PIN_CAN0_STB  | GPIO_OUTPUT_ONE);
-		s32k1xx_pinconfig(PIN_CAN1_STB  | GPIO_OUTPUT_ONE);

 	} else {
 		/* STB low -> active CAN phy */
 		s32k1xx_pinconfig(PIN_CAN0_STB  | GPIO_OUTPUT_ZERO);
-		s32k1xx_pinconfig(PIN_CAN1_STB  | GPIO_OUTPUT_ZERO);
 	}

 #endif
@@ -110,7 +110,7 @@ const struct clock_configuration_s g_initial_clkconfig = {
 		{
 			.mode        = SCG_SPLL_MONITOR_DISABLE,         /* SPLLCM */
 			.div1        = SCG_ASYNC_CLOCK_DIV_BY_1,         /* SPLLDIV1 */
-			.div2        = SCG_ASYNC_CLOCK_DIV_BY_2,         /* SPLLDIV2 */
+			.div2        = SCG_ASYNC_CLOCK_DIV_BY_1,         /* SPLLDIV2 */
 			.prediv      = 1,                                /* PREDIV */
 			.mult        = 40,                               /* MULT */
 			.src         = 0,                                /* SOURCE */
@@ -101,7 +101,7 @@ const struct peripheral_clock_config_s g_peripheral_clockconfig0[] = {
 #else
 		.clkgate = false,
 #endif
-		.clksrc  = CLK_SRC_SPLL_DIV2,
+		.clksrc  = CLK_SRC_SIRC_DIV2,
 	},
 	{
 		.clkname = LPSPI0_CLK,
@@ -110,7 +110,7 @@ const struct peripheral_clock_config_s g_peripheral_clockconfig0[] = {
 #else
 		.clkgate = false,
 #endif
-		.clksrc  = CLK_SRC_SPLL_DIV2,
+		.clksrc  = CLK_SRC_SIRC_DIV2,
 	},
 	{
 		.clkname = LPUART0_CLK,
@@ -119,7 +119,7 @@ const struct peripheral_clock_config_s g_peripheral_clockconfig0[] = {
 #else
 		.clkgate = false,
 #endif
-		.clksrc  = CLK_SRC_SPLL_DIV2,
+		.clksrc  = CLK_SRC_SIRC_DIV2,
 	},
 	{
 		.clkname = LPUART1_CLK,
@@ -128,7 +128,7 @@ const struct peripheral_clock_config_s g_peripheral_clockconfig0[] = {
 #else
 		.clkgate = false,
 #endif
-		.clksrc  = CLK_SRC_SPLL_DIV2,
+		.clksrc  = CLK_SRC_SIRC_DIV2,
 	},
 	{
 		.clkname    = RTC0_CLK,
@@ -1,4 +1,3 @@
-CONFIG_DRIVERS_OSD=n
 CONFIG_DRIVERS_TELEMETRY_FRSKY_TELEMETRY=n
 CONFIG_MODULES_ATTITUDE_ESTIMATOR_Q=n
 CONFIG_MODULES_EKF2=y
@@ -7,7 +7,9 @@ CONFIG_SYSTEMCMDS_BL_UPDATE=n
 CONFIG_DRIVERS_DIFFERENTIAL_PRESSURE_MS4525DO=y
 CONFIG_DRIVERS_DISTANCE_SENSOR_LIGHTWARE_LASER_I2C=y
 CONFIG_DRIVERS_DISTANCE_SENSOR_LIGHTWARE_LASER_SERIAL=y
+CONFIG_DRIVERS_DISTANCE_SENSOR_LL40LS=y
 CONFIG_MODULES_AIRSPEED_SELECTOR=y
 CONFIG_MODULES_FW_ATT_CONTROL=y
 CONFIG_MODULES_FW_AUTOTUNE_ATTITUDE_CONTROL=y
 CONFIG_MODULES_FW_POS_CONTROL_L1=y
+CONFIG_SYSTEMCMDS_PERF=y
@@ -1,2 +1,3 @@
 CONFIG_SYSTEMCMDS_BL_UPDATE=n
 CONFIG_DRIVERS_DISTANCE_SENSOR_LIGHTWARE_LASER_I2C=y
+CONFIG_MODULES_MC_AUTOTUNE_ATTITUDE_CONTROL=y
@@ -1,7 +0,0 @@
-# CONFIG_BOARD_UAVCAN_TIMER_OVERRIDE is not set
-CONFIG_DRIVERS_HEATER=n
-CONFIG_DRIVERS_OSD=n
-CONFIG_DRIVERS_UAVCAN=n
-CONFIG_CYPHAL_BMS_SUBSCRIBER=y
-CONFIG_CYPHAL_UORB_SENSOR_GPS_SUBSCRIBER=y
-CONFIG_DRIVERS_CYPHAL=y
@@ -1,4 +1,3 @@
-# CONFIG_BOARD_UAVCAN_TIMER_OVERRIDE is not set
 CONFIG_COMMON_BAROMETERS=n
 CONFIG_COMMON_DIFFERENTIAL_PRESSURE=n
 CONFIG_COMMON_MAGNETOMETER=n
@@ -23,6 +22,7 @@ CONFIG_DRIVERS_UAVCAN=n
 CONFIG_MODULES_ATTITUDE_ESTIMATOR_Q=n
 CONFIG_MODULES_CAMERA_FEEDBACK=n
 CONFIG_MODULES_ESC_BATTERY=n
+CONFIG_MODULES_GIMBAL=n
 CONFIG_MODULES_GYRO_CALIBRATION=n
 CONFIG_MODULES_GYRO_FFT=n
 CONFIG_MODULES_LANDING_TARGET_ESTIMATOR=n
@@ -32,7 +32,6 @@ CONFIG_MODULES_SIH=n
 CONFIG_MODULES_TEMPERATURE_COMPENSATION=n
 CONFIG_MODULES_UUV_ATT_CONTROL=n
 CONFIG_MODULES_UUV_POS_CONTROL=n
-CONFIG_MODULES_GIMBAL=n
 CONFIG_SYSTEMCMDS_SERIAL_TEST=n
 CONFIG_BOARD_CONSTRAINED_FLASH=y
 CONFIG_BOARD_TESTING=y
@@ -43,7 +43,6 @@ CONFIG_DRIVERS_SMART_BATTERY_BATMON=y
 CONFIG_COMMON_TELEMETRY=y
 CONFIG_DRIVERS_TONE_ALARM=y
 CONFIG_DRIVERS_UAVCAN=y
-CONFIG_BOARD_UAVCAN_TIMER_OVERRIDE=6
 CONFIG_MODULES_AIRSPEED_SELECTOR=y
 CONFIG_MODULES_ATTITUDE_ESTIMATOR_Q=y
 CONFIG_MODULES_BATTERY_STATUS=y
@@ -21,7 +21,6 @@ CONFIG_MODULES_FW_POS_CONTROL=n
 CONFIG_MODULES_FW_POS_CONTROL_L1=n
 CONFIG_MODULES_ROVER_POS_CONTROL=n
 CONFIG_MODULES_SIH=n
-CONFIG_MODULES_TEMPERATURE_COMPENSATION=n
 CONFIG_MODULES_UUV_ATT_CONTROL=n
 CONFIG_MODULES_UUV_POS_CONTROL=n
 CONFIG_MODULES_VTOL_ATT_CONTROL=n
@@ -1,10 +1,8 @@
-# CONFIG_BOARD_UAVCAN_TIMER_OVERRIDE is not set
 CONFIG_COMMON_HYGROMETERS=n
 CONFIG_COMMON_TELEMETRY=n
 CONFIG_DRIVERS_BATT_SMBUS=n
 CONFIG_DRIVERS_CAMERA_CAPTURE=n
 CONFIG_DRIVERS_CAMERA_TRIGGER=n
-CONFIG_COMMON_DIFFERENTIAL_PRESSURE=n
 CONFIG_DRIVERS_HEATER=n
 CONFIG_DRIVERS_IMU_ANALOG_DEVICES_ADIS16448=n
 CONFIG_DRIVERS_IMU_INVENSENSE_ICM20602=n
@@ -1,5 +1,3 @@
-# CONFIG_BOARD_UAVCAN_TIMER_OVERRIDE is not set
-CONFIG_COMMON_DIFFERENTIAL_PRESSURE=n
 CONFIG_COMMON_HYGROMETERS=n
 CONFIG_COMMON_TELEMETRY=n
 CONFIG_DRIVERS_ADC_ADS1115=n
@@ -0,0 +1,6 @@
+CONFIG_DRIVERS_HEATER=n
+CONFIG_DRIVERS_OSD=n
+CONFIG_DRIVERS_UAVCAN=n
+CONFIG_DRIVERS_UAVCAN_V1=y
+CONFIG_UAVCAN_V1_BMS_SUBSCRIBER=y
+CONFIG_UAVCAN_V1_UORB_SENSOR_GPS_SUBSCRIBER=y
@@ -37,7 +37,6 @@ CONFIG_DRIVERS_SAFETY_BUTTON=y
 CONFIG_COMMON_TELEMETRY=y
 CONFIG_DRIVERS_TONE_ALARM=y
 CONFIG_DRIVERS_UAVCAN=y
-CONFIG_BOARD_UAVCAN_TIMER_OVERRIDE=2
 CONFIG_MODULES_AIRSPEED_SELECTOR=y
 CONFIG_MODULES_CAMERA_FEEDBACK=y
 CONFIG_MODULES_COMMANDER=y
@@ -164,7 +164,6 @@ set(msg_files
 	timesync.msg
 	timesync_status.msg
 	trajectory_bezier.msg
-	trajectory_setpoint.msg
 	trajectory_waypoint.msg
 	transponder_report.msg
 	tune_control.msg
@@ -4,9 +4,7 @@ uint64 timestamp_sample	    # the timestamp the data this control response is ba

 uint16 reversible_flags     # bitset which motors are configured to be reversible

-uint8 ACTUATOR_FUNCTION_MOTOR1 = 101
-
-uint8 NUM_CONTROLS = 12
-float32[12] control # range: [-1, 1], where 1 means maximum positive thrust,
-                    # -1 maximum negative (if not supported by the output, <0 maps to NaN),
-                    # and NaN maps to disarmed (stop the motors)
+uint8 NUM_CONTROLS = 8
+float32[8] control # range: [-1, 1], where 1 means maximum positive thrust,
+                   # -1 maximum negative (if not supported by the output, <0 maps to NaN),
+                   # and NaN maps to disarmed (stop the motors)
@@ -7,7 +7,7 @@ uint8 ACTION_RELEASE_CONTROL = 0	# exit test mode for the given function
 uint8 ACTION_DO_CONTROL = 1			# enable actuator test mode

 uint8 FUNCTION_MOTOR1 = 101
-uint8 MAX_NUM_MOTORS  = 12
+uint8 MAX_NUM_MOTORS  = 8
 uint8 FUNCTION_SERVO1 = 201
 uint8 MAX_NUM_SERVOS  = 8

@@ -19,5 +19,3 @@ int8 ACTUATOR_SATURATION_LOWER     = -2 # The actuator is saturated (with a valu
 int8[16] actuator_saturation            # Indicates actuator saturation status.
                                        # Note 1: actuator saturation does not necessarily imply that the thrust setpoint or the torque setpoint were not achieved.
                                        # Note 2: an actuator with limited dynamics can be indicated as upper-saturated even if it as not reached its maximum value.
-
-uint16 handled_motor_failure_mask        # Bitmask of failed motors that were removed from the allocation / effectiveness matrix. Not necessarily identical to the report from FailureDetector
@@ -8,8 +8,6 @@ uint8 esc_address					# Address of current ESC (in most cases 1-8 / must be set

 uint8 esc_state					# State of ESC - depend on Vendor

-uint8 actuator_function				# actuator output function (one of Motor1...MotorN)
-
 uint16 failures					# Bitmask to indicate the internal ESC faults

 uint8 FAILURE_OVER_CURRENT = 0 			# (1 << 0)
@@ -6,9 +6,8 @@ bool fd_pitch
 bool fd_alt
 bool fd_ext
 bool fd_arm_escs
+bool fd_high_wind
 bool fd_battery
 bool fd_imbalanced_prop
-bool fd_motor

 float32 imbalanced_prop_metric      # Metric of the imbalanced propeller check (low-passed)
-uint16 motor_failure_mask           # Bit-mask with motor indices, indicating critical motor failures
@@ -2,24 +2,22 @@
 # These fields are scaled and offset-compensated where possible and do not
 # change with board revisions and sensor updates.

-uint64 timestamp                                # time since system start (microseconds)
+uint64 timestamp				# time since system start (microseconds)

-int32 RELATIVE_TIMESTAMP_INVALID = 2147483647   # (0x7fffffff) If one of the relative timestamps is set to this value, it means the associated sensor values are invalid
+int32 RELATIVE_TIMESTAMP_INVALID = 2147483647	# (0x7fffffff) If one of the relative timestamps is set to this value, it means the associated sensor values are invalid

 # gyro timstamp is equal to the timestamp of the message
-float32[3] gyro_rad                     # average angular rate measured in the FRD body frame XYZ-axis in rad/s over the last gyro sampling period
-uint32 gyro_integral_dt                 # gyro measurement sampling period in microseconds
+float32[3] gyro_rad			# average angular rate measured in the FRD body frame XYZ-axis in rad/s over the last gyro sampling period
+uint32 gyro_integral_dt		# gyro measurement sampling period in microseconds

-int32 accelerometer_timestamp_relative  # timestamp + accelerometer_timestamp_relative = Accelerometer timestamp
-float32[3] accelerometer_m_s2           # average value acceleration measured in the FRD body frame XYZ-axis in m/s^2 over the last accelerometer sampling period
-uint32 accelerometer_integral_dt        # accelerometer measurement sampling period in microseconds
+int32 accelerometer_timestamp_relative	# timestamp + accelerometer_timestamp_relative = Accelerometer timestamp
+float32[3] accelerometer_m_s2		# average value acceleration measured in the FRD body frame XYZ-axis in m/s^2 over the last accelerometer sampling period
+uint32 accelerometer_integral_dt	# accelerometer measurement sampling period in microseconds

 uint8 CLIPPING_X = 1
 uint8 CLIPPING_Y = 2
 uint8 CLIPPING_Z = 4
+uint8 accelerometer_clipping            # bitfield indicating if there was any accelerometer clipping (per axis) during the sampling period

-uint8 accelerometer_clipping    # bitfield indicating if there was any accelerometer clipping (per axis) during the integration time frame
-uint8 gyro_clipping             # bitfield indicating if there was any gyro clipping (per axis) during the integration time frame
-
-uint8 accel_calibration_count   # Calibration changed counter. Monotonically increases whenever accelermeter calibration changes.
-uint8 gyro_calibration_count    # Calibration changed counter. Monotonically increases whenever rate gyro calibration changes.
+uint8 accel_calibration_count  	# Calibration changed counter. Monotonically increases whenever accelermeter calibration changes.
+uint8 gyro_calibration_count   	# Calibration changed counter. Monotonically increases whenever rate gyro calibration changes.
@@ -11,8 +11,6 @@ float32 temperature       # temperature in degrees Celsius

 uint32 error_count

-uint8[3] clip_counter     # clip count per axis in the sample period
-
 uint8 samples             # number of raw samples that went into this message

 uint8 ORB_QUEUE_LENGTH = 8
@@ -59,7 +59,8 @@ rtps:
    send:    true
  - msg:     vehicle_local_position_setpoint
    receive: true
-  - msg:     trajectory_setpoint
+  - msg:     trajectory_setpoint # multi-topic / alias of vehicle_local_position_setpoint
+    base:    vehicle_local_position_setpoint
    receive: true
  - msg:     vehicle_odometry
    send:    true
@@ -1,15 +0,0 @@
-# Trajectory setpoint in NED frame
-# Input to PID position controller.
-# Needs to be kinematically consistent and feasible for smooth flight.
-# setting a value to NaN means the state should not be controlled
-
-uint64 timestamp # time since system start (microseconds)
-
-# NED local world frame
-float32[3] position # in meters
-float32[3] velocity # in meters/second
-float32[3] acceleration # in meters/second^2
-float32[3] jerk # in meters/second^3 (for logging only)
-
-float32 yaw # euler angle of desired attitude in radians -PI..+PI
-float32 yawspeed # angular velocity around NED frame z-axis in radians/second
@@ -1,12 +1,12 @@
 # This is similar to the mavlink message ATTITUDE_QUATERNION, but for onboard use

-uint64 timestamp                # time since system start (microseconds)
+uint64 timestamp		    # time since system start (microseconds)

-uint64 timestamp_sample         # the timestamp of the raw data (microseconds)
+uint64 timestamp_sample     # the timestamp of the raw data (microseconds)

-float32[4] q                    # Quaternion rotation from the FRD body frame to the NED earth frame
-float32[4] delta_q_reset        # Amount by which quaternion has changed during last reset
-uint8 quat_reset_counter        # Quaternion reset counter
+float32[4] q			    # Quaternion rotation from the FRD body frame to the NED earth frame
+float32[4] delta_q_reset 	# Amount by which quaternion has changed during last reset
+uint8 quat_reset_counter	# Quaternion reset counter

 # TOPICS vehicle_attitude vehicle_attitude_groundtruth vehicle_vision_attitude
 # TOPICS estimator_attitude
@@ -72,7 +72,6 @@ uint16 VEHICLE_CMD_MISSION_START = 300			# start running a mission |first_item:
 uint16 VEHICLE_CMD_ACTUATOR_TEST = 310			# Actuator testing command|value [-1,1]|timeout [s]|Empty|Empty|output function|
 uint16 VEHICLE_CMD_CONFIGURE_ACTUATOR = 311		# Actuator configuration command|configuration|Empty|Empty|Empty|output function|
 uint16 VEHICLE_CMD_COMPONENT_ARM_DISARM = 400		# Arms / Disarms a component |1 to arm, 0 to disarm
-uint16 VEHICLE_CMD_RUN_PREARM_CHECKS = 401              # Instructs system to run pre-arm checks.
 uint16 VEHICLE_CMD_INJECT_FAILURE = 420			# Inject artificial failure for testing purposes
 uint16 VEHICLE_CMD_START_RX_PAIR = 500			# Starts receiver pairing |0:Spektrum| 0:Spektrum DSM2, 1:Spektrum DSMX|
 uint16 VEHICLE_CMD_REQUEST_MESSAGE = 512    # Request to send a single instance of the specified message
@@ -8,15 +8,12 @@ uint32 gyro_device_id           # Gyroscope unique device ID for the sensor that

 float32[3] delta_angle          # delta angle about the FRD body frame XYZ-axis in rad over the integration time frame (delta_angle_dt)
 float32[3] delta_velocity       # delta velocity in the FRD body frame XYZ-axis in m/s over the integration time frame (delta_velocity_dt)
-
 uint16 delta_angle_dt           # integration period in microseconds
 uint16 delta_velocity_dt        # integration period in microseconds

 uint8 CLIPPING_X = 1
 uint8 CLIPPING_Y = 2
 uint8 CLIPPING_Z = 4
-
-uint8 delta_angle_clipping     # bitfield indicating if there was any gyro clipping (per axis) during the integration time frame
 uint8 delta_velocity_clipping   # bitfield indicating if there was any accelerometer clipping (per axis) during the integration time frame

 uint8 accel_calibration_count  	# Calibration changed counter. Monotonically increases whenever accelermeter calibration changes.
@@ -4,7 +4,6 @@ uint32 accel_device_id          # unique device ID for the sensor that does not
 uint32 gyro_device_id           # unique device ID for the sensor that does not change between power cycles

 uint32[3] accel_clipping        # total clipping per axis
-uint32[3] gyro_clipping         # total clipping per axis

 uint32 accel_error_count
 uint32 gyro_error_count
@@ -1,19 +1,18 @@
 # Local position setpoint in NED frame
-# Telemetry of PID position controller to monitor tracking.
-# NaN means the state was not controlled
+# setting something to NaN means the state should not be controlled

 uint64 timestamp	# time since system start (microseconds)

 float32 x		# in meters NED
 float32 y		# in meters NED
 float32 z		# in meters NED
-
+float32 yaw		# in radians NED -PI..+PI
+float32 yawspeed	# in radians/sec
 float32 vx		# in meters/sec
 float32 vy		# in meters/sec
 float32 vz		# in meters/sec
-
 float32[3] acceleration # in meters/sec^2
+float32[3] jerk # in meters/sec^3
 float32[3] thrust	# normalized thrust vector in NED

-float32 yaw		# in radians NED -PI..+PI
-float32 yawspeed	# in radians/sec
+# TOPICS vehicle_local_position_setpoint trajectory_setpoint
@@ -1,19 +1,25 @@
+# If you change the order, add or remove arming_state_t states make sure to update the arrays
+# in state_machine_helper.cpp as well.
 uint64 timestamp				# time since system start (microseconds)

-uint8 ARMING_STATE_DISARMED = 1
-uint8 ARMING_STATE_ARMED    = 2
-uint8 ARMING_STATE_MAX      = 3
+uint8 ARMING_STATE_INIT = 0
+uint8 ARMING_STATE_STANDBY = 1
+uint8 ARMING_STATE_ARMED = 2
+uint8 ARMING_STATE_STANDBY_ERROR = 3
+uint8 ARMING_STATE_SHUTDOWN = 4
+uint8 ARMING_STATE_IN_AIR_RESTORE = 5
+uint8 ARMING_STATE_MAX = 6

 # FailureDetector status
-uint16 FAILURE_NONE = 0
-uint16 FAILURE_ROLL = 1              # (1 << 0)
-uint16 FAILURE_PITCH = 2             # (1 << 1)
-uint16 FAILURE_ALT = 4               # (1 << 2)
-uint16 FAILURE_EXT = 8               # (1 << 3)
-uint16 FAILURE_ARM_ESC = 16          # (1 << 4)
-uint16 FAILURE_BATTERY = 32          # (1 << 5)
-uint16 FAILURE_IMBALANCED_PROP = 64  # (1 << 6)
-uint16 FAILURE_MOTOR = 128           # (1 << 7)
+uint8 FAILURE_NONE = 0
+uint8 FAILURE_ROLL = 1 	        # (1 << 0)
+uint8 FAILURE_PITCH = 2	        # (1 << 1)
+uint8 FAILURE_ALT = 4 	        # (1 << 2)
+uint8 FAILURE_EXT = 8 	        # (1 << 3)
+uint8 FAILURE_ARM_ESC = 16      # (1 << 4)
+uint8 FAILURE_HIGH_WIND = 32        # (1 << 5)
+uint8 FAILURE_BATTERY = 64          # (1 << 6)
+uint8 FAILURE_IMBALANCED_PROP = 128 # (1 << 7)

 # HIL
 uint8 HIL_STATE_OFF = 0
@@ -26,7 +32,7 @@ uint8 NAVIGATION_STATE_POSCTL = 2		# Position control mode
 uint8 NAVIGATION_STATE_AUTO_MISSION = 3		# Auto mission mode
 uint8 NAVIGATION_STATE_AUTO_LOITER = 4		# Auto loiter mode
 uint8 NAVIGATION_STATE_AUTO_RTL = 5		# Auto return to launch mode
-uint8 NAVIGATION_STATE_UNUSED3 = 8 	        # Free slot
+uint8 NAVIGATION_STATE_AUTO_LANDENGFAIL = 8 	# Auto land on engine failure
 uint8 NAVIGATION_STATE_UNUSED = 9	        # Free slot
 uint8 NAVIGATION_STATE_ACRO = 10		# Acro mode
 uint8 NAVIGATION_STATE_UNUSED1 = 11		# Free slot
@@ -63,12 +69,13 @@ uint8 system_type			# system type, contains mavlink MAV_TYPE
 uint8 system_id			# system id, contains MAVLink's system ID field
 uint8 component_id			# subsystem / component id, contains MAVLink's component ID field

-uint8 vehicle_type          # Type of vehicle (rotary wing/fixed-wing/rover/airship, see above)
+uint8 vehicle_type          # Type of vehicle (fixed-wing, rotary wing, ground)
                            # If the vehicle is a VTOL, then this value will be VEHICLE_TYPE_ROTARY_WING while flying as a multicopter,
                            # and VEHICLE_TYPE_FIXED_WING when flying as a fixed-wing

 bool is_vtol				# True if the system is VTOL capable
 bool is_vtol_tailsitter                 # True if the system performs a 90° pitch down rotation during transition from MC to FW
+bool vtol_fw_permanent_stab		# True if VTOL should stabilize attitude for fw in manual mode
 bool in_transition_mode			# True if VTOL is doing a transition
 bool in_transition_to_fw		# True if VTOL is doing a transition from MC to FW

@@ -79,10 +86,11 @@ uint8 data_link_lost_counter			# counts unique data link lost events

 bool high_latency_data_link_lost 			# Set to true if the high latency data link (eg. RockBlock Iridium 9603 telemetry module) is lost

+bool engine_failure				# Set to true if an engine failure is detected
 bool mission_failure				# Set to true if mission could not continue/finish
 bool geofence_violated

-uint16 failure_detector_status			# Bitmask containing FailureDetector status [0, 0, 0, 0, 0, FAILURE_ALT, FAILURE_PITCH, FAILURE_ROLL]
+uint8 failure_detector_status			# Bitmask containing FailureDetector status [0, 0, 0, 0, 0, FAILURE_ALT, FAILURE_PITCH, FAILURE_ROLL]

 # see SYS_STATUS mavlink message for the following
 # lower 32 bits are for the base flags, higher 32 bits are or the extended flags
@@ -2,10 +2,8 @@

 uint64 timestamp				# time since system start (microseconds)

-bool preflight_check_passing
-bool prearm_check_passing
-
 bool calibration_enabled
+bool system_sensors_initialized
 bool system_hotplug_timeout		# true if the hotplug sensor search is over
 bool auto_mission_available
 bool angular_velocity_valid
@@ -20,7 +18,6 @@ bool power_input_valid                	# set if input power is valid
 bool battery_healthy                	# set if battery is available and not low
 bool escs_error		      		# set to true if one or more ESCs reporting esc_status are offline
 bool escs_failure		      	# set to true if one or more ESCs reporting esc_status has a failure
-bool overload                           # maximum cpuload exceeded or free memory dangerously low

 bool position_reliant_on_gps
 bool position_reliant_on_optical_flow
@@ -31,6 +28,7 @@ bool flight_terminated

 bool circuit_breaker_engaged_power_check
 bool circuit_breaker_engaged_airspd_check
+bool circuit_breaker_engaged_enginefailure_check
 bool circuit_breaker_flight_termination_disabled
 bool circuit_breaker_engaged_usb_check
 bool circuit_breaker_engaged_posfailure_check	# set to true when the position valid checks have been disabled
@@ -7,6 +7,8 @@ uint8 VEHICLE_VTOL_STATE_FW = 4

 uint64 timestamp			# time since system start (microseconds)

-uint8 vehicle_vtol_state		# current state of the vtol, see VEHICLE_VTOL_STATE
-
+bool vtol_in_rw_mode			# true: vtol vehicle is in rotating wing mode
+bool vtol_in_trans_mode
+bool in_transition_to_fw		# True if VTOL is doing a transition from MC to FW
 bool vtol_transition_failsafe		# vtol in transition failsafe mode
+bool fw_permanent_stab			# In fw mode stabilize attitude even if in manual mode
@@ -55,16 +55,6 @@ endif()
 # build NuttX
 add_subdirectory(NuttX ${PX4_BINARY_DIR}/NuttX)

-# check that CONFIG_ARCH_BOARD_CUSTOM_DIR is in PX4_BOARD_DIR
-if(CONFIG_ARCH_BOARD_CUSTOM_DIR_RELPATH)
-	get_filename_component(nuttx_defconfig_root "${NUTTX_DEFCONFIG}/../.." ABSOLUTE)
-	get_filename_component(nuttx_config_from_defconfig "${NUTTX_DIR}/${CONFIG_ARCH_BOARD_CUSTOM_DIR}" ABSOLUTE)
-
-	if(NOT ${nuttx_defconfig_root} MATCHES ${nuttx_config_from_defconfig})
-		message(FATAL_ERROR "NuttX custom board directory (${CONFIG_ARCH_BOARD_CUSTOM_DIR}) isn't in board directory (${PX4_BOARD_DIR})")
-	endif()
-endif()
-
 set(nuttx_libs)
 set(SCRIPT_PREFIX)
 if("${PX4_BOARD_LABEL}" STREQUAL "bootloader")
@@ -1,221 +0,0 @@
-/****************************************************************************
- *
- *   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.
- *
- ****************************************************************************/
-
-#include "CanardHandle.hpp"
-
-#include <net/if.h>
-#include <sys/ioctl.h>
-#include <string.h>
-
-#include <px4_platform_common/log.h>
-
-#include "o1heap/o1heap.h"
-
-#include "Subscribers/BaseSubscriber.hpp"
-
-#if defined(__PX4_NUTTX)
-# if defined(CONFIG_NET_CAN)
-#  include "CanardSocketCAN.hpp"
-# elif defined(CONFIG_CAN)
-#  include "CanardNuttXCDev.hpp"
-# endif // CONFIG_CAN
-#endif // NuttX
-
-
-O1HeapInstance *cyphal_allocator{nullptr};
-
-static void *memAllocate(CanardInstance *const ins, const size_t amount) { return o1heapAllocate(cyphal_allocator, amount); }
-static void memFree(CanardInstance *const ins, void *const pointer) { o1heapFree(cyphal_allocator, pointer); }
-
-
-CanardHandle::CanardHandle(uint32_t node_id, const size_t capacity, const size_t mtu_bytes)
-{
-	_cyphal_heap = memalign(O1HEAP_ALIGNMENT, HeapSize);
-	cyphal_allocator = o1heapInit(_cyphal_heap, HeapSize, nullptr, nullptr);
-
-	if (cyphal_allocator == nullptr) {
-		PX4_ERR("o1heapInit failed with size %u", HeapSize);
-	}
-
-	_canard_instance = canardInit(&memAllocate, &memFree);
-
-	_canard_instance.node_id = node_id; // Defaults to anonymous; can be set up later at any point.
-
-	_queue = canardTxInit(capacity, mtu_bytes);
-
-#if defined(__PX4_NUTTX)
-# if defined(CONFIG_NET_CAN)
-	_can_interface = new CanardSocketCAN();
-# elif defined(CONFIG_CAN)
-	_can_interface = new CanardNuttXCDev();
-# endif // CONFIG_CAN
-#endif // NuttX
-
-}
-
-CanardHandle::~CanardHandle()
-{
-	_can_interface->close();
-	delete _can_interface;
-	_can_interface = nullptr;
-
-	delete static_cast<uint8_t *>(_cyphal_heap);
-	_cyphal_heap = nullptr;
-
-}
-
-
-bool CanardHandle::init()
-{
-	if (_can_interface) {
-		if (_can_interface->init() == PX4_OK) {
-			return true;
-		}
-	}
-
-	return false;
-}
-
-void CanardHandle::receive()
-{
-	/* Process received messages */
-
-	uint8_t data[64] {};
-	CanardRxFrame received_frame{};
-	received_frame.frame.payload = &data;
-
-	while (_can_interface->receive(&received_frame) > 0) {
-		CanardRxTransfer receive{};
-		CanardRxSubscription *subscription = nullptr;
-		int32_t result = canardRxAccept(&_canard_instance, received_frame.timestamp_usec, &received_frame.frame, 0, &receive,
-						&subscription);
-
-		if (result < 0) {
-			// An error has occurred: either an argument is invalid or we've ran out of memory.
-			// It is possible to statically prove that an out-of-memory will never occur for a given application if
-			// the heap is sized correctly; for background, refer to the Robson's Proof and the documentation for O1Heap.
-			// Reception of an invalid frame is NOT an error.
-			PX4_ERR("Receive error %" PRId32" \n", result);
-
-		} else if (result == 1) {
-			// A transfer has been received, process it.
-			// PX4_INFO("received Port ID: %d", receive.port_id);
-
-			if (subscription != nullptr) {
-				UavcanBaseSubscriber *sub_instance = (UavcanBaseSubscriber *)subscription->user_reference;
-				sub_instance->callback(receive);
-
-			} else {
-				PX4_ERR("No matching sub for %d", receive.metadata.port_id);
-			}
-
-			// Deallocate the dynamic memory afterwards.
-			_canard_instance.memory_free(&_canard_instance, (void *)receive.payload);
-
-		} else {
-			//PX4_INFO("RX canard %d", result);
-		}
-	}
-
-}
-
-void CanardHandle::transmit()
-{
-	// Look at the top of the TX queue.
-	for (const CanardTxQueueItem *ti = NULL; (ti = canardTxPeek(&_queue)) != NULL;) { // Peek at the top of the queue.
-		if ((0U == ti->tx_deadline_usec) || (ti->tx_deadline_usec > hrt_absolute_time())) { // Check the deadline.
-			// Send the frame. Redundant interfaces may be used here.
-			const int tx_res = _can_interface->transmit(*ti);
-
-			if (tx_res < 0) {
-				PX4_ERR("Transmit error %d, frame dropped, errno '%s'", tx_res, strerror(errno));
-
-			} else if (tx_res == 0) {
-				// Timeout - just exit and try again later
-				break;
-			}
-
-		}
-
-		// After the frame is transmitted or if it has timed out while waiting, pop it from the queue and deallocate:
-		_canard_instance.memory_free(&_canard_instance, canardTxPop(&_queue, ti));
-	}
-}
-
-int32_t CanardHandle::TxPush(const CanardMicrosecond             tx_deadline_usec,
-			     const CanardTransferMetadata *const metadata,
-			     const size_t                        payload_size,
-			     const void *const                   payload)
-{
-	return canardTxPush(&_queue, &_canard_instance, tx_deadline_usec, metadata, payload_size, payload);
-}
-
-int8_t CanardHandle::RxSubscribe(const CanardTransferKind    transfer_kind,
-				 const CanardPortID          port_id,
-				 const size_t                extent,
-				 const CanardMicrosecond     transfer_id_timeout_usec,
-				 CanardRxSubscription *const out_subscription)
-{
-	return canardRxSubscribe(&_canard_instance, transfer_kind, port_id, extent, transfer_id_timeout_usec, out_subscription);
-}
-
-int8_t CanardHandle::RxUnsubscribe(const CanardTransferKind transfer_kind,
-				   const CanardPortID       port_id)
-{
-	return canardRxUnsubscribe(&_canard_instance, transfer_kind, port_id);
-}
-
-CanardTreeNode *CanardHandle::getRxSubscriptions(CanardTransferKind kind)
-{
-	return _canard_instance.rx_subscriptions[kind];
-}
-
-O1HeapDiagnostics CanardHandle::getO1HeapDiagnostics()
-{
-	return o1heapGetDiagnostics(cyphal_allocator);
-}
-
-int32_t CanardHandle::mtu()
-{
-	return _queue.mtu_bytes;
-}
-
-CanardNodeID CanardHandle::node_id()
-{
-	return _canard_instance.node_id;
-}
-
-void CanardHandle::set_node_id(CanardNodeID id)
-{
-	_canard_instance.node_id = id;
-}
@@ -1,87 +0,0 @@
-/****************************************************************************
- *
- *   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.
- *
- ****************************************************************************/
-
-#pragma once
-
-#include <canard.h>
-#include "o1heap/o1heap.h"
-#include "CanardInterface.hpp"
-
-class CanardHandle
-{
-	/*
-	* This memory is allocated for the 01Heap allocator used by
-	* libcanard to store incoming/outcoming data
-	* Current size of 8192 bytes is arbitrary, should be optimized further
-	* when more nodes and messages are on the CAN bus
-	*/
-	static constexpr unsigned HeapSize = 8192;
-
-public:
-	CanardHandle(uint32_t node_id, const size_t capacity, const size_t mtu_bytes);
-	~CanardHandle();
-
-	bool init();
-
-	void receive();
-	void transmit();
-
-	int32_t TxPush(const CanardMicrosecond             tx_deadline_usec,
-		       const CanardTransferMetadata *const metadata,
-		       const size_t                        payload_size,
-		       const void *const                   payload);
-
-	int8_t RxSubscribe(const CanardTransferKind    transfer_kind,
-			   const CanardPortID          port_id,
-			   const size_t                extent,
-			   const CanardMicrosecond     transfer_id_timeout_usec,
-			   CanardRxSubscription *const out_subscription);
-	int8_t RxUnsubscribe(const CanardTransferKind transfer_kind,
-			     const CanardPortID       port_id);
-	CanardTreeNode *getRxSubscriptions(CanardTransferKind kind);
-	O1HeapDiagnostics getO1HeapDiagnostics();
-
-	int32_t mtu();
-	CanardNodeID node_id();
-	void set_node_id(CanardNodeID id);
-
-private:
-	CanardInterface *_can_interface;
-
-	CanardInstance _canard_instance;
-
-	CanardTxQueue _queue;
-
-	void *_cyphal_heap{nullptr};
-
-};
@@ -247,10 +247,7 @@ void DShot::update_telemetry_num_motors()

 	if (_mixing_output.useDynamicMixing()) {
 		for (unsigned i = 0; i < _num_outputs; ++i) {
-			if (_mixing_output.isFunctionSet(i)) {
-				_telemetry->actuator_functions[motor_count] = (uint8_t)_mixing_output.outputFunction(i);
-				++motor_count;
-			}
+			motor_count += _mixing_output.isFunctionSet(i);
 		}

 	} else {
@@ -284,26 +281,24 @@ void DShot::init_telemetry(const char *device)
 	update_telemetry_num_motors();
 }

-void DShot::handle_new_telemetry_data(const int telemetry_index, const DShotTelemetry::EscData &data)
+void DShot::handle_new_telemetry_data(const int motor_index, const DShotTelemetry::EscData &data)
 {
 	// fill in new motor data
 	esc_status_s &esc_status = _telemetry->esc_status_pub.get();

-	if (telemetry_index < esc_status_s::CONNECTED_ESC_MAX) {
-		esc_status.esc_online_flags |= 1 << telemetry_index;
+	if (motor_index < esc_status_s::CONNECTED_ESC_MAX) {
+		esc_status.esc_online_flags |= 1 << motor_index;

-		esc_status.esc[telemetry_index].actuator_function = _telemetry->actuator_functions[telemetry_index];
-		esc_status.esc[telemetry_index].timestamp       = data.time;
-		esc_status.esc[telemetry_index].esc_rpm         = (static_cast<int>(data.erpm) * 100) /
-				(_param_mot_pole_count.get() / 2);
-		esc_status.esc[telemetry_index].esc_voltage     = static_cast<float>(data.voltage) * 0.01f;
-		esc_status.esc[telemetry_index].esc_current     = static_cast<float>(data.current) * 0.01f;
-		esc_status.esc[telemetry_index].esc_temperature = static_cast<float>(data.temperature);
+		esc_status.esc[motor_index].timestamp       = data.time;
+		esc_status.esc[motor_index].esc_rpm         = (static_cast<int>(data.erpm) * 100) / (_param_mot_pole_count.get() / 2);
+		esc_status.esc[motor_index].esc_voltage     = static_cast<float>(data.voltage) * 0.01f;
+		esc_status.esc[motor_index].esc_current     = static_cast<float>(data.current) * 0.01f;
+		esc_status.esc[motor_index].esc_temperature = static_cast<float>(data.temperature);
 		// TODO: accumulate consumption and use for battery estimation
 	}

 	// publish when motor index wraps (which is robust against motor timeouts)
-	if (telemetry_index <= _telemetry->last_telemetry_index) {
+	if (motor_index <= _telemetry->last_motor_index) {
 		esc_status.timestamp = hrt_absolute_time();
 		esc_status.esc_connectiontype = esc_status_s::ESC_CONNECTION_TYPE_DSHOT;
 		esc_status.esc_count = _telemetry->handler.numMotors();
@@ -319,7 +314,7 @@ void DShot::handle_new_telemetry_data(const int telemetry_index, const DShotTele
 		esc_status.esc_online_flags = 0;
 	}

-	_telemetry->last_telemetry_index = telemetry_index;
+	_telemetry->last_motor_index = motor_index;
 }

 int DShot::send_command_thread_safe(const dshot_command_t command, const int num_repetitions, const int motor_index)
@@ -455,44 +450,29 @@ bool DShot::updateOutputs(bool stop_motors, uint16_t outputs[MAX_ACTUATORS],
 	} else {
 		int telemetry_index = 0;

+		const uint32_t reversible_outputs = _mixing_output.reversibleOutputs();
+
 		for (int i = 0; i < (int)num_outputs; i++) {

 			uint16_t output = outputs[i];

+			// DShot 3D splits the throttle ranges in two.
+			// This is in terms of DShot values, code below is in terms of actuator_output
+			// Direction 1) 48 is the slowest, 1047 is the fastest.
+			// Direction 2) 1049 is the slowest, 2047 is the fastest.
+			if (_param_dshot_3d_enable.get() || (reversible_outputs & (1u << i))) {
+				if (output >= _param_dshot_3d_dead_l.get() && output <= _param_dshot_3d_dead_h.get()) {
+					output = DSHOT_DISARM_VALUE;
+
+				} else if (output < 1000 && output > 0) { //Todo: allow actuator 0 or dshot 48 to be used
+					output = 999 - output;
+				}
+			}
+
 			if (output == DSHOT_DISARM_VALUE) {
 				up_dshot_motor_command(i, DShot_cmd_motor_stop, telemetry_index == requested_telemetry_index);

 			} else {
-
-				// DShot 3D splits the throttle ranges in two.
-				// This is in terms of DShot values, code below is in terms of actuator_output
-				// Direction 1) 48 is the slowest, 1047 is the fastest.
-				// Direction 2) 1049 is the slowest, 2047 is the fastest.
-				if (_param_dshot_3d_enable.get() || (_reversible_outputs & (1u << i))) {
-					if (output >= _param_dshot_3d_dead_l.get() && output < _param_dshot_3d_dead_h.get()) {
-						output = DSHOT_DISARM_VALUE;
-
-					} else {
-						bool upper_range = output >= 1000;
-
-						if (upper_range) {
-							output -= 1000;
-
-						} else {
-							output = 999 - output; // lower range is inverted
-						}
-
-						float max_output = 999.f;
-						float min_output = max_output * _param_dshot_min.get();
-						output = math::min(max_output, (min_output + output * (max_output - min_output) / max_output));
-
-						if (upper_range) {
-							output += 1000;
-						}
-
-					}
-				}
-
 				up_dshot_motor_data_set(i, math::min(output, static_cast<uint16_t>(DSHOT_MAX_THROTTLE)),
 							telemetry_index == requested_telemetry_index);
 			}
@@ -513,6 +493,8 @@ bool DShot::updateOutputs(bool stop_motors, uint16_t outputs[MAX_ACTUATORS],

 void DShot::Run()
 {
+	SmartLock lock_guard(_lock);
+
 	if (should_exit()) {
 		ScheduleClear();
 		_mixing_output.unregister();
@@ -521,8 +503,6 @@ void DShot::Run()
 		return;
 	}

-	SmartLock lock_guard(_lock);
-
 	perf_begin(_cycle_perf);

 	_mixing_output.update();
@@ -571,13 +551,6 @@ void DShot::Run()

 	handle_vehicle_commands();

-	if (!_mixing_output.armed().armed) {
-		if (_reversible_outputs != _mixing_output.reversibleOutputs()) {
-			_reversible_outputs = _mixing_output.reversibleOutputs();
-			update_params();
-		}
-	}
-
 	// check at end of cycle (updateSubscriptions() can potentially change to a different WorkQueue thread)
 	_mixing_output.updateSubscriptions(true);

@@ -671,13 +644,6 @@ void DShot::update_params()
 	_mixing_output.setAllMinValues(math::constrain(static_cast<int>((_param_dshot_min.get() *
 				       static_cast<float>(DSHOT_MAX_THROTTLE))),
 				       DSHOT_MIN_THROTTLE, DSHOT_MAX_THROTTLE));
-
-	// Do not use the minimum parameter for reversible outputs
-	for (unsigned i = 0; i < _num_outputs; ++i) {
-		if ((1 << i) & _reversible_outputs) {
-			_mixing_output.minValue(i) = DSHOT_MIN_THROTTLE;
-		}
-	}
 }

 int DShot::ioctl(file *filp, int cmd, unsigned long arg)
@@ -127,15 +127,14 @@ private:
 	struct Telemetry {
 		DShotTelemetry handler{};
 		uORB::PublicationMultiData<esc_status_s> esc_status_pub{ORB_ID(esc_status)};
-		int last_telemetry_index{-1};
-		uint8_t actuator_functions[esc_status_s::CONNECTED_ESC_MAX] {};
+		int last_motor_index{-1};
 	};

 	void enable_dshot_outputs(const bool enabled);

 	void init_telemetry(const char *device);

-	void handle_new_telemetry_data(const int telemetry_index, const DShotTelemetry::EscData &data);
+	void handle_new_telemetry_data(const int motor_index, const DShotTelemetry::EscData &data);

 	int request_esc_info();

@@ -148,7 +147,6 @@ private:
 	void handle_vehicle_commands();

 	MixingOutput _mixing_output {PARAM_PREFIX, DIRECT_PWM_OUTPUT_CHANNELS, *this, MixingOutput::SchedulingPolicy::Auto, false, false};
-	uint32_t _reversible_outputs{};

 	Telemetry *_telemetry{nullptr};

@@ -1,6 +1,6 @@
 ############################################################################
 #
-#   Copyright (c) 2021-2022 PX4 Development Team. All rights reserved.
+#   Copyright (c) 2021 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
@@ -1,6 +1,6 @@
 /****************************************************************************
 *
- *   Copyright (c) 2021-2022 PX4 Development Team. All rights reserved.
+ *   Copyright (c) 2021 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
@@ -47,7 +47,7 @@ ICM42670P::ICM42670P(const I2CSPIDriverConfig &config):
 	_px4_accel(get_device_id(), config.rotation),
 	_px4_gyro(get_device_id(), config.rotation)
 {
-	if (config.drdy_gpio != 0) {
+	if (_drdy_gpio != 0) {
 		_drdy_missed_perf = perf_alloc(PC_COUNT, MODULE_NAME": DRDY missed");
 	}

@@ -123,7 +123,7 @@ void ICM42670P::RunImpl()

 	switch (_state) {
 	case STATE::RESET:
-		// SIGNAL_PATH_RESET: Software Reset (auto clear bit)
+		// DEVICE_CONFIG: Software reset configuration
 		RegisterWrite(Register::BANK_0::SIGNAL_PATH_RESET, SIGNAL_PATH_RESET_BIT::SOFT_RESET_DEVICE_CONFIG);
 		_reset_timestamp = now;
 		_failure_count = 0;
@@ -133,7 +133,7 @@ void ICM42670P::RunImpl()

 	case STATE::WAIT_FOR_RESET:
 		if ((RegisterRead(Register::BANK_0::WHO_AM_I) == WHOAMI)
-		    && (RegisterRead(Register::BANK_0::SIGNAL_PATH_RESET) == 0x00)
+		    && (RegisterRead(Register::BANK_0::DEVICE_CONFIG) == 0x04)
 		    && (RegisterRead(Register::BANK_0::INT_STATUS) & INT_STATUS_BIT::RESET_DONE_INT)) {

 			// Wakeup accel and gyro and schedule remaining configuration
@@ -190,19 +190,15 @@ void ICM42670P::RunImpl()
 		break;

 	case STATE::FIFO_READ: {
-			hrt_abstime timestamp_sample = now;
-			uint8_t samples = 0;
+			uint32_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;
+				// scheduled from interrupt if _drdy_fifo_read_samples was set as expected
+				if (_drdy_fifo_read_samples.fetch_and(0) != _fifo_gyro_samples) {
+					perf_count(_drdy_missed_perf);

 				} else {
-					perf_count(_drdy_missed_perf);
+					samples = _fifo_gyro_samples;
 				}

 				// push backup schedule back
@@ -213,6 +209,8 @@ void ICM42670P::RunImpl()
 				// check current FIFO count
 				const uint16_t fifo_count = FIFOReadCount();

+				// PX4_WARN("fifo_count = %d ",fifo_count);
+
 				if (fifo_count >= FIFO::SIZE) {
 					FIFOReset();
 					perf_count(_fifo_overflow_perf);
@@ -224,17 +222,12 @@ void ICM42670P::RunImpl()
 					// 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;
+						// PX4_WARN("samples 1  = %d ",samples);
 					}
 				}
 			}
@@ -242,9 +235,12 @@ void ICM42670P::RunImpl()
 			bool success = false;

 			if (samples >= 1) {
-				if (FIFORead(timestamp_sample, samples)) {
+				// PX4_WARN("samples 2  = %d ",samples);
+				if (FIFORead(now, samples)) {
 					success = true;

+					// PX4_WARN("success  = %d ",success);
+
 					if (_failure_count > 0) {
 						_failure_count--;
 					}
@@ -264,11 +260,9 @@ void ICM42670P::RunImpl()
 			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_mreg1_cfg[_checked_register_mreg1])
 				   ) {
 					_last_config_check_timestamp = now;
 					_checked_register_bank0 = (_checked_register_bank0 + 1) % size_register_bank0_cfg;
-					_checked_register_mreg1 = (_checked_register_mreg1 + 1) % size_register_mreg1_cfg;

 				} else {
 					// register check failed, force reset
@@ -289,8 +283,68 @@ void ICM42670P::RunImpl()
 	}
 }

+void ICM42670P::ConfigureAccel()
+{
+	const uint8_t ACCEL_FS_SEL = RegisterRead(Register::BANK_0::ACCEL_CONFIG0) & (Bit7 | Bit6 | Bit5); // 7:5 ACCEL_FS_SEL
+
+	switch (ACCEL_FS_SEL) {
+	case ACCEL_FS_SEL_2G:
+		_px4_accel.set_scale(CONSTANTS_ONE_G / 16384.f);
+		_px4_accel.set_range(2.f * CONSTANTS_ONE_G);
+		break;
+
+	case ACCEL_FS_SEL_4G:
+		_px4_accel.set_scale(CONSTANTS_ONE_G / 8192.f);
+		_px4_accel.set_range(4.f * CONSTANTS_ONE_G);
+		break;
+
+	case ACCEL_FS_SEL_8G:
+		_px4_accel.set_scale(CONSTANTS_ONE_G / 4096.f);
+		_px4_accel.set_range(8.f * CONSTANTS_ONE_G);
+		break;
+
+	case ACCEL_FS_SEL_16G:
+		_px4_accel.set_scale(CONSTANTS_ONE_G / 2048.f);
+		_px4_accel.set_range(16.f * CONSTANTS_ONE_G);
+		break;
+	}
+}
+
+void ICM42670P::ConfigureGyro()
+{
+	const uint8_t GYRO_FS_SEL = RegisterRead(Register::BANK_0::GYRO_CONFIG0) & (Bit7 | Bit6 | Bit5); // 7:5 GYRO_FS_SEL
+
+	float range_dps = 0.f;
+
+	switch (GYRO_FS_SEL) {
+
+	case GYRO_FS_SEL_250_DPS:
+		range_dps = 250.f;
+		break;
+
+	case GYRO_FS_SEL_500_DPS:
+		range_dps = 500.f;
+		break;
+
+	case GYRO_FS_SEL_1000_DPS:
+		range_dps = 1000.f;
+		break;
+
+	case GYRO_FS_SEL_2000_DPS:
+		range_dps = 2000.f;
+		break;
+	}
+
+	_px4_gyro.set_scale(math::radians(range_dps / 32768.f));
+	_px4_gyro.set_range(math::radians(range_dps));
+}
+
 void ICM42670P::ConfigureSampleRate(int sample_rate)
 {
+	if (sample_rate == 0) {
+		sample_rate = 800; // default to 800 Hz
+	}
+
 	// 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);
@@ -320,6 +374,8 @@ void ICM42670P::ConfigureFIFOWatermark(uint8_t samples)
 	}
 }

+
+
 bool ICM42670P::Configure()
 {
 	// first set and clear all configured register bits
@@ -327,9 +383,7 @@ bool ICM42670P::Configure()
 		RegisterSetAndClearBits(reg_cfg.reg, reg_cfg.set_bits, reg_cfg.clear_bits);
 	}

-	for (const auto &reg_cfg : _register_mreg1_cfg) {
-		RegisterSetAndClearBits(reg_cfg.reg, reg_cfg.set_bits, reg_cfg.clear_bits);
-	}
+	Mreg1Config();

 	// now check that all are configured
 	bool success = true;
@@ -340,17 +394,11 @@ bool ICM42670P::Configure()
 		}
 	}

-	for (const auto &reg_cfg : _register_mreg1_cfg) {
-		if (!RegisterCheck(reg_cfg)) {
-			success = false;
-		}
-	}
+	success = Mreg1Check();

-	_px4_accel.set_scale(CONSTANTS_ONE_G / 2048.f);
-	_px4_accel.set_range(16.f * CONSTANTS_ONE_G);

-	_px4_gyro.set_scale(math::radians(2000.f / 32768.f));
-	_px4_gyro.set_range(math::radians(2000.f));
+	ConfigureAccel();
+	ConfigureGyro();

 	return success;
 }
@@ -363,8 +411,11 @@ int ICM42670P::DataReadyInterruptCallback(int irq, void *context, void *arg)

 void ICM42670P::DataReady()
 {
-	_drdy_timestamp_sample.store(hrt_absolute_time());
-	ScheduleNow();
+	uint32_t expected = 0;
+
+	if (_drdy_fifo_read_samples.compare_exchange(&expected, _fifo_gyro_samples)) {
+		ScheduleNow();
+	}
 }

 bool ICM42670P::DataReadyInterruptConfigure()
@@ -394,19 +445,18 @@ bool ICM42670P::RegisterCheck(const T &reg_cfg)
 	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;
 }

-uint8_t ICM42670P::RegisterRead(Register::BANK_0 reg)
+template <typename T>
+uint8_t ICM42670P::RegisterRead(T reg)
 {
 	uint8_t cmd[2] {};
 	cmd[0] = static_cast<uint8_t>(reg) | DIR_READ;
@@ -414,50 +464,14 @@ uint8_t ICM42670P::RegisterRead(Register::BANK_0 reg)
 	return cmd[1];
 }

-uint8_t ICM42670P::RegisterRead(Register::MREG1 reg)
-{
-	// BLK_SEL_R must be set to 0
-	RegisterWrite(Register::BANK_0::BLK_SEL_R, 0x00);
-
-	// MADDR_R must be set to 0x14 (address of the MREG1 register being accessed)
-	RegisterWrite(Register::BANK_0::MADDR_R, (uint8_t)reg);
-
-	// Wait for 10 µs
-	px4_udelay(10);
-
-	// Read register M_R to access the value in MREG1 register 0x14
-	uint8_t value = RegisterRead(Register::BANK_0::M_R);
-
-	// Wait for 10 µs
-	// Host must not access any other register for 10 µs once MREG1, MREG2 or MREG3 access is kicked off.
-	px4_udelay(10);
-
-	return value;
-}
-
-void ICM42670P::RegisterWrite(Register::BANK_0 reg, uint8_t value)
+template <typename T>
+void ICM42670P::RegisterWrite(T reg, uint8_t value)
 {
 	uint8_t cmd[2] { (uint8_t)reg, value };
+
 	transfer(cmd, cmd, sizeof(cmd));
 }

-void ICM42670P::RegisterWrite(Register::MREG1 reg, uint8_t value)
-{
-	// BLK_SEL_W must be set to 0
-	RegisterWrite(Register::BANK_0::BLK_SEL_W, 0x00);
-
-	// MADDR_W must be set to address of the MREG1 register being accessed
-	RegisterWrite(Register::BANK_0::MADDR_W, (uint8_t)reg);
-
-	// M_W must be set to the desired value
-	RegisterWrite(Register::BANK_0::M_W, value);
-
-	// Wait for 10 µs
-	// Host must not access any other register for 10 µs once MREG1, MREG2 or MREG3 access is kicked off.
-	px4_udelay(10);
-}
-
-
 template <typename T>
 void ICM42670P::RegisterSetAndClearBits(T reg, uint8_t setbits, uint8_t clearbits)
 {
@@ -484,10 +498,12 @@ uint16_t ICM42670P::FIFOReadCount()
 	return combine(fifo_count_buf[1], fifo_count_buf[2]);
 }

+
+
 bool ICM42670P::FIFORead(const hrt_abstime &timestamp_sample, uint8_t samples)
 {
 	FIFOTransferBuffer buffer{};
-	const size_t transfer_size = math::min(samples * sizeof(FIFO::DATA) + 6, FIFO::SIZE);
+	const size_t transfer_size = math::min(samples * sizeof(FIFO::DATA) + 4 + 2, FIFO::SIZE);

 	if (transfer((uint8_t *)&buffer, (uint8_t *)&buffer, transfer_size) != PX4_OK) {
 		perf_count(_bad_transfer_perf);
@@ -502,6 +518,7 @@ bool ICM42670P::FIFORead(const hrt_abstime &timestamp_sample, uint8_t samples)

 	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();
@@ -535,18 +552,6 @@ bool ICM42670P::FIFORead(const hrt_abstime &timestamp_sample, uint8_t samples)
 		} 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_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) {
@@ -558,6 +563,7 @@ bool ICM42670P::FIFORead(const hrt_abstime &timestamp_sample, uint8_t samples)
 		}
 	}

+	// PX4_WARN("valid_samples  = %d ",valid_samples);
 	if (valid_samples > 0) {
 		ProcessGyro(timestamp_sample, buffer.f, valid_samples);
 		ProcessAccel(timestamp_sample, buffer.f, valid_samples);
@@ -571,30 +577,18 @@ void ICM42670P::FIFOReset()
 {
 	perf_count(_fifo_reset_perf);

-	// FIFO flush requires the following programming sequence:
-	//  Write FIFO_FLUSH = 1
-	//  Wait for 1.5 µs
-	//  Read FIFO_FLUSH, it should now be 0
-
 	// SIGNAL_PATH_RESET: FIFO flush
 	RegisterSetBits(Register::BANK_0::SIGNAL_PATH_RESET, SIGNAL_PATH_RESET_BIT::FIFO_FLUSH);
-	px4_udelay(2); // Wait for 1.5 µs
-
-	const uint8_t SIGNAL_PATH_RESET = RegisterRead(Register::BANK_0::SIGNAL_PATH_RESET);
-
-	if ((SIGNAL_PATH_RESET & SIGNAL_PATH_RESET_BIT::FIFO_FLUSH) != 0) {
-		PX4_DEBUG("SIGNAL_PATH_RESET FIFO_FLUSH failed");
-	}

 	// reset while FIFO is disabled
-	_drdy_timestamp_sample.store(0);
+	_drdy_fifo_read_samples.store(0);
 }

 void ICM42670P::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 = samples;
+	accel.samples = 0;
 	accel.dt = FIFO_SAMPLE_DT;

 	for (int i = 0; i < samples; i++) {
@@ -604,15 +598,18 @@ void ICM42670P::ProcessAccel(const hrt_abstime &timestamp_sample, const FIFO::DA

 		// 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;
-		accel.y[i] = math::negate(accel_y);
-		accel.z[i] = math::negate(accel_z);
+		accel.x[accel.samples] = accel_x;
+		accel.y[accel.samples] = (accel_y == INT16_MIN) ? INT16_MAX : -accel_y;
+		accel.z[accel.samples] = (accel_z == INT16_MIN) ? INT16_MAX : -accel_z;
+		accel.samples++;
 	}

 	_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));

-	_px4_accel.updateFIFO(accel);
+	if (accel.samples > 0) {
+		_px4_accel.updateFIFO(accel);
+	}
 }

 void ICM42670P::ProcessGyro(const hrt_abstime &timestamp_sample, const FIFO::DATA fifo[], const uint8_t samples)
@@ -630,8 +627,8 @@ void ICM42670P::ProcessGyro(const hrt_abstime &timestamp_sample, const FIFO::DAT
 		// 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;
-		gyro.y[i] = math::negate(gyro_y);
-		gyro.z[i] = math::negate(gyro_z);
+		gyro.y[i] = (gyro_y == INT16_MIN) ? INT16_MAX : -gyro_y;
+		gyro.z[i] = (gyro_z == INT16_MIN) ? INT16_MAX : -gyro_z;
 	}

 	_px4_gyro.set_error_count(perf_event_count(_bad_register_perf) + perf_event_count(_bad_transfer_perf) +
@@ -661,3 +658,88 @@ void ICM42670P::UpdateTemperature()
 		_px4_gyro.set_temperature(TEMP_degC);
 	}
 }
+
+
+uint8_t ICM42670P::RegisterReadBank1(uint8_t reg)
+{
+	uint8_t value;
+
+	RegisterWrite((uint8_t)Register::BANK_0::BLK_SEL_R, 0x00);
+	RegisterWrite((uint8_t)Register::BANK_0::MADDR_R, reg);
+	ScheduleDelayed(10_us);
+	value = RegisterRead((uint8_t)Register::BANK_0::M_R);
+	ScheduleDelayed(10_us);
+	RegisterWrite((uint8_t)Register::BANK_0::BLK_SEL_R, 0x00);
+
+	return value;
+}
+
+void ICM42670P::RegisterWriteBank1(uint8_t reg, uint8_t value)
+{
+	RegisterWrite((uint8_t)Register::BANK_0::BLK_SEL_W, 0x00);
+	RegisterWrite((uint8_t)Register::BANK_0::MADDR_W, reg);
+	RegisterWrite((uint8_t)Register::BANK_0::M_W, value);
+	ScheduleDelayed(10_us);
+	RegisterWrite((uint8_t)Register::BANK_0::BLK_SEL_W, 0x00);
+}
+
+void ICM42670P::Mreg1Config()
+{
+	uint8_t data;
+	uint8_t set_bits;
+	uint8_t clear_bits;
+
+	clear_bits = Bit7 | Bit6 | Bit4 | Bit3;
+	set_bits = FIFO_CONFIG5_BIT::FIFO_WM_GT_TH  | FIFO_CONFIG5_BIT::FIFO_TMST_FSYNC_EN | FIFO_CONFIG5_BIT::FIFO_GYRO_EN |
+		   FIFO_CONFIG5_BIT::FIFO_ACCEL_EN;
+
+	data = RegisterReadBank1(0x01);
+	data &= ~clear_bits;
+	data |= set_bits;
+	RegisterWriteBank1(0x01, data);
+
+
+	clear_bits = Bit7 | Bit6 | Bit5 | Bit4 | Bit2 | Bit1 | Bit0;
+	set_bits = INT_CONFIG0_BIT::CLEAR_ON_FIFO_READ;
+	data = RegisterReadBank1(0x04);
+	data &= ~clear_bits;
+	data |= set_bits;
+	RegisterWriteBank1(0x04, data);
+}
+
+bool ICM42670P::Mreg1Check()
+{
+	uint8_t set_bits;
+	uint8_t clear_bits;
+	uint8_t reg_value;
+	bool success;
+	success = true;
+
+	reg_value = RegisterReadBank1(0x01);
+
+	clear_bits = Bit7 | Bit6 | Bit4 | Bit3;
+	set_bits = FIFO_CONFIG5_BIT::FIFO_WM_GT_TH  | FIFO_CONFIG5_BIT::FIFO_TMST_FSYNC_EN | FIFO_CONFIG5_BIT::FIFO_GYRO_EN |
+		   FIFO_CONFIG5_BIT::FIFO_ACCEL_EN;
+
+	if (set_bits && ((reg_value & set_bits) != set_bits)) {
+		success = false;
+	}
+
+	if (clear_bits && ((reg_value & clear_bits) != 0)) {
+		success = false;
+	}
+
+	reg_value = RegisterReadBank1(0x04);
+	clear_bits = Bit7 | Bit6 | Bit5 | Bit4 | Bit2 | Bit1 | Bit0;
+	set_bits = INT_CONFIG0_BIT::CLEAR_ON_FIFO_READ;
+
+	if (set_bits && ((reg_value & set_bits) != set_bits)) {
+		success = false;
+	}
+
+	if (clear_bits && ((reg_value & clear_bits) != 0)) {
+		success = false;
+	}
+
+	return success;
+}
@@ -1,6 +1,6 @@
 /****************************************************************************
 *
- *   Copyright (c) 2021-2022 PX4 Development Team. All rights reserved.
+ *   Copyright (c) 2021 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
@@ -57,7 +57,6 @@ class ICM42670P : public device::SPI, public I2CSPIDriver<ICM42670P>
 public:
 	ICM42670P(const I2CSPIDriverConfig &config);
 	~ICM42670P() override;
-
 	static void print_usage();

 	void RunImpl();
@@ -69,12 +68,12 @@ private:
 	void exit_and_cleanup() override;

 	// Sensor Configuration
-	static constexpr float FIFO_SAMPLE_DT{1e6f / 1600.f};     // 1600 Hz accel & gyro ODR configured
+	static constexpr float FIFO_SAMPLE_DT{1e6f / 800.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};

 	// 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))};
+	static constexpr uint32_t FIFO_MAX_SAMPLES{math::min(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 {
@@ -86,8 +85,10 @@ private:
 		uint8_t FIFO_COUNTL{0};
 		FIFO::DATA f[FIFO_MAX_SAMPLES] {};
 	};
+
+
 	// ensure no struct padding
-	static_assert(sizeof(FIFOTransferBuffer) == (6 + FIFO_MAX_SAMPLES *sizeof(FIFO::DATA)));
+	static_assert(sizeof(FIFOTransferBuffer) == (4 + 2 + FIFO_MAX_SAMPLES *sizeof(FIFO::DATA)));

 	struct register_bank0_config_t {
 		Register::BANK_0 reg;
@@ -95,17 +96,13 @@ private:
 		uint8_t clear_bits{0};
 	};

-	struct register_mreg1_config_t {
-		Register::MREG1 reg;
-		uint8_t set_bits{0};
-		uint8_t clear_bits{0};
-	};
-
 	int probe() override;

 	bool Reset();

 	bool Configure();
+	void ConfigureAccel();
+	void ConfigureGyro();
 	void ConfigureSampleRate(int sample_rate);
 	void ConfigureFIFOWatermark(uint8_t samples);

@@ -114,13 +111,9 @@ private:
 	bool DataReadyInterruptConfigure();
 	bool DataReadyInterruptDisable();

-	uint8_t RegisterRead(Register::BANK_0 reg);
-	uint8_t RegisterRead(Register::MREG1 reg);
-
-	void RegisterWrite(Register::BANK_0 reg, uint8_t value);
-	void RegisterWrite(Register::MREG1 reg, uint8_t value);
-
 	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); }
@@ -133,6 +126,12 @@ private:
 	void ProcessGyro(const hrt_abstime &timestamp_sample, const FIFO::DATA fifo[], const uint8_t samples);
 	void UpdateTemperature();

+	uint8_t RegisterReadBank1(uint8_t reg);
+	void RegisterWriteBank1(uint8_t reg, uint8_t value);
+
+	void Mreg1Config();
+	bool Mreg1Check();
+
 	const spi_drdy_gpio_t _drdy_gpio;

 	PX4Accelerometer _px4_accel;
@@ -150,7 +149,9 @@ private:
 	hrt_abstime _temperature_update_timestamp{0};
 	int _failure_count{0};

-	px4::atomic<hrt_abstime> _drdy_timestamp_sample{0};
+	enum REG_BANK_SEL_BIT _last_register_bank {REG_BANK_SEL_BIT::USER_BANK_0};
+
+	px4::atomic<uint32_t> _drdy_fifo_read_samples{0};
 	bool _data_ready_interrupt_enabled{false};

 	enum class STATE : uint8_t {
@@ -158,32 +159,26 @@ private:
 		WAIT_FOR_RESET,
 		CONFIGURE,
 		FIFO_READ,
-	} _state{STATE::RESET};
+	};
+
+	STATE _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))};
+	uint32_t _fifo_gyro_samples{static_cast<uint32_t>(_fifo_empty_interval_us / (1000000 / GYRO_RATE))};

 	uint8_t _checked_register_bank0{0};
 	static constexpr uint8_t size_register_bank0_cfg{10};
 	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::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_2000_DPS_SET | GYRO_CONFIG0_BIT::GYRO_ODR_1600HZ_SET, GYRO_CONFIG0_BIT::GYRO_FS_SEL_2000_DPS_CLEAR | GYRO_CONFIG0_BIT::GYRO_ODR_1600HZ_CLEAR },
-		{ Register::BANK_0::ACCEL_CONFIG0,        ACCEL_CONFIG0_BIT::ACCEL_UI_FS_SEL_16G_SET | ACCEL_CONFIG0_BIT::ACCEL_ODR_1600HZ_SET, ACCEL_CONFIG0_BIT::ACCEL_UI_FS_SEL_16G_SET | ACCEL_CONFIG0_BIT::ACCEL_ODR_1600HZ_CLEAR },
-		{ Register::BANK_0::GYRO_CONFIG1,         0, GYRO_CONFIG1_BIT::GYRO_UI_FILT_BW_BYPASSED_CLEAR },
-		{ Register::BANK_0::ACCEL_CONFIG1,        0, ACCEL_CONFIG1_BIT::ACCEL_UI_FILT_BW_BYPASSED_CLEAR },
-		{ Register::BANK_0::FIFO_CONFIG1,         FIFO_CONFIG1_BIT::FIFO_MODE_STOP_ON_FULL, FIFO_CONFIG1_BIT::FIFO_BYPASS },
-		{ 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_SOURCE0,          INT_SOURCE0_BIT::FIFO_THS_INT1_EN, 0 },
-	};
-
-	uint8_t _checked_register_mreg1{0};
-	static constexpr uint8_t size_register_mreg1_cfg{2};
-	register_mreg1_config_t _register_mreg1_cfg[size_register_mreg1_cfg] {
-		// Register                              | Set bits, Clear bits
-		{ Register::MREG1::FIFO_CONFIG5,          FIFO_CONFIG5_BIT::FIFO_GYRO_EN | FIFO_CONFIG5_BIT::FIFO_ACCEL_EN, 0 },
-		{ Register::MREG1::INT_CONFIG0,           INT_CONFIG0_BIT::FIFO_THS_INT_CLEAR, 0 },
+		// 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_CONFIG1,   FIFO_CONFIG1_BIT::FIFO_MODE_STOP_ON_FULL, Bit0 },
+		{ 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_ODR_1600Hz | GYRO_CONFIG0_BIT::GYRO_FS_SEL_2000_DPS, Bit7 | Bit3 | Bit0 },
+		{ Register::BANK_0::ACCEL_CONFIG0, ACCEL_CONFIG0_BIT::ACCEL_ODR_1600Hz | ACCEL_CONFIG0_BIT::ACCEL_FS_SEL_16G, Bit6 | Bit5 | Bit3 | Bit0 },
+		{ Register::BANK_0::GYRO_CONFIG1,  GYRO_CONFIG1_BIT::GYRO_UI_FILT_BW_34Hz, Bit1 },
+		{ Register::BANK_0::ACCEL_CONFIG1, ACCEL_CONFIG1_BIT::ACCEL_UI_FILT_BW_34Hz, Bit1 },
+		{ 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_SOURCE0,   INT_SOURCE0_BIT::FIFO_THS_INT1_EN, 0 },
 	};
 };
@@ -1,6 +1,6 @@
 /****************************************************************************
 *
- *   Copyright (c) 2021-2022 PX4 Development Team. All rights reserved.
+ *   Copyright (c) 2021 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
@@ -54,7 +54,7 @@ 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 uint32_t SPI_SPEED = 12 * 1000 * 1000; // 24 MHz SPI
 static constexpr uint8_t DIR_READ = 0x80;

 static constexpr uint8_t WHOAMI = 0x67;
@@ -66,13 +66,21 @@ namespace Register
 {

 enum class BANK_0 : uint8_t {
-	SIGNAL_PATH_RESET = 0x02,
+	DEVICE_CONFIG     = 0x01,

 	INT_CONFIG        = 0x06,

+
 	TEMP_DATA1        = 0x09,
 	TEMP_DATA0        = 0x0A,

+	INT_STATUS        = 0x3A,
+	FIFO_COUNTH       = 0x3D,
+	FIFO_COUNTL       = 0x3E,
+	FIFO_DATA         = 0x3F,
+
+	SIGNAL_PATH_RESET = 0x02,
+
 	PWR_MGMT0         = 0x1F,
 	GYRO_CONFIG0      = 0x20,
 	ACCEL_CONFIG0     = 0x21,
@@ -82,93 +90,87 @@ enum class BANK_0 : uint8_t {
 	FIFO_CONFIG1      = 0x28,
 	FIFO_CONFIG2      = 0x29,
 	FIFO_CONFIG3      = 0x2A,
+
 	INT_SOURCE0       = 0x2B,

-	INT_STATUS        = 0x3A,
-	FIFO_COUNTH       = 0x3D,
-	FIFO_COUNTL       = 0x3E,
-	FIFO_DATA         = 0x3F,
-
 	WHO_AM_I          = 0x75,
+	// REG_BANK_SEL      = 0x76,

 	BLK_SEL_W         = 0x79,
 	MADDR_W           = 0x7A,
 	M_W               = 0x7B,
+
 	BLK_SEL_R         = 0x7C,
 	MADDR_R           = 0x7D,
 	M_R               = 0x7E,
+
 };

-enum class MREG1 : uint8_t {
-	FIFO_CONFIG5 = 0x01,
-
-	INT_CONFIG0  = 0x04,
+enum class MREG_1 : uint8_t {
+	FIFO_CONFIG5_MREG1      = 0x01,
+	INT_CONFIG0_MREG1       = 0x04,
 };

+enum class MREG_2 : uint8_t {
+	OTP_CTRL7_MREG2  = 0x06,
+};
+
+enum class MREG_3 : uint8_t {
+	XA_ST_DATA_MREG3 = 0x00,
+	YA_ST_DATA_MREG3 = 0x01,
+	ZA_ST_DATA_MREG3 = 0x02,
+	XG_ST_DATA_MREG3 = 0x03,
+	YG_ST_DATA_MREG3 = 0x04,
+	ZG_ST_DATA_MREG3 = 0x05,
+};
+
+
+
 };

 //---------------- BANK0 Register bits

 // SIGNAL_PATH_RESET
 enum SIGNAL_PATH_RESET_BIT : uint8_t {
-	SOFT_RESET_DEVICE_CONFIG  = Bit4, // 1: Software reset enabled
-	FIFO_FLUSH                = Bit2, // When set to 1, FIFO will get flushed
+	SOFT_RESET_DEVICE_CONFIG  = Bit4, //
+	FIFO_FLUSH                = Bit2,
 };

 // INT_CONFIG
 enum INT_CONFIG_BIT : uint8_t {
-	INT1_MODE          = Bit2, // INT1 interrupt mode 1: Latched mode
-	INT1_DRIVE_CIRCUIT = Bit1, // INT1 drive circuit 1: Push pull
-	INT1_POLARITY      = Bit0, // INT1 interrupt polarity 0: Active low
+	INT1_MODE           = Bit2,
+	INT1_DRIVE_CIRCUIT  = Bit1,
+	INT1_POLARITY       = Bit0,
 };
-
-// PWR_MGMT0
-enum PWR_MGMT0_BIT : uint8_t {
-	// 3:2 GYRO_MODE
-	GYRO_MODE_LOW_NOISE  = Bit3 | Bit2, // 11: Places gyroscope in Low Noise (LN) Mode
-	// 1:0 ACCEL_MODE
-	ACCEL_MODE_LOW_NOISE = Bit1 | Bit0, // 11: Places accelerometer in Low Noise (LN) Mode
-};
-
-// GYRO_CONFIG0
-enum GYRO_CONFIG0_BIT : uint8_t {
-	// 6:5 GYRO_FS_SEL
-	GYRO_FS_SEL_2000_DPS_SET   = 0,           // 0b000 = ±2000dps
-	GYRO_FS_SEL_2000_DPS_CLEAR = Bit6 | Bit5,
-
-	// 3:0 GYRO_ODR
-	GYRO_ODR_1600HZ_SET        = Bit2 | Bit0, // 0101: 1600Hz
-	GYRO_ODR_1600HZ_CLEAR      = Bit3 | Bit1,
-};
-
-// ACCEL_CONFIG0
-enum ACCEL_CONFIG0_BIT : uint8_t {
-	// 6:5 ACCEL_UI_FS_SEL
-	ACCEL_UI_FS_SEL_16G_SET   = 0,           // 000: ±16g
-	ACCEL_UI_FS_SEL_16G_CLEAR = Bit6 | Bit5,
-
-	// 3:0 ACCEL_ODR
-	ACCEL_ODR_1600HZ_SET      = Bit2 | Bit0, // 0101: 1600Hz
-	ACCEL_ODR_1600HZ_CLEAR    = Bit3 | Bit1,
-};
-
 // GYRO_CONFIG1
 enum GYRO_CONFIG1_BIT : uint8_t {
-	// 2:0 GYRO_UI_FILT_BW
-	GYRO_UI_FILT_BW_BYPASSED_CLEAR = Bit2 | Bit1 | Bit0, // 000: Low pass filter bypassed
+
+	// 2:0 GYRO_ODR
+	GYRO_UI_FILT_BW_16Hz   = Bit2 | Bit1 | Bit0,   // 111: 16Hz
+	GYRO_UI_FILT_BW_25Hz   = Bit2 | Bit1,          // 110: 25Hz
+	GYRO_UI_FILT_BW_34Hz   = Bit2 | Bit0,  	// 101: 34Hz
+	GYRO_UI_FILT_BW_53Hz   = Bit2,         	// 100: 53Hz
+	GYRO_UI_FILT_BW_73Hz   = Bit1 | Bit0,  	// 011: 73Hz
+	GYRO_UI_FILT_BW_121Hz  = Bit1,        		// 010: 121Hz
+	GYRO_UI_FILT_BW_180Hz  = Bit0,        		// 001: 180Hz
 };

 // ACCEL_CONFIG1
 enum ACCEL_CONFIG1_BIT : uint8_t {
-	// 2:0 ACCEL_UI_FILT_BW
-	ACCEL_UI_FILT_BW_BYPASSED_CLEAR = Bit2 | Bit1 | Bit0, // 000: Low pass filter bypassed
-};

+	// 2:0 ACCEL_ODR
+	ACCEL_UI_FILT_BW_16Hz   = Bit2 | Bit1 | Bit0,   // 111: 16Hz
+	ACCEL_UI_FILT_BW_25Hz   = Bit2 | Bit1,          // 110: 25Hz
+	ACCEL_UI_FILT_BW_34Hz   = Bit2 | Bit0,  	// 101: 34Hz
+	ACCEL_UI_FILT_BW_53Hz   = Bit2,         	// 100: 53Hz
+	ACCEL_UI_FILT_BW_73Hz   = Bit1 | Bit0,  	// 011: 73Hz
+	ACCEL_UI_FILT_BW_121Hz  = Bit1,        		// 010: 121Hz
+	ACCEL_UI_FILT_BW_180Hz  = Bit0,        		// 001: 180Hz
+};
 // FIFO_CONFIG1
 enum FIFO_CONFIG1_BIT : uint8_t {
-	// FIFO_MODE
-	FIFO_MODE_STOP_ON_FULL = Bit1, // 1: STOP-on-FULL Mode
-	FIFO_BYPASS            = Bit0, // 0: FIFO is not bypassed
+	// 1 FIFO_MODE
+	FIFO_MODE_STOP_ON_FULL = Bit1, // 11: STOP-on-FULL Mode
 };

 // INT_STATUS
@@ -178,63 +180,113 @@ enum INT_STATUS_BIT : uint8_t {
 	FIFO_FULL_INT  = Bit1,
 };

-// INT_SOURCE0
-enum INT_SOURCE0_BIT : uint8_t {
-	RESET_DONE_INT1_EN = Bit4, // 1: Reset done interrupt routed to INT1
-	FIFO_THS_INT1_EN   = Bit2, // FIFO threshold interrupt routed to INT1
+// 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 {
+	// 6:5 GYRO_FS_SEL
+	GYRO_FS_SEL_2000_DPS = 0,            // 0b000 = ±2000dps
+	GYRO_FS_SEL_1000_DPS = Bit5,         // 0b001 = ±1000 dps
+	GYRO_FS_SEL_500_DPS  = Bit6,         // 0b010 = ±500 dps
+	GYRO_FS_SEL_250_DPS  = Bit6 | Bit5,  // 0b011 = ±250 dps

-//---------------- USER BANK MREG1 Register bits
+	// 3:0 GYRO_ODR
+	GYRO_ODR_1600Hz      = Bit2 | Bit0,         // 0101: 1600Hz
+	GYRO_ODR_800Hz       = Bit2 | Bit1,         // 0110: 800Hz
+	GYRO_ODR_400Hz       = Bit2 | Bit1 | Bit0,  // 0111: 400Hz
+	GYRO_ODR_200Hz       = Bit3,         // 1000: 200Hz
+};
+
+// ACCEL_CONFIG0
+enum ACCEL_CONFIG0_BIT : uint8_t {
+	// 6:5 ACCEL_FS_SEL
+	ACCEL_FS_SEL_16G = 0,           // 000: ±16g
+	ACCEL_FS_SEL_8G  = Bit5,        // 001: ±8g
+	ACCEL_FS_SEL_4G  = Bit6,        // 010: ±4g
+	ACCEL_FS_SEL_2G  = Bit6 | Bit5, // 011: ±2g
+
+	// 3:0 ACCEL_ODR
+	ACCEL_ODR_1600Hz  = Bit2 | Bit0,         // 0101: 1600Hz
+	ACCEL_ODR_800Hz  = Bit2 | Bit1,         // 0110: 800Hz
+	ACCEL_ODR_400Hz   = Bit2 | Bit1 | Bit0,  // 0111: 400Hz
+	ACCEL_ODR_200Hz   = Bit3,         	// 1000: 200Hz
+};

 // FIFO_CONFIG5
 enum FIFO_CONFIG5_BIT : uint8_t {
-	FIFO_WM_GT_TH = Bit5, // 0: Trigger FIFO Watermark interrupt when FIFO_COUNT = FIFO_WM
-	FIFO_GYRO_EN  = Bit1, // 1: Enables Gyro packets to go to FIFO
-	FIFO_ACCEL_EN = Bit0, // 1: Enables Accel packets to go to FIFO
+	FIFO_RESUME_PARTIAL_RD = Bit4,
+	FIFO_WM_GT_TH          = Bit5,
+	FIFO_HIRES_EN          = Bit3,
+	FIFO_TMST_FSYNC_EN     = Bit2,
+	FIFO_GYRO_EN           = Bit1,
+	FIFO_ACCEL_EN          = Bit0,
 };

 // INT_CONFIG0
 enum INT_CONFIG0_BIT : uint8_t {
 	// 3:2 FIFO_THS_INT_CLEAR
-	FIFO_THS_INT_CLEAR = Bit3, // 10: Clear on FIFO data 1Byte Read
+	CLEAR_ON_FIFO_READ = Bit3,
+};
+
+// INT_SOURCE0
+enum INT_SOURCE0_BIT : uint8_t {
+	ST_INT1_END        = Bit7,
+	FSYNC_INT1_EN      = Bit6,
+	PLL_RDY_INT1_EN    = Bit5,
+	RESET_DONE_INT1_EN = Bit4,
+	DRDY_INT1_EN       = Bit3,
+	FIFO_THS_INT1_EN   = Bit2, // FIFO threshold interrupt routed to INT1
+	FIFO_FULL_INT1_EN  = Bit1,
+	AGC_RDY_INT1_EN    = Bit0,
+};
+
+// REG_BANK_SEL
+enum REG_BANK_SEL_BIT : uint8_t {
+	USER_BANK_0 = 0,           // 0: Select USER BANK 0.
+	USER_BANK_1 = Bit4,        // 1: Select USER BANK 1.
+	USER_BANK_2 = Bit5,        // 2: Select USER BANK 2.
+	USER_BANK_3 = Bit5 | Bit4, // 3: Select USER BANK 3.
 };

 namespace FIFO
 {
-static constexpr size_t SIZE = 1024;
+static constexpr size_t SIZE = 2048;

 // FIFO_DATA layout when FIFO_CONFIG1 has FIFO_GYRO_EN and FIFO_ACCEL_EN set

 // Packet 3
 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 temperature;   // Temperature[7:0]
-	uint8_t TimeStamp_h;   // TimeStamp[15:8]
-	uint8_t TimeStamp_l;   // TimeStamp[7:0]
+	uint8_t ACCEL_DATA_X1;
+	uint8_t ACCEL_DATA_X0;
+	uint8_t ACCEL_DATA_Y1;
+	uint8_t ACCEL_DATA_Y0;
+	uint8_t ACCEL_DATA_Z1;
+	uint8_t ACCEL_DATA_Z0;
+	uint8_t GYRO_DATA_X1;
+	uint8_t GYRO_DATA_X0;
+	uint8_t GYRO_DATA_Y1;
+	uint8_t GYRO_DATA_Y0;
+	uint8_t GYRO_DATA_Z1;
+	uint8_t GYRO_DATA_Z0;
+	uint8_t temperature;  // Temperature[7:0]
+	uint8_t timestamp_l;
+	uint8_t timestamp_h;
 };

 // 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_ACCEL           = Bit6,
+	HEADER_GYRO            = Bit5,
+	HEADER_20              = Bit4,
 	HEADER_TIMESTAMP_FSYNC = Bit3 | Bit2,
-	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
+	HEADER_ODR_ACCEL       = Bit1,
+	HEADER_ODR_GYRO        = Bit0,
 };

 }
@@ -126,6 +126,8 @@ bool LinuxPWMOut::updateOutputs(bool stop_motors, uint16_t outputs[MAX_ACTUATORS

 void LinuxPWMOut::Run()
 {
+	SmartLock lock_guard(_lock);
+
 	if (should_exit()) {
 		ScheduleClear();
 		_mixing_output.unregister();
@@ -134,8 +136,6 @@ void LinuxPWMOut::Run()
 		return;
 	}

-	SmartLock lock_guard(_lock);
-
 	perf_begin(_cycle_perf);
 	perf_count(_interval_perf);

@@ -408,6 +408,10 @@ OSDatxxxx::get_flight_mode(uint8_t nav_state)
 		flight_mode = "AUTO";
 		break;

+	case vehicle_status_s::NAVIGATION_STATE_AUTO_LANDENGFAIL:
+		flight_mode = "FAILURE";
+		break;
+
 	case vehicle_status_s::NAVIGATION_STATE_ACRO:
 		flight_mode = "ACRO";
 		break;
@@ -376,6 +376,8 @@ bool PCA9685Wrapper::updateOutputs(bool stop_motors, uint16_t *outputs, unsigned

 void PCA9685Wrapper::Run()
 {
+	SmartLock lock_guard(_lock);
+
 	if (should_exit()) {
 		ScheduleClear();
 		_mixing_output.unregister();
@@ -389,8 +391,6 @@ void PCA9685Wrapper::Run()
 		return;
 	}

-	SmartLock lock_guard(_lock);
-
 	perf_begin(_cycle_perf);

 	switch (_state) {
--- a/Show More
+++ b/Show More