FailureDetector: check if ESCs have current

And increase the timeout to 300ms, as some ESCs only update with 10Hz.
TEST PositionControl: use aux2 to switch between thrust vs attitude
2026-06-04 02:00:04 +08:00 · 2022-06-03 09:53:56 +02:00 · 2022-06-03 09:52:45 +02:00 · 2022-05-31 14:33:27 +02:00 · 2022-05-31 14:16:33 +02:00 · 2022-05-23 15:37:48 +02:00
266 changed files with 12550 additions and 8254 deletions
@@ -105,6 +105,7 @@ pipeline {
                     "px4_fmu-v5_stackcheck",
                     "px4_fmu-v5_uavcanv0periph",
                     "px4_fmu-v5x_default",
+                     "px4_fmu-v6c_default",
                     "px4_fmu-v6u_default",
                     "px4_fmu-v6x_default",
                     "px4_io-v2_default",
@@ -794,7 +794,7 @@ void resetParameters() {
 void runTests() {

  // test loading a range of airframes
-  sh './Tools/HIL/test_airframes.sh `find /dev/serial -name *usb-*` 1000 1001 2100 3000 4001 6001 8001 10016'
+  sh './Tools/HIL/test_airframes.sh `find /dev/serial -name *usb-*` 2100 3000 4001 6001 8001 10016'

  resetParameters()

@@ -1,32 +1,34 @@
 ---
-name: Bug report
+name: 🐛 Bug report
 about: Create a report to help us improve
+labels: bug-report

 ---

-**Describe the bug**
+## Describe the bug
 A clear and concise description of the bug.

-**To Reproduce**
+## To Reproduce
 Steps to reproduce the behavior:
 1. Drone switched on '...'
 2. Uploaded mission '....' (attach QGC mission file)
 3. Took off '....'
 4. See error

-**Expected behavior**
+## Expected behavior
 A clear and concise description of what you expected to happen.

-**Log Files and Screenshots**
+## Log Files and Screenshots
 *Always* provide a link to the flight log file:
 - Download the flight log file from the vehicle ([tutorial](https://docs.px4.io/master/en/getting_started/flight_reporting.html)).
- Share the link to a log showing the problem on [PX4 Flight Review](http://logs.px4.io/).
+- Upload the log to the [PX4 Flight Review](http://logs.px4.io/)
+- Share the link to the log (Copy and paste the URL of the log)

 Add screenshots to help explain your problem.

-**Drone (please complete the following information):**
+## Drone (please complete the following information):
 - Describe the type of drone.
 - Photo of the IMU / autopilot setup if possible.

-**Additional context**
+## Additional context
 Add any other context about the problem here.
@@ -1,19 +1,20 @@
 ---
 name: 🚀 Feature Request
 about: Suggest an idea for this project
+labels: feature-request

 ---

 For general questions please use [PX4 Discuss](http://discuss.px4.io/) or Slack (you can find an invite link on this project README).

-**Describe problem solved by the proposed feature**
+## Describe problem solved by the proposed feature
 A clear and concise description of the problem, if any, this feature will solve. E.g. I'm always frustrated when ...

-**Describe your preferred solution**
+## Describe your preferred solution
 A clear and concise description of what you want to happen.

-**Describe possible alternatives**
+## Describe possible alternatives
 A clear and concise description of alternative solutions or features you've considered.

-**Additional context**
+## Additional context
 Add any other context or screenshots for the feature request here.
@@ -1,10 +1,13 @@
 ---
 name: ⛔ Support Question
-about: See [PX4 Discuss](http://discuss.px4.io/) for questions about using PX4.
+about: See http://discuss.px4.io/ for questions about using PX4.

 ---

-We use GitHub issues only to discuss PX4 bugs and new features. For
-questions about using PX4 or related components, please use [PX4 Discuss](http://discuss.px4.io/).
+## Attention! Please read the note below
+
+We use GitHub issues only to discuss PX4 bugs and new features.
+
+**For questions about using PX4 or related components, please use [PX4 Discuss](http://discuss.px4.io/).**

 Thanks!
@@ -1,9 +1,11 @@
 ---
 name: ⛔ Documentation Issue
-about: See https://github.com/PX4/Devguide for documentation issues
+about: See https://github.com/PX4/px4_user_guide for documentation issues

 ---

-PX4 has dedicated repositories for developer documentation (https://github.com/PX4/Devguide) and user documentation (https://github.com/PX4/px4_user_guide).
+## Attention! Please read the note below
+
+**Please submit the documentation issue to the [User Guide](https://github.com/PX4/px4_user_guide) repository.**

 Thanks!
@@ -1,17 +1,17 @@
 Please use [PX4 Discuss](http://discuss.px4.io/) or [Slack](http://slack.px4.io/) to align on pull requests if necessary. You can then open draft pull requests to get early feedback.

-**Describe problem solved by this pull request**
-A clear and concise description of the problem this proposed change will solve.
+## Describe problem solved by this pull request
+A clear and concise description of the problem this proposed change will solve. Or, what it will improve.
 E.g. For this use case I ran into...

-**Describe your solution**
+## Describe your solution
 A clear and concise description of what you have implemented.

-**Describe possible alternatives**
+## Describe possible alternatives
 A clear and concise description of alternative solutions or features you've considered.

-**Test data / coverage**
+## Test data / coverage
 How was it tested? What cases were covered? Logs uploaded to https://review.px4.io/ and screenshots of the important plot parts.

-**Additional context**
+## Additional context
 Add any other related context or media.
@@ -61,6 +61,7 @@ jobs:
          px4_fmu-v4pro,
          px4_fmu-v5,
          px4_fmu-v5x,
+          px4_fmu-v6c,
          px4_fmu-v6u,
          px4_fmu-v6x,
          raspberrypi_pico,
@@ -325,6 +325,7 @@ px4io_update: px4_io-v2_default cubepilot_io-v2_default
 	cp build/px4_io-v2_default/px4_io-v2_default.bin boards/px4/fmu-v5/extras/px4_io-v2_default.bin
 	cp build/px4_io-v2_default/px4_io-v2_default.bin boards/px4/fmu-v5x/extras/px4_io-v2_default.bin
 	cp build/px4_io-v2_default/px4_io-v2_default.bin boards/px4/fmu-v6x/extras/px4_io-v2_default.bin
+	cp build/px4_io-v2_default/px4_io-v2_default.bin boards/px4/fmu-v6c/extras/px4_io-v2_default.bin
 	# cubepilot_io-v2_default
 	cp build/cubepilot_io-v2_default/cubepilot_io-v2_default.bin boards/cubepilot/cubeorange/extras/cubepilot_io-v2_default.bin
 	cp build/cubepilot_io-v2_default/cubepilot_io-v2_default.bin boards/cubepilot/cubeyellow/extras/cubepilot_io-v2_default.bin
@@ -7,6 +7,11 @@

 . ${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

@@ -1,6 +1,4 @@

-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
@@ -61,6 +61,8 @@ param set-default HIL_ACT_FUNC6 400

 param set SYS_HITL 1

+param set UAVCAN_ENABLE 0
+
 # disable some checks to allow to fly
 # - with usb
 param set-default CBRK_USB_CHK 197848
@@ -15,6 +15,8 @@ set MIXER quad_x

 param set SYS_HITL 1

+param set UAVCAN_ENABLE 0
+
 param set-default CA_ROTOR_COUNT 4
 param set-default CA_ROTOR0_PX 0.15
 param set-default CA_ROTOR0_PY 0.15
@@ -94,6 +94,8 @@ param set-default HIL_ACT_FUNC8 203

 param set SYS_HITL 1

+param set UAVCAN_ENABLE 0
+
 # disable some checks to allow to fly
 # - with usb
 param set-default CBRK_USB_CHK 197848
@@ -131,7 +131,6 @@ 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

@@ -136,8 +136,11 @@ class firmware(object):

        self.image = bytearray(zlib.decompress(base64.b64decode(self.desc['image'])))

-        # pad image to 4-byte length
-        while ((len(self.image) % 4) != 0):
+        # PX4 bootloader in theory requires only 4-byte padding,
+        # but a bug exists with the flash block caching where if the data
+        # does not fill up the block, it will not be written (left as all 1s).
+        # So pad up to the flash block size (8 words).
+        while ((len(self.image) % (4 * 8)) != 0):
            self.image.extend(b'\xff')

    def property(self, propname):
@@ -200,13 +200,12 @@ 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
-		gazebo_version=11
+	elif [[ "${UBUNTU_RELEASE}" == "22.04" ]]; then
+		java_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 \
@@ -220,20 +219,30 @@ 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$gazebo_version \
+		$gazebo_packages \
 		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 \
@@ -9,14 +9,15 @@ 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_DRIVERS_MAGNETOMETER_QMC5883L=y
+CONFIG_COMMON_DISTANCE_SENSOR=n
 CONFIG_COMMON_LIGHT=n
-CONFIG_COMMON_MAGNETOMETER=y
+CONFIG_COMMON_MAGNETOMETER=n
 CONFIG_COMMON_OPTICAL_FLOW=y
 CONFIG_DRIVERS_OSD=y
 CONFIG_DRIVERS_PWM_OUT=y
@@ -159,10 +159,12 @@ 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_1,         /* SPLLDIV2 */
+			.div2        = SCG_ASYNC_CLOCK_DIV_BY_2,         /* 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_SIRC_DIV2,
+		.clksrc  = CLK_SRC_SPLL_DIV2,
 	},
 	{
 		.clkname = LPSPI0_CLK,
@@ -110,7 +110,7 @@ const struct peripheral_clock_config_s g_peripheral_clockconfig0[] = {
 #else
 		.clkgate = false,
 #endif
-		.clksrc  = CLK_SRC_SIRC_DIV2,
+		.clksrc  = CLK_SRC_SPLL_DIV2,
 	},
 	{
 		.clkname = LPUART0_CLK,
@@ -119,7 +119,7 @@ const struct peripheral_clock_config_s g_peripheral_clockconfig0[] = {
 #else
 		.clkgate = false,
 #endif
-		.clksrc  = CLK_SRC_SIRC_DIV2,
+		.clksrc  = CLK_SRC_SPLL_DIV2,
 	},
 	{
 		.clkname = LPUART1_CLK,
@@ -128,7 +128,7 @@ const struct peripheral_clock_config_s g_peripheral_clockconfig0[] = {
 #else
 		.clkgate = false,
 #endif
-		.clksrc  = CLK_SRC_SIRC_DIV2,
+		.clksrc  = CLK_SRC_SPLL_DIV2,
 	},
 	{
 		.clkname    = RTC0_CLK,
@@ -9,5 +9,4 @@ CONFIG_DRIVERS_DISTANCE_SENSOR_LIGHTWARE_LASER_I2C=y
 CONFIG_DRIVERS_DISTANCE_SENSOR_LIGHTWARE_LASER_SERIAL=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
@@ -0,0 +1,3 @@
+CONFIG_BOARD_TOOLCHAIN="arm-none-eabi"
+CONFIG_BOARD_ARCHITECTURE="cortex-m7"
+CONFIG_BOARD_ROMFSROOT=""
@@ -0,0 +1,91 @@
+CONFIG_BOARD_TOOLCHAIN="arm-none-eabi"
+CONFIG_BOARD_ARCHITECTURE="cortex-m7"
+CONFIG_BOARD_SERIAL_GPS1="/dev/ttyS0"
+CONFIG_BOARD_SERIAL_GPS2="/dev/ttyS6"
+CONFIG_BOARD_SERIAL_TEL1="/dev/ttyS5"
+CONFIG_BOARD_SERIAL_TEL2="/dev/ttyS3"
+CONFIG_BOARD_SERIAL_TEL3="/dev/ttyS1"
+CONFIG_DRIVERS_ADC_BOARD_ADC=y
+CONFIG_COMMON_BAROMETERS=y
+CONFIG_DRIVERS_BATT_SMBUS=y
+CONFIG_DRIVERS_CAMERA_CAPTURE=y
+CONFIG_DRIVERS_CAMERA_TRIGGER=y
+CONFIG_COMMON_DIFFERENTIAL_PRESSURE=y
+CONFIG_COMMON_DISTANCE_SENSOR=y
+CONFIG_DRIVERS_DSHOT=y
+CONFIG_DRIVERS_GPS=y
+CONFIG_DRIVERS_HEATER=y
+CONFIG_DRIVERS_IMU_BOSCH_BMI055=y
+CONFIG_DRIVERS_IMU_INVENSENSE_ICM42688P=y
+CONFIG_COMMON_LIGHT=y
+CONFIG_COMMON_MAGNETOMETER=y
+CONFIG_COMMON_OPTICAL_FLOW=y
+CONFIG_DRIVERS_POWER_MONITOR_INA226=y
+CONFIG_DRIVERS_POWER_MONITOR_INA228=y
+CONFIG_DRIVERS_POWER_MONITOR_INA238=y
+CONFIG_DRIVERS_PWM_OUT=y
+CONFIG_DRIVERS_PWM_OUT_SIM=y
+CONFIG_DRIVERS_PX4IO=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_BATTERY_STATUS=y
+CONFIG_MODULES_CAMERA_FEEDBACK=y
+CONFIG_MODULES_COMMANDER=y
+CONFIG_MODULES_CONTROL_ALLOCATOR=y
+CONFIG_MODULES_DATAMAN=y
+CONFIG_MODULES_EKF2=y
+CONFIG_MODULES_ESC_BATTERY=y
+CONFIG_MODULES_EVENTS=y
+CONFIG_MODULES_FLIGHT_MODE_MANAGER=y
+CONFIG_MODULES_FW_ATT_CONTROL=y
+CONFIG_MODULES_FW_AUTOTUNE_ATTITUDE_CONTROL=y
+CONFIG_MODULES_FW_POS_CONTROL_L1=y
+CONFIG_MODULES_GIMBAL=y
+CONFIG_MODULES_GYRO_CALIBRATION=y
+CONFIG_MODULES_GYRO_FFT=y
+CONFIG_MODULES_LAND_DETECTOR=y
+CONFIG_MODULES_LANDING_TARGET_ESTIMATOR=y
+CONFIG_MODULES_LOAD_MON=y
+CONFIG_MODULES_LOGGER=y
+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=y
+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_ROVER_POS_CONTROL=y
+CONFIG_MODULES_SENSORS=y
+CONFIG_MODULES_SIH=y
+CONFIG_MODULES_TEMPERATURE_COMPENSATION=y
+CONFIG_MODULES_VTOL_ATT_CONTROL=y
+CONFIG_SYSTEMCMDS_ACTUATOR_TEST=y
+CONFIG_SYSTEMCMDS_BL_UPDATE=y
+CONFIG_SYSTEMCMDS_DMESG=y
+CONFIG_SYSTEMCMDS_HARDFAULT_LOG=y
+CONFIG_SYSTEMCMDS_I2CDETECT=y
+CONFIG_SYSTEMCMDS_LED_CONTROL=y
+CONFIG_SYSTEMCMDS_MFT=y
+CONFIG_SYSTEMCMDS_MIXER=y
+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=y
+CONFIG_SYSTEMCMDS_SERIAL_TEST=y
+CONFIG_SYSTEMCMDS_SYSTEM_TIME=y
+CONFIG_SYSTEMCMDS_TOP=y
+CONFIG_SYSTEMCMDS_TOPIC_LISTENER=y
+CONFIG_SYSTEMCMDS_TUNE_CONTROL=y
+CONFIG_SYSTEMCMDS_UORB=y
+CONFIG_SYSTEMCMDS_VER=y
+CONFIG_SYSTEMCMDS_WORK_QUEUE=y
@@ -0,0 +1,13 @@
+{
+    "board_id": 56,
+    "magic": "PX4FWv1",
+    "description": "Firmware for the PX4FMUv6C board",
+    "image": "",
+    "build_time": 0,
+    "summary": "PX4FMUv6C",
+    "version": "0.1",
+    "image_size": 0,
+    "image_maxsize": 1966080,
+    "git_identity": "",
+    "board_revision": 0
+}
@@ -0,0 +1,20 @@
+#!/bin/sh
+#
+# board specific defaults
+#------------------------------------------------------------------------------
+
+
+param set-default BAT1_V_DIV 18.1
+param set-default BAT2_V_DIV 18.1
+
+param set-default BAT1_A_PER_V 36.367515152
+param set-default BAT2_A_PER_V 36.367515152
+
+# Mavlink ethernet (CFG 1000)
+param set-default MAV_2_CONFIG 1000
+param set-default MAV_2_BROADCAST 1
+param set-default MAV_2_MODE 0
+param set-default MAV_2_RADIO_CTL 0
+param set-default MAV_2_RATE 100000
+param set-default MAV_2_REMOTE_PRT 14550
+param set-default MAV_2_UDP_PRT 14550
@@ -0,0 +1,21 @@
+#!/bin/sh
+#
+# PX4 FMUv6C specific board sensors init
+#------------------------------------------------------------------------------
+board_adc start
+
+# Internal SPI bus BMI055 accel/gyro
+bmi055 -A -R 4 -s start
+bmi055 -G -R 4 -s start
+
+# Internal SPI bus ICM42688p
+icm42688p -R 6 -s start
+
+# Internal barometer on I2C4
+ms5611 -I -b 4 -a 0x77 start
+
+# Internal compass on IMU I2C4
+ist8310 -I -b 4 -a 0xc -R 6 start
+
+# External compass on GPS/I2C1 (the 3rd external bus): standard Holybro Pixhawk 4 or CUAV V5 GPS/compass puck (with lights, safety button, and buzzer)
+ist8310 -X -b 1 -R 10 start
@@ -0,0 +1,17 @@
+#
+# For a description of the syntax of this configuration file,
+# see misc/tools/kconfig-language.txt.
+#
+config BOARD_HAS_PROBES
+	bool "Board provides GPIO or other Hardware for signaling to timing analyze."
+	default y
+	---help---
+		This board provides GPIO FMU-CH1-5, CAP1-6 as PROBE_1-11 to provide timing signals from selected drivers.
+
+config BOARD_USE_PROBES
+	bool "Enable the use the board provided FMU-CH1-5, CAP1-6 as PROBE_1-11"
+	default n
+	depends on BOARD_HAS_PROBES
+
+	---help---
+		Select to use GPIO FMU-CH1-5, CAP1-6  to provide timing signals from selected drivers.
@@ -0,0 +1,96 @@
+#
+# This file is autogenerated: PLEASE DO NOT EDIT IT.
+#
+# You can use "make menuconfig" to make any modifications to the installed .config file.
+# You can then do "make savedefconfig" to generate a new defconfig file that includes your
+# modifications.
+#
+# CONFIG_DEV_CONSOLE is not set
+# CONFIG_DISABLE_PSEUDOFS_OPERATIONS is not set
+# CONFIG_SPI_EXCHANGE is not set
+# CONFIG_STM32H7_SYSCFG is not set
+CONFIG_ARCH="arm"
+CONFIG_ARCH_BOARD_CUSTOM=y
+CONFIG_ARCH_BOARD_CUSTOM_DIR="../../../../boards/px4/fmu-v6c/nuttx-config"
+CONFIG_ARCH_BOARD_CUSTOM_DIR_RELPATH=y
+CONFIG_ARCH_BOARD_CUSTOM_NAME="px4"
+CONFIG_ARCH_CHIP="stm32h7"
+CONFIG_ARCH_CHIP_STM32H753II=y
+CONFIG_ARCH_CHIP_STM32H7=y
+CONFIG_ARCH_INTERRUPTSTACK=512
+CONFIG_ARMV7M_BASEPRI_WAR=y
+CONFIG_ARMV7M_ICACHE=y
+CONFIG_ARMV7M_MEMCPY=y
+CONFIG_ARMV7M_USEBASEPRI=y
+CONFIG_BOARDCTL_RESET=y
+CONFIG_BOARD_INITTHREAD_PRIORITY=254
+CONFIG_BOARD_LATE_INITIALIZE=y
+CONFIG_BOARD_LOOPSPERMSEC=95150
+CONFIG_BOARD_RESET_ON_ASSERT=2
+CONFIG_C99_BOOL8=y
+CONFIG_CDCACM=y
+CONFIG_CDCACM_IFLOWCONTROL=y
+CONFIG_CDCACM_PRODUCTID=0x0038
+CONFIG_CDCACM_PRODUCTSTR="PX4 BL FMU v6C.x"
+CONFIG_CDCACM_RXBUFSIZE=600
+CONFIG_CDCACM_TXBUFSIZE=12000
+CONFIG_CDCACM_VENDORID=0x3185
+CONFIG_CDCACM_VENDORSTR="Auterion"
+CONFIG_CLOCK_MONOTONIC=y
+CONFIG_DEBUG_FULLOPT=y
+CONFIG_DEBUG_SYMBOLS=y
+CONFIG_DEFAULT_SMALL=y
+CONFIG_DISABLE_MQUEUE=y
+CONFIG_DISABLE_PTHREAD=y
+CONFIG_EXPERIMENTAL=y
+CONFIG_FDCLONE_DISABLE=y
+CONFIG_FDCLONE_STDIO=y
+CONFIG_HAVE_CXX=y
+CONFIG_HAVE_CXXINITIALIZE=y
+CONFIG_IDLETHREAD_STACKSIZE=750
+CONFIG_LIBC_FLOATINGPOINT=y
+CONFIG_LIBC_LONG_LONG=y
+CONFIG_LIBC_STRERROR=y
+CONFIG_LIB_BOARDCTL=y
+CONFIG_MEMSET_64BIT=y
+CONFIG_MEMSET_OPTSPEED=y
+CONFIG_PREALLOC_TIMERS=50
+CONFIG_PTHREAD_STACK_MIN=512
+CONFIG_RAM_SIZE=245760
+CONFIG_RAM_START=0x20010000
+CONFIG_RAW_BINARY=y
+CONFIG_SDCLONE_DISABLE=y
+CONFIG_SERIAL_TERMIOS=y
+CONFIG_SIG_DEFAULT=y
+CONFIG_SIG_SIGALRM_ACTION=y
+CONFIG_SIG_SIGUSR1_ACTION=y
+CONFIG_SIG_SIGUSR2_ACTION=y
+CONFIG_SPI=y
+CONFIG_STACK_COLORATION=y
+CONFIG_START_DAY=30
+CONFIG_START_MONTH=11
+CONFIG_STDIO_BUFFER_SIZE=32
+CONFIG_STM32H7_BKPSRAM=y
+CONFIG_STM32H7_DMA1=y
+CONFIG_STM32H7_DMA2=y
+CONFIG_STM32H7_OTGFS=y
+CONFIG_STM32H7_PROGMEM=y
+CONFIG_STM32H7_SERIAL_DISABLE_REORDERING=y
+CONFIG_STM32H7_TIM1=y
+CONFIG_STM32H7_UART7=y
+CONFIG_SYSTEMTICK_HOOK=y
+CONFIG_SYSTEM_CDCACM=y
+CONFIG_TASK_NAME_SIZE=24
+CONFIG_TTY_SIGINT=y
+CONFIG_TTY_SIGINT_CHAR=0x03
+CONFIG_TTY_SIGTSTP=y
+CONFIG_UART7_RXBUFSIZE=512
+CONFIG_UART7_RXDMA=y
+CONFIG_UART7_TXBUFSIZE=512
+CONFIG_UART7_TXDMA=y
+CONFIG_USBDEV=y
+CONFIG_USBDEV_BUSPOWERED=y
+CONFIG_USBDEV_MAXPOWER=500
+CONFIG_USEC_PER_TICK=1000
+CONFIG_USERMAIN_STACKSIZE=2944
+CONFIG_USER_ENTRYPOINT="bootloader_main"
@@ -0,0 +1,505 @@
+/************************************************************************************
+ * nuttx-configs/px4_fmu-v6c/include/board.h
+ *
+ *   Copyright (C) 2016-2019 Gregory Nutt. All rights reserved.
+ *   Authors: David Sidrane <david.sidrane@nscdg.com>
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright
+ *    notice, this list of conditions and the following disclaimer.
+ * 2. Redistributions in binary form must reproduce the above copyright
+ *    notice, this list of conditions and the following disclaimer in
+ *    the documentation and/or other materials provided with the
+ *    distribution.
+ * 3. Neither the name NuttX nor the names of its contributors may be
+ *    used to endorse or promote products derived from this software
+ *    without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
+ * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
+ * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *
+ ************************************************************************************/
+#ifndef __NUTTX_CONFIG_PX4_FMU_V6C_INCLUDE_BOARD_H
+#define __NUTTX_CONFIG_PX4_FMU_V6C_INCLUDE_BOARD_H
+
+/************************************************************************************
+ * Included Files
+ ************************************************************************************/
+
+#include "board_dma_map.h"
+
+#include <nuttx/config.h>
+
+#ifndef __ASSEMBLY__
+# include <stdint.h>
+#endif
+
+#include "stm32_rcc.h"
+#include "stm32_sdmmc.h"
+
+/************************************************************************************
+ * Pre-processor Definitions
+ ************************************************************************************/
+
+/* Clocking *************************************************************************/
+/* The px4_fmu-v6C  board provides the following clock sources:
+ *
+ *   X1: 16 MHz crystal for HSE
+ *
+ * So we have these clock source available within the STM32
+ *
+ *   HSI: 16 MHz RC factory-trimmed
+ *   HSE: 16 MHz crystal for HSE
+ */
+
+#define STM32_BOARD_XTAL        16000000ul
+
+#define STM32_HSI_FREQUENCY     16000000ul
+#define STM32_LSI_FREQUENCY     32000
+#define STM32_HSE_FREQUENCY     STM32_BOARD_XTAL
+#define STM32_LSE_FREQUENCY     32768
+
+/* Main PLL Configuration.
+ *
+ * PLL source is HSE = 16,000,000
+ *
+ * PLL_VCOx = (STM32_HSE_FREQUENCY / PLLM) * PLLN
+ * Subject to:
+ *
+ *     1 <= PLLM <= 63
+ *     4 <= PLLN <= 512
+ *   150 MHz <= PLL_VCOL <= 420MHz
+ *   192 MHz <= PLL_VCOH <= 836MHz
+ *
+ * SYSCLK  = PLL_VCO / PLLP
+ * CPUCLK  = SYSCLK / D1CPRE
+ * Subject to
+ *
+ *   PLLP1   = {2, 4, 6, 8, ..., 128}
+ *   PLLP2,3 = {2, 3, 4, ..., 128}
+ *   CPUCLK <= 480 MHz
+ */
+
+#define STM32_BOARD_USEHSE
+
+#define STM32_PLLCFG_PLLSRC      RCC_PLLCKSELR_PLLSRC_HSE
+
+/* PLL1, wide 4 - 8 MHz input, enable DIVP, DIVQ, DIVR
+ *
+ *   PLL1_VCO = (16,000,000 / 1) * 60 = 960 MHz
+ *
+ *   PLL1P = PLL1_VCO/2  = 960 MHz / 2   = 480 MHz
+ *   PLL1Q = PLL1_VCO/4  = 960 MHz / 4   = 240 MHz
+ *   PLL1R = PLL1_VCO/8  = 960 MHz / 8   = 120 MHz
+ */
+
+#define STM32_PLLCFG_PLL1CFG    (RCC_PLLCFGR_PLL1VCOSEL_WIDE | \
+				 RCC_PLLCFGR_PLL1RGE_4_8_MHZ | \
+				 RCC_PLLCFGR_DIVP1EN | \
+				 RCC_PLLCFGR_DIVQ1EN | \
+				 RCC_PLLCFGR_DIVR1EN)
+#define STM32_PLLCFG_PLL1M       RCC_PLLCKSELR_DIVM1(1)
+#define STM32_PLLCFG_PLL1N       RCC_PLL1DIVR_N1(60)
+#define STM32_PLLCFG_PLL1P       RCC_PLL1DIVR_P1(2)
+#define STM32_PLLCFG_PLL1Q       RCC_PLL1DIVR_Q1(4)
+#define STM32_PLLCFG_PLL1R       RCC_PLL1DIVR_R1(8)
+
+#define STM32_VCO1_FREQUENCY     ((STM32_HSE_FREQUENCY / 1) * 60)
+#define STM32_PLL1P_FREQUENCY    (STM32_VCO1_FREQUENCY / 2)
+#define STM32_PLL1Q_FREQUENCY    (STM32_VCO1_FREQUENCY / 4)
+#define STM32_PLL1R_FREQUENCY    (STM32_VCO1_FREQUENCY / 8)
+
+/* PLL2 */
+
+#define STM32_PLLCFG_PLL2CFG     (RCC_PLLCFGR_PLL2VCOSEL_WIDE | \
+				  RCC_PLLCFGR_PLL2RGE_4_8_MHZ | \
+				  RCC_PLLCFGR_DIVP2EN | \
+				  RCC_PLLCFGR_DIVQ2EN | \
+				  RCC_PLLCFGR_DIVR2EN)
+#define STM32_PLLCFG_PLL2M       RCC_PLLCKSELR_DIVM2(4)
+#define STM32_PLLCFG_PLL2N       RCC_PLL2DIVR_N2(48)
+#define STM32_PLLCFG_PLL2P       RCC_PLL2DIVR_P2(2)
+#define STM32_PLLCFG_PLL2Q       RCC_PLL2DIVR_Q2(2)
+#define STM32_PLLCFG_PLL2R       RCC_PLL2DIVR_R2(2)
+
+#define STM32_VCO2_FREQUENCY     ((STM32_HSE_FREQUENCY / 4) * 48)
+#define STM32_PLL2P_FREQUENCY    (STM32_VCO2_FREQUENCY / 2)
+#define STM32_PLL2Q_FREQUENCY    (STM32_VCO2_FREQUENCY / 2)
+#define STM32_PLL2R_FREQUENCY    (STM32_VCO2_FREQUENCY / 2)
+
+/* PLL3 */
+
+#define STM32_PLLCFG_PLL3CFG    (RCC_PLLCFGR_PLL3VCOSEL_WIDE | \
+				 RCC_PLLCFGR_PLL3RGE_4_8_MHZ | \
+				 RCC_PLLCFGR_DIVQ3EN)
+#define STM32_PLLCFG_PLL3M      RCC_PLLCKSELR_DIVM3(4)
+#define STM32_PLLCFG_PLL3N      RCC_PLL3DIVR_N3(48)
+#define STM32_PLLCFG_PLL3P      RCC_PLL3DIVR_P3(2)
+#define STM32_PLLCFG_PLL3Q      RCC_PLL3DIVR_Q3(4)
+#define STM32_PLLCFG_PLL3R      RCC_PLL3DIVR_R3(2)
+
+#define STM32_VCO3_FREQUENCY    ((STM32_HSE_FREQUENCY / 4) * 48)
+#define STM32_PLL3P_FREQUENCY   (STM32_VCO3_FREQUENCY / 2)
+#define STM32_PLL3Q_FREQUENCY   (STM32_VCO3_FREQUENCY / 4)
+#define STM32_PLL3R_FREQUENCY   (STM32_VCO3_FREQUENCY / 2)
+
+/* SYSCLK = PLL1P = 480MHz
+ * CPUCLK = SYSCLK / 1 = 480 MHz
+ */
+
+#define STM32_RCC_D1CFGR_D1CPRE  (RCC_D1CFGR_D1CPRE_SYSCLK)
+#define STM32_SYSCLK_FREQUENCY   (STM32_PLL1P_FREQUENCY)
+#define STM32_CPUCLK_FREQUENCY   (STM32_SYSCLK_FREQUENCY / 1)
+
+/* Configure Clock Assignments */
+
+/* AHB clock (HCLK) is SYSCLK/2 (240 MHz max)
+ * HCLK1 = HCLK2 = HCLK3 = HCLK4 = 240
+ */
+
+#define STM32_RCC_D1CFGR_HPRE   RCC_D1CFGR_HPRE_SYSCLKd2        /* HCLK  = SYSCLK / 2 */
+#define STM32_ACLK_FREQUENCY    (STM32_CPUCLK_FREQUENCY / 2)    /* ACLK in D1, HCLK3 in D1 */
+#define STM32_HCLK_FREQUENCY    (STM32_CPUCLK_FREQUENCY / 2)    /* HCLK in D2, HCLK4 in D3 */
+#define STM32_BOARD_HCLK        STM32_HCLK_FREQUENCY            /* same as above, to satisfy compiler */
+
+/* APB1 clock (PCLK1) is HCLK/2 (120 MHz) */
+
+#define STM32_RCC_D2CFGR_D2PPRE1  RCC_D2CFGR_D2PPRE1_HCLKd2       /* PCLK1 = HCLK / 2 */
+#define STM32_PCLK1_FREQUENCY     (STM32_HCLK_FREQUENCY/2)
+
+/* APB2 clock (PCLK2) is HCLK/2 (120 MHz) */
+
+#define STM32_RCC_D2CFGR_D2PPRE2  RCC_D2CFGR_D2PPRE2_HCLKd2       /* PCLK2 = HCLK / 2 */
+#define STM32_PCLK2_FREQUENCY     (STM32_HCLK_FREQUENCY/2)
+
+/* APB3 clock (PCLK3) is HCLK/2 (120 MHz) */
+
+#define STM32_RCC_D1CFGR_D1PPRE   RCC_D1CFGR_D1PPRE_HCLKd2        /* PCLK3 = HCLK / 2 */
+#define STM32_PCLK3_FREQUENCY     (STM32_HCLK_FREQUENCY/2)
+
+/* APB4 clock (PCLK4) is HCLK/4 (120 MHz) */
+
+#define STM32_RCC_D3CFGR_D3PPRE   RCC_D3CFGR_D3PPRE_HCLKd2       /* PCLK4 = HCLK / 2 */
+#define STM32_PCLK4_FREQUENCY     (STM32_HCLK_FREQUENCY/2)
+
+/* Timer clock frequencies */
+
+/* Timers driven from APB1 will be twice PCLK1 */
+
+#define STM32_APB1_TIM2_CLKIN   (2*STM32_PCLK1_FREQUENCY)
+#define STM32_APB1_TIM3_CLKIN   (2*STM32_PCLK1_FREQUENCY)
+#define STM32_APB1_TIM4_CLKIN   (2*STM32_PCLK1_FREQUENCY)
+#define STM32_APB1_TIM5_CLKIN   (2*STM32_PCLK1_FREQUENCY)
+#define STM32_APB1_TIM6_CLKIN   (2*STM32_PCLK1_FREQUENCY)
+#define STM32_APB1_TIM7_CLKIN   (2*STM32_PCLK1_FREQUENCY)
+#define STM32_APB1_TIM12_CLKIN  (2*STM32_PCLK1_FREQUENCY)
+#define STM32_APB1_TIM13_CLKIN  (2*STM32_PCLK1_FREQUENCY)
+#define STM32_APB1_TIM14_CLKIN  (2*STM32_PCLK1_FREQUENCY)
+
+/* Timers driven from APB2 will be twice PCLK2 */
+
+#define STM32_APB2_TIM1_CLKIN   (2*STM32_PCLK2_FREQUENCY)
+#define STM32_APB2_TIM8_CLKIN   (2*STM32_PCLK2_FREQUENCY)
+#define STM32_APB2_TIM15_CLKIN  (2*STM32_PCLK2_FREQUENCY)
+#define STM32_APB2_TIM16_CLKIN  (2*STM32_PCLK2_FREQUENCY)
+#define STM32_APB2_TIM17_CLKIN  (2*STM32_PCLK2_FREQUENCY)
+
+/* Kernel Clock Configuration
+ *
+ * Note: look at Table 54 in ST Manual
+ */
+
+/* I2C123 clock source */
+
+#define STM32_RCC_D2CCIP2R_I2C123SRC RCC_D2CCIP2R_I2C123SEL_HSI
+
+/* I2C4 clock source */
+
+#define STM32_RCC_D3CCIPR_I2C4SRC    RCC_D3CCIPR_I2C4SEL_HSI
+
+/* SPI123 clock source */
+
+#define STM32_RCC_D2CCIP1R_SPI123SRC RCC_D2CCIP1R_SPI123SEL_PLL2
+
+/* SPI45 clock source */
+
+#define STM32_RCC_D2CCIP1R_SPI45SRC  RCC_D2CCIP1R_SPI45SEL_PLL2
+
+/* SPI6 clock source */
+
+#define STM32_RCC_D3CCIPR_SPI6SRC    RCC_D3CCIPR_SPI6SEL_PLL2
+
+/* USB 1 and 2 clock source */
+
+#define STM32_RCC_D2CCIP2R_USBSRC    RCC_D2CCIP2R_USBSEL_PLL3
+
+/* ADC 1 2 3 clock source */
+
+#define STM32_RCC_D3CCIPR_ADCSEL     RCC_D3CCIPR_ADCSEL_PLL2
+
+/* FDCAN 1 2 clock source */
+
+#define STM32_RCC_D2CCIP1R_FDCANSEL  RCC_D2CCIP1R_FDCANSEL_HSE   /* FDCAN 1 2 clock source */
+
+#define STM32_FDCANCLK               STM32_HSE_FREQUENCY
+
+/* FLASH wait states
+ *
+ *  ------------ ---------- -----------
+ *  Vcore        MAX ACLK   WAIT STATES
+ *  ------------ ---------- -----------
+ *  1.15-1.26 V     70 MHz    0
+ *  (VOS1 level)   140 MHz    1
+ *                 210 MHz    2
+ *  1.05-1.15 V     55 MHz    0
+ *  (VOS2 level)   110 MHz    1
+ *                 165 MHz    2
+ *                 220 MHz    3
+ *  0.95-1.05 V     45 MHz    0
+ *  (VOS3 level)    90 MHz    1
+ *                 135 MHz    2
+ *                 180 MHz    3
+ *                 225 MHz    4
+ *  ------------ ---------- -----------
+ */
+
+#define BOARD_FLASH_WAITSTATES 2
+
+/* SDMMC definitions ********************************************************/
+
+/* Init 400kHz, freq = PLL1Q/(2*div)  div =  PLL1Q/(2*freq) */
+
+#define STM32_SDMMC_INIT_CLKDIV     (300 << STM32_SDMMC_CLKCR_CLKDIV_SHIFT)
+
+/* 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   (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    (5 << STM32_SDMMC_CLKCR_CLKDIV_SHIFT)
+#else
+#  define STM32_SDMMC_SDXFR_CLKDIV    (100 << STM32_SDMMC_CLKCR_CLKDIV_SHIFT)
+#endif
+
+#define STM32_SDMMC_CLKCR_EDGE      STM32_SDMMC_CLKCR_NEGEDGE
+
+/* LED definitions ******************************************************************/
+/* The PX4 FMUV6C board has three, LED_GREEN a Green LED, LED_BLUE a Blue LED and
+ * LED_RED a Red LED, that can be controlled by software.
+ *
+ * If CONFIG_ARCH_LEDS is not defined, then the user can control the LEDs in any way.
+ * The following definitions are used to access individual LEDs.
+ */
+
+/* LED index values for use with board_userled() */
+
+/* LED definitions ******************************************************************/
+/* The px4_fmu-v6c board has three, LED_GREEN a Green LED, LED_BLUE a Blue LED and
+ * LED_RED a Red LED, that can be controlled by software.
+ *
+ * If CONFIG_ARCH_LEDS is not defined, then the user can control the LEDs in any way.
+ * The following definitions are used to access individual LEDs.
+ */
+
+/* LED index values for use with board_userled() */
+
+#define BOARD_LED1        0
+#define BOARD_LED2        1
+#define BOARD_LED3        2
+#define BOARD_NLEDS       3
+
+#define BOARD_LED_RED     BOARD_LED1
+#define BOARD_LED_GREEN   BOARD_LED2
+#define BOARD_LED_BLUE    BOARD_LED3
+
+/* LED bits for use with board_userled_all() */
+
+#define BOARD_LED1_BIT    (1 << BOARD_LED1)
+#define BOARD_LED2_BIT    (1 << BOARD_LED2)
+#define BOARD_LED3_BIT    (1 << BOARD_LED3)
+
+/* If CONFIG_ARCH_LEDS is defined, the usage by the board port is defined in
+ * include/board.h and src/stm32_leds.c. The LEDs are used to encode OS-related
+ * events as follows:
+ *
+ *
+ *   SYMBOL                     Meaning                      LED state
+ *                                                        Red   Green Blue
+ *   ----------------------  --------------------------  ------ ------ ----*/
+
+#define LED_STARTED        0 /* NuttX has been started   OFF    OFF   OFF  */
+#define LED_HEAPALLOCATE   1 /* Heap has been allocated  OFF    OFF   ON   */
+#define LED_IRQSENABLED    2 /* Interrupts enabled       OFF    ON    OFF  */
+#define LED_STACKCREATED   3 /* Idle stack created       OFF    ON    ON   */
+#define LED_INIRQ          4 /* In an interrupt          N/C    N/C   GLOW */
+#define LED_SIGNAL         5 /* In a signal handler      N/C    GLOW  N/C  */
+#define LED_ASSERTION      6 /* An assertion failed      GLOW   N/C   GLOW */
+#define LED_PANIC          7 /* The system has crashed   Blink  OFF   N/C  */
+#define LED_IDLE           8 /* MCU is is sleep mode     ON     OFF   OFF  */
+
+/* Thus if the Green LED is statically on, NuttX has successfully booted and
+ * is, apparently, running normally.  If the Red LED is flashing at
+ * approximately 2Hz, then a fatal error has been detected and the system
+ * has halted.
+ */
+
+/* Alternate function pin selections ************************************************/
+
+#define GPIO_USART1_RX   GPIO_USART1_RX_2    /* PA10 */
+#define GPIO_USART1_TX   GPIO_USART1_TX_3    /* PB6 */
+
+#define GPIO_USART2_RX   GPIO_USART2_RX_1   /* PA3   */
+#define GPIO_USART2_TX   GPIO_USART2_TX_2   /* PD5   */
+
+#define GPIO_USART3_RX   GPIO_USART3_RX_3   /* PD9   */
+#define GPIO_USART3_TX   GPIO_USART3_TX_3   /* PD8   */
+
+#define GPIO_UART5_RX    GPIO_UART5_RX_3    /* PD2  */
+#define GPIO_UART5_TX    GPIO_UART5_TX_3    /* PC12 */
+// GPIO_UART5_RTS   no remap                /* PC8  */
+// GPIO_UART5_CTS  No remap                 /* PC9  */
+
+#define GPIO_USART6_RX   GPIO_USART6_RX_1   /* PC7 */
+#define GPIO_USART6_TX   GPIO_USART6_TX_1   /* PC6  */
+
+#define GPIO_UART7_RX    GPIO_UART7_RX_3    /* PE7  */
+#define GPIO_UART7_TX    GPIO_UART7_TX_3    /* PE8  */
+#define GPIO_UART7_RTS   GPIO_UART7_RTS_1   /* PE9  */
+#define GPIO_UART7_CTS   GPIO_UART7_CTS_1   /* PE10 */
+
+#define GPIO_UART8_RX    GPIO_UART8_RX_1    /* PE0 */
+#define GPIO_UART8_TX    GPIO_UART8_TX_1    /* PE1 */
+
+
+/* CAN
+ *
+ * CAN1 is routed to transceiver.
+ * CAN2 is routed to transceiver.
+ */
+#define GPIO_CAN1_RX     GPIO_CAN1_RX_3     /* PD0  */
+#define GPIO_CAN1_TX     GPIO_CAN1_TX_3     /* PD1  */
+#define GPIO_CAN2_RX     GPIO_CAN2_RX_2     /* PB5 */
+#define GPIO_CAN2_TX     GPIO_CAN2_TX_1     /* PB13  */
+
+/* SPI
+ * SPI1 is sensors
+ * SPI2 is FRAM
+ *
+ */
+
+#define ADJ_SLEW_RATE(p) (((p) & ~GPIO_SPEED_MASK) | (GPIO_SPEED_2MHz))
+
+#define GPIO_SPI1_MISO   GPIO_SPI1_MISO_1               /* PA6  */
+#define GPIO_SPI1_MOSI   GPIO_SPI1_MOSI_1               /* PA7  */
+#define GPIO_SPI1_SCK    ADJ_SLEW_RATE(GPIO_SPI1_SCK_1) /* PA5  */
+
+#define GPIO_SPI2_MISO   GPIO_SPI2_MISO_2               /* PC2  */
+#define GPIO_SPI2_MOSI   GPIO_SPI2_MOSI_3               /* PC3  */
+#define GPIO_SPI2_SCK    ADJ_SLEW_RATE(GPIO_SPI2_SCK_5) /* PD3  */
+
+/* I2C
+ *
+ *   The optional _GPIO configurations allow the I2C driver to manually
+ *   reset the bus to clear stuck slaves.  They match the pin configuration,
+ *   but are normally-high GPIOs.
+ *
+ */
+
+#define GPIO_I2C1_SCL GPIO_I2C1_SCL_2       /* PB8  */
+#define GPIO_I2C1_SDA GPIO_I2C1_SDA_1       /* PB7  */
+
+#define GPIO_I2C1_SCL_GPIO                  (GPIO_OUTPUT | GPIO_OPENDRAIN |GPIO_SPEED_50MHz | GPIO_OUTPUT_SET | GPIO_PORTB | GPIO_PIN8)
+#define GPIO_I2C1_SDA_GPIO                  (GPIO_OUTPUT | GPIO_OPENDRAIN |GPIO_SPEED_50MHz | GPIO_OUTPUT_SET | GPIO_PORTB | GPIO_PIN7)
+
+#define GPIO_I2C2_SCL GPIO_I2C2_SCL_1       /* PB10 */
+#define GPIO_I2C2_SDA GPIO_I2C2_SDA_1       /* PB11*/
+
+#define GPIO_I2C2_SCL_GPIO                  (GPIO_OUTPUT | GPIO_OPENDRAIN |GPIO_SPEED_50MHz | GPIO_OUTPUT_SET | GPIO_PORTB | GPIO_PIN10)
+#define GPIO_I2C2_SDA_GPIO                  (GPIO_OUTPUT | GPIO_OPENDRAIN |GPIO_SPEED_50MHz | GPIO_OUTPUT_SET | GPIO_PORTB | GPIO_PIN11)
+
+#define GPIO_I2C4_SCL GPIO_I2C4_SCL_1       /* PD12 */
+#define GPIO_I2C4_SDA GPIO_I2C4_SDA_1       /* PD13 */
+
+#define GPIO_I2C4_SCL_GPIO                  (GPIO_OUTPUT | GPIO_OPENDRAIN | GPIO_SPEED_50MHz | GPIO_OUTPUT_SET | GPIO_PORTD | GPIO_PIN12)
+#define GPIO_I2C4_SDA_GPIO                  (GPIO_OUTPUT | GPIO_OPENDRAIN | GPIO_SPEED_50MHz | GPIO_OUTPUT_SET | GPIO_PORTD | GPIO_PIN13)
+
+/* SDMMC2
+ *
+ *      VDD 3.3
+ *      GND
+ *      SDMMC2_CK                           PD6
+ *      SDMMC2_CMD                          PD7
+ *      SDMMC2_D0                           PB14
+ *      SDMMC2_D1                           PB15
+ *      SDMMC2_D2                           PB3
+ *      SDMMC2_D3                           PB4
+ */
+
+#define GPIO_SDMMC2_CK   GPIO_SDMMC2_CK_1  /* PD6  */
+#define GPIO_SDMMC2_CMD  GPIO_SDMMC2_CMD_1 /* PD7  */
+//      GPIO_SDMMC2_D0   No Remap          /* PB14 */
+//      GPIO_SDMMC2_D1   No Remap          /* PB15 */
+#define GPIO_SDMMC2_D2   GPIO_SDMMC2_D2_2  /* PB3  */
+//      GPIO_SDMMC2_D3    No Remap         /* PB4  */
+
+/* USB
+ *
+ *      OTG_FS_DM                           PA11
+ *      OTG_FS_DP                           PA12
+ *      VBUS                                PA9
+ */
+
+
+/* Board provides GPIO or other Hardware for signaling to timing analyzer */
+
+#if defined(CONFIG_BOARD_USE_PROBES)
+# include "stm32_gpio.h"
+# define PROBE_N(n) (1<<((n)-1))
+# define PROBE_1    (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_2MHz|GPIO_OUTPUT_CLEAR|GPIO_PORTA|GPIO_PIN8)   /* PA8  AUX1 */
+# define PROBE_2    (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_2MHz|GPIO_OUTPUT_CLEAR|GPIO_PORTE|GPIO_PIN11)  /* PE11 AUX2 */
+# define PROBE_3    (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_2MHz|GPIO_OUTPUT_CLEAR|GPIO_PORTE|GPIO_PIN13)  /* PE13 AUX3 */
+# define PROBE_4    (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_2MHz|GPIO_OUTPUT_CLEAR|GPIO_PORTE|GPIO_PIN14)  /* PE14 AUX4 */
+# define PROBE_5    (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_2MHz|GPIO_OUTPUT_CLEAR|GPIO_PORTD|GPIO_PIN14)  /* PD14 AUX5 */
+# define PROBE_6    (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_2MHz|GPIO_OUTPUT_CLEAR|GPIO_PORTD|GPIO_PIN15)  /* PD15 AUX6 */
+# define PROBE_7    (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_2MHz|GPIO_OUTPUT_CLEAR|GPIO_PORTA|GPIO_PIN0)   /* PA0  AUX7 */
+# define PROBE_8    (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_2MHz|GPIO_OUTPUT_CLEAR|GPIO_PORTA|GPIO_PIN2)   /* PA1  AUX8 */
+
+# define PROBE_INIT(mask) \
+	do { \
+		if ((mask)& PROBE_N(1)) { stm32_configgpio(PROBE_1); } \
+		if ((mask)& PROBE_N(2)) { stm32_configgpio(PROBE_2); } \
+		if ((mask)& PROBE_N(3)) { stm32_configgpio(PROBE_3); } \
+		if ((mask)& PROBE_N(4)) { stm32_configgpio(PROBE_4); } \
+		if ((mask)& PROBE_N(5)) { stm32_configgpio(PROBE_5); } \
+		if ((mask)& PROBE_N(6)) { stm32_configgpio(PROBE_6); } \
+		if ((mask)& PROBE_N(7)) { stm32_configgpio(PROBE_7); } \
+		if ((mask)& PROBE_N(8)) { stm32_configgpio(PROBE_8); } \
+	} while(0)
+
+# define PROBE(n,s)  do {stm32_gpiowrite(PROBE_##n,(s));}while(0)
+# define PROBE_MARK(n) PROBE(n,false);PROBE(n,true)
+#else
+# define PROBE_INIT(mask)
+# define PROBE(n,s)
+# define PROBE_MARK(n)
+#endif
+
+#endif  /*__NUTTX_CONFIG_PX4_FMU_V6C_INCLUDE_BOARD_H  */
@@ -0,0 +1,79 @@
+/****************************************************************************
+ *
+ *   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.
+ *
+ ****************************************************************************/
+
+#pragma once
+
+// DMAMUX1 Using at most 8 Channels on DMA1 --------   Assigned
+//                                                     V
+
+#define DMAMAP_SPI1_RX    DMAMAP_DMA12_SPI1RX_0     /* 1 DMA1:37 BMI055, ICM-42688-P */
+#define DMAMAP_SPI1_TX    DMAMAP_DMA12_SPI1TX_0     /* 2 DMA1:38 BMI055, ICM-42688-P  */
+
+#define DMAMAP_SPI2_RX    DMAMAP_DMA12_SPI2RX_0     /* 3 DMA1:39 FRAM */
+#define DMAMAP_SPI2_TX    DMAMAP_DMA12_SPI2TX_0     /* 4 DMA1:40 FRAM */
+
+//#define DMAMAP_USART1_RX  DMAMAP_DMA12_USART1RX_0 /*  DMA1:41  GPS1 */
+//#define DMAMAP_USART1_TX  DMAMAP_DMA12_USART1TX_0 /*  DMA1:42  GPS1 */
+
+//#define DMAMAP_USART2_RX  DMAMAP_DMA12_USART2RX_0 /*  DMA1:43 Telem3 */
+//#define DMAMAP_USART2_TX  DMAMAP_DMA12_USART2TX_0 /*  DMA1:44 Telem3 */
+
+//#define DMAMAP_USART3_RX  DMAMAP_DMA12_USART3RX_0 /*  DMA1:45 DEBUG */
+//#define DMAMAP_USART3_TX  DMAMAP_DMA12_USART3TX_0 /*  DMA1:46 DEBUG */
+
+//#define DMAMAP_UART4_RX   DMAMAP_DMA12_UART4RX_0  /*  DMA1:63 EXT2 */
+//#define DMAMAP_UART4_TX   DMAMAP_DMA12_UART4TX_0  /*  DMA1:64 EXT2 */
+
+#define DMAMAP_USART6_RX  DMAMAP_DMA12_USART6RX_0   /* 5 DMA1:71 PX4IO */
+#define DMAMAP_USART6_TX  DMAMAP_DMA12_USART6TX_0   /* 6 DMA1:72 PX4IO */
+
+// Assigned in timer_config.cpp
+
+// Timer 4                                          /* 7 DMA1:32 TIM4UP */
+// Timer 5                                          /* 8 DMA1:50 TIM5UP */
+
+// DMAMUX2 Using at most 8 Channels on DMA2 --------   Assigned
+//                                                     V
+
+
+#define DMAMAP_USART3_RX  DMAMAP_DMA12_USART3RX_1   /* 3 DMA2:45 DEBUG */
+#define DMAMAP_USART3_TX  DMAMAP_DMA12_USART3TX_1   /* 4 DMA2:46 DEBUG */
+
+#define DMAMAP_UART5_RX   DMAMAP_DMA12_UART5RX_1    /* 5 DMA2:65 TELEM2 */
+#define DMAMAP_UART5_TX   DMAMAP_DMA12_UART5TX_1    /* 6 DMA2:66  TELEM2 */
+
+#define DMAMAP_UART7_RX   DMAMAP_DMA12_UART7RX_1    /* 7 DMA1:79  TELEM1 */
+#define DMAMAP_UART7_TX   DMAMAP_DMA12_UART7TX_1    /* 8 DMA1:80  TELEM1 */
+
+//#define DMAMAP_UART8_RX   DMAMAP_DMA12_UART8RX_1  /*  DMA1:81  GPS2 */
+//#define DMAMAP_UART8_TX   DMAMAP_DMA12_UART8TX_1  /*  DMA1:82  GPS2 */
@@ -0,0 +1,241 @@
+#
+# This file is autogenerated: PLEASE DO NOT EDIT IT.
+#
+# You can use "make menuconfig" to make any modifications to the installed .config file.
+# You can then do "make savedefconfig" to generate a new defconfig file that includes your
+# modifications.
+#
+# CONFIG_DISABLE_ENVIRON is not set
+# CONFIG_DISABLE_PSEUDOFS_OPERATIONS is not set
+# CONFIG_MMCSD_HAVE_CARDDETECT is not set
+# CONFIG_MMCSD_HAVE_WRITEPROTECT is not set
+# CONFIG_MMCSD_MMCSUPPORT is not set
+# CONFIG_MMCSD_SPI is not set
+# CONFIG_NSH_DISABLEBG is not set
+# CONFIG_NSH_DISABLESCRIPT is not set
+# CONFIG_NSH_DISABLE_DF is not set
+# CONFIG_NSH_DISABLE_EXEC is not set
+# CONFIG_NSH_DISABLE_EXIT is not set
+# CONFIG_NSH_DISABLE_GET is not set
+# CONFIG_NSH_DISABLE_ITEF is not set
+# CONFIG_NSH_DISABLE_LOOPS is not set
+# CONFIG_NSH_DISABLE_MKFATFS is not set
+# CONFIG_NSH_DISABLE_SEMICOLON is not set
+# CONFIG_NSH_DISABLE_TIME is not set
+CONFIG_ARCH="arm"
+CONFIG_ARCH_BOARD_CUSTOM=y
+CONFIG_ARCH_BOARD_CUSTOM_DIR="../../../../boards/px4/fmu-v6c/nuttx-config"
+CONFIG_ARCH_BOARD_CUSTOM_DIR_RELPATH=y
+CONFIG_ARCH_BOARD_CUSTOM_NAME="px4"
+CONFIG_ARCH_CHIP="stm32h7"
+CONFIG_ARCH_CHIP_STM32H743VI=y
+CONFIG_ARCH_CHIP_STM32H7=y
+CONFIG_ARCH_INTERRUPTSTACK=512
+CONFIG_ARCH_STACKDUMP=y
+CONFIG_ARMV7M_BASEPRI_WAR=y
+CONFIG_ARMV7M_DCACHE=y
+CONFIG_ARMV7M_DTCM=y
+CONFIG_ARMV7M_ICACHE=y
+CONFIG_ARMV7M_MEMCPY=y
+CONFIG_ARMV7M_USEBASEPRI=y
+CONFIG_ARM_MPU_EARLY_RESET=y
+CONFIG_BOARDCTL_RESET=y
+CONFIG_BOARD_CRASHDUMP=y
+CONFIG_BOARD_LOOPSPERMSEC=95751
+CONFIG_BOARD_RESET_ON_ASSERT=2
+CONFIG_BUILTIN=y
+CONFIG_C99_BOOL8=y
+CONFIG_CDCACM=y
+CONFIG_CDCACM_IFLOWCONTROL=y
+CONFIG_CDCACM_PRODUCTID=0x0038
+CONFIG_CDCACM_PRODUCTSTR="PX4 FMU v6C.x"
+CONFIG_CDCACM_RXBUFSIZE=600
+CONFIG_CDCACM_TXBUFSIZE=12000
+CONFIG_CDCACM_VENDORID=0x3185
+CONFIG_CDCACM_VENDORSTR="Auterion"
+CONFIG_CLOCK_MONOTONIC=y
+CONFIG_DEBUG_FULLOPT=y
+CONFIG_DEBUG_HARDFAULT_ALERT=y
+CONFIG_DEBUG_MEMFAULT=y
+CONFIG_DEBUG_SYMBOLS=y
+CONFIG_DEFAULT_SMALL=y
+CONFIG_DEV_FIFO_SIZE=0
+CONFIG_DEV_PIPE_MAXSIZE=1024
+CONFIG_DEV_PIPE_SIZE=70
+CONFIG_EXPERIMENTAL=y
+CONFIG_FAT_DMAMEMORY=y
+CONFIG_FAT_LCNAMES=y
+CONFIG_FAT_LFN=y
+CONFIG_FAT_LFN_ALIAS_HASH=y
+CONFIG_FDCLONE_STDIO=y
+CONFIG_FS_BINFS=y
+CONFIG_FS_CROMFS=y
+CONFIG_FS_FAT=y
+CONFIG_FS_FATTIME=y
+CONFIG_FS_PROCFS=y
+CONFIG_FS_PROCFS_INCLUDE_PROGMEM=y
+CONFIG_FS_PROCFS_MAX_TASKS=64
+CONFIG_FS_PROCFS_REGISTER=y
+CONFIG_FS_ROMFS=y
+CONFIG_GRAN=y
+CONFIG_GRAN_INTR=y
+CONFIG_HAVE_CXX=y
+CONFIG_HAVE_CXXINITIALIZE=y
+CONFIG_I2C=y
+CONFIG_I2C_RESET=y
+CONFIG_IDLETHREAD_STACKSIZE=750
+CONFIG_IOB_NBUFFERS=24
+CONFIG_IOB_NCHAINS=24
+CONFIG_LIBC_FLOATINGPOINT=y
+CONFIG_LIBC_LONG_LONG=y
+CONFIG_LIBC_STRERROR=y
+CONFIG_MEMSET_64BIT=y
+CONFIG_MEMSET_OPTSPEED=y
+CONFIG_MMCSD=y
+CONFIG_MMCSD_SDIO=y
+CONFIG_MMCSD_SDIOWAIT_WRCOMPLETE=y
+CONFIG_MM_IOB=y
+CONFIG_MM_REGIONS=4
+CONFIG_MTD=y
+CONFIG_MTD_BYTE_WRITE=y
+CONFIG_MTD_PARTITION=y
+CONFIG_MTD_PROGMEM=y
+CONFIG_MTD_RAMTRON=y
+CONFIG_NAME_MAX=40
+CONFIG_NSH_ARCHINIT=y
+CONFIG_NSH_ARGCAT=y
+CONFIG_NSH_BUILTIN_APPS=y
+CONFIG_NSH_CMDPARMS=y
+CONFIG_NSH_CROMFSETC=y
+CONFIG_NSH_LINELEN=128
+CONFIG_NSH_MAXARGUMENTS=15
+CONFIG_NSH_NESTDEPTH=8
+CONFIG_NSH_QUOTE=y
+CONFIG_NSH_ROMFSETC=y
+CONFIG_NSH_ROMFSSECTSIZE=128
+CONFIG_NSH_STRERROR=y
+CONFIG_NSH_VARS=y
+CONFIG_OTG_ID_GPIO_DISABLE=y
+CONFIG_PIPES=y
+CONFIG_PREALLOC_TIMERS=50
+CONFIG_PRIORITY_INHERITANCE=y
+CONFIG_PTHREAD_MUTEX_ROBUST=y
+CONFIG_PTHREAD_STACK_MIN=512
+CONFIG_RAMTRON_SETSPEED=y
+CONFIG_RAM_SIZE=245760
+CONFIG_RAM_START=0x20010000
+CONFIG_RAW_BINARY=y
+CONFIG_READLINE_CMD_HISTORY=y
+CONFIG_READLINE_TABCOMPLETION=y
+CONFIG_RTC_DATETIME=y
+CONFIG_SCHED_ATEXIT=y
+CONFIG_SCHED_HPWORK=y
+CONFIG_SCHED_HPWORKPRIORITY=249
+CONFIG_SCHED_HPWORKSTACKSIZE=1280
+CONFIG_SCHED_INSTRUMENTATION=y
+CONFIG_SCHED_INSTRUMENTATION_EXTERNAL=y
+CONFIG_SCHED_LPWORK=y
+CONFIG_SCHED_LPWORKPRIORITY=50
+CONFIG_SCHED_LPWORKSTACKSIZE=1632
+CONFIG_SCHED_WAITPID=y
+CONFIG_SDCLONE_DISABLE=y
+CONFIG_SEM_NNESTPRIO=8
+CONFIG_SEM_PREALLOCHOLDERS=0
+CONFIG_SERIAL_IFLOWCONTROL_WATERMARKS=y
+CONFIG_SERIAL_TERMIOS=y
+CONFIG_SIG_DEFAULT=y
+CONFIG_SIG_SIGALRM_ACTION=y
+CONFIG_SIG_SIGUSR1_ACTION=y
+CONFIG_SIG_SIGUSR2_ACTION=y
+CONFIG_SIG_SIGWORK=4
+CONFIG_STACK_COLORATION=y
+CONFIG_START_DAY=30
+CONFIG_START_MONTH=11
+CONFIG_STDIO_BUFFER_SIZE=256
+CONFIG_STM32H7_ADC1=y
+CONFIG_STM32H7_ADC3=y
+CONFIG_STM32H7_BBSRAM=y
+CONFIG_STM32H7_BBSRAM_FILES=5
+CONFIG_STM32H7_BDMA=y
+CONFIG_STM32H7_BKPSRAM=y
+CONFIG_STM32H7_DMA1=y
+CONFIG_STM32H7_DMA2=y
+CONFIG_STM32H7_DMACAPABLE=y
+CONFIG_STM32H7_FLOWCONTROL_BROKEN=y
+CONFIG_STM32H7_I2C1=y
+CONFIG_STM32H7_I2C2=y
+CONFIG_STM32H7_I2C4=y
+CONFIG_STM32H7_I2C_DYNTIMEO=y
+CONFIG_STM32H7_I2C_DYNTIMEO_STARTSTOP=10
+CONFIG_STM32H7_OTGFS=y
+CONFIG_STM32H7_PROGMEM=y
+CONFIG_STM32H7_RTC=y
+CONFIG_STM32H7_RTC_HSECLOCK=y
+CONFIG_STM32H7_RTC_MAGIC_REG=1
+CONFIG_STM32H7_SAVE_CRASHDUMP=y
+CONFIG_STM32H7_SDMMC2=y
+CONFIG_STM32H7_SERIALBRK_BSDCOMPAT=y
+CONFIG_STM32H7_SERIAL_DISABLE_REORDERING=y
+CONFIG_STM32H7_SPI1=y
+CONFIG_STM32H7_SPI1_DMA=y
+CONFIG_STM32H7_SPI1_DMA_BUFFER=1024
+CONFIG_STM32H7_SPI2=y
+CONFIG_STM32H7_SPI2_DMA=y
+CONFIG_STM32H7_SPI2_DMA_BUFFER=4096
+CONFIG_STM32H7_SPI_DMA=y
+CONFIG_STM32H7_TIM12=y
+CONFIG_STM32H7_TIM1=y
+CONFIG_STM32H7_TIM4=y
+CONFIG_STM32H7_TIM5=y
+CONFIG_STM32H7_UART5=y
+CONFIG_STM32H7_UART7=y
+CONFIG_STM32H7_UART8=y
+CONFIG_STM32H7_USART1=y
+CONFIG_STM32H7_USART2=y
+CONFIG_STM32H7_USART3=y
+CONFIG_STM32H7_USART6=y
+CONFIG_STM32H7_USART_BREAKS=y
+CONFIG_STM32H7_USART_INVERT=y
+CONFIG_STM32H7_USART_SINGLEWIRE=y
+CONFIG_STM32H7_USART_SWAP=y
+CONFIG_SYSTEM_CDCACM=y
+CONFIG_SYSTEM_NSH=y
+CONFIG_TASK_NAME_SIZE=24
+CONFIG_UART5_IFLOWCONTROL=y
+CONFIG_UART5_OFLOWCONTROL=y
+CONFIG_UART5_RXDMA=y
+CONFIG_UART5_TXBUFSIZE=3000
+CONFIG_UART5_TXDMA=y
+CONFIG_UART7_BAUD=57600
+CONFIG_UART7_IFLOWCONTROL=y
+CONFIG_UART7_OFLOWCONTROL=y
+CONFIG_UART7_RXBUFSIZE=600
+CONFIG_UART7_RXDMA=y
+CONFIG_UART7_TXBUFSIZE=3000
+CONFIG_UART7_TXDMA=y
+CONFIG_UART8_BAUD=57600
+CONFIG_UART8_RXBUFSIZE=600
+CONFIG_UART8_TXBUFSIZE=1500
+CONFIG_USART1_BAUD=57600
+CONFIG_USART1_RXBUFSIZE=600
+CONFIG_USART1_TXBUFSIZE=1500
+CONFIG_USART2_BAUD=57600
+CONFIG_USART2_RXBUFSIZE=600
+CONFIG_USART2_TXBUFSIZE=3000
+CONFIG_USART3_BAUD=57600
+CONFIG_USART3_RXBUFSIZE=180
+CONFIG_USART3_RXDMA=y
+CONFIG_USART3_SERIAL_CONSOLE=y
+CONFIG_USART3_TXBUFSIZE=1500
+CONFIG_USART3_TXDMA=y
+CONFIG_USART6_BAUD=57600
+CONFIG_USART6_RXBUFSIZE=600
+CONFIG_USART6_TXBUFSIZE=1500
+CONFIG_USBDEV=y
+CONFIG_USBDEV_BUSPOWERED=y
+CONFIG_USBDEV_MAXPOWER=500
+CONFIG_USEC_PER_TICK=1000
+CONFIG_USERMAIN_STACKSIZE=2944
+CONFIG_USER_ENTRYPOINT="nsh_main"
+CONFIG_WATCHDOG=y
+CONFIG_WQUEUE_NOTIFIER=y
@@ -0,0 +1,215 @@
+/****************************************************************************
+ * scripts/script.ld
+ *
+ *   Copyright (C) 2016, 2019 Gregory Nutt. All rights reserved.
+ *   Author: Gregory Nutt <gnutt@nuttx.org>
+ *
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright
+ *    notice, this list of conditions and the following disclaimer.
+ * 2. Redistributions in binary form must reproduce the above copyright
+ *    notice, this list of conditions and the following disclaimer in
+ *    the documentation and/or other materials provided with the
+ *    distribution.
+ * 3. Neither the name NuttX nor the names of its contributors may be
+ *    used to endorse or promote products derived from this software
+ *    without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
+ * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
+ * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *
+ ****************************************************************************/
+
+/* The PX4 FMUV6C uses an STM32H753II has 2048Kb of main FLASH memory.
+ * The flash memory is partitioned into a User Flash memory and a System
+ * Flash memory. Each of these memories has two banks:
+ *
+ *   1) User Flash memory:
+ *
+ *      Bank 1: Start address 0x0800:0000 to 0x080F:FFFF with 8 sectors, 128Kb each
+ *      Bank 2: Start address 0x0810:0000 to 0x081F:FFFF with 8 sectors, 128Kb each
+ *
+ *   2) System Flash memory:
+ *
+ *      Bank 1: Start address 0x1FF0:0000 to 0x1FF1:FFFF with 1 x 128Kb sector
+ *      Bank 1: Start address 0x1FF4:0000 to 0x1FF5:FFFF with 1 x 128Kb sector
+ *
+ *   3) User option bytes for user configuration, only in Bank 1.
+ *
+ * In the STM32H743II, two different boot spaces can be selected through
+ * the BOOT pin and the boot base address programmed in the BOOT_ADD0 and
+ * BOOT_ADD1 option bytes:
+ *
+ *   1) BOOT=0: Boot address defined by user option byte BOOT_ADD0[15:0].
+ *      ST programmed value: Flash memory at 0x0800:0000
+ *   2) BOOT=1: Boot address defined by user option byte BOOT_ADD1[15:0].
+ *      ST programmed value: System bootloader at 0x1FF0:0000
+ *
+ * The PX4 FMUV6C has a test point on board, the BOOT0 pin is at ground so by
+ * default, the STM32 will boot to address 0x0800:0000 in FLASH unless the test
+ * point is pulled to 3.3V.then the boot will be from 0x1FF0:0000
+ *
+ * The STM32H743II also has 1024Kb of data SRAM.
+ * SRAM is split up into several blocks and into three power domains:
+ *
+ *   1) TCM SRAMs are dedicated to the Cortex-M7 and are accessible with
+ *      0 wait states by the Cortex-M7 and by MDMA through AHBS slave bus
+ *
+ *      1.1) 128Kb of DTCM-RAM beginning at address 0x2000:0000
+ *
+ *           The DTCM-RAM is organized as 2 x 64Kb DTCM-RAMs on 2 x 32 bit
+ *           DTCM ports. The DTCM-RAM could be used for critical real-time
+ *           data, such as interrupt service routines or stack / heap memory.
+ *           Both DTCM-RAMs can be used in parallel (for load/store operations)
+ *           thanks to the Cortex-M7 dual issue capability.
+ *
+ *      1.2)  64Kb of ITCM-RAM beginning at address 0x0000:0000
+ *
+ *           This RAM is connected to ITCM 64-bit interface designed for
+ *           execution of critical real-times routines by the CPU.
+ *
+ *   2) AXI SRAM (D1 domain) accessible by all system masters except BDMA
+ *      through D1 domain AXI bus matrix
+ *
+ *      2.1) 512Kb of SRAM beginning at address 0x2400:0000
+ *
+ *   3) AHB SRAM (D2 domain) accessible by all system masters except BDMA
+ *      through D2 domain AHB bus matrix
+ *
+ *      3.1) 128Kb of SRAM1 beginning at address 0x3000:0000
+ *      3.2) 128Kb of SRAM2 beginning at address 0x3002:0000
+ *      3.3)  32Kb of SRAM3 beginning at address 0x3004:0000
+ *
+ *      SRAM1 - SRAM3 are one contiguous block: 288Kb at address 0x3000:0000
+ *
+ *   4) AHB SRAM (D3 domain) accessible by most of system masters
+ *      through D3 domain AHB bus matrix
+ *
+ *      4.1)  64Kb of SRAM4 beginning at address 0x3800:0000
+ *      4.1)   4Kb of backup RAM beginning at address 0x3880:0000
+ *
+ * When booting from FLASH, FLASH memory is aliased to address 0x0000:0000
+ * where the code expects to begin execution by jumping to the entry point in
+ * the 0x0800:0000 address range.
+ *
+ * The bootloader uses the first sector of the flash, which is 128K in length.
+ */
+
+MEMORY
+{
+	itcm  (rwx) : ORIGIN = 0x00000000, LENGTH =   64K
+	flash (rx)  : ORIGIN = 0x08000000, LENGTH =  128K
+	dtcm1 (rwx) : ORIGIN = 0x20000000, LENGTH =   64K
+	dtcm2 (rwx) : ORIGIN = 0x20010000, LENGTH =   64K
+	sram  (rwx) : ORIGIN = 0x24000000, LENGTH =  512K
+	sram1 (rwx) : ORIGIN = 0x30000000, LENGTH =  128K
+	sram2 (rwx) : ORIGIN = 0x30020000, LENGTH =  128K
+	sram3 (rwx) : ORIGIN = 0x30040000, LENGTH =   32K
+	sram4 (rwx) : ORIGIN = 0x38000000, LENGTH =   64K
+	bbram (rwx) : ORIGIN = 0x38800000, LENGTH =    4K
+}
+
+OUTPUT_ARCH(arm)
+EXTERN(_vectors)
+ENTRY(_stext)
+
+/*
+ * Ensure that abort() is present in the final object.  The exception handling
+ * code pulled in by libgcc.a requires it (and that code cannot be easily avoided).
+ */
+EXTERN(abort)
+EXTERN(_bootdelay_signature)
+
+SECTIONS
+{
+	.text : {
+		_stext = ABSOLUTE(.);
+		*(.vectors)
+		. = ALIGN(32);
+		/*
+		This signature provides the bootloader with a way to delay booting
+		*/
+		_bootdelay_signature = ABSOLUTE(.);
+		FILL(0xffecc2925d7d05c5)
+		. += 8;
+		*(.text .text.*)
+		*(.fixup)
+		*(.gnu.warning)
+		*(.rodata .rodata.*)
+		*(.gnu.linkonce.t.*)
+		*(.glue_7)
+		*(.glue_7t)
+		*(.got)
+		*(.gcc_except_table)
+		*(.gnu.linkonce.r.*)
+		_etext = ABSOLUTE(.);
+
+	} > flash
+
+	/*
+	 * Init functions (static constructors and the like)
+	 */
+	.init_section : {
+		_sinit = ABSOLUTE(.);
+		KEEP(*(.init_array .init_array.*))
+		_einit = ABSOLUTE(.);
+	} > flash
+
+
+	.ARM.extab : {
+		*(.ARM.extab*)
+	} > flash
+
+	__exidx_start = ABSOLUTE(.);
+	.ARM.exidx : {
+		*(.ARM.exidx*)
+	} > flash
+	__exidx_end = ABSOLUTE(.);
+
+	_eronly = ABSOLUTE(.);
+
+	.data : {
+		_sdata = ABSOLUTE(.);
+		*(.data .data.*)
+		*(.gnu.linkonce.d.*)
+		CONSTRUCTORS
+		_edata = ABSOLUTE(.);
+	} > sram AT > flash
+
+	.bss : {
+		_sbss = ABSOLUTE(.);
+		*(.bss .bss.*)
+		*(.gnu.linkonce.b.*)
+		*(COMMON)
+		. = ALIGN(4);
+		_ebss = ABSOLUTE(.);
+	} > sram
+
+	/* Stabs debugging sections. */
+	.stab 0 : { *(.stab) }
+	.stabstr 0 : { *(.stabstr) }
+	.stab.excl 0 : { *(.stab.excl) }
+	.stab.exclstr 0 : { *(.stab.exclstr) }
+	.stab.index 0 : { *(.stab.index) }
+	.stab.indexstr 0 : { *(.stab.indexstr) }
+	.comment 0 : { *(.comment) }
+	.debug_abbrev 0 : { *(.debug_abbrev) }
+	.debug_info 0 : { *(.debug_info) }
+	.debug_line 0 : { *(.debug_line) }
+	.debug_pubnames 0 : { *(.debug_pubnames) }
+	.debug_aranges 0 : { *(.debug_aranges) }
+}
@@ -0,0 +1,223 @@
+/****************************************************************************
+ * scripts/script.ld
+ *
+ *   Copyright (C) 2016, 2019 Gregory Nutt. All rights reserved.
+ *   Author: Gregory Nutt <gnutt@nuttx.org>
+ *
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright
+ *    notice, this list of conditions and the following disclaimer.
+ * 2. Redistributions in binary form must reproduce the above copyright
+ *    notice, this list of conditions and the following disclaimer in
+ *    the documentation and/or other materials provided with the
+ *    distribution.
+ * 3. Neither the name NuttX nor the names of its contributors may be
+ *    used to endorse or promote products derived from this software
+ *    without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
+ * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
+ * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *
+ ****************************************************************************/
+
+/* The PX4 FMUV6C uses an STM32H753II has 2048Kb of main FLASH memory.
+ * The flash memory is partitioned into a User Flash memory and a System
+ * Flash memory. Each of these memories has two banks:
+ *
+ *   1) User Flash memory:
+ *
+ *      Bank 1: Start address 0x0800:0000 to 0x080F:FFFF with 8 sectors, 128Kb each
+ *      Bank 2: Start address 0x0810:0000 to 0x081F:FFFF with 8 sectors, 128Kb each
+ *
+ *   2) System Flash memory:
+ *
+ *      Bank 1: Start address 0x1FF0:0000 to 0x1FF1:FFFF with 1 x 128Kb sector
+ *      Bank 1: Start address 0x1FF4:0000 to 0x1FF5:FFFF with 1 x 128Kb sector
+ *
+ *   3) User option bytes for user configuration, only in Bank 1.
+ *
+ * In the STM32H743II, two different boot spaces can be selected through
+ * the BOOT pin and the boot base address programmed in the BOOT_ADD0 and
+ * BOOT_ADD1 option bytes:
+ *
+ *   1) BOOT=0: Boot address defined by user option byte BOOT_ADD0[15:0].
+ *      ST programmed value: Flash memory at 0x0800:0000
+ *   2) BOOT=1: Boot address defined by user option byte BOOT_ADD1[15:0].
+ *      ST programmed value: System bootloader at 0x1FF0:0000
+ *
+ * The PX4 FMUV6C has a test point on board, the BOOT0 pin is at ground so by
+ * default, the STM32 will boot to address 0x0800:0000 in FLASH unless the test
+ * point is pulled to 3.3V.then the boot will be from 0x1FF0:0000
+ *
+ * The STM32H743II also has 1024Kb of data SRAM.
+ * SRAM is split up into several blocks and into three power domains:
+ *
+ *   1) TCM SRAMs are dedicated to the Cortex-M7 and are accessible with
+ *      0 wait states by the Cortex-M7 and by MDMA through AHBS slave bus
+ *
+ *      1.1) 128Kb of DTCM-RAM beginning at address 0x2000:0000
+ *
+ *           The DTCM-RAM is organized as 2 x 64Kb DTCM-RAMs on 2 x 32 bit
+ *           DTCM ports. The DTCM-RAM could be used for critical real-time
+ *           data, such as interrupt service routines or stack / heap memory.
+ *           Both DTCM-RAMs can be used in parallel (for load/store operations)
+ *           thanks to the Cortex-M7 dual issue capability.
+ *
+ *      1.2)  64Kb of ITCM-RAM beginning at address 0x0000:0000
+ *
+ *           This RAM is connected to ITCM 64-bit interface designed for
+ *           execution of critical real-times routines by the CPU.
+ *
+ *   2) AXI SRAM (D1 domain) accessible by all system masters except BDMA
+ *      through D1 domain AXI bus matrix
+ *
+ *      2.1) 512Kb of SRAM beginning at address 0x2400:0000
+ *
+ *   3) AHB SRAM (D2 domain) accessible by all system masters except BDMA
+ *      through D2 domain AHB bus matrix
+ *
+ *      3.1) 128Kb of SRAM1 beginning at address 0x3000:0000
+ *      3.2) 128Kb of SRAM2 beginning at address 0x3002:0000
+ *      3.3)  32Kb of SRAM3 beginning at address 0x3004:0000
+ *
+ *      SRAM1 - SRAM3 are one contiguous block: 288Kb at address 0x3000:0000
+ *
+ *   4) AHB SRAM (D3 domain) accessible by most of system masters
+ *      through D3 domain AHB bus matrix
+ *
+ *      4.1)  64Kb of SRAM4 beginning at address 0x3800:0000
+ *      4.1)   4Kb of backup RAM beginning at address 0x3880:0000
+ *
+ * When booting from FLASH, FLASH memory is aliased to address 0x0000:0000
+ * where the code expects to begin execution by jumping to the entry point in
+ * the 0x0800:0000 address range.
+ */
+
+MEMORY
+{
+	itcm  (rwx) : ORIGIN = 0x00000000, LENGTH =   64K
+	flash (rx)  : ORIGIN = 0x08020000, LENGTH = 1920K
+	dtcm1 (rwx) : ORIGIN = 0x20000000, LENGTH =   64K
+	dtcm2 (rwx) : ORIGIN = 0x20010000, LENGTH =   64K
+	sram  (rwx) : ORIGIN = 0x24000000, LENGTH =  512K
+	sram1 (rwx) : ORIGIN = 0x30000000, LENGTH =  128K
+	sram2 (rwx) : ORIGIN = 0x30020000, LENGTH =  128K
+	sram3 (rwx) : ORIGIN = 0x30040000, LENGTH =   32K
+	sram4 (rwx) : ORIGIN = 0x38000000, LENGTH =   64K
+	bbram (rwx) : ORIGIN = 0x38800000, LENGTH =    4K
+}
+
+OUTPUT_ARCH(arm)
+EXTERN(_vectors)
+ENTRY(_stext)
+
+/*
+ * Ensure that abort() is present in the final object.  The exception handling
+ * code pulled in by libgcc.a requires it (and that code cannot be easily avoided).
+ */
+EXTERN(abort)
+EXTERN(_bootdelay_signature)
+EXTERN(board_get_manifest)
+
+SECTIONS
+{
+	.text : {
+		_stext = ABSOLUTE(.);
+		*(.vectors)
+		. = ALIGN(32);
+		/*
+		This signature provides the bootloader with a way to delay booting
+		*/
+		_bootdelay_signature = ABSOLUTE(.);
+		FILL(0xffecc2925d7d05c5)
+		. += 8;
+		*(.text .text.*)
+		*(.fixup)
+		*(.gnu.warning)
+		*(.rodata .rodata.*)
+		*(.gnu.linkonce.t.*)
+		*(.glue_7)
+		*(.glue_7t)
+		*(.got)
+		*(.gcc_except_table)
+		*(.gnu.linkonce.r.*)
+		_etext = ABSOLUTE(.);
+
+	} > flash
+
+	/*
+	 * Init functions (static constructors and the like)
+	 */
+	.init_section : {
+		_sinit = ABSOLUTE(.);
+		KEEP(*(.init_array .init_array.*))
+		_einit = ABSOLUTE(.);
+	} > flash
+
+
+	.ARM.extab : {
+		*(.ARM.extab*)
+	} > flash
+
+	__exidx_start = ABSOLUTE(.);
+	.ARM.exidx : {
+		*(.ARM.exidx*)
+	} > flash
+	__exidx_end = ABSOLUTE(.);
+
+	_eronly = ABSOLUTE(.);
+
+	.data : {
+		_sdata = ABSOLUTE(.);
+		*(.data .data.*)
+		*(.gnu.linkonce.d.*)
+		CONSTRUCTORS
+		_edata = ABSOLUTE(.);
+	} > sram AT > flash
+
+	.bss : {
+		_sbss = ABSOLUTE(.);
+		*(.bss .bss.*)
+		*(.gnu.linkonce.b.*)
+		*(COMMON)
+		. = ALIGN(4);
+		_ebss = ABSOLUTE(.);
+	} > sram
+
+	/* Emit the the D3 power domain section for locating BDMA data */
+
+	.sram4_reserve (NOLOAD) :
+	{
+		*(.sram4)
+		. = ALIGN(4);
+		_sram4_heap_start = ABSOLUTE(.);
+	} > sram4
+
+	/* Stabs debugging sections. */
+	.stab 0 : { *(.stab) }
+	.stabstr 0 : { *(.stabstr) }
+	.stab.excl 0 : { *(.stab.excl) }
+	.stab.exclstr 0 : { *(.stab.exclstr) }
+	.stab.index 0 : { *(.stab.index) }
+	.stab.indexstr 0 : { *(.stab.indexstr) }
+	.comment 0 : { *(.comment) }
+	.debug_abbrev 0 : { *(.debug_abbrev) }
+	.debug_info 0 : { *(.debug_info) }
+	.debug_line 0 : { *(.debug_line) }
+	.debug_pubnames 0 : { *(.debug_pubnames) }
+	.debug_aranges 0 : { *(.debug_aranges) }
+}
@@ -0,0 +1,75 @@
+############################################################################
+#
+#   Copyright (c) 2016 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.
+#
+############################################################################
+if("${PX4_BOARD_LABEL}" STREQUAL  "bootloader")
+	add_compile_definitions(BOOTLOADER)
+	add_library(drivers_board
+		bootloader_main.c
+		init.c
+		usb.c
+		timer_config.cpp
+	)
+	target_link_libraries(drivers_board
+		PRIVATE
+			nuttx_arch # sdio
+			nuttx_drivers # sdio
+			px4_layer #gpio
+			arch_io_pins # iotimer
+			bootloader
+	)
+	target_include_directories(drivers_board PRIVATE ${PX4_SOURCE_DIR}/platforms/nuttx/src/bootloader/common)
+
+else()
+	add_library(drivers_board
+		can.c
+		i2c.cpp
+		init.c
+		led.c
+		mtd.cpp
+		manifest.c
+		sdio.c
+		spi.cpp
+		timer_config.cpp
+		usb.c
+	)
+	add_dependencies(drivers_board arch_board_hw_info)
+
+	target_link_libraries(drivers_board
+		PRIVATE
+			arch_spi
+			arch_board_hw_info
+			drivers__led # drv_led_start
+			nuttx_arch # sdio
+			nuttx_drivers # sdio
+			px4_layer
+	)
+endif()
@@ -0,0 +1,321 @@
+/****************************************************************************
+ *
+ *   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.
+ *
+ ****************************************************************************/
+
+/**
+ * @file board_config.h
+ *
+ * PX4FMU-v6c internal definitions
+ */
+
+#pragma once
+
+/****************************************************************************************************
+ * Included Files
+ ****************************************************************************************************/
+
+#include <px4_platform_common/px4_config.h>
+#include <nuttx/compiler.h>
+#include <stdint.h>
+
+
+#include <stm32_gpio.h>
+
+/****************************************************************************************************
+ * Definitions
+ ****************************************************************************************************/
+
+#undef TRACE_PINS
+
+/* PX4IO connection configuration */
+
+#define BOARD_USES_PX4IO_VERSION       2
+#define PX4IO_SERIAL_DEVICE            "/dev/ttyS4"
+#define PX4IO_SERIAL_TX_GPIO           GPIO_USART6_TX
+#define PX4IO_SERIAL_RX_GPIO           GPIO_USART6_RX
+#define PX4IO_SERIAL_BASE              STM32_USART6_BASE
+#define PX4IO_SERIAL_VECTOR            STM32_IRQ_USART6
+#define PX4IO_SERIAL_TX_DMAMAP         DMAMAP_USART6_TX
+#define PX4IO_SERIAL_RX_DMAMAP         DMAMAP_USART6_RX
+#define PX4IO_SERIAL_RCC_REG           STM32_RCC_APB2ENR
+#define PX4IO_SERIAL_RCC_EN            RCC_APB2ENR_USART6EN
+#define PX4IO_SERIAL_CLOCK             STM32_PCLK2_FREQUENCY
+#define PX4IO_SERIAL_BITRATE           1500000               /* 1.5Mbps -> max rate for IO */
+
+/* PX4FMU GPIOs ***********************************************************************************/
+
+
+/* LEDs are driven with push pull Anodes to 3.3V */
+
+#define GPIO_nLED_RED        /* PD10 */  (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_50MHz|GPIO_OUTPUT_SET|GPIO_PORTD|GPIO_PIN10)
+#define GPIO_nLED_BLUE       /* PD11 */  (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_50MHz|GPIO_OUTPUT_SET|GPIO_PORTD|GPIO_PIN11)
+
+#define BOARD_HAS_CONTROL_STATUS_LEDS      1
+#define BOARD_OVERLOAD_LED     LED_RED
+#define BOARD_ARMED_STATE_LED  LED_BLUE
+
+/*
+ * ADC channels
+ *
+ * These are the channel numbers of the ADCs of the microcontroller that
+ * can be used by the Px4 Firmware in the adc driver
+ */
+
+/* ADC defines to be used in sensors.cpp to read from a particular channel */
+
+#define ADC1_CH(n)                  (n)
+
+/* N.B. there is no offset mapping needed for ADC3 because */
+#define ADC3_CH(n)                  (n)
+
+/* We are only use ADC3 for REV/VER. */
+
+/* Define GPIO pins used as ADC N.B. Channel numbers must match below  */
+
+#define PX4_ADC_GPIO  \
+	/* PC4  */  GPIO_ADC12_INP4,   \
+	/* PB1  */  GPIO_ADC12_INP5,   \
+	/* PC5  */  GPIO_ADC12_INP8,   \
+	/* PC0  */  GPIO_ADC123_INP10, \
+	/* PC1  */  GPIO_ADC123_INP11, \
+	/* PA2  */  GPIO_ADC12_INP14,  \
+	/* PA4  */  GPIO_ADC12_INP18   \
+
+/* Define Channel numbers must match above GPIO pin IN(n)*/
+#define ADC_BATTERY1_CURRENT_CHANNEL            /* PC4 */  ADC1_CH(4)
+#define ADC_BATTERY2_VOLTAGE_CHANNEL            /* PB1 */  ADC1_CH(5)
+#define ADC_BATTERY1_VOLTAGE_CHANNEL            /* PC5 */  ADC1_CH(8)
+#define ADC_HW_REV_SENSE_CHANNEL                /* PC0 */  ADC3_CH(10)
+#define ADC_HW_VER_SENSE_CHANNEL                /* PC1 */  ADC3_CH(11)
+#define ADC_BATTERY2_CURRENT_CHANNEL            /* PA2 */  ADC1_CH(14)
+#define ADC_SCALED_V5_CHANNEL                   /* PA4 */  ADC1_CH(18)
+
+#define ADC_CHANNELS \
+	((1 << ADC_BATTERY1_CURRENT_CHANNEL) | \
+	 (1 << ADC_BATTERY2_VOLTAGE_CHANNEL) | \
+	 (1 << ADC_BATTERY1_VOLTAGE_CHANNEL) | \
+	 (1 << ADC_BATTERY2_CURRENT_CHANNEL) | \
+	 (1 << ADC_SCALED_V5_CHANNEL       ))
+
+#define HW_REV_VER_ADC_BASE STM32_ADC3_BASE
+
+#define SYSTEM_ADC_BASE STM32_ADC1_BASE
+
+/* HW has to large of R termination on ADC todo:change when HW value is chosen */
+#define BOARD_ADC_OPEN_CIRCUIT_V     (5.6f)
+
+/* HW Version and Revision drive signals Default to 1 to detect */
+#define BOARD_HAS_HW_VERSIONING
+
+#define GPIO_HW_VER_REV_DRIVE  /* PE12 */ (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_2MHz|GPIO_OUTPUT_SET|GPIO_PORTE|GPIO_PIN12)
+#define GPIO_HW_REV_SENSE      /* PC0 */  GPIO_ADC123_INP10
+#define GPIO_HW_VER_SENSE      /* PC1 */  GPIO_ADC123_INP11
+#define HW_INFO_INIT           {'V','6','C','x', 'x',0}
+#define HW_INFO_INIT_VER       3 /* Offset in above string of the VER */
+#define HW_INFO_INIT_REV       4 /* Offset in above string of the REV */
+
+#define BOARD_NUM_SPI_CFG_HW_VERSIONS 2 // Rev 0, 10 Sensor sets
+//                 Base/FMUM
+#define V6C00   HW_VER_REV(0x0,0x0) // FMUV6C,                 Rev 0
+#define V6C10   HW_VER_REV(0x1,0x0) // NO PX4IO,               Rev 0
+
+
+/* HEATER
+ * PWM in future
+ */
+#define GPIO_HEATER_OUTPUT   /* PB9  T17CH1 */ (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_2MHz|GPIO_OUTPUT_CLEAR|GPIO_PORTB|GPIO_PIN9)
+#define HEATER_OUTPUT_EN(on_true)	       px4_arch_gpiowrite(GPIO_HEATER_OUTPUT, (on_true))
+
+/* PWM
+ */
+#define DIRECT_PWM_OUTPUT_CHANNELS   8
+
+
+/* Power supply control and monitoring GPIOs */
+
+#define GPIO_nPOWER_IN_A                /* PA15  */ (GPIO_INPUT|GPIO_PULLUP|GPIO_PORTA|GPIO_PIN15)
+#define GPIO_nPOWER_IN_B                /* PB12  */ (GPIO_INPUT|GPIO_PULLUP|GPIO_PORTB|GPIO_PIN12)
+#define GPIO_nPOWER_IN_C                /* PE15  */ (GPIO_INPUT|GPIO_PULLUP|GPIO_PORTE|GPIO_PIN15)
+
+#define GPIO_nVDD_BRICK1_VALID          GPIO_nPOWER_IN_A /* Brick 1 Is Chosen */
+#define GPIO_nVDD_BRICK2_VALID          GPIO_nPOWER_IN_B /* Brick 2 Is Chosen  */
+#define BOARD_NUMBER_BRICKS             2
+#define GPIO_nVDD_USB_VALID             GPIO_nPOWER_IN_C /* USB     Is Chosen */
+
+#define GPIO_VDD_5V_PERIPH_nEN          /* PE2  */ (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_2MHz|GPIO_OUTPUT_SET|GPIO_PORTE|GPIO_PIN2)
+#define GPIO_VDD_5V_PERIPH_nOC          /* PE3  */ (GPIO_INPUT |GPIO_FLOAT|GPIO_PORTE|GPIO_PIN3)
+#define GPIO_VDD_5V_HIPOWER_nEN         /* PC10 */ (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_2MHz|GPIO_OUTPUT_SET|GPIO_PORTC|GPIO_PIN10)
+#define GPIO_VDD_5V_HIPOWER_nOC         /* PC11 */ (GPIO_INPUT |GPIO_FLOAT|GPIO_PORTC|GPIO_PIN11)
+#define GPIO_VDD_3V3_SENSORS_EN         /* PB2  */ (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_2MHz|GPIO_OUTPUT_CLEAR|GPIO_PORTB|GPIO_PIN2)
+
+/* Define True logic Power Control in arch agnostic form */
+
+#define VDD_5V_PERIPH_EN(on_true)          px4_arch_gpiowrite(GPIO_VDD_5V_PERIPH_nEN, !(on_true))
+#define VDD_5V_HIPOWER_EN(on_true)         px4_arch_gpiowrite(GPIO_VDD_5V_HIPOWER_nEN, !(on_true))
+#define VDD_3V3_SENSORS_EN(on_true)       px4_arch_gpiowrite(GPIO_VDD_3V3_SENSORS4_EN, (on_true))
+
+/* Tone alarm output */
+
+#define TONE_ALARM_TIMER        3  /* Timer 3 */
+#define TONE_ALARM_CHANNEL      3  /* PB0 GPIO_TIM3_CH3OUT_1 */
+
+#define GPIO_BUZZER_1           /* PPB0 */ (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_2MHz|GPIO_OUTPUT_CLEAR|GPIO_PORTB|GPIO_PIN0)
+
+#define GPIO_TONE_ALARM_IDLE    GPIO_BUZZER_1
+#define GPIO_TONE_ALARM         GPIO_TIM3_CH3OUT_1
+
+/* USB OTG FS
+ *
+ * PA9  OTG_FS_VBUS VBUS sensing
+ */
+#define GPIO_OTGFS_VBUS         /* PA9 */ (GPIO_INPUT|GPIO_PULLDOWN|GPIO_SPEED_100MHz|GPIO_PORTA|GPIO_PIN9)
+
+/* High-resolution timer */
+#define HRT_TIMER               8  /* use timer8 for the HRT */
+#define HRT_TIMER_CHANNEL       3  /* use capture/compare channel 3 */
+
+/* PWM input driver. Use FMU AUX5 pins attached to timer4 channel 3 */
+#define PWMIN_TIMER                       4
+#define PWMIN_TIMER_CHANNEL    /* T4C3 */ 3
+#define GPIO_PWM_IN            /* PD14 */ GPIO_TIM4_CH3IN_2
+
+#define SDIO_SLOTNO                    0  /* Only one slot */
+#define SDIO_MINOR                     0
+
+/* SD card bringup does not work if performed on the IDLE thread because it
+ * will cause waiting.  Use either:
+ *
+ *  CONFIG_LIB_BOARDCTL=y, OR
+ *  CONFIG_BOARD_INITIALIZE=y && CONFIG_BOARD_INITTHREAD=y
+ */
+
+#if defined(CONFIG_BOARD_INITIALIZE) && !defined(CONFIG_LIB_BOARDCTL) && \
+   !defined(CONFIG_BOARD_INITTHREAD)
+#  warning SDIO initialization cannot be perfomed on the IDLE thread
+#endif
+
+/* By Providing BOARD_ADC_USB_CONNECTED (using the px4_arch abstraction)
+ * this board support the ADC system_power interface, and therefore
+ * provides the true logic GPIO BOARD_ADC_xxxx macros.
+ */
+#define BOARD_ADC_USB_CONNECTED (px4_arch_gpioread(GPIO_OTGFS_VBUS))
+#define BOARD_ADC_USB_VALID     (!px4_arch_gpioread(GPIO_nVDD_USB_VALID))
+
+/* FMUv6C never powers off the Servo rail */
+
+#define BOARD_ADC_SERVO_VALID     (1)
+
+#define BOARD_ADC_BRICK1_VALID  (!px4_arch_gpioread(GPIO_nVDD_BRICK1_VALID))
+#define BOARD_ADC_BRICK2_VALID  (!px4_arch_gpioread(GPIO_nVDD_BRICK2_VALID))
+
+#define BOARD_ADC_PERIPH_5V_OC  (!px4_arch_gpioread(GPIO_VDD_5V_PERIPH_nOC))
+#define BOARD_ADC_HIPOWER_5V_OC (!px4_arch_gpioread(GPIO_VDD_5V_HIPOWER_nOC))
+
+
+/* This board provides a DMA pool and APIs */
+#define BOARD_DMA_ALLOC_POOL_SIZE 5120
+
+/* This board provides the board_on_reset interface */
+
+#define BOARD_HAS_ON_RESET 1
+
+#define PX4_GPIO_INIT_LIST { \
+		PX4_ADC_GPIO,                     \
+		GPIO_HW_VER_REV_DRIVE,            \
+		GPIO_CAN1_TX,                     \
+		GPIO_CAN1_RX,                     \
+		GPIO_CAN2_TX,                     \
+		GPIO_CAN2_RX,                     \
+		GPIO_HEATER_OUTPUT,               \
+		GPIO_nPOWER_IN_A,                 \
+		GPIO_nPOWER_IN_B,                 \
+		GPIO_nPOWER_IN_C,                 \
+		GPIO_VDD_5V_PERIPH_nEN,           \
+		GPIO_VDD_5V_PERIPH_nOC,           \
+		GPIO_VDD_5V_HIPOWER_nEN,          \
+		GPIO_VDD_5V_HIPOWER_nOC,          \
+		GPIO_VDD_3V3_SENSORS_EN,          \
+		GPIO_TONE_ALARM_IDLE,             \
+	}
+
+#define BOARD_ENABLE_CONSOLE_BUFFER
+
+#define PX4_I2C_BUS_MTD      4,5
+
+
+#define BOARD_NUM_IO_TIMERS 5
+
+__BEGIN_DECLS
+
+/****************************************************************************************************
+ * Public Types
+ ****************************************************************************************************/
+
+/****************************************************************************************************
+ * Public data
+ ****************************************************************************************************/
+
+#ifndef __ASSEMBLY__
+
+/****************************************************************************************************
+ * Public Functions
+ ****************************************************************************************************/
+
+/****************************************************************************
+ * Name: stm32_sdio_initialize
+ *
+ * Description:
+ *   Initialize SDIO-based MMC/SD card support
+ *
+ ****************************************************************************/
+
+int stm32_sdio_initialize(void);
+
+/****************************************************************************************************
+ * Name: stm32_spiinitialize
+ *
+ * Description:
+ *   Called to configure SPI chip select GPIO pins for the PX4FMU board.
+ *
+ ****************************************************************************************************/
+
+extern void stm32_spiinitialize(void);
+
+extern void stm32_usbinitialize(void);
+
+extern void board_peripheral_reset(int ms);
+
+#include <px4_platform_common/board_common.h>
+
+#endif /* __ASSEMBLY__ */
+
+__END_DECLS
@@ -1,6 +1,6 @@
-/***************************************************************************
+/****************************************************************************
 *
- *   Copyright (c) 2013-2014 PX4 Development Team. All rights reserved.
+ *   Copyright (c) 2019, 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
@@ -30,44 +30,33 @@
 * POSSIBILITY OF SUCH DAMAGE.
 *
 ****************************************************************************/
+
 /**
- * @file enginefailure.h
- * Helper class for a fixedwing engine failure mode
+ * @file bootloader_main.c
 *
- * @author Thomas Gubler <thomasgubler@gmail.com>
- */
+ * FMU-specific early startup code for bootloader
+*/

-#pragma once
+#include "board_config.h"
+#include "bl.h"

-#include "navigator_mode.h"
-#include "mission_block.h"
+#include <nuttx/config.h>
+#include <nuttx/board.h>
+#include <chip.h>
+#include <stm32_uart.h>
+#include <arch/board/board.h>
+#include "arm_internal.h"
+#include <px4_platform_common/init.h>

-class Navigator;
+extern int sercon_main(int c, char **argv);

-class EngineFailure : public MissionBlock
+void board_late_initialize(void)
 {
-public:
-	EngineFailure(Navigator *navigator);
-	~EngineFailure() = default;
+	sercon_main(0, NULL);
+}

-	void on_inactive() override;
-	void on_activation() override;
-	void on_active() override;
-
-private:
-	enum EFState {
-		EF_STATE_NONE = 0,
-		EF_STATE_LOITERDOWN = 1,
-	} _ef_state{EF_STATE_NONE};
-
-	/**
-	 * Set the DLL item
-	 */
-	void		set_ef_item();
-
-	/**
-	 * Move to next EF item
-	 */
-	void		advance_ef();
-
-};
+extern void sys_tick_handler(void);
+void board_timerhook(void)
+{
+	sys_tick_handler();
+}
@@ -0,0 +1,128 @@
+/****************************************************************************
+ *
+ *   Copyright (C) 2012 PX4 Development Team. All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright
+ *    notice, this list of conditions and the following disclaimer.
+ * 2. Redistributions in binary form must reproduce the above copyright
+ *    notice, this list of conditions and the following disclaimer in
+ *    the documentation and/or other materials provided with the
+ *    distribution.
+ * 3. Neither the name PX4 nor the names of its contributors may be
+ *    used to endorse or promote products derived from this software
+ *    without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
+ * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
+ * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *
+ ****************************************************************************/
+
+/**
+ * @file px4fmu_can.c
+ *
+ * Board-specific CAN functions.
+ */
+
+#ifdef CONFIG_CAN
+
+#include <errno.h>
+#include <debug.h>
+
+#include <nuttx/can/can.h>
+#include <arch/board/board.h>
+
+#include "chip.h"
+#include "arm_arch.h"
+
+#include "chip.h"
+#include "stm32_can.h"
+#include "board_config.h"
+
+#ifdef CONFIG_CAN
+
+/************************************************************************************
+ * Pre-processor Definitions
+ ************************************************************************************/
+/* Configuration ********************************************************************/
+
+#if defined(CONFIG_STM32_CAN1) && defined(CONFIG_STM32_CAN2)
+#  warning "Both CAN1 and CAN2 are enabled.  Assuming only CAN1."
+#  undef CONFIG_STM32_CAN2
+#endif
+
+#ifdef CONFIG_STM32_CAN1
+#  define CAN_PORT 1
+#else
+#  define CAN_PORT 2
+#endif
+
+/************************************************************************************
+ * Private Functions
+ ************************************************************************************/
+
+/************************************************************************************
+ * Public Functions
+ ************************************************************************************/
+int can_devinit(void);
+
+/************************************************************************************
+ * Name: can_devinit
+ *
+ * Description:
+ *   All STM32 architectures must provide the following interface to work with
+ *   examples/can.
+ *
+ ************************************************************************************/
+
+int can_devinit(void)
+{
+	static bool initialized = false;
+	struct can_dev_s *can;
+	int ret;
+
+	/* Check if we have already initialized */
+
+	if (!initialized) {
+		/* Call stm32_caninitialize() to get an instance of the CAN interface */
+
+		can = stm32_caninitialize(CAN_PORT);
+
+		if (can == NULL) {
+			canerr("ERROR:  Failed to get CAN interface\n");
+			return -ENODEV;
+		}
+
+		/* Register the CAN driver at "/dev/can0" */
+
+		ret = can_register("/dev/can0", can);
+
+		if (ret < 0) {
+			canerr("ERROR: can_register failed: %d\n", ret);
+			return ret;
+		}
+
+		/* Now we are initialized */
+
+		initialized = true;
+	}
+
+	return OK;
+}
+
+#endif
+
+#endif /* CONFIG_CAN */
@@ -0,0 +1,128 @@
+/*
+ * hw_config.h
+ *
+ *  Created on: May 17, 2015
+ *      Author: david_s5
+ */
+
+#ifndef HW_CONFIG_H_
+#define HW_CONFIG_H_
+
+/****************************************************************************
+ * 10-8--2016:
+ *  To simplify the ripple effect on the tools, we will be using
+ *  /dev/serial/by-id/<asterisk>PX4<asterisk> to locate PX4 devices. Therefore
+ *  moving forward all Bootloaders must contain the prefix "PX4 BL "
+ *  in the USBDEVICESTRING
+ *  This Change will be made in an upcoming BL release
+ ****************************************************************************/
+/*
+ * Define usage to configure a bootloader
+ *
+ *
+ * Constant                example          Usage
+ * APP_LOAD_ADDRESS     0x08004000            - The address in Linker Script, where the app fw is org-ed
+ * BOOTLOADER_DELAY     5000                  - Ms to wait while under USB pwr or bootloader request
+ * BOARD_FMUV2
+ * INTERFACE_USB        1                     - (Optional) Scan and use the USB interface for bootloading
+ * INTERFACE_USART      1                     - (Optional) Scan and use the Serial interface for bootloading
+ * USBDEVICESTRING      "PX4 BL FMU v2.x"     - USB id string
+ * USBPRODUCTID         0x0011                - PID Should match defconfig
+ * BOOT_DELAY_ADDRESS   0x000001a0            - (Optional) From the linker script from Linker Script to get a custom
+ *                                               delay provided by an APP FW
+ * BOARD_TYPE           9                     - Must match .prototype boad_id
+ * _FLASH_KBYTES        (*(uint16_t *)0x1fff7a22) - Run time flash size detection
+ * BOARD_FLASH_SECTORS  ((_FLASH_KBYTES == 0x400) ? 11 : 23) - Run time determine the physical last sector
+ * BOARD_FLASH_SECTORS   11                   - Hard coded zero based last sector
+ * BOARD_FLASH_SIZE     (_FLASH_KBYTES*1024)-   Total Flash size of device, determined at run time.
+ *                         (1024 * 1024)      - Hard coded Total Flash of device - The bootloader and app reserved will be deducted
+ *                                              programmatically
+ *
+ * BOARD_FIRST_FLASH_SECTOR_TO_ERASE  2        - Optional sectors index in the flash_sectors table  (F4 only), to begin erasing.
+ *                                               This is to allow sectors to be reserved for app fw usage. That will NOT be erased
+ *                                               during a FW upgrade.
+ *                                               The default is 0, and selects the first sector to be erased, as the 0th entry in the
+ *                                               flash_sectors table. Which is the second physical sector of FLASH in the device.
+ *                                               The first physical sector of FLASH is used by the bootloader, and is not defined
+ *                                               in the table.
+ *
+ * APP_RESERVATION_SIZE (BOARD_FIRST_FLASH_SECTOR_TO_ERASE * 16 * 1024) - Number of bytes reserved by the APP FW. This number plus
+ *                                                                        BOOTLOADER_RESERVATION_SIZE  will be deducted from
+ *                                                                        BOARD_FLASH_SIZE to determine the size of the App FW
+ *                                                                        and hence the address space of FLASH to erase and program.
+ * USBMFGSTRING            "PX4 AP"            - Optional USB MFG string (default is '3D Robotics' if not defined.)
+ * SERIAL_BREAK_DETECT_DISABLED                -  Optional prevent break selection on Serial port from entering or staying in BL
+ *
+ * * Other defines are somewhat self explanatory.
+ */
+
+/* Boot device selection list*/
+#define USB0_DEV       0x01
+#define SERIAL0_DEV    0x02
+#define SERIAL1_DEV    0x04
+
+#define APP_LOAD_ADDRESS               0x08020000
+#define BOOTLOADER_DELAY               5000
+#define INTERFACE_USB                  1
+#define INTERFACE_USB_CONFIG           "/dev/ttyACM0"
+#define BOARD_VBUS                     MK_GPIO_INPUT(GPIO_OTGFS_VBUS)
+
+//#define USE_VBUS_PULL_DOWN
+#define INTERFACE_USART                1
+#define INTERFACE_USART_CONFIG         "/dev/ttyS0,115200"
+#define BOOT_DELAY_ADDRESS             0x000001a0
+#define BOARD_TYPE                     56
+#define _FLASH_KBYTES                  (*(uint32_t *)0x1FF1E880)
+#define BOARD_FLASH_SECTORS            (15)
+#define BOARD_FLASH_SIZE               (_FLASH_KBYTES * 1024)
+
+#define OSC_FREQ                       16
+
+#define BOARD_PIN_LED_ACTIVITY         GPIO_nLED_BLUE // BLUE
+#define BOARD_PIN_LED_BOOTLOADER       GPIO_nLED_RED // RED
+#define BOARD_LED_ON                   0
+#define BOARD_LED_OFF                  1
+
+#define SERIAL_BREAK_DETECT_DISABLED   1
+
+/*
+ * Uncommenting this allows to force the bootloader through
+ * a PWM output pin. As this can accidentally initialize
+ * an ESC prematurely, it is not recommended. This feature
+ * has not been used and hence defaults now to off.
+ *
+ * # define BOARD_FORCE_BL_PIN_OUT         (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_2MHz|GPIO_OUTPUT_SET|GPIO_PORTE|GPIO_PIN14)
+ * # define BOARD_FORCE_BL_PIN_IN          (GPIO_INPUT|GPIO_PULLUP|GPIO_PORTE|GPIO_PIN11)
+ *
+ * # define BOARD_POWER_PIN_OUT           (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_2MHz|GPIO_OUTPUT_SET|GPIO_PORTA|GPIO_PIN4)
+ * # define BOARD_POWER_ON                 1
+ * # define BOARD_POWER_OFF                0
+ * # undef  BOARD_POWER_PIN_RELEASE        // Leave pin enabling Power - un comment to release (disable power)
+ *
+*/
+
+#if !defined(ARCH_SN_MAX_LENGTH)
+# define ARCH_SN_MAX_LENGTH 12
+#endif
+
+#if !defined(APP_RESERVATION_SIZE)
+#  define APP_RESERVATION_SIZE 0
+#endif
+
+#if !defined(BOARD_FIRST_FLASH_SECTOR_TO_ERASE)
+#  define BOARD_FIRST_FLASH_SECTOR_TO_ERASE 1
+#endif
+
+#if !defined(USB_DATA_ALIGN)
+# define USB_DATA_ALIGN
+#endif
+
+#ifndef BOOT_DEVICES_SELECTION
+#  define BOOT_DEVICES_SELECTION USB0_DEV|SERIAL0_DEV|SERIAL1_DEV
+#endif
+
+#ifndef BOOT_DEVICES_FILTER_ONUSB
+#  define BOOT_DEVICES_FILTER_ONUSB USB0_DEV|SERIAL0_DEV|SERIAL1_DEV
+#endif
+
+#endif /* HW_CONFIG_H_ */
@@ -0,0 +1,40 @@
+/****************************************************************************
+ *
+ *   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 <px4_arch/i2c_hw_description.h>
+
+constexpr px4_i2c_bus_t px4_i2c_buses[I2C_BUS_MAX_BUS_ITEMS] = {
+	initI2CBusExternal(1),
+	initI2CBusExternal(2),
+	initI2CBusInternal(4),
+};
@@ -0,0 +1,259 @@
+/****************************************************************************
+ *
+ *   Copyright (c) 2012-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.
+ *
+ ****************************************************************************/
+
+/**
+ * @file init.c
+ *
+ * PX4FMU-specific early startup code.  This file implements the
+ * board_app_initialize() function that is called early by nsh during startup.
+ *
+ * Code here is run before the rcS script is invoked; it should start required
+ * subsystems and perform board-specific initialization.
+ */
+
+/****************************************************************************
+ * Included Files
+ ****************************************************************************/
+
+#include "board_config.h"
+
+#include <stdbool.h>
+#include <stdio.h>
+#include <string.h>
+#include <debug.h>
+#include <errno.h>
+#include <syslog.h>
+
+#include <nuttx/config.h>
+#include <nuttx/board.h>
+#include <nuttx/spi/spi.h>
+#include <nuttx/sdio.h>
+#include <nuttx/mmcsd.h>
+#include <nuttx/analog/adc.h>
+#include <nuttx/mm/gran.h>
+#include <chip.h>
+#include <stm32_uart.h>
+#include <arch/board/board.h>
+#include "arm_internal.h"
+
+#include <drivers/drv_hrt.h>
+#include <drivers/drv_board_led.h>
+#include <systemlib/px4_macros.h>
+#include <px4_arch/io_timer.h>
+#include <px4_platform_common/init.h>
+#include <px4_platform/gpio.h>
+#include <px4_platform/board_determine_hw_info.h>
+#include <px4_platform/board_dma_alloc.h>
+
+/****************************************************************************
+ * Pre-Processor Definitions
+ ****************************************************************************/
+
+/* Configuration ************************************************************/
+
+/*
+ * Ideally we'd be able to get these from arm_internal.h,
+ * but since we want to be able to disable the NuttX use
+ * of leds for system indication at will and there is no
+ * separate switch, we need to build independent of the
+ * CONFIG_ARCH_LEDS configuration switch.
+ */
+__BEGIN_DECLS
+extern void led_init(void);
+extern void led_on(int led);
+extern void led_off(int led);
+__END_DECLS
+
+
+/************************************************************************************
+ * Name: board_peripheral_reset
+ *
+ * Description:
+ *
+ ************************************************************************************/
+__EXPORT void board_peripheral_reset(int ms)
+{
+	/* set the peripheral rails off */
+
+	VDD_5V_PERIPH_EN(false);
+	board_control_spi_sensors_power(false, 0xffff);
+
+	/* wait for the peripheral rail to reach GND */
+	usleep(ms * 1000);
+	syslog(LOG_DEBUG, "reset done, %d ms\n", ms);
+
+	/* re-enable power */
+
+	/* switch the peripheral rail back on */
+	board_control_spi_sensors_power(true, 0xffff);
+	VDD_5V_PERIPH_EN(true);
+
+}
+
+/************************************************************************************
+ * Name: board_on_reset
+ *
+ * Description:
+ * Optionally provided function called on entry to board_system_reset
+ * It should perform any house keeping prior to the rest.
+ *
+ * status - 1 if resetting to boot loader
+ *          0 if just resetting
+ *
+ ************************************************************************************/
+__EXPORT void board_on_reset(int status)
+{
+	for (int i = 0; i < DIRECT_PWM_OUTPUT_CHANNELS; ++i) {
+		px4_arch_configgpio(io_timer_channel_get_gpio_output(i));
+	}
+
+	if (status >= 0) {
+		up_mdelay(6);
+	}
+}
+
+/************************************************************************************
+ * Name: stm32_boardinitialize
+ *
+ * Description:
+ *   All STM32 architectures must provide the following entry point.  This entry point
+ *   is called early in the initialization -- after all memory has been configured
+ *   and mapped but before any devices have been initialized.
+ *
+ ************************************************************************************/
+
+__EXPORT void
+stm32_boardinitialize(void)
+{
+	board_on_reset(-1); /* Reset PWM first thing */
+
+	/* configure LEDs */
+
+	board_autoled_initialize();
+
+	/* configure pins */
+
+	const uint32_t gpio[] = PX4_GPIO_INIT_LIST;
+	px4_gpio_init(gpio, arraySize(gpio));
+
+	/* configure USB interfaces */
+
+	stm32_usbinitialize();
+}
+
+/****************************************************************************
+ * Name: board_app_initialize
+ *
+ * Description:
+ *   Perform application specific initialization.  This function is never
+ *   called directly from application code, but only indirectly via the
+ *   (non-standard) boardctl() interface using the command BOARDIOC_INIT.
+ *
+ * Input Parameters:
+ *   arg - The boardctl() argument is passed to the board_app_initialize()
+ *         implementation without modification.  The argument has no
+ *         meaning to NuttX; the meaning of the argument is a contract
+ *         between the board-specific initalization logic and the the
+ *         matching application logic.  The value cold be such things as a
+ *         mode enumeration value, a set of DIP switch switch settings, a
+ *         pointer to configuration data read from a file or serial FLASH,
+ *         or whatever you would like to do with it.  Every implementation
+ *         should accept zero/NULL as a default configuration.
+ *
+ * Returned Value:
+ *   Zero (OK) is returned on success; a negated errno value is returned on
+ *   any failure to indicate the nature of the failure.
+ *
+ ****************************************************************************/
+
+__EXPORT int board_app_initialize(uintptr_t arg)
+{
+#if !defined(BOOTLOADER)
+
+	/* Power on Interfaces */
+	VDD_5V_PERIPH_EN(true);
+	VDD_5V_HIPOWER_EN(true);
+
+	/* Need hrt running before using the ADC */
+
+	px4_platform_init();
+
+
+	if (OK == board_determine_hw_info()) {
+		syslog(LOG_INFO, "[boot] Rev 0x%1x : Ver 0x%1x %s\n", board_get_hw_revision(), board_get_hw_version(),
+		       board_get_hw_type_name());
+
+	} else {
+		syslog(LOG_ERR, "[boot] Failed to read HW revision and version\n");
+	}
+
+	stm32_spiinitialize();
+
+	board_spi_reset(10, 0xffff);
+
+	/* configure the DMA allocator */
+
+	if (board_dma_alloc_init() < 0) {
+		syslog(LOG_ERR, "[boot] DMA alloc FAILED\n");
+	}
+
+#  if defined(SERIAL_HAVE_RXDMA)
+	// set up the serial DMA polling at 1ms intervals for received bytes that have not triggered a DMA event.
+	static struct hrt_call serial_dma_call;
+	hrt_call_every(&serial_dma_call, 1000, 1000, (hrt_callout)stm32_serial_dma_poll, NULL);
+#  endif
+
+	/* initial LED state */
+	drv_led_start();
+	led_off(LED_RED);
+	led_off(LED_BLUE);
+
+	if (board_hardfault_init(2, true) != 0) {
+		led_on(LED_RED);
+	}
+
+#  ifdef CONFIG_MMCSD
+	int ret = stm32_sdio_initialize();
+
+	if (ret != OK) {
+		led_on(LED_RED);
+	}
+
+#  endif /* CONFIG_MMCSD */
+
+	/* Configure the HW based on the manifest */
+
+	px4_platform_configure();
+#endif
+	return OK;
+}
@@ -0,0 +1,235 @@
+/****************************************************************************
+ *
+ *   Copyright (c) 2013 PX4 Development Team. All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright
+ *    notice, this list of conditions and the following disclaimer.
+ * 2. Redistributions in binary form must reproduce the above copyright
+ *    notice, this list of conditions and the following disclaimer in
+ *    the documentation and/or other materials provided with the
+ *    distribution.
+ * 3. Neither the name PX4 nor the names of its contributors may be
+ *    used to endorse or promote products derived from this software
+ *    without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
+ * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
+ * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *
+ ****************************************************************************/
+
+/**
+ * @file px4fmu2_led.c
+ *
+ * PX4FMU LED backend.
+ */
+
+#include <px4_platform_common/px4_config.h>
+
+#include <stdbool.h>
+
+#include "chip.h"
+#include "stm32_gpio.h"
+#include "board_config.h"
+
+#include <nuttx/board.h>
+#include <arch/board/board.h>
+
+/*
+ * Ideally we'd be able to get these from arm_internal.h,
+ * but since we want to be able to disable the NuttX use
+ * of leds for system indication at will and there is no
+ * separate switch, we need to build independent of the
+ * CONFIG_ARCH_LEDS configuration switch.
+ */
+__BEGIN_DECLS
+extern void led_init(void);
+extern void led_on(int led);
+extern void led_off(int led);
+extern void led_toggle(int led);
+__END_DECLS
+
+#ifdef CONFIG_ARCH_LEDS
+static bool nuttx_owns_leds = true;
+//                                B  R  S  G
+//                                0  1  2  3
+static const uint8_t xlatpx4[] = {1, 2, 4, 0};
+#  define xlat(p) xlatpx4[(p)]
+static uint32_t g_ledmap[] = {
+	GPIO_nLED_GREEN,   // Indexed by BOARD_LED_GREEN
+	GPIO_nLED_BLUE,    // Indexed by BOARD_LED_BLUE
+	GPIO_nLED_RED,     // Indexed by BOARD_LED_RED
+	GPIO_nSAFETY_SWITCH_LED_OUT,  // Indexed by LED_SAFETY by xlatpx4
+};
+
+#else
+
+#  define xlat(p) (p)
+static uint32_t g_ledmap[] = {
+	GPIO_nLED_BLUE,                     // Indexed by LED_BLUE
+	GPIO_nLED_RED,                      // Indexed by LED_RED, LED_AMBER
+	0,                                  // Indexed by LED_SAFETY (defaulted to an input)
+	0,                                  // Indexed by LED_GREEN
+};
+
+#endif
+
+__EXPORT void led_init(void)
+{
+	for (size_t l = 0; l < (sizeof(g_ledmap) / sizeof(g_ledmap[0])); l++) {
+		if (g_ledmap[l] != 0) {
+			stm32_configgpio(g_ledmap[l]);
+		}
+	}
+}
+
+static void phy_set_led(int led, bool state)
+{
+	/* Drive Low to switch on */
+
+	if (g_ledmap[led] != 0) {
+		stm32_gpiowrite(g_ledmap[led], !state);
+	}
+}
+
+static bool phy_get_led(int led)
+{
+	/* If Low it is on */
+	if (g_ledmap[led] != 0) {
+		return !stm32_gpioread(g_ledmap[led]);
+	}
+
+	return false;
+}
+
+__EXPORT void led_on(int led)
+{
+	phy_set_led(xlat(led), true);
+}
+
+__EXPORT void led_off(int led)
+{
+	phy_set_led(xlat(led), false);
+}
+
+__EXPORT void led_toggle(int led)
+{
+	phy_set_led(xlat(led), !phy_get_led(xlat(led)));
+}
+
+#ifdef CONFIG_ARCH_LEDS
+/****************************************************************************
+ * Public Functions
+ ****************************************************************************/
+
+/****************************************************************************
+ * Name: board_autoled_initialize
+ ****************************************************************************/
+
+void board_autoled_initialize(void)
+{
+	led_init();
+}
+
+/****************************************************************************
+ * Name: board_autoled_on
+ ****************************************************************************/
+
+void board_autoled_on(int led)
+{
+	if (!nuttx_owns_leds) {
+		return;
+	}
+
+	switch (led) {
+	default:
+		break;
+
+	case LED_HEAPALLOCATE:
+		phy_set_led(BOARD_LED_BLUE, true);
+		break;
+
+	case LED_IRQSENABLED:
+		phy_set_led(BOARD_LED_BLUE, false);
+		phy_set_led(BOARD_LED_GREEN, true);
+		break;
+
+	case LED_STACKCREATED:
+		phy_set_led(BOARD_LED_GREEN, true);
+		phy_set_led(BOARD_LED_BLUE, true);
+		break;
+
+	case LED_INIRQ:
+		phy_set_led(BOARD_LED_BLUE, true);
+		break;
+
+	case LED_SIGNAL:
+		phy_set_led(BOARD_LED_GREEN, true);
+		break;
+
+	case LED_ASSERTION:
+		phy_set_led(BOARD_LED_RED, true);
+		phy_set_led(BOARD_LED_BLUE, true);
+		break;
+
+	case LED_PANIC:
+		phy_set_led(BOARD_LED_RED, true);
+		break;
+
+	case LED_IDLE : /* IDLE */
+		phy_set_led(BOARD_LED_RED, true);
+		break;
+	}
+}
+
+/****************************************************************************
+ * Name: board_autoled_off
+ ****************************************************************************/
+
+void board_autoled_off(int led)
+{
+	if (!nuttx_owns_leds) {
+		return;
+	}
+
+	switch (led) {
+	default:
+		break;
+
+	case LED_SIGNAL:
+		phy_set_led(BOARD_LED_GREEN, false);
+		break;
+
+	case LED_INIRQ:
+		phy_set_led(BOARD_LED_BLUE, false);
+		break;
+
+	case LED_ASSERTION:
+		phy_set_led(BOARD_LED_RED, false);
+		phy_set_led(BOARD_LED_BLUE, false);
+		break;
+
+	case LED_PANIC:
+		phy_set_led(BOARD_LED_RED, false);
+		break;
+
+	case LED_IDLE : /* IDLE */
+		phy_set_led(BOARD_LED_RED, false);
+		break;
+	}
+}
+
+#endif /* CONFIG_ARCH_LEDS */
@@ -0,0 +1,147 @@
+/****************************************************************************
+ *
+ *   Copyright (c) 2018, 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
+ * are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright
+ *    notice, this list of conditions and the following disclaimer.
+ * 2. Redistributions in binary form must reproduce the above copyright
+ *    notice, this list of conditions and the following disclaimer in
+ *    the documentation and/or other materials provided with the
+ *    distribution.
+ * 3. Neither the name PX4 nor the names of its contributors may be
+ *    used to endorse or promote products derived from this software
+ *    without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
+ * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
+ * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *
+ ****************************************************************************/
+
+/**
+ * @file manifest.c
+ *
+ * This module supplies the interface to the manifest of hardware that is
+ * optional and dependent on the HW REV and HW VER IDs
+ *
+ * The manifest allows the system to know whether a hardware option
+ * say for example the PX4IO is an no-pop option vs it is broken.
+ *
+ */
+
+/****************************************************************************
+ * Included Files
+ ****************************************************************************/
+
+#include <nuttx/config.h>
+#include <board_config.h>
+
+#include <inttypes.h>
+#include <stdbool.h>
+#include <syslog.h>
+
+#include "systemlib/px4_macros.h"
+
+/****************************************************************************
+ * Pre-Processor Definitions
+ ****************************************************************************/
+
+typedef struct {
+	uint32_t                hw_ver_rev; /* the version and revision */
+	const px4_hw_mft_item_t *mft;       /* The first entry */
+	uint32_t                entries;    /* the lenght of the list */
+} px4_hw_mft_list_entry_t;
+
+typedef px4_hw_mft_list_entry_t *px4_hw_mft_list_entry;
+#define px4_hw_mft_list_uninitialized (px4_hw_mft_list_entry) -1
+
+static const px4_hw_mft_item_t device_unsupported = {0, 0, 0};
+
+// List of components on a specific board configuration
+// The index of those components is given by the enum (px4_hw_mft_item_id_t)
+// declared in board_common.h
+static const px4_hw_mft_item_t hw_mft_list_v0600[] = {
+	{
+		.present     = 1,
+		.mandatory   = 1,
+		.connection  = px4_hw_con_onboard,
+	},
+};
+
+static const px4_hw_mft_item_t hw_mft_list_v0610[] = {
+	{
+		.present     = 0,
+		.mandatory   = 0,
+		.connection  = px4_hw_con_unknown,
+	},
+	{
+		.present     = 1,
+		.mandatory   = 1,
+		.connection  = px4_hw_con_onboard,
+	},
+};
+
+
+static px4_hw_mft_list_entry_t mft_lists[] = {
+//  ver_rev
+	{V6C00, hw_mft_list_v0600, arraySize(hw_mft_list_v0600)},
+	{V6C10, hw_mft_list_v0610, arraySize(hw_mft_list_v0610)}, // No PX4IO
+};
+
+/************************************************************************************
+ * Name: board_query_manifest
+ *
+ * Description:
+ *   Optional returns manifest item.
+ *
+ * Input Parameters:
+ *   manifest_id - the ID for the manifest item to retrieve
+ *
+ * Returned Value:
+ *   0 - item is not in manifest => assume legacy operations
+ *   pointer to a manifest item
+ *
+ ************************************************************************************/
+
+__EXPORT px4_hw_mft_item board_query_manifest(px4_hw_mft_item_id_t id)
+{
+	static px4_hw_mft_list_entry boards_manifest = px4_hw_mft_list_uninitialized;
+
+	if (boards_manifest == px4_hw_mft_list_uninitialized) {
+		uint32_t ver_rev = board_get_hw_version() << 8;
+		ver_rev |= board_get_hw_revision();
+
+		for (unsigned i = 0; i < arraySize(mft_lists); i++) {
+			if (mft_lists[i].hw_ver_rev == ver_rev) {
+				boards_manifest = &mft_lists[i];
+				break;
+			}
+		}
+
+		if (boards_manifest == px4_hw_mft_list_uninitialized) {
+			syslog(LOG_ERR, "[boot] Board %4" PRIx32 " is not supported!\n", ver_rev);
+		}
+	}
+
+	px4_hw_mft_item rv = &device_unsupported;
+
+	if (boards_manifest != px4_hw_mft_list_uninitialized &&
+	    id < boards_manifest->entries) {
+		rv = &boards_manifest->mft[id];
+	}
+
+	return rv;
+}
@@ -0,0 +1,104 @@
+/****************************************************************************
+ *
+ *   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 <nuttx/spi/spi.h>
+#include <px4_platform_common/px4_manifest.h>
+//                                                              KiB BS    nB
+static const px4_mft_device_t spi2 = {             // FM25V02A on FMUM 32K 512 X 64
+	.bus_type = px4_mft_device_t::SPI,
+	.devid    = SPIDEV_FLASH(0)
+};
+
+static const px4_mft_device_t i2c4 = {             // 24LC64T on IMU   8K 32 X 256
+	.bus_type =  px4_mft_device_t::I2C,
+	.devid    =  PX4_MK_I2C_DEVID(4, 0x50)
+};
+
+
+static const px4_mtd_entry_t fmum_fram = {
+	.device = &spi2,
+	.npart = 2,
+	.partd = {
+		{
+			.type = MTD_PARAMETERS,
+			.path = "/fs/mtd_params",
+			.nblocks = 32
+		},
+		{
+			.type = MTD_WAYPOINTS,
+			.path = "/fs/mtd_waypoints",
+			.nblocks = 32
+
+		}
+	},
+};
+
+static const px4_mtd_entry_t imu_eeprom = {
+	.device = &i2c4,
+	.npart = 2,
+	.partd = {
+		{
+			.type = MTD_CALDATA,
+			.path = "/fs/mtd_caldata",
+			.nblocks = 248
+		},
+		{
+			.type = MTD_ID,
+			.path = "/fs/mtd_id",
+			.nblocks = 8 // 256 = 32 * 8
+		}
+	},
+};
+
+static const px4_mtd_manifest_t board_mtd_config = {
+	.nconfigs   = 2,
+	.entries = {
+		&fmum_fram,
+		&imu_eeprom
+	}
+};
+
+static const px4_mft_entry_s mtd_mft = {
+	.type = MTD,
+	.pmft = (void *) &board_mtd_config,
+};
+
+static const px4_mft_s mft = {
+	.nmft = 1,
+	.mfts = &mtd_mft
+};
+
+const px4_mft_s *board_get_manifest(void)
+{
+	return &mft;
+}
@@ -0,0 +1,177 @@
+/****************************************************************************
+ *
+ *   Copyright (C) 2014, 2016 Gregory Nutt. All rights reserved.
+ *   Author: Gregory Nutt <gnutt@nuttx.org>
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright
+ *    notice, this list of conditions and the following disclaimer.
+ * 2. Redistributions in binary form must reproduce the above copyright
+ *    notice, this list of conditions and the following disclaimer in
+ *    the documentation and/or other materials provided with the
+ *    distribution.
+ * 3. Neither the name NuttX nor the names of its contributors may be
+ *    used to endorse or promote products derived from this software
+ *    without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
+ * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
+ * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *
+ ****************************************************************************/
+
+/****************************************************************************
+ * Included Files
+ ****************************************************************************/
+
+#include <nuttx/config.h>
+#include <board_config.h>
+
+#include <stdbool.h>
+#include <stdio.h>
+#include <debug.h>
+#include <errno.h>
+
+#include <nuttx/sdio.h>
+#include <nuttx/mmcsd.h>
+
+#include "chip.h"
+#include "board_config.h"
+#include "stm32_gpio.h"
+#include "stm32_sdmmc.h"
+
+#ifdef CONFIG_MMCSD
+
+
+/****************************************************************************
+ * Pre-processor Definitions
+ ****************************************************************************/
+
+/* Card detections requires card support and a card detection GPIO */
+
+#define HAVE_NCD   1
+#if !defined(GPIO_SDMMC1_NCD)
+#  undef HAVE_NCD
+#endif
+
+/****************************************************************************
+ * Private Data
+ ****************************************************************************/
+
+static FAR struct sdio_dev_s *sdio_dev;
+#ifdef HAVE_NCD
+static bool g_sd_inserted = 0xff; /* Impossible value */
+#endif
+
+/****************************************************************************
+ * Private Functions
+ ****************************************************************************/
+
+/****************************************************************************
+ * Name: stm32_ncd_interrupt
+ *
+ * Description:
+ *   Card detect interrupt handler.
+ *
+ ****************************************************************************/
+
+#ifdef HAVE_NCD
+static int stm32_ncd_interrupt(int irq, FAR void *context)
+{
+	bool present;
+
+	present = !stm32_gpioread(GPIO_SDMMC1_NCD);
+
+	if (sdio_dev && present != g_sd_inserted) {
+		sdio_mediachange(sdio_dev, present);
+		g_sd_inserted = present;
+	}
+
+	return OK;
+}
+#endif
+
+/****************************************************************************
+ * Public Functions
+ ****************************************************************************/
+
+/****************************************************************************
+ * Name: stm32_sdio_initialize
+ *
+ * Description:
+ *   Initialize SDIO-based MMC/SD card support
+ *
+ ****************************************************************************/
+
+int stm32_sdio_initialize(void)
+{
+	int ret;
+
+#ifdef HAVE_NCD
+	/* Card detect */
+
+	bool cd_status;
+
+	/* Configure the card detect GPIO */
+
+	stm32_configgpio(GPIO_SDMMC1_NCD);
+
+	/* Register an interrupt handler for the card detect pin */
+
+	stm32_gpiosetevent(GPIO_SDMMC1_NCD, true, true, true, stm32_ncd_interrupt);
+#endif
+
+	/* Mount the SDIO-based MMC/SD block driver */
+	/* First, get an instance of the SDIO interface */
+
+	finfo("Initializing SDIO slot %d\n", SDIO_SLOTNO);
+
+	sdio_dev = sdio_initialize(SDIO_SLOTNO);
+
+	if (!sdio_dev) {
+		syslog(LOG_ERR, "[boot] Failed to initialize SDIO slot %d\n", SDIO_SLOTNO);
+		return -ENODEV;
+	}
+
+	/* Now bind the SDIO interface to the MMC/SD driver */
+
+	finfo("Bind SDIO to the MMC/SD driver, minor=%d\n", SDIO_MINOR);
+
+	ret = mmcsd_slotinitialize(SDIO_MINOR, sdio_dev);
+
+	if (ret != OK) {
+		syslog(LOG_ERR, "[boot] Failed to bind SDIO to the MMC/SD driver: %d\n", ret);
+		return ret;
+	}
+
+	finfo("Successfully bound SDIO to the MMC/SD driver\n");
+
+#ifdef HAVE_NCD
+	/* Use SD card detect pin to check if a card is g_sd_inserted */
+
+	cd_status = !stm32_gpioread(GPIO_SDMMC1_NCD);
+	finfo("Card detect : %d\n", cd_status);
+
+	sdio_mediachange(sdio_dev, cd_status);
+#else
+	/* Assume that the SD card is inserted.  What choice do we have? */
+
+	sdio_mediachange(sdio_dev, true);
+#endif
+
+	return OK;
+}
+
+#endif /* CONFIG_MMCSD */
@@ -0,0 +1,61 @@
+/****************************************************************************
+ *
+ *   Copyright (C) 2020, 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.
+ *
+ ****************************************************************************/
+
+#include <px4_arch/spi_hw_description.h>
+#include <drivers/drv_sensor.h>
+#include <nuttx/spi/spi.h>
+
+constexpr px4_spi_bus_all_hw_t px4_spi_buses_all_hw[BOARD_NUM_SPI_CFG_HW_VERSIONS] = {
+	initSPIHWVersion(V6C00, {
+		initSPIBus(SPI::Bus::SPI1, {
+			initSPIDevice(DRV_GYR_DEVTYPE_BMI055,  SPI::CS{GPIO::PortC, GPIO::Pin14}, SPI::DRDY{GPIO::PortE, GPIO::Pin5}),
+			initSPIDevice(DRV_ACC_DEVTYPE_BMI055,  SPI::CS{GPIO::PortC, GPIO::Pin15}, SPI::DRDY{GPIO::PortE, GPIO::Pin4}),
+			initSPIDevice(DRV_IMU_DEVTYPE_ICM42688P, SPI::CS{GPIO::PortC, GPIO::Pin13}, SPI::DRDY{GPIO::PortE, GPIO::Pin6}),
+		}, {GPIO::PortB, GPIO::Pin2}),
+		initSPIBus(SPI::Bus::SPI2, {
+			initSPIDevice(SPIDEV_FLASH(0), SPI::CS{GPIO::PortD, GPIO::Pin4})
+		}),
+	}),
+	initSPIHWVersion(V6C10, {
+		initSPIBus(SPI::Bus::SPI1, {
+			initSPIDevice(DRV_GYR_DEVTYPE_BMI055,  SPI::CS{GPIO::PortC, GPIO::Pin14}, SPI::DRDY{GPIO::PortE, GPIO::Pin5}),
+			initSPIDevice(DRV_ACC_DEVTYPE_BMI055,  SPI::CS{GPIO::PortC, GPIO::Pin15}, SPI::DRDY{GPIO::PortE, GPIO::Pin4}),
+			initSPIDevice(DRV_IMU_DEVTYPE_ICM42688P, SPI::CS{GPIO::PortC, GPIO::Pin13}, SPI::DRDY{GPIO::PortE, GPIO::Pin6}),
+		}, {GPIO::PortB, GPIO::Pin2}),
+		initSPIBus(SPI::Bus::SPI2, {
+			initSPIDevice(SPIDEV_FLASH(0), SPI::CS{GPIO::PortD, GPIO::Pin4})
+		}),
+	}),
+};
+
+static constexpr bool unused = validateSPIConfig(px4_spi_buses_all_hw);
@@ -0,0 +1,73 @@
+/****************************************************************************
+ *
+ *   Copyright (C) 2012 PX4 Development Team. All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright
+ *    notice, this list of conditions and the following disclaimer.
+ * 2. Redistributions in binary form must reproduce the above copyright
+ *    notice, this list of conditions and the following disclaimer in
+ *    the documentation and/or other materials provided with the
+ *    distribution.
+ * 3. Neither the name PX4 nor the names of its contributors may be
+ *    used to endorse or promote products derived from this software
+ *    without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
+ * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
+ * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *
+ ****************************************************************************/
+
+#include <px4_arch/io_timer_hw_description.h>
+
+/* Timer allocation
+ *
+ * TIM1_CH1  T FMU_CH1
+ * TIM1_CH2  T FMU_CH2
+ * TIM1_CH3  T FMU_CH3
+ * TIM1_CH4  T FMU_CH4
+ *
+ * TIM4_CH3  T FMU_CH5
+ * TIM4_CH4  T FMU_CH6
+ *
+ * TIM5_CH1  T FMU_CH7
+ * TIM5_CH2  T FMU_CH8
+ *
+ * TIM17_CH1 T HEATER                 > PWM OUT or GPIO
+ *
+ * TIM3_CH3  T BUZZER_1              - Driven by other driver
+ */
+
+constexpr io_timers_t io_timers[MAX_IO_TIMERS] = {
+	initIOTimer(Timer::Timer1, DMA{DMA::Index1}),
+	initIOTimer(Timer::Timer4, DMA{DMA::Index1}),
+	initIOTimer(Timer::Timer5),
+	initIOTimer(Timer::Timer17),
+};
+
+constexpr timer_io_channels_t timer_io_channels[MAX_TIMER_IO_CHANNELS] = {
+	initIOTimerChannel(io_timers, {Timer::Timer1, Timer::Channel1}, {GPIO::PortA, GPIO::Pin8}),
+	initIOTimerChannel(io_timers, {Timer::Timer1, Timer::Channel2}, {GPIO::PortE, GPIO::Pin11}),
+	initIOTimerChannel(io_timers, {Timer::Timer1, Timer::Channel3}, {GPIO::PortE, GPIO::Pin13}),
+	initIOTimerChannel(io_timers, {Timer::Timer1, Timer::Channel4}, {GPIO::PortE, GPIO::Pin14}),
+	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::Timer5, Timer::Channel1}, {GPIO::PortA, GPIO::Pin0}),
+	initIOTimerChannel(io_timers, {Timer::Timer5, Timer::Channel2}, {GPIO::PortA, GPIO::Pin1}),
+};
+
+constexpr io_timers_channel_mapping_t io_timers_channel_mapping =
+	initIOTimerChannelMapping(io_timers, timer_io_channels);
@@ -0,0 +1,105 @@
+/****************************************************************************
+ *
+ *   Copyright (C) 2016 PX4 Development Team. All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright
+ *    notice, this list of conditions and the following disclaimer.
+ * 2. Redistributions in binary form must reproduce the above copyright
+ *    notice, this list of conditions and the following disclaimer in
+ *    the documentation and/or other materials provided with the
+ *    distribution.
+ * 3. Neither the name PX4 nor the names of its contributors may be
+ *    used to endorse or promote products derived from this software
+ *    without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
+ * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
+ * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *
+ ****************************************************************************/
+
+/**
+ * @file px4fmu_usb.c
+ *
+ * Board-specific USB functions.
+ */
+
+/************************************************************************************
+ * Included Files
+ ************************************************************************************/
+
+#include <px4_platform_common/px4_config.h>
+
+#include <sys/types.h>
+#include <stdint.h>
+#include <stdbool.h>
+#include <debug.h>
+
+#include <nuttx/usb/usbdev.h>
+#include <nuttx/usb/usbdev_trace.h>
+
+#include <arm_arch.h>
+#include <chip.h>
+#include <stm32_gpio.h>
+#include <stm32_otg.h>
+#include "board_config.h"
+
+/************************************************************************************
+ * Definitions
+ ************************************************************************************/
+
+/************************************************************************************
+ * Private Functions
+ ************************************************************************************/
+
+/************************************************************************************
+ * Public Functions
+ ************************************************************************************/
+
+/************************************************************************************
+ * Name: stm32_usbinitialize
+ *
+ * Description:
+ *   Called to setup USB-related GPIO pins for the PX4FMU board.
+ *
+ ************************************************************************************/
+
+__EXPORT void stm32_usbinitialize(void)
+{
+	/* The OTG FS has an internal soft pull-up */
+
+	/* Configure the OTG FS VBUS sensing GPIO, Power On, and Overcurrent GPIOs */
+
+#ifdef CONFIG_STM32H7_OTGFS
+	stm32_configgpio(GPIO_OTGFS_VBUS);
+#endif
+}
+
+/************************************************************************************
+ * Name:  stm32_usbsuspend
+ *
+ * Description:
+ *   Board logic must provide the stm32_usbsuspend logic if the USBDEV driver is
+ *   used.  This function is called whenever the USB enters or leaves suspend mode.
+ *   This is an opportunity for the board logic to shutdown clocks, power, etc.
+ *   while the USB is suspended.
+ *
+ ************************************************************************************/
+
+__EXPORT void stm32_usbsuspend(FAR struct usbdev_s *dev, bool resume)
+{
+	uinfo("resume: %d\n", resume);
+}
@@ -25,17 +25,23 @@ then
 	ina238 -X -b 2 -t 2 -k start
 fi

+if ver hwtypecmp V6X04 V6X14
+then
+	# Internal SPI bus ICM20649
+	icm20649 -s -R 6 start
+else
 # Internal SPI BMI088
-bmi088 -A -R 4 -s start
-bmi088 -G -R 4 -s start
+	bmi088 -A -R 4 -s start
+	bmi088 -G -R 4 -s start
+fi

 # Internal SPI bus ICM42688p
 icm42688p -R 6 -s start

-if ver hwtypecmp V6X03 V6X13
+if ver hwtypecmp V6X03 V6X13 V6X04 V6X14
 then
 	# Internal SPI bus ICM-42670-P (hard-mounted)
-	icm42670p -R 14 -s start
+	icm42670p -R 10 -s start
 else
 	# Internal SPI bus ICM-20649 (hard-mounted)
 	icm20649 -R 14 -s start
@@ -215,13 +215,15 @@
 #define HW_INFO_INIT_VER       3 /* Offset in above string of the VER */
 #define HW_INFO_INIT_REV       4 /* Offset in above string of the REV */

-#define BOARD_NUM_SPI_CFG_HW_VERSIONS 2 // Rev 0 and Rev 3 Sensor sets
+#define BOARD_NUM_SPI_CFG_HW_VERSIONS 3 // Rev 0 and Rev 3,4 Sensor sets
 //                 Base/FMUM
 #define V6X00   HW_VER_REV(0x0,0x0) // FMUV6X,                 Rev 0
 #define V6X01   HW_VER_REV(0x0,0x1) // FMUV6X,     BMI388 I2C2 Rev 1
 #define V6X03   HW_VER_REV(0x0,0x3) // FMUV6X,     Sensor Set  Rev 3
+#define V6X04   HW_VER_REV(0x0,0x4) // FMUV6X,     Sensor Set  Rev 4
 #define V6X10   HW_VER_REV(0x1,0x0) // NO PX4IO,               Rev 0
 #define V6X13   HW_VER_REV(0x1,0x3) // NO PX4IO,   Sensor Set  Rev 3
+#define V6X14   HW_VER_REV(0x1,0x4) // NO PX4IO,   Sensor Set  Rev 4


 /* HEATER
@@ -102,6 +102,8 @@ static px4_hw_mft_list_entry_t mft_lists[] = {
 	{V6X01, hw_mft_list_v0600, arraySize(hw_mft_list_v0600)}, // BMP388 moved to I2C2
 	{V6X03, hw_mft_list_v0600, arraySize(hw_mft_list_v0600)}, // BMP388 moved to I2C2, Sensor Set 3
 	{V6X13, hw_mft_list_v0610, arraySize(hw_mft_list_v0610)}, // No PX4IO BMP388 moved to I2C2, Sensor Set 3
+	{V6X04, hw_mft_list_v0600, arraySize(hw_mft_list_v0600)}, // BMP388 moved to I2C2, Sensor Set 4
+	{V6X14, hw_mft_list_v0610, arraySize(hw_mft_list_v0610)}, // No PX4IO BMP388 moved to I2C2, Sensor Set 4
 };

 /************************************************************************************
@@ -83,6 +83,29 @@ constexpr px4_spi_bus_all_hw_t px4_spi_buses_all_hw[BOARD_NUM_SPI_CFG_HW_VERSION
 			initSPIConfigExternal(SPI::CS{GPIO::PortA, GPIO::Pin15}, SPI::DRDY{GPIO::PortD, GPIO::Pin12}),
 		}),
 	}),
+
+	initSPIHWVersion(HW_VER_REV(0, 4), {
+		initSPIBus(SPI::Bus::SPI1, {
+			initSPIDevice(DRV_IMU_DEVTYPE_ICM42670P, SPI::CS{GPIO::PortI, GPIO::Pin9}, SPI::DRDY{GPIO::PortF, GPIO::Pin2}),
+		}, {GPIO::PortI, GPIO::Pin11}),
+		initSPIBus(SPI::Bus::SPI2, {
+			initSPIDevice(DRV_IMU_DEVTYPE_ICM42688P, SPI::CS{GPIO::PortH, GPIO::Pin5}, SPI::DRDY{GPIO::PortA, GPIO::Pin10}),
+		}, {GPIO::PortF, GPIO::Pin4}),
+		initSPIBus(SPI::Bus::SPI3, {
+			initSPIDevice(DRV_IMU_DEVTYPE_ICM20649, SPI::CS{GPIO::PortI, GPIO::Pin4}, SPI::DRDY{GPIO::PortI, GPIO::Pin7}),
+		}, {GPIO::PortE, GPIO::Pin7}),
+		//  initSPIBus(SPI::Bus::SPI4, {
+		//    // no devices
+		// TODO: if enabled, remove GPIO_VDD_3V3_SENSORS4_EN from board_config.h
+		//  }, {GPIO::PortG, GPIO::Pin8}),
+		initSPIBus(SPI::Bus::SPI5, {
+			initSPIDevice(SPIDEV_FLASH(0), SPI::CS{GPIO::PortG, GPIO::Pin7})
+		}),
+		initSPIBusExternal(SPI::Bus::SPI6, {
+			initSPIConfigExternal(SPI::CS{GPIO::PortI, GPIO::Pin10}, SPI::DRDY{GPIO::PortD, GPIO::Pin11}),
+			initSPIConfigExternal(SPI::CS{GPIO::PortA, GPIO::Pin15}, SPI::DRDY{GPIO::PortD, GPIO::Pin12}),
+		}),
+	}),
 };

 static constexpr bool unused = validateSPIConfig(px4_spi_buses_all_hw);
@@ -137,7 +137,6 @@ set(msg_files
 	rc_parameter_map.msg
 	rpm.msg
 	rtl_time_estimate.msg
-	safety.msg
 	satellite_info.msg
 	sensor_accel.msg
 	sensor_accel_fifo.msg
@@ -164,6 +163,7 @@ set(msg_files
 	timesync.msg
 	timesync_status.msg
 	trajectory_bezier.msg
+	trajectory_setpoint.msg
 	trajectory_waypoint.msg
 	transponder_report.msg
 	tune_control.msg
@@ -4,6 +4,8 @@ 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),
@@ -19,3 +19,5 @@ 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,6 +8,8 @@ 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)
@@ -8,5 +8,7 @@ bool fd_ext
 bool fd_arm_escs
 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
@@ -19,7 +19,7 @@ bool status_rc_ppm
 bool status_rc_sbus
 bool status_rc_st24
 bool status_rc_sumd
-bool status_safety_off
+bool status_safety_button_event # px4io safety button was pressed for longer than 1 second

 # PX4IO alarms (PX4IO_P_STATUS_ALARMS)
 bool alarm_pwm_error
@@ -1,3 +0,0 @@
-uint64 timestamp		# time since system start (microseconds)
-bool safety_switch_available		# Set to true if a safety switch is connected
-bool safety_off			# Set to true if safety is off
@@ -2,22 +2,24 @@
 # 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 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 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.
@@ -11,6 +11,8 @@ 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,8 +59,7 @@ rtps:
    send:    true
  - msg:     vehicle_local_position_setpoint
    receive: true
-  - msg:     trajectory_setpoint # multi-topic / alias of vehicle_local_position_setpoint
-    base:    vehicle_local_position_setpoint
+  - msg:     trajectory_setpoint
    receive: true
  - msg:     vehicle_odometry
    send:    true
@@ -0,0 +1,15 @@
+# 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
@@ -8,12 +8,15 @@ 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,6 +4,7 @@ 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,18 +1,19 @@
 # Local position setpoint in NED frame
-# setting something to NaN means the state should not be controlled
+# Telemetry of PID position controller to monitor tracking.
+# NaN means the state was not 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

-# TOPICS vehicle_local_position_setpoint trajectory_setpoint
+float32 yaw		# in radians NED -PI..+PI
+float32 yawspeed	# in radians/sec
@@ -1,5 +1,3 @@
-# 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_INIT = 0
@@ -11,14 +9,15 @@ uint8 ARMING_STATE_IN_AIR_RESTORE = 5
 uint8 ARMING_STATE_MAX = 6

 # FailureDetector status
-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_BATTERY = 32          # (1 << 5)
-uint8 FAILURE_IMBALANCED_PROP = 64 # (1 << 6)
+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)

 # HIL
 uint8 HIL_STATE_OFF = 0
@@ -31,7 +30,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_AUTO_LANDENGFAIL = 8 	# Auto land on engine failure
+uint8 NAVIGATION_STATE_UNUSED3 = 8 	        # Free slot
 uint8 NAVIGATION_STATE_UNUSED = 9	        # Free slot
 uint8 NAVIGATION_STATE_ACRO = 10		# Acro mode
 uint8 NAVIGATION_STATE_UNUSED1 = 11		# Free slot
@@ -68,13 +67,12 @@ 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 (fixed-wing, rotary wing, ground)
+uint8 vehicle_type          # Type of vehicle (rotary wing/fixed-wing/rover/airship, see above)
                            # 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

@@ -85,11 +83,10 @@ 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

-uint8 failure_detector_status			# Bitmask containing FailureDetector status [0, 0, 0, 0, 0, FAILURE_ALT, FAILURE_PITCH, FAILURE_ROLL]
+uint16 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
@@ -119,3 +116,6 @@ uint8 latest_disarming_reason
 uint64 armed_time       # Arming timestamp (microseconds)

 uint64 takeoff_time     # Takeoff timestamp (microseconds)
+
+bool safety_button_available	# Set to true if a safety button is connected
+bool safety_off					# Set to true if safety is off
@@ -28,7 +28,6 @@ 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,8 +7,6 @@ uint8 VEHICLE_VTOL_STATE_FW = 4

 uint64 timestamp			# time since system start (microseconds)

-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
+uint8 vehicle_vtol_state		# current state of the vtol, see VEHICLE_VTOL_STATE
+
 bool vtol_transition_failsafe		# vtol in transition failsafe mode
-bool fw_permanent_stab			# In fw mode stabilize attitude even if in manual mode
@@ -92,7 +92,13 @@ enum Timer {
 	Timer13,
 	Timer14,
 #ifdef STM32_TIM15_BASE
-	Timer15
+	Timer15,
+#endif
+#ifdef STM32_TIM16_BASE
+	Timer16,
+#endif
+#ifdef STM32_TIM17_BASE
+	Timer17
 #endif
 };
 enum Channel {
@@ -161,6 +167,16 @@ static inline constexpr uint32_t timerBaseRegister(Timer::Timer timer)
 	case Timer::Timer15: return STM32_TIM15_BASE;
 #endif

+#ifdef STM32_TIM16_BASE
+
+	case Timer::Timer16: return STM32_TIM16_BASE;
+#endif
+
+#ifdef STM32_TIM17_BASE
+
+	case Timer::Timer17: return STM32_TIM17_BASE;
+#endif
+
 	default: break;
 	}

@@ -86,6 +86,8 @@ static inline constexpr uint32_t getTimerUpdateDMAMap(Timer::Timer timer, const
 	case Timer::Timer13:
 	case Timer::Timer14:
 	case Timer::Timer15:
+	case Timer::Timer16:
+	case Timer::Timer17:
 		break;
 	}

@@ -49,6 +49,8 @@ static inline constexpr timer_io_channels_t initIOTimerGPIOInOut(Timer::TimerCha
 	switch (timer.timer) {
 	case Timer::Timer1:
 	case Timer::Timer2:
+	case Timer::Timer16:
+	case Timer::Timer17:
 		gpio_af = GPIO_AF1;
 		break;

@@ -274,6 +276,28 @@ static inline constexpr io_timers_t initIOTimer(Timer::Timer timer, DMA dshot_dm
 		ret.vectorno =  STM32_IRQ_TIM15;
 #ifdef CONFIG_STM32_TIM15
 		nuttx_config_timer_enabled = true;
+#endif
+		break;
+
+	case Timer::Timer16:
+		ret.base = STM32_TIM16_BASE;
+		ret.clock_register = STM32_RCC_APB2ENR;
+		ret.clock_bit = RCC_APB2ENR_TIM16EN;
+		ret.clock_freq = STM32_APB2_TIM16_CLKIN;
+		ret.vectorno =  STM32_IRQ_TIM16;
+#ifdef CONFIG_STM32_TIM16
+		nuttx_config_timer_enabled = true;
+#endif
+		break;
+
+	case Timer::Timer17:
+		ret.base = STM32_TIM17_BASE;
+		ret.clock_register = STM32_RCC_APB2ENR;
+		ret.clock_bit = RCC_APB2ENR_TIM17EN;
+		ret.clock_freq = STM32_APB2_TIM17_CLKIN;
+		ret.vectorno =  STM32_IRQ_TIM17;
+#ifdef CONFIG_STM32_TIM17
+		nuttx_config_timer_enabled = true;
 #endif
 		break;
 	}
@@ -52,18 +52,18 @@
 #endif // NuttX


-O1HeapInstance *uavcan_allocator{nullptr};
+O1HeapInstance *cyphal_allocator{nullptr};

-static void *memAllocate(CanardInstance *const ins, const size_t amount) { return o1heapAllocate(uavcan_allocator, amount); }
-static void memFree(CanardInstance *const ins, void *const pointer) { o1heapFree(uavcan_allocator, pointer); }
+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)
 {
-	_uavcan_heap = memalign(O1HEAP_ALIGNMENT, HeapSize);
-	uavcan_allocator = o1heapInit(_uavcan_heap, HeapSize, nullptr, nullptr);
+	_cyphal_heap = memalign(O1HEAP_ALIGNMENT, HeapSize);
+	cyphal_allocator = o1heapInit(_cyphal_heap, HeapSize, nullptr, nullptr);

-	if (uavcan_allocator == nullptr) {
+	if (cyphal_allocator == nullptr) {
 		PX4_ERR("o1heapInit failed with size %u", HeapSize);
 	}

@@ -89,8 +89,8 @@ CanardHandle::~CanardHandle()
 	delete _can_interface;
 	_can_interface = nullptr;

-	delete static_cast<uint8_t *>(_uavcan_heap);
-	_uavcan_heap = nullptr;
+	delete static_cast<uint8_t *>(_cyphal_heap);
+	_cyphal_heap = nullptr;

 }

@@ -200,6 +200,11 @@ 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;
@@ -34,6 +34,7 @@
 #pragma once

 #include <canard.h>
+#include "o1heap/o1heap.h"
 #include "CanardInterface.hpp"

 class CanardHandle
@@ -68,6 +69,7 @@ public:
 	int8_t RxUnsubscribe(const CanardTransferKind transfer_kind,
 			     const CanardPortID       port_id);
 	CanardTreeNode *getRxSubscriptions(CanardTransferKind kind);
+	O1HeapDiagnostics getO1HeapDiagnostics();

 	int32_t mtu();
 	CanardNodeID node_id();
@@ -80,6 +82,6 @@ private:

 	CanardTxQueue _queue;

-	void *_uavcan_heap{nullptr};
+	void *_cyphal_heap{nullptr};

 };
@@ -245,12 +245,12 @@ void CyphalNode::print_info()
 	perf_print_counter(_cycle_perf);
 	perf_print_counter(_interval_perf);

-	/*O1HeapDiagnostics heap_diagnostics = o1heapGetDiagnostics(uavcan_allocator);
+	O1HeapDiagnostics heap_diagnostics = _canard_handle.getO1HeapDiagnostics();

 	PX4_INFO("Heap status %zu/%zu Peak alloc %zu Peak req %zu OOM count %" PRIu64,
 		 heap_diagnostics.allocated, heap_diagnostics.capacity,
 		 heap_diagnostics.peak_allocated, heap_diagnostics.peak_request_size,
-		 heap_diagnostics.oom_count);*/
+		 heap_diagnostics.oom_count);

 	_pub_manager.printInfo();

@@ -188,12 +188,6 @@ typedef uint16_t	servo_position_t;
 /** get the maximum PWM value the output will send */
 #define PWM_SERVO_GET_MAX_PWM	_PX4_IOC(_PWM_SERVO_BASE, 19)

-/** force safety switch off (to disable use of safety switch) */
-#define PWM_SERVO_SET_FORCE_SAFETY_OFF		_PX4_IOC(_PWM_SERVO_BASE, 25)
-
-/** force safety switch on (to enable use of safety switch) */
-#define PWM_SERVO_SET_FORCE_SAFETY_ON		_PX4_IOC(_PWM_SERVO_BASE, 28)
-
 /*
 *
 *
@@ -247,7 +247,10 @@ void DShot::update_telemetry_num_motors()

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

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

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

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

-		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);
+		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);
 		// TODO: accumulate consumption and use for battery estimation
 	}

 	// publish when motor index wraps (which is robust against motor timeouts)
-	if (motor_index <= _telemetry->last_motor_index) {
+	if (telemetry_index <= _telemetry->last_telemetry_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();
@@ -314,7 +319,7 @@ void DShot::handle_new_telemetry_data(const int motor_index, const DShotTelemetr
 		esc_status.esc_online_flags = 0;
 	}

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

 int DShot::send_command_thread_safe(const dshot_command_t command, const int num_repetitions, const int motor_index)
@@ -450,29 +455,44 @@ 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);
 			}
@@ -493,8 +513,6 @@ 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();
@@ -503,6 +521,8 @@ void DShot::Run()
 		return;
 	}

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

 	_mixing_output.update();
@@ -551,6 +571,13 @@ 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);

@@ -644,6 +671,13 @@ 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,14 +127,15 @@ private:
 	struct Telemetry {
 		DShotTelemetry handler{};
 		uORB::PublicationMultiData<esc_status_s> esc_status_pub{ORB_ID(esc_status)};
-		int last_motor_index{-1};
+		int last_telemetry_index{-1};
+		uint8_t actuator_functions[esc_status_s::CONNECTED_ESC_MAX] {};
 	};

 	void enable_dshot_outputs(const bool enabled);

 	void init_telemetry(const char *device);

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

 	int request_esc_info();

@@ -147,6 +148,7 @@ 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 PX4 Development Team. All rights reserved.
+#   Copyright (c) 2021-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
@@ -1,6 +1,6 @@
 /****************************************************************************
 *
- *   Copyright (c) 2021 PX4 Development Team. All rights reserved.
+ *   Copyright (c) 2021-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
@@ -47,7 +47,7 @@ ICM42670P::ICM42670P(const I2CSPIDriverConfig &config):
 	_px4_accel(get_device_id(), config.rotation),
 	_px4_gyro(get_device_id(), config.rotation)
 {
-	if (_drdy_gpio != 0) {
+	if (config.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:
-		// DEVICE_CONFIG: Software reset configuration
+		// SIGNAL_PATH_RESET: Software Reset (auto clear bit)
 		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::DEVICE_CONFIG) == 0x04)
+		    && (RegisterRead(Register::BANK_0::SIGNAL_PATH_RESET) == 0x00)
 		    && (RegisterRead(Register::BANK_0::INT_STATUS) & INT_STATUS_BIT::RESET_DONE_INT)) {

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

 	case STATE::FIFO_READ: {
-			uint32_t samples = 0;
+			hrt_abstime timestamp_sample = now;
+			uint8_t samples = 0;

 			if (_data_ready_interrupt_enabled) {
-				// 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);
+				// scheduled from interrupt if _drdy_timestamp_sample was set as expected
+				const hrt_abstime drdy_timestamp_sample = _drdy_timestamp_sample.fetch_and(0);
+
+				if ((now - drdy_timestamp_sample) < _fifo_empty_interval_us) {
+					timestamp_sample = drdy_timestamp_sample;
+					samples = _fifo_gyro_samples;

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

 				// push backup schedule back
@@ -209,8 +213,6 @@ 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);
@@ -222,12 +224,17 @@ 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);
 					}
 				}
 			}
@@ -235,12 +242,9 @@ void ICM42670P::RunImpl()
 			bool success = false;

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

-					// PX4_WARN("success  = %d ",success);
-
 					if (_failure_count > 0) {
 						_failure_count--;
 					}
@@ -260,9 +264,11 @@ 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
@@ -283,68 +289,8 @@ 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);
@@ -374,8 +320,6 @@ void ICM42670P::ConfigureFIFOWatermark(uint8_t samples)
 	}
 }

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

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

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

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

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

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

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

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

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

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

-template <typename T>
-void ICM42670P::RegisterWrite(T reg, uint8_t value)
+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)
 {
 	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)
 {
@@ -498,12 +484,10 @@ 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) + 4 + 2, FIFO::SIZE);
+	const size_t transfer_size = math::min(samples * sizeof(FIFO::DATA) + 6, FIFO::SIZE);

 	if (transfer((uint8_t *)&buffer, (uint8_t *)&buffer, transfer_size) != PX4_OK) {
 		perf_count(_bad_transfer_perf);
@@ -518,7 +502,6 @@ 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();
@@ -552,6 +535,18 @@ 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) {
@@ -563,7 +558,6 @@ 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);
@@ -577,18 +571,30 @@ 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_fifo_read_samples.store(0);
+	_drdy_timestamp_sample.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 = 0;
+	accel.samples = samples;
 	accel.dt = FIFO_SAMPLE_DT;

 	for (int i = 0; i < samples; i++) {
@@ -598,18 +604,15 @@ 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[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++;
+		accel.x[i] = accel_x;
+		accel.y[i] = math::negate(accel_y);
+		accel.z[i] = math::negate(accel_z);
 	}

 	_px4_accel.set_error_count(perf_event_count(_bad_register_perf) + perf_event_count(_bad_transfer_perf) +
 				   perf_event_count(_fifo_empty_perf) + perf_event_count(_fifo_overflow_perf));

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

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

 	_px4_gyro.set_error_count(perf_event_count(_bad_register_perf) + perf_event_count(_bad_transfer_perf) +
@@ -658,88 +661,3 @@ 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 PX4 Development Team. All rights reserved.
+ *   Copyright (c) 2021-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
@@ -57,6 +57,7 @@ class ICM42670P : public device::SPI, public I2CSPIDriver<ICM42670P>
 public:
 	ICM42670P(const I2CSPIDriverConfig &config);
 	~ICM42670P() override;
+
 	static void print_usage();

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

 	// Sensor Configuration
-	static constexpr float FIFO_SAMPLE_DT{1e6f / 800.f};     // 8000 Hz accel & gyro ODR configured
+	static constexpr float FIFO_SAMPLE_DT{1e6f / 1600.f};     // 1600 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 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))};
+	static constexpr int32_t FIFO_MAX_SAMPLES{math::min(FIFO::SIZE / sizeof(FIFO::DATA), sizeof(sensor_gyro_fifo_s::x) / sizeof(sensor_gyro_fifo_s::x[0]), sizeof(sensor_accel_fifo_s::x) / sizeof(sensor_accel_fifo_s::x[0]) * (int)(GYRO_RATE / ACCEL_RATE))};

 	// Transfer data
 	struct FIFOTransferBuffer {
@@ -85,10 +86,8 @@ private:
 		uint8_t FIFO_COUNTL{0};
 		FIFO::DATA f[FIFO_MAX_SAMPLES] {};
 	};
-
-
 	// ensure no struct padding
-	static_assert(sizeof(FIFOTransferBuffer) == (4 + 2 + FIFO_MAX_SAMPLES *sizeof(FIFO::DATA)));
+	static_assert(sizeof(FIFOTransferBuffer) == (6 + FIFO_MAX_SAMPLES *sizeof(FIFO::DATA)));

 	struct register_bank0_config_t {
 		Register::BANK_0 reg;
@@ -96,13 +95,17 @@ 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);

@@ -111,9 +114,13 @@ 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); }
@@ -126,12 +133,6 @@ 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;
@@ -149,9 +150,7 @@ private:
 	hrt_abstime _temperature_update_timestamp{0};
 	int _failure_count{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};
+	px4::atomic<hrt_abstime> _drdy_timestamp_sample{0};
 	bool _data_ready_interrupt_enabled{false};

 	enum class STATE : uint8_t {
@@ -159,26 +158,32 @@ private:
 		WAIT_FOR_RESET,
 		CONFIGURE,
 		FIFO_READ,
-	};
-
-	STATE _state{STATE::RESET};
+	} _state{STATE::RESET};

 	uint16_t _fifo_empty_interval_us{1250}; // default 1250 us / 800 Hz transfer interval
-	uint32_t _fifo_gyro_samples{static_cast<uint32_t>(_fifo_empty_interval_us / (1000000 / GYRO_RATE))};
+	int32_t _fifo_gyro_samples{static_cast<int32_t>(_fifo_empty_interval_us / (1000000 / GYRO_RATE))};

 	uint8_t _checked_register_bank0{0};
 	static constexpr uint8_t size_register_bank0_cfg{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::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 },
+		// 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 },
 	};
 };
@@ -1,6 +1,6 @@
 /****************************************************************************
 *
- *   Copyright (c) 2021 PX4 Development Team. All rights reserved.
+ *   Copyright (c) 2021-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
@@ -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 = 12 * 1000 * 1000; // 24 MHz SPI
+static constexpr uint32_t SPI_SPEED = 24 * 1000 * 1000; // 24 MHz SPI
 static constexpr uint8_t DIR_READ = 0x80;

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

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

 	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,
@@ -90,87 +82,93 @@ 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 MREG_1 : uint8_t {
-	FIFO_CONFIG5_MREG1      = 0x01,
-	INT_CONFIG0_MREG1       = 0x04,
+enum class MREG1 : uint8_t {
+	FIFO_CONFIG5 = 0x01,
+
+	INT_CONFIG0  = 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, //
-	FIFO_FLUSH                = Bit2,
+	SOFT_RESET_DEVICE_CONFIG  = Bit4, // 1: Software reset enabled
+	FIFO_FLUSH                = Bit2, // When set to 1, FIFO will get flushed
 };

 // INT_CONFIG
 enum INT_CONFIG_BIT : uint8_t {
-	INT1_MODE           = Bit2,
-	INT1_DRIVE_CIRCUIT  = Bit1,
-	INT1_POLARITY       = Bit0,
+	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
 };
+
+// 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_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
+	// 2:0 GYRO_UI_FILT_BW
+	GYRO_UI_FILT_BW_BYPASSED_CLEAR = Bit2 | Bit1 | Bit0, // 000: Low pass filter bypassed
 };

 // ACCEL_CONFIG1
 enum ACCEL_CONFIG1_BIT : uint8_t {
-
-	// 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
+	// 2:0 ACCEL_UI_FILT_BW
+	ACCEL_UI_FILT_BW_BYPASSED_CLEAR = Bit2 | Bit1 | Bit0, // 000: Low pass filter bypassed
 };
+
 // FIFO_CONFIG1
 enum FIFO_CONFIG1_BIT : uint8_t {
-	// 1 FIFO_MODE
-	FIFO_MODE_STOP_ON_FULL = Bit1, // 11: STOP-on-FULL Mode
+	// FIFO_MODE
+	FIFO_MODE_STOP_ON_FULL = Bit1, // 1: STOP-on-FULL Mode
+	FIFO_BYPASS            = Bit0, // 0: FIFO is not bypassed
 };

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

-// PWR_MGMT0
-enum PWR_MGMT0_BIT : uint8_t {
-	GYRO_MODE_LOW_NOISE  = Bit3 | Bit2, // 11: Places gyroscope in Low Noise (LN) Mode
-	ACCEL_MODE_LOW_NOISE = Bit1 | Bit0, // 11: Places accelerometer in Low Noise (LN) Mode
+// 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
 };

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

-	// 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
-};
+//---------------- USER BANK MREG1 Register bits

 // FIFO_CONFIG5
 enum FIFO_CONFIG5_BIT : uint8_t {
-	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,
+	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
 };

 // INT_CONFIG0
 enum INT_CONFIG0_BIT : uint8_t {
 	// 3:2 FIFO_THS_INT_CLEAR
-	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.
+	FIFO_THS_INT_CLEAR = Bit3, // 10: Clear on FIFO data 1Byte Read
 };

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

 // 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;
-	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;
+	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]
 };

 // 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,
-	HEADER_GYRO            = Bit5,
-	HEADER_20              = Bit4,
+	HEADER_ACCEL           = Bit6, // 1: Packet is sized so that accel data have location in the packet, FIFO_ACCEL_EN must be 1
+	HEADER_GYRO            = Bit5, // 1: Packet is sized so that gyro data have location in the packet, FIFO_GYRO_EN must be1
+	HEADER_20              = Bit4, // 1: Packet has a new and valid sample of extended 20-bit data for gyro and/or accel
 	HEADER_TIMESTAMP_FSYNC = Bit3 | Bit2,
-	HEADER_ODR_ACCEL       = Bit1,
-	HEADER_ODR_GYRO        = Bit0,
+	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
 };

 }
@@ -126,8 +126,6 @@ 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();
@@ -136,6 +134,8 @@ void LinuxPWMOut::Run()
 		return;
 	}

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

@@ -408,10 +408,6 @@ 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,8 +376,6 @@ bool PCA9685Wrapper::updateOutputs(bool stop_motors, uint16_t *outputs, unsigned

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

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

 	switch (_state) {
@@ -492,9 +492,7 @@ int PCA9685Wrapper::ioctl(cdev::file_t *filep, int cmd, unsigned long arg)
 		break;

 	case PWM_SERVO_SET_ARM_OK:
-	case PWM_SERVO_SET_FORCE_SAFETY_OFF:
 	case PWM_SERVO_CLEAR_ARM_OK:
-	case PWM_SERVO_SET_FORCE_SAFETY_ON:
 	case PWM_SERVO_ARM:
 	case PWM_SERVO_DISARM:
 		break;
--- a/Show More
+++ b/Show More
Author	SHA1	Message	Date
Beat Küng	767cb3dc79	FailureDetector: check if ESCs have current And increase the timeout to 300ms, as some ESCs only update with 10Hz.	2022-06-03 09:53:56 +02:00
Beat Küng	b7c721f3f6	TEST PositionControl: use aux2 to switch between thrust vs attitude	2022-06-03 09:52:45 +02:00
Beat Küng	588e1572e3	mc_rate_control: pass through 3D thrust	2022-05-31 14:33:27 +02:00
Beat Küng	923b5b15bd	TEST: allow omnicopter to rotate via AUX1 & use 3D thrust in pos/mission mode	2022-05-31 14:16:33 +02:00
Beat Küng	1decf904d7	failure_detector: allow disabling attitude failure (as already documented)	2022-05-23 15:37:48 +02:00
Beat Küng	49956f2c44	control_allocator: show motor axis for MC (as advanced)	2022-05-23 15:37:14 +02:00
Alex Mikhalev	8267f4fd1e	scripts: Hotfix px_uploader.py to avoid PX4BL bug Signed-off-by: Alex Mikhalev <alex@corvus-robotics.com>	2022-05-23 15:37:06 +02:00
Igor Misic	12a81979a8	px4io: replace safety_off state with safety button event (#19558 ) internal PX4IO safety_off state is removed and replaced with a normal safety button event. From this 'commit' commander is taking care of the PX4IO safety.	2022-05-23 06:54:37 +02:00
Igor Misic	08dcc72e1f	commander/safety: replace safety.msg with Safety class (#19558 )	2022-05-23 06:54:37 +02:00
Daniel Agar	b800600a6c	boards: px4_fmu-v2_fixeding disable fw_autotune_attitude_control to save flash	2022-05-22 12:32:24 -04:00
Daniel Agar	61d5a23445	Jenkinsfile: hardware test rack skip runing HITL airframes	2022-05-22 12:31:08 -04:00
Julian Oes	47bcee70c4	ROMFS: disable UAVCAN in HITL Without this, uavcan creates MixingOutput classes which then create empty actuator_outputs publications. This then prevents the motor output in HITL to be forwarded to the simulator via mavlink.	2022-05-21 17:53:20 -04:00
bresch	00e88a02c0	baro bias: improve innovation sequence monitoring The baro observation noise parameter is often over-estimated in order as a measure to mitigate temporary offsets in the readings due to wind gusts or poor pressure compensation tuning. The side effect is that the innovation sequence monitoring based on normalized innovation struggles to detect an offset in the state because the innovation isn't statistically significant. To counter this issue, a simpler check is added to trigger the process noise boost when the innovation has the same sign for a long period of time.	2022-05-21 22:37:04 +02:00
PX4 BuildBot	c3f10c2cb1	Update submodule sitl_gazebo to latest Sat May 21 12:38:49 UTC 2022 - sitl_gazebo in PX4/Firmware (`422be90140`): https://github.com/PX4/PX4-SITL_gazebo/commit/48440d7b5c78a21182415266334981f1163f4b2c - sitl_gazebo current upstream: https://github.com/PX4/PX4-SITL_gazebo/commit/5eb5df80450a412076bfc24e7dd343839020f056 - Changes: https://github.com/PX4/PX4-SITL_gazebo/compare/48440d7b5c78a21182415266334981f1163f4b2c...5eb5df80450a412076bfc24e7dd343839020f056 5eb5df8 2022-04-28 stmoon - ROS2_PLUGIN set OFF by default c8f4d5b 2022-04-28 stmoon - update the motor failure plugin for ros2	2022-05-21 14:58:42 -04:00
Daniel Agar	db4545e442	Update world_magnetic_model to latest Sat May 21 11:14:10 UTC 2022	2022-05-21 14:57:55 -04:00
mcsauder	6b0788ff46	Standardize variable naming and formatting across vehicle attitude controller files.	2022-05-21 12:22:18 -04:00
Junwoo Hwang	422be90140	Update Issue and Pull Request Templates 1. Change the paragraph headings to proper Markdown headings (easier to link / structure the Markdown) 2. Move PULL_REQUEST_TEMPLATE into .github folder 3. Change description in Issue template and remove outdated DevGuide Repository information 4. Add a bug emoji to bug report isue template 5. Support automatically adding labels 'bug-report' and 'feature-request' to easily sort / filter appropriate issues in Github	2022-05-20 21:01:14 -04:00
Julian Oes	d08e5eae2d	sitl_gazebo: update submodule This fixes the issue where HITL doesn't connect over USB.	2022-05-20 21:00:33 -04:00
Daniel Agar	c40631c136	commander: handle RC loss timeout if manual_control_setpoint stops publishing (#19680 ) Co-authored-by: Matthias Grob <maetugr@gmail.com>	2022-05-20 19:37:30 -04:00
David Sidrane	2bdeb36ea3	px4_fmu-v6x:Add Rev 4 Sensors	2022-05-20 15:52:32 -04:00
David Sidrane	9d0e57230a	boards: new px4_fmu-v6c board support (#19544 )	2022-05-20 15:49:06 -04:00
Julian Oes	33a77c225c	commander: lockdown is not termination We use lockdown to prevent outputs like motors and servos from being active in HITL simulation. This means that we can't treat the lockdown flag as a flight_terminated, otherwise we can't arm in HITL at all.	2022-05-20 09:43:32 -04:00
Nicolas MARTIN	a0cb7f6258	HITL: undefined time_remaining_s should be NAN	2022-05-20 09:38:43 -04:00
Nico van Duijn	04071b9456	Commander: ignore MAV_CMD_REQUEST_MSG This commit adds the MAV_CMD_REQUEST_MESSAGE to the list of vehicle commands which are ignored without generating a warning sound.	2022-05-20 15:36:42 +02:00
Matthias Grob	38e027ee45	ArmStateMachine: remove dependency on armed.armed To have the internal state as single source of truth for the arming state within Commander.	2022-05-20 13:51:51 +02:00
Matthias Grob	37c485ce89	ArmStateMachine: move arm state into the class	2022-05-20 13:51:51 +02:00
Matthias Grob	47532ca07b	ArmStateMachine: replace state name array with method	2022-05-20 13:51:51 +02:00
alessandro	c5bbf4553b	ubuntu.sh: fix gazebo and openjdk for 22.04	2022-05-20 06:38:09 +02:00
Matthias Grob	887fe7dba2	commander_params: shorten low battery action delay I got multiple times the feedback now that a consistent delay is helpful and makes sense but the default delay is too long for low battery action where you're trying to come back in time and possibly the emergency reaction kicks in while the critical action is executing which leads to a longer accumulated delay.	2022-05-19 21:54:09 +02:00
Daniel Agar	fd4b62032e	commander: publish actuator_armed first on any change	2022-05-19 12:47:50 -04:00
PX4 BuildBot	9518b65f93	Update submodule mavlink to latest Thu May 19 00:38:59 UTC 2022 - mavlink in PX4/Firmware (87c73145b36a835b1635de0498a5613a7af5cafc): https://github.com/mavlink/mavlink/commit/a1cb2c0e71f191f04da3d92d92db8702a7e91ff6 - mavlink current upstream: https://github.com/mavlink/mavlink/commit/99e82cad70494903a23a67de08ff9cbb5918d8f3 - Changes: https://github.com/mavlink/mavlink/compare/a1cb2c0e71f191f04da3d92d92db8702a7e91ff6...99e82cad70494903a23a67de08ff9cbb5918d8f3 99e82cad 2022-05-19 Hamish Willee - Deprecated GPS_INJECT_DATA (#1842) bf3df03d 2022-05-16 Hamish Willee - WIND_COV - accuracy units are m/s (#1844) a73d4864 2022-05-11 Hamish Willee - development.xml FIGURE_EIGHT_EXECUTION_STATUS - add a note about it being set at lower rate (#1841)	2022-05-18 21:41:08 -04:00
mcsauder	e8da98fd14	Add gyro clipping to mirror accel clipping monitoring instances.	2022-05-18 21:16:05 -04:00
mcsauder	ea10eacb99	Replace EKF/common.h #defines with enums.	2022-05-18 09:25:19 -04:00
Silvan Fuhrer	8f2c84d36f	VTOL paras: add min of 0.1 to transition times Signed-off-by: Silvan Fuhrer <silvan@auterion.com>	2022-05-18 10:01:04 +02:00
Silvan Fuhrer	c29ca6c959	tailsitter: guard against division by 0 Signed-off-by: Silvan Fuhrer <silvan@auterion.com>	2022-05-18 10:01:04 +02:00
Silvan Fuhrer	3cf07e1be5	VTOL: rename params _PTCH to _PITCH Signed-off-by: Silvan Fuhrer <silvan@auterion.com>	2022-05-18 10:01:04 +02:00
Silvan Fuhrer	7c5f0121a5	VTOL: remove some unsued variables Signed-off-by: Silvan Fuhrer <silvan@auterion.com>	2022-05-18 10:01:04 +02:00
Silvan Fuhrer	2b7efeacca	vtol_vehicle_status: replace several status bools by single vehicle_vtol_state Replace vtol_in_rw_mode, vtol_in_trans_mode, in_transition_to_fw by vehicle_vtol_state. Signed-off-by: Silvan Fuhrer <silvan@auterion.com>	2022-05-18 10:01:04 +02:00
Silvan Fuhrer	635f64a2e5	Commander: remove permanent stabilization option for fixed-wing flight Signed-off-by: Silvan Fuhrer <silvan@auterion.com>	2022-05-18 10:01:04 +02:00
Silvan Fuhrer	d8444df11c	Set tailsitter flag via vehicle status Removes the necessity of including vtol_type.h in other modules. Signed-off-by: Silvan Fuhrer <silvan@auterion.com>	2022-05-18 10:01:04 +02:00
Silvan Fuhrer	7292ce483c	VTOL: move to cpp params API Signed-off-by: Silvan Fuhrer <silvan@auterion.com>	2022-05-18 10:01:04 +02:00
Daniel Agar	ff16131874	icm42670p run at full speed	2022-05-17 08:14:22 -07:00
Daniel Agar	83daf648ef	drivers/imu/invensense/icm42670p: cleanup and small fixes	2022-05-17 08:14:22 -07:00
Peter van der Perk	3ea492b7a2	UCANS32K146 Fix CAN1 STB pin	2022-05-17 03:13:05 -07:00
Matthias Grob	02e11eddce	mavlink_mission: add more specific information to the error message	2022-05-17 07:49:00 +02:00
Junwoo Hwang	146f0cafe0	Add get*Expo() functions to the Sticks library To return Exponential Values, which is helpful for reducing the sensitivity of the stick around the centered value (0), since it's exponential curve. Useful for user adjustment implementations, where accidentally touching the stick wouldn't have so much effect when using the Exponential value, compared to using the raw position value.	2022-05-16 23:55:05 +02:00
Matthias Grob	4bd2d4cf35	rc_update: add unit tests for mode slot To verify RC mode switch and mode button functionality works as expected.	2022-05-16 14:37:29 +02:00
achim	3fe4c6e3f5	boards: matek h743-mini specify drivers to still fit to flash - the code has become a bit bigger again that now the drivers have to be specified a bit more precisely to still fit into the flash.	2022-05-13 14:05:04 -04:00
Beat Küng	32df76ca8a	dshot: handle DSHOT_MIN for reversible outputs Also ensures there's no deadband if dead_l == dead_h (the default).	2022-05-13 14:04:01 -04:00
Matthias Grob	57a0289627	trajectory_setpoint: correct comment typo "kinematically" Co-authored-by: Mathieu Bresciani <brescianimathieu@gmail.com>	2022-05-12 17:19:48 +02:00
Matthias Grob	4017f4bb0b	vehicle_local_position_setpoint: reorder fields for clarity	2022-05-12 17:19:48 +02:00
Matthias Grob	b67fbac296	uuv_pos_control: siplify passing on trajectory position setpoint	2022-05-12 17:19:48 +02:00
Matthias Grob	200124f094	mavlink_receiver: check entire Vectors for NAN Note: Behavioral change Inf also results in the flag being true.	2022-05-12 17:19:48 +02:00
Matthias Grob	8ca28f3796	Separate message for trajectory setpoint	2022-05-12 17:19:48 +02:00
Beat Küng	9166b6953d	output drivers: init SmartLock after exit_and_cleanup This fixes an invalid memory access when exiting the module: exit_and_cleanup destroys the object, but lock_guard is destructed after and accesses the lock.	2022-05-12 08:16:35 -04:00
Beat Küng	0a9378e0f6	mavsdk_tests: ensure motor is stopped for motor failure test	2022-05-12 07:58:56 +02:00
Alessandro Simovic	b5a3c58a95	sitl: loosen some timeouts The typhoon_h480 model would not always complete takeoff in 30 seconds or finish the mission within 60 seconds.	2022-05-12 07:58:56 +02:00
Alessandro Simovic	5c1932dcca	disable engine failure detector for SITL VTOL Tried increasing ESC timeout for VTOL first VTOL sitl tests are failing because the ESC telemetry seems to be coming in at 0.3 Hz	2022-05-12 07:58:56 +02:00
Alessandro Simovic	06f69cd469	Add SITL tests for control allocation	2022-05-12 07:58:56 +02:00
Beat Küng	0f860045f7	failure_detector: add failure injector class for motor telemetry This allows to test motor failures via 'failure motor off -i 1' on a real system.	2022-05-12 07:58:56 +02:00
Alessandro Simovic	4640f395d7	simulator_mavlink: Add ESC telemetry	2022-05-12 07:58:56 +02:00
Alessandro Simovic	20ccfbb719	control_allocator: remove failed motor from effectiveness - limit to handling only 1 motor failure - Log which motor failures are handled - Remove motor from effectiveness matrix without recomputing the scale / normalization	2022-05-12 07:58:56 +02:00
Alessandro Simovic	fb71e7587c	failure_detector: add motor/ESC failure detection	2022-05-12 07:58:56 +02:00
Beat Küng	dfd934fbdb	esc_report: add actuator_function	2022-05-12 07:58:56 +02:00
Hamish Willee	aab2feb8f5	pwm.cpp: remove the test example (#19587 )	2022-05-12 07:56:13 +02:00
alexklimaj	0f69f8ced8	Fix uavcan battery causing immediate RTL time remaining low	2022-05-11 21:48:12 -04:00
Daniel Agar	fba7c972d1	drivers/rc_input: ensure RC inversion is disabled initially and latch RC_INPUT_PROTO conservatively - this allows jumping straight to a non-SBUS RC protocol - increased the scan time per protocol 300->500 ms, which the newer DSM parser seems to need in some cases. - only set RC_INPUT_PROTO if we've had a successful RC lock for > 3 seconds	2022-05-11 14:30:41 -04:00
Daniel Agar	c772e5230f	commander: remove compile time dependencies on non-commander parameters - this allows builds with mavlink fully disabled - move commander MAN_ARM_GESTURE, RC_MAP_ARM_SW, MC_AIRMODE checks to manual_control	2022-05-11 10:14:23 -04:00
Beat Küng	8d36ba6727	log_writer_file: fix corner case when mission log is enabled Normally _should_run for mission is only ever true if _should_run for the normal log is. There are exceptions though: - the log buffer fails to allocate - there was a write failure (e.g. due to SD card removal) In that situation, the writer would not wait anymore but busy-loop.	2022-05-11 10:06:35 -04:00
Beat Küng	ebbe08bc86	log_writer_file: protect access to _should_run, use px4::atomicbool for _exit_thread	2022-05-11 10:06:35 -04:00
Peter van der Perk	0053aeec97	Cyphal restore O1Heap statistics	2022-05-11 09:49:18 -04:00
Peter van der Perk	e62e8b58d1	Update UCANS32K146 clocking to use XTAL and support higher pheriphal freq	2022-05-11 06:02:39 -07:00