WIP: mRo ctrl-zero-h7

boards: mRo pixracer pro disable USART6 (conflicts with SPI6)
boards: mRo pixracer pro fix incorrect SPI2 sck
2026-06-20 03:30:34 +08:00 · 2021-02-16 21:51:42 -05:00 · 2021-02-16 21:48:57 -05:00 · 2021-02-16 21:26:12 -05:00 · 2021-02-16 15:16:19 -05:00 · 2021-02-16 13:02:14 -05:00
36 changed files with 2639 additions and 296 deletions
@@ -94,7 +94,7 @@ jobs:
        PX4_HOME_LON: ${{matrix.config.longitude}}
        PX4_HOME_ALT: ${{matrix.config.altitude}}
        PX4_CMAKE_BUILD_TYPE: ${{matrix.config.build_type}}
-      run: test/mavsdk_tests/mavsdk_test_runner.py --speed-factor 20 --abort-early --model ${{matrix.config.model}} test/mavsdk_tests/configs/sitl.json
+      run: test/mavsdk_tests/mavsdk_test_runner.py --speed-factor 20 --abort-early --model ${{matrix.config.model}} --upload test/mavsdk_tests/configs/sitl.json

    - name: Look at core files
      if: failure()
@@ -106,10 +106,6 @@ jobs:
        name: coredump
        path: px4.core

-    - name: Upload logs to flight review
-      if: failure()
-      run: ./Tools/upload_log.py -q --description "${GITHUB_WORKFLOW} ${GITHUB_RUN_ID}" --feedback "${GITHUB_WORKFLOW} ${GITHUB_RUN_ID} ${GITHUB_REPOSITORY} ${GITHUB_REF}" --source CI ${GITHUB_WORKSPACE}/build/px4_sitl_default/tmp_mavsdk_tests/rootfs/log/*/*.ulg
-
    - name: Upload px4 binary
      if: failure()
      uses: actions/upload-artifact@v2-preview
@@ -131,6 +131,16 @@ CONFIG:
      buildType: MinSizeRel
      settings:
        CONFIG: mro_ctrl-zero-f7_default
+    mro_pixracerpro_bootloader:
+      short: mro_pixracerpro_bootloader
+      buildType: MinSizeRel
+      settings:
+        CONFIG: mro_pixracerpro_bootloader
+    mro_pixracerpro_default:
+      short: mro_pixracerpro_default
+      buildType: MinSizeRel
+      settings:
+        CONFIG: mro_pixracerpro_default
    mro_x21-777_default:
      short: mro_x2.1-777
      buildType: MinSizeRel
@@ -0,0 +1,9 @@
+
+px4_add_board(
+	PLATFORM nuttx
+	VENDOR mro
+	MODEL ctrl-zero-h7
+	LABEL bootloader
+	TOOLCHAIN arm-none-eabi
+	ARCHITECTURE cortex-m7
+)
@@ -0,0 +1,130 @@
+
+px4_add_board(
+	PLATFORM nuttx
+	VENDOR mro
+	MODEL ctrl-zero-h7
+	LABEL default
+	TOOLCHAIN arm-none-eabi
+	ARCHITECTURE cortex-m7
+	ROMFSROOT px4fmu_common
+	BUILD_BOOTLOADER
+	TESTING
+	UAVCAN_INTERFACES 1
+	SERIAL_PORTS
+		TEL1:/dev/ttyS0
+		TEL2:/dev/ttyS1
+		GPS1:/dev/ttyS2
+		#RC:/dev/ttyS3
+		#CONSOLE:/dev/ttyS4
+		#FRSKY:/dev/ttyS5
+	DRIVERS
+		adc/board_adc
+		#barometer # all available barometer drivers
+		barometer/dps310
+		batt_smbus
+		camera_capture
+		camera_trigger
+		differential_pressure # all available differential pressure drivers
+		distance_sensor # all available distance sensor drivers
+		dshot
+		gps
+		#imu # all available imu drivers
+		imu/bosch/bmi088
+		imu/invensense/icm20602
+		imu/invensense/icm20948
+		irlock
+		lights/blinkm
+		lights/rgbled
+		lights/rgbled_ncp5623c
+		magnetometer # all available magnetometer drivers
+		mkblctrl
+		optical_flow # all available optical flow drivers
+		#osd
+		pca9685
+		power_monitor/ina226
+		#protocol_splitter
+		#pwm_input
+		pwm_out_sim
+		pwm_out
+		rc_input
+		roboclaw
+		rpm
+		#safety_button TODO
+		tap_esc
+		telemetry # all available telemetry drivers
+		test_ppm
+		tone_alarm
+		uavcan
+	MODULES
+		airspeed_selector
+		attitude_estimator_q
+		battery_status
+		camera_feedback
+		commander
+		dataman
+		ekf2
+		esc_battery
+		events
+		flight_mode_manager
+		fw_att_control
+		fw_pos_control_l1
+		land_detector
+		landing_target_estimator
+		load_mon
+		local_position_estimator
+		logger
+		mavlink
+		mc_att_control
+		mc_hover_thrust_estimator
+		mc_pos_control
+		mc_rate_control
+		#micrortps_bridge
+		navigator
+		rc_update
+		rover_pos_control
+		sensors
+		sih
+		temperature_compensation
+		vmount
+		vtol_att_control
+	SYSTEMCMDS
+		bl_update
+		dmesg
+		dumpfile
+		esc_calib
+		gpio
+		hardfault_log
+		i2cdetect
+		led_control
+		mft
+		mixer
+		motor_ramp
+		motor_test
+		mtd
+		nshterm
+		param
+		perf
+		pwm
+		reboot
+		reflect
+		sd_bench
+		system_time
+		tests # tests and test runner
+		top
+		topic_listener
+		tune_control
+		usb_connected
+		ver
+		work_queue
+	EXAMPLES
+		fake_magnetometer
+		fixedwing_control # Tutorial code from https://px4.io/dev/example_fixedwing_control
+		hello
+		hwtest # Hardware test
+		#matlab_csv_serial
+		px4_mavlink_debug # Tutorial code from http://dev.px4.io/en/debug/debug_values.html
+		px4_simple_app # Tutorial code from http://dev.px4.io/en/apps/hello_sky.html
+		rover_steering_control # Rover example app
+		uuv_example_app
+		work_item
+	)
@@ -0,0 +1,13 @@
+{
+    "board_id": 1023,
+    "magic": "mRo-ctrl-zero-h7",
+    "description": "Firmware for the mRo-ctrl-zero-h7 board",
+    "image": "",
+    "build_time": 0,
+    "summary": "mRo-ctrl-zero-h7",
+    "version": "0.1",
+    "image_size": 0,
+    "image_maxsize": 1966080,
+    "git_identity": "",
+    "board_revision": 0
+}
@@ -0,0 +1,42 @@
+#!/bin/sh
+#
+# board specific defaults
+#------------------------------------------------------------------------------
+
+#
+# Bootloader upgrade
+#
+set BL_FILE /etc/extras/mro_ctrl-zero-h7_bootloader.bin
+if [ -f $BL_FILE ]
+then
+	if param compare SYS_BL_UPDATE 1
+	then
+		param set SYS_BL_UPDATE 0
+		param save
+		echo "BL update..."
+		bl_update $BL_FILE
+		echo "BL update done"
+		reboot
+	fi
+fi
+unset BL_FILE
+
+
+param set-default BAT_V_DIV 10.1
+param set-default BAT1_V_DIV 10.1
+
+param set-default BAT_A_PER_V 24
+param set-default BAT1_A_PER_V 24
+
+# Multi-EKF
+param set-default EKF2_MULTI_IMU 3
+param set-default SENS_IMU_MODE 0
+param set-default EKF2_MULTI_MAG 3
+param set-default SENS_MAG_MODE 0
+
+param set-default UAVCAN_ENABLE 2
+
+
+set LOGGER_BUF 64
+
+safety_button start
@@ -0,0 +1,7 @@
+#!/bin/sh
+#
+# Board specific MAVLink startup script.
+#------------------------------------------------------------------------------
+
+# Start MAVLink on the USB port
+mavlink start -d /dev/ttyACM0
@@ -0,0 +1,18 @@
+#!/bin/sh
+#
+# board specific sensors init
+#------------------------------------------------------------------------------
+board_adc start
+
+# Internal ICM-20602
+icm20602 -s -b 1 -R 8 start
+
+# Internal SPI bus BMI088 accel & gyro
+bmi088 -A -s -b 5 -R 8 start
+bmi088 -G -s -b 5 -R 8 start
+
+# Internal ICM-20948 (with magnetometer)
+icm20948 -s -b 1 -R 8 -M start
+
+# Interal DPS310 (barometer)
+dps310 -s -b 2 start
@@ -0,0 +1,95 @@
+#
+# 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="../nuttx-config"
+CONFIG_ARCH_BOARD_CUSTOM_DIR_RELPATH=y
+CONFIG_ARCH_BOARD_CUSTOM_NAME="px4"
+CONFIG_ARCH_CHIP="stm32h7"
+CONFIG_ARCH_CHIP_STM32H743ZI=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_PRODUCTID=0x1023
+CONFIG_CDCACM_PRODUCTSTR="PX4 BL mRo ControlZeroH7"
+CONFIG_CDCACM_RXBUFSIZE=600
+CONFIG_CDCACM_TXBUFSIZE=12000
+CONFIG_CDCACM_VENDORID=0x26ac
+CONFIG_CDCACM_VENDORSTR="mRo"
+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_MAX_TASKS=8
+CONFIG_MEMSET_64BIT=y
+CONFIG_MEMSET_OPTSPEED=y
+CONFIG_MM_REGIONS=3
+CONFIG_NFILE_DESCRIPTORS=5
+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_OTGFS=y
+CONFIG_STM32H7_PROGMEM=y
+CONFIG_STM32H7_SERIAL_DISABLE_REORDERING=y
+CONFIG_STM32H7_TIM1=y
+CONFIG_STM32H7_USART3=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_SIGSTP=y
+CONFIG_USART3_RXBUFSIZE=600
+CONFIG_USART3_TXBUFSIZE=300
+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,270 @@
+/************************************************************************************
+ * nuttx-config/include/board.h
+ *
+ *   Copyright (C) 2020 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.
+ *
+ ************************************************************************************/
+#pragma once
+
+#include "board_dma_map.h"
+
+#include <nuttx/config.h>
+
+#ifndef __ASSEMBLY__
+# include <stdint.h>
+#endif
+
+#include "stm32_rcc.h"
+#include "stm32_sdmmc.h"
+
+/* Clocking *************************************************************************/
+/* The board provides the following clock sources:
+ *
+ *   X1: 24 MHz crystal for HSE
+ *
+ * So we have these clock source available within the STM32
+ *
+ *   HSI: 16 MHz RC factory-trimmed internal oscillator
+ *   HSE: 24 MHz crystal for HSE
+ */
+#define STM32_BOARD_XTAL        24000000ul
+
+#define STM32_HSI_FREQUENCY     16000000ul
+#define STM32_LSI_FREQUENCY     32000
+#define STM32_HSE_FREQUENCY     STM32_BOARD_XTAL
+#define STM32_LSE_FREQUENCY     0
+
+/* Main PLL Configuration.
+ *
+ * PLL source is HSE = 24,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_PLLCFG_PLLSRC      RCC_PLLCKSELR_PLLSRC_HSE
+
+/* PLL1, wide 4 - 8 MHz input, enable DIVP, DIVQ, DIVR
+ *
+ *   PLL1_VCO = (24,000,000 / 2) * 80 = 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(2)
+#define STM32_PLLCFG_PLL1N       RCC_PLL1DIVR_N1(80)
+#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 / 2) * 80)
+#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(32)
+#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) * 32)
+#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(32)
+#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) * 32)
+#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/4 (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
+ */
+#define STM32_RCC_D2CCIP2R_I2C123SRC RCC_D2CCIP2R_I2C123SEL_HSI  /* I2C123 clock source */
+#define STM32_RCC_D2CCIP1R_SPI123SRC RCC_D2CCIP1R_SPI123SEL_PLL2 /* SPI123 clock source */
+#define STM32_RCC_D2CCIP1R_SPI45SRC  RCC_D2CCIP1R_SPI45SEL_PLL2  /* SPI45 clock source */
+#define STM32_RCC_D3CCIPR_SPI6SRC    RCC_D3CCIPR_SPI6SEL_PLL2    /* SPI6 clock source */
+#define STM32_RCC_D2CCIP2R_USBSRC    RCC_D2CCIP2R_USBSEL_PLL3    /* USB 1 and 2 clock source */
+#define STM32_RCC_D3CCIPR_ADCSEL     RCC_D3CCIPR_ADCSEL_PLL2     /* ADC 1 2 3 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 */
+#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
+
+
+/* UART/USART */
+#define GPIO_USART2_RX   GPIO_USART2_RX_2       /* PD6 */
+#define GPIO_USART2_TX   GPIO_USART2_TX_2       /* PD5 */
+#define GPIO_USART2_RTS  GPIO_USART2_RTS_2      /* PD4 */
+#define GPIO_USART2_CTS  GPIO_USART2_CTS_NSS_2  /* PD3 */
+
+#define GPIO_USART3_RX   GPIO_USART3_RX_3       /* PD9 */
+#define GPIO_USART3_TX   GPIO_USART3_TX_3       /* PD8 */
+#define GPIO_USART3_RTS  GPIO_USART3_RTS_2      /* PD12 */
+#define GPIO_USART3_CTS  GPIO_USART3_CTS_NSS_2  /* PD11 */
+
+#define GPIO_UART4_RX    GPIO_UART4_RX_1        /* PA1 */
+#define GPIO_UART4_TX    GPIO_UART4_TX_1        /* PA0 */
+
+#define GPIO_USART6_RX   GPIO_USART6_RX_1       /* PC7 */
+#define GPIO_USART6_TX   GPIO_USART6_TX_2       /* PG14 */
+
+#define GPIO_UART7_RX    GPIO_UART7_RX_1        /* PE7 */
+#define GPIO_UART7_TX    GPIO_UART7_TX_1        /* PE8 */
+
+#define GPIO_UART8_RX    GPIO_UART8_RX_1        /* PE0 */
+#define GPIO_UART8_TX    GPIO_UART8_TX_1        /* PE1 */
+
+
+/* CAN */
+#define GPIO_CAN1_RX     GPIO_CAN1_RX_3     /* PD0  */
+#define GPIO_CAN1_TX     GPIO_CAN1_TX_3     /* PD1 */
+
+
+/* SPI */
+#define ADJ_SLEW_RATE(p) (((p) & ~GPIO_SPEED_MASK) | (GPIO_SPEED_2MHz))
+
+#define GPIO_SPI1_SCK    ADJ_SLEW_RATE(GPIO_SPI1_SCK_1) /* PA5  */
+#define GPIO_SPI1_MISO   GPIO_SPI1_MISO_1               /* PA6  */
+#define GPIO_SPI1_MOSI   GPIO_SPI1_MOSI_1               /* PA7  */
+
+#define GPIO_SPI2_SCK    ADJ_SLEW_RATE(GPIO_SPI2_SCK_2) /* PB10 */
+#define GPIO_SPI2_MISO   GPIO_SPI2_MISO_1               /* PB14 */
+#define GPIO_SPI2_MOSI   GPIO_SPI2_MOSI_1               /* PB15 */
+
+#define GPIO_SPI5_SCK    ADJ_SLEW_RATE(GPIO_SPI5_SCK_1) /* PF7  */
+#define GPIO_SPI5_MISO   GPIO_SPI5_MISO_1               /* PF8  */
+#define GPIO_SPI5_MOSI   GPIO_SPI5_MOSI_1               /* PF9  */
+
+
+/* I2C */
+#define GPIO_I2C1_SCL GPIO_I2C1_SCL_2       /* PB8  */
+#define GPIO_I2C1_SDA GPIO_I2C1_SDA_2       /* PB9  */
@@ -0,0 +1,44 @@
+/****************************************************************************
+ *
+ *   Copyright (c) 2021 PX4 Development Team. All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * 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
+#define DMAMAP_SPI1_RX    DMAMAP_DMA12_SPI1RX_0 /* DMA1:37 */
+#define DMAMAP_SPI1_TX    DMAMAP_DMA12_SPI1TX_0 /* DMA1:38 */
+
+#define DMAMAP_USART6_RX   DMAMAP_DMA12_USART6RX_1 /* DMA1:71 */
+#define DMAMAP_USART6_TX   DMAMAP_DMA12_USART6TX_1 /* DMA1:72 */
+
+#define DMAMAP_SPI5_RX    DMAMAP_DMA12_SPI5RX_0 /* DMA1:83 */
+#define DMAMAP_SPI5_TX    DMAMAP_DMA12_SPI5TX_0 /* DMA1:84 */
@@ -0,0 +1,233 @@
+#
+# 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_SEMICOLON is not set
+# CONFIG_NSH_DISABLE_TIME is not set
+CONFIG_ARCH="arm"
+CONFIG_ARCH_BOARD_CUSTOM=y
+CONFIG_ARCH_BOARD_CUSTOM_DIR="../nuttx-config"
+CONFIG_ARCH_BOARD_CUSTOM_DIR_RELPATH=y
+CONFIG_ARCH_BOARD_CUSTOM_NAME="px4"
+CONFIG_ARCH_CHIP="stm32h7"
+CONFIG_ARCH_CHIP_STM32H743II=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_BOARDCTL_RESET=y
+CONFIG_BOARD_CRASHDUMP=y
+CONFIG_BOARD_LOOPSPERMSEC=95150
+CONFIG_BOARD_RESET_ON_ASSERT=2
+CONFIG_BUILTIN=y
+CONFIG_C99_BOOL8=y
+CONFIG_CDCACM=y
+CONFIG_CDCACM_PRODUCTID=0x1023
+CONFIG_CDCACM_PRODUCTSTR="mRoControlZeroH7"
+CONFIG_CDCACM_RXBUFSIZE=600
+CONFIG_CDCACM_TXBUFSIZE=12000
+CONFIG_CDCACM_VENDORID=0x26ac
+CONFIG_CDCACM_VENDORSTR="mRo"
+CONFIG_CLOCK_MONOTONIC=y
+CONFIG_DEBUG_FULLOPT=y
+CONFIG_DEBUG_HARDFAULT_ALERT=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_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_LIBC_FLOATINGPOINT=y
+CONFIG_LIBC_LONG_LONG=y
+CONFIG_LIBC_STRERROR=y
+CONFIG_MAX_TASKS=64
+CONFIG_MEMSET_64BIT=y
+CONFIG_MEMSET_OPTSPEED=y
+CONFIG_MMCSD=y
+CONFIG_MMCSD_SDIO=y
+CONFIG_MMCSD_SDIOWAIT_WRCOMPLETE=y
+CONFIG_MM_REGIONS=3
+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_NFILE_DESCRIPTORS=12
+CONFIG_NSH_ARCHINIT=y
+CONFIG_NSH_ARCHROMFS=y
+CONFIG_NSH_ARGCAT=y
+CONFIG_NSH_BUILTIN_APPS=y
+CONFIG_NSH_CMDPARMS=y
+CONFIG_NSH_CROMFSETC=y
+CONFIG_NSH_DISABLE_IFCONFIG=y
+CONFIG_NSH_DISABLE_IFUPDOWN=y
+CONFIG_NSH_DISABLE_MB=y
+CONFIG_NSH_DISABLE_MH=y
+CONFIG_NSH_DISABLE_TELNETD=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_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_RAMTRON_WRITEWAIT=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_SDMMC1_SDIO_MODE=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_BBSRAM=y
+CONFIG_STM32H7_BBSRAM_FILES=5
+CONFIG_STM32H7_BKPSRAM=y
+CONFIG_STM32H7_DMA1=y
+CONFIG_STM32H7_DMA2=y
+CONFIG_STM32H7_DMACAPABLE=y
+CONFIG_STM32H7_DTCMEXCLUDE=y
+CONFIG_STM32H7_DTCM_PROCFS=y
+CONFIG_STM32H7_FLOWCONTROL_BROKEN=y
+CONFIG_STM32H7_I2C1=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_MAGIC_REG=1
+CONFIG_STM32H7_SAVE_CRASHDUMP=y
+CONFIG_STM32H7_SDMMC1=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_SPI5=y
+CONFIG_STM32H7_SPI5_DMA=y
+CONFIG_STM32H7_SPI5_DMA_BUFFER=1024
+CONFIG_STM32H7_SPI_DMA=y
+CONFIG_STM32H7_SPI_DMATHRESHOLD=8
+CONFIG_STM32H7_TIM1=y
+CONFIG_STM32H7_TIM4=y
+CONFIG_STM32H7_TIM8=y
+CONFIG_STM32H7_UART4=y
+CONFIG_STM32H7_UART7=y
+CONFIG_STM32H7_UART8=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_TTY_SIGINT=y
+CONFIG_TTY_SIGSTP=y
+CONFIG_UART4_BAUD=57600
+CONFIG_UART4_RXBUFSIZE=600
+CONFIG_UART4_TXBUFSIZE=1500
+CONFIG_UART7_BAUD=57600
+CONFIG_UART7_RXBUFSIZE=600
+CONFIG_UART7_SERIAL_CONSOLE=y
+CONFIG_UART7_TXBUFSIZE=1500
+CONFIG_UART8_BAUD=57600
+CONFIG_UART8_RXBUFSIZE=600
+CONFIG_UART8_TXBUFSIZE=1500
+CONFIG_USART2_BAUD=57600
+CONFIG_USART2_IFLOWCONTROL=y
+CONFIG_USART2_OFLOWCONTROL=y
+CONFIG_USART2_RXBUFSIZE=600
+CONFIG_USART2_TXBUFSIZE=1500
+CONFIG_USART3_BAUD=57600
+CONFIG_USART3_IFLOWCONTROL=y
+CONFIG_USART3_OFLOWCONTROL=y
+CONFIG_USART3_RXBUFSIZE=600
+CONFIG_USART3_TXBUFSIZE=3000
+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
@@ -0,0 +1,223 @@
+/****************************************************************************
+ * scripts/script.ld
+ *
+ *   Copyright (C) 2020 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 board uses an STM32H743II 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
+ *
+ * There's a switch on board, the BOOT0 pin is at ground so by default,
+ * the STM32 will boot to address 0x0800:0000 in FLASH unless the switch is
+ * drepresed, then the boot will be from 0x1FF0:0000
+ *
+ * The STM32H743ZI 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
+{
+	FLASH     (rx) : ORIGIN = 0x08000000, LENGTH = 2048K
+	DTCM1    (rwx) : ORIGIN = 0x20000000, LENGTH =   64K
+	DTCM2    (rwx) : ORIGIN = 0x20010000, LENGTH =   64K
+
+	ITCM_RAM (rwx) : ORIGIN = 0x00000000, LENGTH =   64K
+	DTCM_RAM (rwx) : ORIGIN = 0x20000000, LENGTH =  128K
+	AXI_SRAM (rwx) : ORIGIN = 0x24000000, LENGTH =  512K /* D1 domain AXI bus */
+	SRAM1    (rwx) : ORIGIN = 0x30000000, LENGTH =  128K /* D2 domain AHB bus */
+	SRAM2    (rwx) : ORIGIN = 0x30020000, LENGTH =  128K /* D2 domain AHB bus */
+	SRAM3    (rwx) : ORIGIN = 0x30040000, LENGTH =   32K /* D2 domain AHB bus */
+	SRAM4    (rwx) : ORIGIN = 0x38000000, LENGTH =   64K /* D3 domain */
+	BKPRAM   (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(.);
+	} > AXI_SRAM AT > FLASH
+
+	.bss : {
+		_sbss = ABSOLUTE(.);
+		*(.bss .bss.*)
+		*(.gnu.linkonce.b.*)
+		*(COMMON)
+		. = ALIGN(4);
+		_ebss = ABSOLUTE(.);
+	} > AXI_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) }
+
+	.ramfunc : {
+		_sramfuncs = .;
+		*(.ramfunc  .ramfunc.*)
+		. = ALIGN(4);
+		_eramfuncs = .;
+	} > ITCM_RAM AT > FLASH
+
+	_framfuncs = LOADADDR(.ramfunc);
+}
@@ -0,0 +1,221 @@
+/****************************************************************************
+ * scripts/script.ld
+ *
+ *   Copyright (C) 2021 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 board uses an STM32H743XIH6 and 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
+ *
+ * There's a switch on board, the BOOT0 pin is at ground so by default,
+ * the STM32 will boot to address 0x0800:0000 in FLASH unless the switch is
+ * drepresed, then the boot will be from 0x1FF0:0000
+ *
+ * The STM32H743ZI 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_RAM  (rwx) : ORIGIN = 0x00000000, LENGTH =   64K
+	FLASH      (rx) : ORIGIN = 0x08020000, LENGTH = 1920K
+
+	DTCM1_RAM (rwx) : ORIGIN = 0x20000000, LENGTH =   64K
+	DTCM2_RAM (rwx) : ORIGIN = 0x20010000, LENGTH =   64K
+	AXI_SRAM  (rwx) : ORIGIN = 0x24000000, LENGTH =  512K /* D1 domain AXI bus */
+	SRAM1     (rwx) : ORIGIN = 0x30000000, LENGTH =  128K /* D2 domain AHB bus */
+	SRAM2     (rwx) : ORIGIN = 0x30020000, LENGTH =  128K /* D2 domain AHB bus */
+	SRAM3     (rwx) : ORIGIN = 0x30040000, LENGTH =   32K /* D2 domain AHB bus */
+	SRAM4     (rwx) : ORIGIN = 0x38000000, LENGTH =   64K /* D3 domain */
+	BKPRAM    (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(.);
+	} > AXI_SRAM AT > FLASH
+
+	.bss : {
+		_sbss = ABSOLUTE(.);
+		*(.bss .bss.*)
+		*(.gnu.linkonce.b.*)
+		*(COMMON)
+		. = ALIGN(4);
+		_ebss = ABSOLUTE(.);
+	} > AXI_SRAM
+
+	/* Emit the the D3 power domain section for locating BDMA data */
+
+	.sram4 (NOLOAD) :
+	{
+	} > 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,64 @@
+############################################################################
+#
+#   Copyright (c) 2021 PX4 Development Team. All rights reserved.
+#
+# Redistribution and use in source and binary forms, with or without
+# modification, are permitted provided that the following conditions
+# 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_library(drivers_board
+		bootloader_main.c
+		usb.c
+	)
+	target_link_libraries(drivers_board
+		PRIVATE
+			nuttx_arch
+			nuttx_drivers
+			bootloader
+	)
+	target_include_directories(drivers_board PRIVATE ${PX4_SOURCE_DIR}/platforms/nuttx/src/bootloader)
+
+else()
+	add_library(drivers_board
+		i2c.cpp
+		init.c
+		led.c
+		spi.cpp
+		timer_config.cpp
+		usb.c
+	)
+
+	target_link_libraries(drivers_board
+		PRIVATE
+			arch_spi
+			drivers__led
+			nuttx_arch
+			nuttx_drivers
+			px4_layer
+	)
+endif()
@@ -0,0 +1,186 @@
+/****************************************************************************
+ *
+ *   Copyright (c) 2021 PX4 Development Team. All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * 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
+ *
+ * Board internal definitions
+ */
+
+#pragma once
+
+#include <px4_platform_common/px4_config.h>
+#include <nuttx/compiler.h>
+#include <stdint.h>
+#include <stm32_gpio.h>
+
+/* LEDs are driven with push open drain to support Anode to 5V or 3.3V */
+#define GPIO_nLED_RED   /* PB11 */ (GPIO_OUTPUT|GPIO_OPENDRAIN|GPIO_SPEED_50MHz|GPIO_OUTPUT_SET|GPIO_PORTB|GPIO_PIN11)
+#define GPIO_nLED_GREEN /* PB1  */ (GPIO_OUTPUT|GPIO_OPENDRAIN|GPIO_SPEED_50MHz|GPIO_OUTPUT_SET|GPIO_PORTB|GPIO_PIN1)
+#define GPIO_nLED_BLUE  /* PB3  */ (GPIO_OUTPUT|GPIO_OPENDRAIN|GPIO_SPEED_50MHz|GPIO_OUTPUT_SET|GPIO_PORTB|GPIO_PIN3)
+
+#define BOARD_HAS_CONTROL_STATUS_LEDS      1
+#define BOARD_OVERLOAD_LED     LED_RED
+#define BOARD_ARMED_STATE_LED  LED_BLUE
+
+/* ADC channels */
+#define PX4_ADC_GPIO  \
+	/* PA2 */  GPIO_ADC12_INP14,  \
+	/* PA3 */  GPIO_ADC12_INP15,  \
+	/* PA4 */  GPIO_ADC12_INP18,  \
+	/* PC1 */  GPIO_ADC123_INP11
+
+/* Define Channel numbers must match above GPIO pins */
+#define ADC_BATTERY_VOLTAGE_CHANNEL       14 /* PA2 BATT_VOLT_SENS */
+#define ADC_BATTERY_CURRENT_CHANNEL       15 /* PA3 BATT_CURRENT_SENS */
+#define ADC_SCALED_V5_CHANNEL             18 /* PA4 VDD_5V_SENS */
+#define ADC_RC_RSSI_CHANNEL               11 /* PC1 */
+
+#define ADC_CHANNELS \
+	((1 << ADC_BATTERY_VOLTAGE_CHANNEL)       | \
+	 (1 << ADC_BATTERY_CURRENT_CHANNEL)       | \
+	 (1 << ADC_SCALED_V5_CHANNEL)             | \
+	 (1 << ADC_RC_RSSI_CHANNEL))
+
+/* HW has to large of R termination on ADC todo:change when HW value is chosen */
+#define BOARD_ADC_OPEN_CIRCUIT_V     (5.6f)
+
+/* CAN Silence: Silent mode control */
+#define GPIO_CAN1_SILENT_S0  /* PF5  */ (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_2MHz|GPIO_OUTPUT_CLEAR|GPIO_PORTF|GPIO_PIN5)
+
+/* PWM */
+#define DIRECT_PWM_OUTPUT_CHANNELS  8
+#define DIRECT_INPUT_TIMER_CHANNELS  8
+
+/* Power supply control and monitoring GPIOs */
+#define GPIO_nPOWER_IN_A                /* PB5 */ (GPIO_INPUT|GPIO_PULLUP|GPIO_PORTB|GPIO_PIN5)
+
+#define GPIO_VDD_BRICK1_VALID          GPIO_nPOWER_IN_A /* Brick 1 Is Chosen */
+#define BOARD_NUMBER_BRICKS             1
+
+#define GPIO_VDD_3V3_SPEKTRUM_POWER_EN  /* PE4  */ (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_2MHz|GPIO_OUTPUT_CLEAR|GPIO_PORTE|GPIO_PIN4)
+
+/* Define True logic Power Control in arch agnostic form */
+#define VDD_3V3_SPEKTRUM_POWER_EN(on_true) px4_arch_gpiowrite(GPIO_VDD_3V3_SPEKTRUM_POWER_EN, (!on_true))
+#define READ_VDD_3V3_SPEKTRUM_POWER_EN()   (px4_arch_gpioread(GPIO_VDD_3V3_SPEKTRUM_POWER_EN) == 0)
+
+/* Tone alarm output */
+#define TONE_ALARM_TIMER        2  /* timer 2 */
+#define TONE_ALARM_CHANNEL      1  /* PA15 TIM2_CH1 */
+
+#define GPIO_BUZZER_1           /* PA15 */ (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_2MHz|GPIO_OUTPUT_CLEAR|GPIO_PORTA|GPIO_PIN15)
+
+#define GPIO_TONE_ALARM_IDLE    GPIO_BUZZER_1
+#define GPIO_TONE_ALARM         GPIO_TIM2_CH1OUT_2
+
+/* USB OTG FS */
+#define GPIO_OTGFS_VBUS         /* PA9 */ (GPIO_INPUT|GPIO_PULLDOWN|GPIO_SPEED_100MHz|GPIO_PORTA|GPIO_PIN9)
+
+/* High-resolution timer */
+#define HRT_TIMER               3  /* use timer3 for the HRT */
+#define HRT_TIMER_CHANNEL       1 /* use capture/compare channel 1 */
+
+#define HRT_PPM_CHANNEL         /* T3C3 */  3  /* use capture/compare channel 3 */
+#define GPIO_PPM_IN             /* PB0 T3C3 */ GPIO_TIM3_CH3IN_1
+
+/* RC Serial port */
+#define RC_SERIAL_PORT          "/dev/ttyS3"
+
+#define GPIO_RSSI_IN            /* PC1  */ (GPIO_INPUT|GPIO_PULLUP|GPIO_PORTC|GPIO_PIN1)
+
+/* Safety Switch: Enable the FMU to control it if there is no px4io fixme:This should be BOARD_SAFETY_LED(__ontrue) */
+#define GPIO_SAFETY_SWITCH_IN              /* PC4 */ (GPIO_INPUT|GPIO_PULLUP|GPIO_PORTC|GPIO_PIN4)
+/* Enable the FMU to use the switch it if there is no px4io fixme:This should be BOARD_SAFTY_BUTTON() */
+#define GPIO_BTN_SAFETY GPIO_SAFETY_SWITCH_IN /* Enable the FMU to control it if there is no px4io */
+
+/* Power switch controls ******************************************************/
+#define SPEKTRUM_POWER(_on_true)           VDD_3V3_SPEKTRUM_POWER_EN(_on_true)
+
+/*
+ * Board has a separate RC_IN
+ *
+ * GPIO PPM_IN on PB0 T3CH3
+ * SPEKTRUM_RX (it's TX or RX in Bind) on UART6 PC7
+ *   Inversion is possible in the UART and can drive GPIO_PPM_IN as an output
+ */
+#define GPIO_PPM_IN_AS_OUT             (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_2MHz|GPIO_OUTPUT_SET|GPIO_PORTB|GPIO_PIN0)
+#define SPEKTRUM_RX_AS_GPIO_OUTPUT()   px4_arch_configgpio(GPIO_PPM_IN_AS_OUT)
+#define SPEKTRUM_RX_AS_UART()          /* Can be left as uart */
+#define SPEKTRUM_OUT(_one_true)        px4_arch_gpiowrite(GPIO_PPM_IN_AS_OUT, (_one_true))
+
+/* 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     BOARD_ADC_USB_CONNECTED
+#define BOARD_ADC_SERVO_VALID   (1) /* never powers off the Servo rail */
+#define BOARD_ADC_BRICK_VALID   (px4_arch_gpioread(GPIO_VDD_BRICK1_VALID))
+
+/* 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 BOARD_HAS_STATIC_MANIFEST 1
+
+#define BOARD_HAS_PWM  DIRECT_PWM_OUTPUT_CHANNELS
+
+#define BOARD_NUM_IO_TIMERS 3
+
+#define BOARD_DSHOT_MOTOR_ASSIGNMENT {3, 2, 1, 0, 4, 5, 6, 7};
+
+#define BOARD_ENABLE_CONSOLE_BUFFER
+
+#define PX4_GPIO_INIT_LIST { \
+		PX4_ADC_GPIO,                     \
+		GPIO_CAN1_TX,                     \
+		GPIO_CAN1_RX,                     \
+		GPIO_CAN1_SILENT_S0,              \
+		GPIO_nPOWER_IN_A,                 \
+		GPIO_VDD_3V3_SPEKTRUM_POWER_EN,   \
+		GPIO_TONE_ALARM_IDLE,             \
+		GPIO_SAFETY_SWITCH_IN,            \
+		GPIO_OTGFS_VBUS,                  \
+	}
+
+__BEGIN_DECLS
+#ifndef __ASSEMBLY__
+
+extern void stm32_spiinitialize(void);
+extern void board_peripheral_reset(int ms);
+
+#include <px4_platform_common/board_common.h>
+#endif /* __ASSEMBLY__ */
+__END_DECLS
@@ -0,0 +1,76 @@
+/****************************************************************************
+ *
+ *   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.
+ *
+ ****************************************************************************/
+
+/**
+ * @file bootloader_main.c
+ *
+ * FMU-specific early startup code for bootloader
+*/
+
+#include "board_config.h"
+#include "bl.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>
+
+extern int sercon_main(int c, char **argv);
+
+__EXPORT void board_on_reset(int status) {}
+
+__EXPORT void stm32_boardinitialize(void)
+{
+	/* configure USB interfaces */
+	stm32_configgpio(GPIO_OTGFS_VBUS);
+}
+
+__EXPORT int board_app_initialize(uintptr_t arg)
+{
+	return 0;
+}
+
+void board_late_initialize(void)
+{
+	px4_platform_console_init();
+	sercon_main(0, NULL);
+}
+
+extern void sys_tick_handler(void);
+void board_timerhook(void)
+{
+	sys_tick_handler();
+}
@@ -0,0 +1,135 @@
+/****************************************************************************
+ *
+ *   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
+
+/****************************************************************************
+ * 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                     1023
+#define _FLASH_KBYTES                  (*(uint32_t *)0x1FF1E880)
+#define BOARD_FLASH_SECTORS            (15)
+#define BOARD_FLASH_SIZE               (_FLASH_KBYTES * 1024)
+
+#define OSC_FREQ                       24
+
+#define BOARD_PIN_LED_ACTIVITY         GPIO_nLED_RED
+#define BOARD_PIN_LED_BOOTLOADER       GPIO_nLED_RED
+#define BOARD_LED_ON                   0
+#define BOARD_LED_OFF                  1
+
+#define SERIAL_BREAK_DETECT_DISABLED   1
+
+#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
@@ -0,0 +1,38 @@
+/****************************************************************************
+ *
+ *   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),
+};
@@ -0,0 +1,201 @@
+/****************************************************************************
+ *
+ *   Copyright (c) 2021 PX4 Development Team. All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * 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
+ *
+ * board-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 initialisation.
+ */
+
+#include "board_config.h"
+
+#include <nuttx/config.h>
+#include <nuttx/board.h>
+#include <nuttx/sdio.h>
+#include <nuttx/mmcsd.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_dma_alloc.h>
+
+__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)
+{
+	bool last = READ_VDD_3V3_SPEKTRUM_POWER_EN();
+	/* Keep Spektum on to discharge rail*/
+	VDD_3V3_SPEKTRUM_POWER_EN(false);
+
+	/* 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 */
+	VDD_3V3_SPEKTRUM_POWER_EN(last);
+}
+
+/************************************************************************************
+ * 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(PX4_MAKE_GPIO_INPUT(io_timer_channel_get_as_pwm_input(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)
+{
+	/* Reset PWM first thing */
+	board_on_reset(-1);
+
+	/* configure pins */
+	const uint32_t gpio[] = PX4_GPIO_INIT_LIST;
+	px4_gpio_init(gpio, arraySize(gpio));
+	board_control_spi_sensors_power_configgpio();
+
+	/* configure LEDs */
+	board_autoled_initialize();
+}
+
+/****************************************************************************
+ * 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;
+ *
+ * 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)
+{
+	/* Power on Interfaces */
+	board_control_spi_sensors_power(true, 0xffff);
+	VDD_3V3_SPEKTRUM_POWER_EN(true);
+
+	px4_platform_init();
+
+	stm32_spiinitialize();
+
+	/* configure the DMA allocator */
+	if (board_dma_alloc_init() < 0) {
+		syslog(LOG_ERR, "[boot] DMA alloc FAILED\n");
+	}
+
+	/* initial LED state */
+	drv_led_start();
+	led_off(LED_RED);
+	led_on(LED_GREEN); // Indicate Power.
+	led_off(LED_BLUE);
+
+	if (board_hardfault_init(2, true) != 0) {
+		led_on(LED_RED);
+	}
+
+#ifdef CONFIG_MMCSD
+	/* Mount the SDIO-based MMC/SD block driver */
+	/* First, get an instance of the SDIO interface */
+	struct sdio_dev_s *sdio_dev = sdio_initialize(0); // SDIO_SLOTNO 0 Only one slot
+
+	if (!sdio_dev) {
+		syslog(LOG_ERR, "[boot] Failed to initialize SDIO slot %d\n", 0);
+		return ERROR;
+	}
+
+	if (mmcsd_slotinitialize(0, sdio_dev) != OK) {
+		syslog(LOG_ERR, "[boot] Failed to bind SDIO to the MMC/SD driver\n");
+		return ERROR;
+	}
+
+	/* Assume that the SD card is inserted.  What choice do we have? */
+	sdio_mediachange(sdio_dev, true);
+#endif /* CONFIG_MMCSD */
+
+	/* Configure the HW based on the manifest */
+
+	px4_platform_configure();
+
+	return OK;
+}
@@ -0,0 +1,111 @@
+/****************************************************************************
+ *
+ *   Copyright (c) 2021 PX4 Development Team. All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * 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 led.c
+ *
+ * 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
+
+static uint32_t g_ledmap[] = {
+	GPIO_nLED_BLUE,                     // Indexed by LED_BLUE
+	GPIO_nLED_RED,                      // Indexed by LED_RED
+	GPIO_nLED_GREEN,                    // Indexed by LED_GREEN
+};
+
+__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(led, true);
+}
+
+__EXPORT void led_off(int led)
+{
+	phy_set_led(led, false);
+}
+
+__EXPORT void led_toggle(int led)
+{
+	phy_set_led(led, !phy_get_led(led));
+}
@@ -0,0 +1,53 @@
+/****************************************************************************
+ *
+ *   Copyright (C) 2021 PX4 Development Team. All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * 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_t px4_spi_buses[SPI_BUS_MAX_BUS_ITEMS] = {
+	initSPIBus(SPI::Bus::SPI1, {
+		initSPIDevice(DRV_IMU_DEVTYPE_ICM20602, SPI::CS{GPIO::PortC, GPIO::Pin2}, SPI::DRDY{GPIO::PortD, GPIO::Pin15}),
+		initSPIDevice(DRV_IMU_DEVTYPE_ICM20948, SPI::CS{GPIO::PortE, GPIO::Pin15}, SPI::DRDY{GPIO::PortE, GPIO::Pin12}),
+	}, {GPIO::PortE, GPIO::Pin3}),
+	initSPIBus(SPI::Bus::SPI2, {
+		initSPIDevice(SPIDEV_FLASH(0), SPI::CS{GPIO::PortD, GPIO::Pin10}),
+		initSPIDevice(DRV_BARO_DEVTYPE_DPS310, SPI::CS{GPIO::PortD, GPIO::Pin7}),
+	}),
+	initSPIBus(SPI::Bus::SPI5, {
+		initSPIDevice(DRV_GYR_DEVTYPE_BMI088, SPI::CS{GPIO::PortF, GPIO::Pin10}, SPI::DRDY{GPIO::PortF, GPIO::Pin3}),
+		initSPIDevice(DRV_ACC_DEVTYPE_BMI088, SPI::CS{GPIO::PortF, GPIO::Pin6}, SPI::DRDY{GPIO::PortF, GPIO::Pin1}),
+	}),
+};
+
+static constexpr bool unused = validateSPIConfig(px4_spi_buses);
@@ -0,0 +1,54 @@
+/****************************************************************************
+ *
+ *   Copyright (C) 2021 PX4 Development Team. All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * 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>
+
+constexpr io_timers_t io_timers[MAX_IO_TIMERS] = {
+	initIOTimer(Timer::Timer1, DMA{DMA::Index1}),
+	initIOTimer(Timer::Timer4, DMA{DMA::Index1}),
+	initIOTimer(Timer::Timer8, DMA{DMA::Index1}),
+};
+
+constexpr timer_io_channels_t timer_io_channels[MAX_TIMER_IO_CHANNELS] = {
+	initIOTimerChannel(io_timers, {Timer::Timer1, Timer::Channel4}, {GPIO::PortE, GPIO::Pin14}),
+	initIOTimerChannel(io_timers, {Timer::Timer1, Timer::Channel3}, {GPIO::PortE, GPIO::Pin13}),
+	initIOTimerChannel(io_timers, {Timer::Timer1, Timer::Channel2}, {GPIO::PortE, GPIO::Pin11}),
+	initIOTimerChannel(io_timers, {Timer::Timer1, Timer::Channel1}, {GPIO::PortE, GPIO::Pin9}),
+	initIOTimerChannel(io_timers, {Timer::Timer4, Timer::Channel2}, {GPIO::PortD, GPIO::Pin13}),
+	initIOTimerChannel(io_timers, {Timer::Timer4, Timer::Channel3}, {GPIO::PortD, GPIO::Pin14}),
+	initIOTimerChannel(io_timers, {Timer::Timer8, Timer::Channel1}, {GPIO::PortI, GPIO::Pin5}),
+	initIOTimerChannel(io_timers, {Timer::Timer8, Timer::Channel2}, {GPIO::PortI, GPIO::Pin6}),
+};
+
+constexpr io_timers_channel_mapping_t io_timers_channel_mapping =
+	initIOTimerChannelMapping(io_timers, timer_io_channels);
@@ -0,0 +1,58 @@
+/****************************************************************************
+ *
+ *   Copyright (C) 2021 PX4 Development Team. All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * 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 usb.c
+ *
+ * Board-specific USB functions.
+ */
+
+#include "board_config.h"
+#include <nuttx/usb/usbdev.h>
+#include <nuttx/usb/usbdev_trace.h>
+#include <stm32_otg.h>
+
+/************************************************************************************
+ * 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);
+}
@@ -237,8 +237,8 @@
 #define GPIO_UART4_TX    GPIO_UART4_TX_2       /* PA0  */
 #define GPIO_UART4_RX    GPIO_UART4_RX_2       /* PA1  */

-#define GPIO_USART6_TX   GPIO_USART6_TX_2      /* PG14 */
-#define GPIO_USART6_RX   GPIO_USART6_RX_1      /* PC7  */
+//#define GPIO_USART6_TX   GPIO_USART6_TX_2      /* PG14 */
+//#define GPIO_USART6_RX   GPIO_USART6_RX_1      /* PC7  */

 #define GPIO_UART7_TX    GPIO_UART7_TX_3       /* PE8  */
 #define GPIO_UART7_RX    GPIO_UART7_RX_3       /* PE7  */
@@ -262,7 +262,7 @@
 #define GPIO_SPI1_MISO   GPIO_SPI1_MISO_1               /* PA6  */
 #define GPIO_SPI1_MOSI   GPIO_SPI1_MOSI_1               /* PA7  */

-#define GPIO_SPI2_SCK    ADJ_SLEW_RATE(GPIO_SPI2_SCK_2) /* PB10 */
+#define GPIO_SPI2_SCK    ADJ_SLEW_RATE(GPIO_SPI2_SCK_3) /* PB10 */
 #define GPIO_SPI2_MISO   GPIO_SPI2_MISO_1               /* PB14 */
 #define GPIO_SPI2_MOSI   GPIO_SPI2_MOSI_1               /* PB15 */

@@ -198,7 +198,6 @@ 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
@@ -228,9 +227,6 @@ CONFIG_USART3_IFLOWCONTROL=y
 CONFIG_USART3_OFLOWCONTROL=y
 CONFIG_USART3_RXBUFSIZE=600
 CONFIG_USART3_TXBUFSIZE=3000
-CONFIG_USART6_BAUD=57600
-CONFIG_USART6_RXBUFSIZE=600
-CONFIG_USART6_TXBUFSIZE=1500
 CONFIG_USBDEV=y
 CONFIG_USBDEV_BUSPOWERED=y
 CONFIG_USBDEV_MAXPOWER=500
@@ -68,6 +68,7 @@ set(msg_files
 	geofence_result.msg
 	gps_dump.msg
 	gps_inject_data.msg
+	heater_status.msg
 	home_position.msg
 	hover_thrust_estimate.msg
 	input_rc.msg
@@ -127,11 +128,11 @@ set(msg_files
 	sensor_selection.msg
 	sensors_status_imu.msg
 	system_power.msg
+	takeoff_status.msg
 	task_stack_info.msg
 	tecs_status.msg
 	telemetry_status.msg
 	test_motor.msg
-	takeoff_status.msg
 	timesync.msg
 	timesync_status.msg
 	trajectory_bezier.msg
@@ -0,0 +1,19 @@
+uint64 timestamp	# time since system start (microseconds)
+
+uint32 device_id
+
+bool heater_on
+
+float32 temperature_sensor
+float32 temperature_target
+
+uint32 controller_period_usec
+uint32 controller_time_on_usec
+
+float32 proportional_value
+float32 integrator_value
+float32 feed_forward_value
+
+uint8 MODE_GPIO  = 1
+uint8 MODE_PX4IO = 2
+uint8 mode
@@ -313,6 +313,8 @@ rtps:
    id: 148
  - msg: takeoff_status
    id: 149
+  - msg: heater_status
+    id: 150
  ########## multi topics: begin ##########
  - msg: actuator_controls_0
    id: 170
@@ -73,17 +73,11 @@ Heater::Heater() :
 	}

 #endif
-
-	heater_enable();
 }

 Heater::~Heater()
 {
 	heater_disable();
-
-#ifdef HEATER_PX4IO
-	px4_close(_io_fd);
-#endif
 }

 int Heater::custom_command(int argc, char *argv[])
@@ -97,28 +91,35 @@ int Heater::custom_command(int argc, char *argv[])
 	return print_usage("Unrecognized command.");
 }

-uint32_t Heater::get_sensor_id()
-{
-	return _sensor_accel.device_id;
-}
-
 void Heater::heater_disable()
 {
 	// Reset heater to off state.
 #ifdef HEATER_PX4IO
-	px4_ioctl(_io_fd, PX4IO_HEATER_CONTROL, HEATER_MODE_DISABLED);
+	if (_io_fd >= 0) {
+		px4_ioctl(_io_fd, PX4IO_HEATER_CONTROL, HEATER_MODE_DISABLED);
+	}
+
 #endif
+
 #ifdef HEATER_GPIO
 	px4_arch_configgpio(GPIO_HEATER_OUTPUT);
 #endif
 }

-void Heater::heater_enable()
+void Heater::heater_initialize()
 {
 	// Initialize heater to off state.
 #ifdef HEATER_PX4IO
-	px4_ioctl(_io_fd, PX4IO_HEATER_CONTROL, HEATER_MODE_OFF);
+	if (_io_fd < 0) {
+		_io_fd = px4_open(IO_HEATER_DEVICE_PATH, O_RDWR);
+	}
+
+	if (_io_fd >= 0) {
+		px4_ioctl(_io_fd, PX4IO_HEATER_CONTROL, HEATER_MODE_OFF);
+	}
+
 #endif
+
 #ifdef HEATER_GPIO
 	px4_arch_configgpio(GPIO_HEATER_OUTPUT);
 #endif
@@ -127,8 +128,13 @@ void Heater::heater_enable()
 void Heater::heater_off()
 {
 #ifdef HEATER_PX4IO
-	px4_ioctl(_io_fd, PX4IO_HEATER_CONTROL, HEATER_MODE_OFF);
+
+	if (_io_fd >= 0) {
+		px4_ioctl(_io_fd, PX4IO_HEATER_CONTROL, HEATER_MODE_OFF);
+	}
+
 #endif
+
 #ifdef HEATER_GPIO
 	px4_arch_gpiowrite(GPIO_HEATER_OUTPUT, 0);
 #endif
@@ -137,64 +143,170 @@ void Heater::heater_off()
 void Heater::heater_on()
 {
 #ifdef HEATER_PX4IO
-	px4_ioctl(_io_fd, PX4IO_HEATER_CONTROL, HEATER_MODE_ON);
+
+	if (_io_fd >= 0) {
+		px4_ioctl(_io_fd, PX4IO_HEATER_CONTROL, HEATER_MODE_ON);
+	}
+
 #endif
+
 #ifdef HEATER_GPIO
 	px4_arch_gpiowrite(GPIO_HEATER_OUTPUT, 1);
 #endif
 }

-void Heater::initialize_topics()
+bool Heater::initialize_topics()
 {
-	// Get the total number of accelerometer instances.
-	uint8_t number_of_imus = orb_group_count(ORB_ID(sensor_accel));
+	for (uint8_t i = 0; i < ORB_MULTI_MAX_INSTANCES; i++) {
+		uORB::SubscriptionData<sensor_accel_s> sensor_accel_sub{ORB_ID(sensor_accel), i};

-	// Get the total number of accelerometer instances and check each instance for the correct ID.
-	for (uint8_t x = 0; x < number_of_imus; x++) {
-		_sensor_accel.device_id = 0;
+		if (sensor_accel_sub.get().timestamp != 0 && sensor_accel_sub.get().device_id != 0
+		    && PX4_ISFINITE(sensor_accel_sub.get().temperature)) {

-		while (_sensor_accel.device_id == 0) {
-			_sensor_accel_sub = uORB::Subscription{ORB_ID(sensor_accel), x};
-
-			if (!_sensor_accel_sub.advertised()) {
-				px4_usleep(100);
-				continue;
+			// If the correct ID is found, exit the for-loop with _sensor_accel_sub pointing to the correct instance.
+			if (sensor_accel_sub.get().device_id == (uint32_t)_param_sens_temp_id.get()) {
+				_sensor_accel_sub.ChangeInstance(i);
+				_sensor_device_id = sensor_accel_sub.get().device_id;
+				return true;
 			}
-
-			_sensor_accel_sub.copy(&_sensor_accel);
-		}
-
-		// If the correct ID is found, exit the for-loop with _sensor_accel_sub pointing to the correct instance.
-		if (_sensor_accel.device_id == (uint32_t)_param_sens_temp_id.get()) {
-			break;
 		}
 	}

-	// Exit the driver if the sensor ID does not match the desired sensor.
-	if (_sensor_accel.device_id != (uint32_t)_param_sens_temp_id.get()) {
-		request_stop();
-		PX4_ERR("Could not identify IMU sensor.");
-	}
+	return false;
 }

-int Heater::print_status()
+void Heater::Run()
 {
-	float _feedforward_value = _param_sens_imu_temp_ff.get();
+	if (should_exit()) {
+#if defined(HEATER_PX4IO)

-	PX4_INFO("Sensor ID: %d,\tSetpoint: %3.2fC,\t Sensor Temperature: %3.2fC,\tDuty Cycle (usec): %d",
-		 _sensor_accel.device_id,
-		 static_cast<double>(_param_sens_imu_temp.get()),
-		 static_cast<double>(_sensor_accel.temperature),
-		 _controller_period_usec);
-	PX4_INFO("Feed Forward control effort: %3.2f%%,\tProportional control effort: %3.2f%%,\tIntegrator control effort: %3.3f%%,\t Heater cycle: %3.2f%%",
-		 static_cast<double>(_feedforward_value * 100),
-		 static_cast<double>(_proportional_value * 100),
-		 static_cast<double>(_integrator_value * 100),
-		 static_cast<double>(static_cast<float>(_controller_time_on_usec) / static_cast<float>(_controller_period_usec) * 100));
+		// must be closed from wq thread
+		if (_io_fd >= 0) {
+			px4_close(_io_fd);
+		}

+#endif
+		exit_and_cleanup();
+		return;
+	}
+
+	// check for parameter updates
+	if (_parameter_update_sub.updated()) {
+		// clear update
+		parameter_update_s pupdate;
+		_parameter_update_sub.copy(&pupdate);
+
+		// update parameters from storage
+		ModuleParams::updateParams();
+	}
+
+	if (_sensor_device_id == 0) {
+		if (initialize_topics()) {
+			heater_initialize();
+
+		} else {
+			// if sensor still not found try again in 1 second
+			ScheduleDelayed(1_s);
+			return;
+		}
+	}
+
+	sensor_accel_s sensor_accel;
+
+	if (_heater_on) {
+		// Turn the heater off.
+		heater_off();
+		_heater_on = false;
+
+	} else if (_sensor_accel_sub.update(&sensor_accel)) {
+		float temperature_delta {0.f};
+
+		// Update the current IMU sensor temperature if valid.
+		if (PX4_ISFINITE(sensor_accel.temperature)) {
+			temperature_delta = _param_sens_imu_temp.get() - sensor_accel.temperature;
+			_temperature_last = sensor_accel.temperature;
+		}
+
+		_proportional_value = temperature_delta * _param_sens_imu_temp_p.get();
+		_integrator_value += temperature_delta * _param_sens_imu_temp_i.get();
+
+		if (fabs(_param_sens_imu_temp_i.get()) <= 0.0001) {
+			_integrator_value = 0.f;
+		}
+
+		// Guard against integrator wind up.
+		_integrator_value = math::constrain(_integrator_value, -0.25f, 0.25f);
+
+		_controller_time_on_usec = static_cast<int>((_param_sens_imu_temp_ff.get() + _proportional_value +
+					   _integrator_value) * static_cast<float>(_controller_period_usec));
+
+		_controller_time_on_usec = math::constrain(_controller_time_on_usec, 0, _controller_period_usec);
+
+		_heater_on = true;
+		heater_on();
+	}
+
+	// Schedule the next cycle.
+	if (_heater_on) {
+		ScheduleDelayed(_controller_time_on_usec);
+
+	} else {
+		ScheduleDelayed(_controller_period_usec - _controller_time_on_usec);
+	}
+
+
+	// publish status
+	heater_status_s status{};
+	status.heater_on = _heater_on;
+	status.device_id = _sensor_device_id;
+	status.temperature_sensor = _temperature_last;
+	status.temperature_target = _param_sens_imu_temp.get();
+	status.controller_period_usec = _controller_period_usec;
+	status.controller_time_on_usec = _controller_time_on_usec;
+	status.proportional_value = _proportional_value;
+	status.integrator_value = _integrator_value;
+	status.feed_forward_value = _param_sens_imu_temp_ff.get();
+
+#ifdef HEATER_PX4IO
+	status.mode |= heater_status_s::MODE_PX4IO;
+#endif
+#ifdef HEATER_GPIO
+	status.mode |= heater_status_s::MODE_GPIO;
+#endif
+
+	status.timestamp = hrt_absolute_time();
+	_heater_status_pub.publish(status);
+}
+
+int Heater::start()
+{
+	// Exit the driver if the sensor ID does not match the desired sensor.
+	if (_param_sens_temp_id.get() == 0) {
+		PX4_ERR("Valid SENS_TEMP_ID required");
+		request_stop();
+		return PX4_ERROR;
+	}
+
+	ScheduleNow();
 	return PX4_OK;
 }

+int Heater::task_spawn(int argc, char *argv[])
+{
+	Heater *heater = new Heater();
+
+	if (!heater) {
+		PX4_ERR("driver allocation failed");
+		return PX4_ERROR;
+	}
+
+	_object.store(heater);
+	_task_id = task_id_is_work_queue;
+
+	heater->start();
+	return 0;
+}
+
 int Heater::print_usage(const char *reason)
 {
 	if (reason) {
@@ -216,101 +328,7 @@ This task can be started at boot from the startup scripts by setting SENS_EN_THE
 	return 0;
 }

-void Heater::Run()
-{
-	if (should_exit()) {
-		exit_and_cleanup();
-		return;
-	}
-
-	if (_heater_on) {
-		// Turn the heater off.
-		heater_off();
-		_heater_on = false;
-
-	} else {
-		update_params(false);
-
-		_sensor_accel_sub.update(&_sensor_accel);
-
-		float temperature_delta {0.f};
-
-		// Update the current IMU sensor temperature if valid.
-		if (!isnan(_sensor_accel.temperature)) {
-			temperature_delta = _param_sens_imu_temp.get() - _sensor_accel.temperature;
-		}
-
-		_proportional_value = temperature_delta * _param_sens_imu_temp_p.get();
-		_integrator_value += temperature_delta * _param_sens_imu_temp_i.get();
-
-		if (fabs(_param_sens_imu_temp_i.get()) <= 0.0001) {
-			_integrator_value = 0.f;
-		}
-
-		// Guard against integrator wind up.
-		_integrator_value = math::constrain(_integrator_value, -0.25f, 0.25f);
-
-		_controller_time_on_usec = static_cast<int>((_param_sens_imu_temp_ff.get() + _proportional_value +
-						             _integrator_value) * static_cast<float>(_controller_period_usec));
-
-		_controller_time_on_usec = math::constrain(_controller_time_on_usec, 0, _controller_period_usec);
-
-		_heater_on = true;
-		heater_on();
-	}
-
-	// Schedule the next cycle.
-	if (_heater_on) {
-		ScheduleDelayed(_controller_time_on_usec);
-
-	} else {
-		ScheduleDelayed(_controller_period_usec - _controller_time_on_usec);
-	}
-}
-
-int Heater::start()
-{
-	update_params(true);
-	initialize_topics();
-
-	// Allow sufficient time for all additional sensors and processes to start.
-	ScheduleDelayed(100000);
-	return PX4_OK;
-}
-
-int Heater::task_spawn(int argc, char *argv[])
-{
-	Heater *heater = new Heater();
-
-	if (!heater) {
-		PX4_ERR("driver allocation failed");
-		return PX4_ERROR;
-	}
-
-	_object.store(heater);
-	_task_id = task_id_is_work_queue;
-
-	heater->start();
-	return 0;
-}
-
-void Heater::update_params(const bool force)
-{
-	// check for parameter updates
-	if (_parameter_update_sub.updated() || force) {
-		// clear update
-		parameter_update_s pupdate;
-		_parameter_update_sub.copy(&pupdate);
-
-		// update parameters from storage
-		ModuleParams::updateParams();
-	}
-}
-
-/**
- * Main entry point for the heater driver module
- */
-int heater_main(int argc, char *argv[])
+extern "C" __EXPORT int heater_main(int argc, char *argv[])
 {
 	return Heater::main(argc, argv);
 }
@@ -47,7 +47,9 @@
 #include <px4_platform_common/module.h>
 #include <px4_platform_common/module_params.h>
 #include <px4_platform_common/px4_work_queue/ScheduledWorkItem.hpp>
+#include <uORB/Publication.hpp>
 #include <uORB/SubscriptionInterval.hpp>
+#include <uORB/topics/heater_status.h>
 #include <uORB/topics/parameter_update.h>
 #include <uORB/topics/sensor_accel.h>

@@ -57,13 +59,6 @@ using namespace time_literals;

 #define CONTROLLER_PERIOD_DEFAULT 100000

-/**
- * @brief IMU Heater Controller driver used to maintain consistent
- *        temparature at the IMU.
- */
-extern "C" __EXPORT int heater_main(int argc, char *argv[]);
-
-
 class Heater : public ModuleBase<Heater>, public ModuleParams, public px4::ScheduledWorkItem
 {
 public:
@@ -97,30 +92,6 @@ public:
 	 */
 	static int task_spawn(int argc, char *argv[]);

-	/**
-	 * @brief Sets and/or reports the heater controller time period value in microseconds.
-	 * @param argv Pointer to the input argument array.
-	 * @return Returns 0 iff successful, -1 otherwise.
-	 */
-	int controller_period(char *argv[]);
-
-	/** @brief Returns the id of the target sensor. */
-	uint32_t get_sensor_id();
-
-	/**
-	 * @brief Sets and/or reports the heater controller integrator gain value.
-	 * @param argv Pointer to the input argument array.
-	 * @return Returns the heater integrator gain value iff successful, 0.0f otherwise.
-	 */
-	float integrator(char *argv[]);
-
-	/**
-	 * @brief Sets and/or reports the heater controller proportional gain value.
-	 * @param argv Pointer to the input argument array.
-	 * @return Returns the heater proportional gain value iff successful, 0.0f otherwise.
-	 */
-	float proportional(char *argv[]);
-
 	/**
 	 * @brief Initiates the heater driver work queue, starts a new background task,
 	 *        and fails if it is already running.
@@ -128,25 +99,10 @@ public:
 	 */
 	int start();

-	/**
-	 * @brief Reports curent status and diagnostic information about the heater driver.
-	 * @return Returns 0 iff successful, -1 otherwise.
-	 */
-	int print_status();
-
-	/**
-	 * @brief Sets and/or reports the heater target temperature.
-	 * @param argv Pointer to the input argument array.
-	 * @return Returns the heater target temperature value iff successful, -1.0f otherwise.
-	 */
-	float temperature_setpoint(char *argv[]);
-
-protected:
+private:

 	/** @brief Called once to initialize uORB topics. */
-	void initialize_topics();
-
-private:
+	bool initialize_topics();

 	/** @brief Calculates the heater element on/off time and schedules the next cycle. */
 	void Run() override;
@@ -158,7 +114,7 @@ private:
 	void update_params(const bool force = false);

 	/** Enables / configures the heater (either by GPIO or PX4IO). */
-	void heater_enable();
+	void heater_initialize();

 	/** Disnables the heater (either by GPIO or PX4IO). */
 	void heater_disable();
@@ -174,7 +130,7 @@ private:

 	/** File descriptor for PX4IO for heater ioctl's */
 #if defined(HEATER_PX4IO)
-	int _io_fd;
+	int _io_fd_ {-1};
 #endif

 	bool _heater_on = false;
@@ -185,10 +141,15 @@ private:
 	float _integrator_value   = 0.0f;
 	float _proportional_value = 0.0f;

+	uORB::Publication<heater_status_s> _heater_status_pub{ORB_ID(heater_status)};
+
 	uORB::SubscriptionInterval _parameter_update_sub{ORB_ID(parameter_update), 1_s};

 	uORB::Subscription _sensor_accel_sub{ORB_ID(sensor_accel)};
-	sensor_accel_s _sensor_accel{};
+
+	uint32_t _sensor_device_id{0};
+
+	float _temperature_last{NAN};

 	DEFINE_PARAMETERS(
 		(ParamFloat<px4::params::SENS_IMU_TEMP_FF>) _param_sens_imu_temp_ff,
@@ -1944,11 +1944,12 @@ Commander::run()
 		}

 		/* start mission result check */
-		const auto prev_mission_instance_count = _mission_result_sub.get().instance_count;
-
-		if (_mission_result_sub.update()) {
+		if (_mission_result_sub.updated()) {
 			const mission_result_s &mission_result = _mission_result_sub.get();

+			const auto prev_mission_instance_count = mission_result.instance_count;
+			_mission_result_sub.update();
+
 			// if mission_result is valid for the current mission
 			const bool mission_result_ok = (mission_result.timestamp > _boot_timestamp)
 						       && (mission_result.instance_count > 0);
@@ -1956,7 +1957,6 @@ Commander::run()
 			_status_flags.condition_auto_mission_available = mission_result_ok && mission_result.valid;

 			if (mission_result_ok) {
-
 				if (_status.mission_failure != mission_result.failure) {
 					_status.mission_failure = mission_result.failure;
 					_status_changed = true;
@@ -1984,6 +1984,20 @@ Commander::run()
 					}
 				}
 			}
+
+			// Transition main state to loiter or auto-mission after takeoff is completed.
+			if (_armed.armed && !_land_detector.landed
+			    && (_status.nav_state == vehicle_status_s::NAVIGATION_STATE_AUTO_TAKEOFF)
+			    && (mission_result.timestamp >= _status.nav_state_timestamp)
+			    && mission_result.finished) {
+
+				if ((_param_takeoff_finished_action.get() == 1) && _status_flags.condition_auto_mission_available) {
+					main_state_transition(_status, commander_state_s::MAIN_STATE_AUTO_MISSION, _status_flags, &_internal_state);
+
+				} else {
+					main_state_transition(_status, commander_state_s::MAIN_STATE_AUTO_LOITER, _status_flags, &_internal_state);
+				}
+			}
 		}

 		// update manual_control_setpoint before geofence (which might check sticks or switches)
@@ -2381,25 +2395,6 @@ Commander::run()
 			}
 		}

-		/* Reset main state to loiter or auto-mission after takeoff is completed.
-		 * Sometimes, the mission result topic is outdated and the mission is still signaled
-		 * as finished even though we only just started with the takeoff. Therefore, we also
-		 * check the timestamp of the mission_result topic. */
-		if (_internal_state.main_state == commander_state_s::MAIN_STATE_AUTO_TAKEOFF
-		    && (_mission_result_sub.get().timestamp >= _internal_state.timestamp)
-		    && _mission_result_sub.get().finished) {
-
-			const bool mission_available = (_mission_result_sub.get().timestamp > _boot_timestamp)
-						       && (_mission_result_sub.get().instance_count > 0) && _mission_result_sub.get().valid;
-
-			if ((_param_takeoff_finished_action.get() == 1) && mission_available) {
-				main_state_transition(_status, commander_state_s::MAIN_STATE_AUTO_MISSION, _status_flags, &_internal_state);
-
-			} else {
-				main_state_transition(_status, commander_state_s::MAIN_STATE_AUTO_LOITER, _status_flags, &_internal_state);
-			}
-		}
-
 		/* check if we are disarmed and there is a better mode to wait in */
 		if (!_armed.armed) {
 			/* if there is no radio control but GPS lock the user might want to fly using
@@ -62,6 +62,7 @@ void LoggedTopics::add_default_topics()
 	add_topic("cpuload");
 	add_topic("esc_status", 250);
 	add_topic("generator_status");
+	add_topic("heater_status");
 	add_topic("home_position");
 	add_topic("hover_thrust_estimate", 100);
 	add_topic("input_rc", 500);
@@ -45,7 +45,7 @@ void AutopilotTester::connect(const std::string uri)

 	std::cout << time_str() << "Waiting for system connect" << std::endl;
 	REQUIRE(poll_condition_with_timeout(
-	[this]() { return _mavsdk.is_connected(); }, adjust_to_lockstep_speed(std::chrono::seconds(25))));
+	[this]() { return _mavsdk.is_connected(); }, std::chrono::seconds(25)));

 	auto &system = _mavsdk.system();

@@ -179,15 +179,7 @@ void AutopilotTester::prepare_square_mission(MissionOptions mission_options)

 	_mission->set_return_to_launch_after_mission(mission_options.rtl_at_end);

-	std::promise<void> prom;
-	auto fut = prom.get_future();
-
-	_mission->upload_mission_async(mission_plan, [&prom](Mission::Result result) {
-		REQUIRE(Mission::Result::Success == result);
-		prom.set_value();
-	});
-
-	REQUIRE(fut.wait_for(std::chrono::seconds(2)) == std::future_status::ready);
+	REQUIRE(_mission->upload_mission(mission_plan) == Mission::Result::Success);
 }

 void AutopilotTester::prepare_straight_mission(MissionOptions mission_options)
@@ -203,15 +195,7 @@ void AutopilotTester::prepare_straight_mission(MissionOptions mission_options)

 	_mission->set_return_to_launch_after_mission(mission_options.rtl_at_end);

-	std::promise<void> prom;
-	auto fut = prom.get_future();
-
-	_mission->upload_mission_async(mission_plan, [&prom](Mission::Result result) {
-		REQUIRE(Mission::Result::Success == result);
-		prom.set_value();
-	});
-
-	REQUIRE(fut.wait_for(std::chrono::seconds(2)) == std::future_status::ready);
+	REQUIRE(_mission->upload_mission(mission_plan) == Mission::Result::Success);
 }

 void AutopilotTester::execute_mission()
@@ -361,29 +345,29 @@ void AutopilotTester::fly_forward_in_posctl()
 	const unsigned manual_control_rate_hz = 50;

 	// Send something to make sure RC is available.
-	for (unsigned i = 0; i < 5 * manual_control_rate_hz; ++i) {
+	for (unsigned i = 0; i < 1 * manual_control_rate_hz; ++i) {
 		CHECK(_manual_control->set_manual_control_input(0.f, 0.f, 0.5f, 0.f) == ManualControl::Result::Success);
-		std::this_thread::sleep_for(adjust_to_lockstep_speed(std::chrono::milliseconds(1000 / manual_control_rate_hz)));
+		sleep_for(std::chrono::milliseconds(1000 / manual_control_rate_hz));
 	}

 	CHECK(_manual_control->start_position_control() == ManualControl::Result::Success);

-	// Climb up for 10 seconds
-	for (unsigned i = 0; i < 10 * manual_control_rate_hz; ++i) {
+	// Climb up for 20 seconds
+	for (unsigned i = 0; i < 20 * manual_control_rate_hz; ++i) {
 		CHECK(_manual_control->set_manual_control_input(0.f, 0.f, 1.f, 0.f) == ManualControl::Result::Success);
-		std::this_thread::sleep_for(adjust_to_lockstep_speed(std::chrono::milliseconds(1000 / manual_control_rate_hz)));
+		sleep_for(std::chrono::milliseconds(1000 / manual_control_rate_hz));
 	}

 	// Fly forward for 60 seconds
 	for (unsigned i = 0; i < 60 * manual_control_rate_hz; ++i) {
 		CHECK(_manual_control->set_manual_control_input(0.5f, 0.f, 0.5f, 0.f) == ManualControl::Result::Success);
-		std::this_thread::sleep_for(adjust_to_lockstep_speed(std::chrono::milliseconds(1000 / manual_control_rate_hz)));
+		sleep_for(std::chrono::milliseconds(1000 / manual_control_rate_hz));
 	}

 	// Descend until disarmed
 	for (unsigned i = 0; i < 60 * manual_control_rate_hz; ++i) {
 		CHECK(_manual_control->set_manual_control_input(0.f, 0.f, 0.0f, 0.f) == ManualControl::Result::Success);
-		std::this_thread::sleep_for(adjust_to_lockstep_speed(std::chrono::milliseconds(1000 / manual_control_rate_hz)));
+		sleep_for(std::chrono::milliseconds(1000 / manual_control_rate_hz));

 		if (!_telemetry->in_air()) {
 			break;
@@ -396,29 +380,29 @@ void AutopilotTester::fly_forward_in_altctl()
 	const unsigned manual_control_rate_hz = 50;

 	// Send something to make sure RC is available.
-	for (unsigned i = 0; i < 5 * manual_control_rate_hz; ++i) {
+	for (unsigned i = 0; i < 1 * manual_control_rate_hz; ++i) {
 		CHECK(_manual_control->set_manual_control_input(0.f, 0.f, 0.5f, 0.f) == ManualControl::Result::Success);
-		std::this_thread::sleep_for(adjust_to_lockstep_speed(std::chrono::milliseconds(1000 / manual_control_rate_hz)));
+		sleep_for(std::chrono::milliseconds(1000 / manual_control_rate_hz));
 	}

 	CHECK(_manual_control->start_altitude_control() == ManualControl::Result::Success);

-	// Climb up for 10 seconds
-	for (unsigned i = 0; i < 10 * manual_control_rate_hz; ++i) {
+	// Climb up for 20 seconds
+	for (unsigned i = 0; i < 20 * manual_control_rate_hz; ++i) {
 		CHECK(_manual_control->set_manual_control_input(0.f, 0.f, 1.f, 0.f) == ManualControl::Result::Success);
-		std::this_thread::sleep_for(adjust_to_lockstep_speed(std::chrono::milliseconds(1000 / manual_control_rate_hz)));
+		sleep_for(std::chrono::milliseconds(1000 / manual_control_rate_hz));
 	}

 	// Fly forward for 60 seconds
 	for (unsigned i = 0; i < 60 * manual_control_rate_hz; ++i) {
 		CHECK(_manual_control->set_manual_control_input(0.5f, 0.f, 0.5f, 0.f) == ManualControl::Result::Success);
-		std::this_thread::sleep_for(adjust_to_lockstep_speed(std::chrono::milliseconds(1000 / manual_control_rate_hz)));
+		sleep_for(std::chrono::milliseconds(1000 / manual_control_rate_hz));
 	}

 	// Descend until disarmed
 	for (unsigned i = 0; i < 60 * manual_control_rate_hz; ++i) {
 		CHECK(_manual_control->set_manual_control_input(0.f, 0.f, 0.0f, 0.f) == ManualControl::Result::Success);
-		std::this_thread::sleep_for(adjust_to_lockstep_speed(std::chrono::milliseconds(1000 / manual_control_rate_hz)));
+		sleep_for(std::chrono::milliseconds(1000 / manual_control_rate_hz));

 		if (!_telemetry->in_air()) {
 			break;
@@ -619,28 +603,3 @@ void AutopilotTester::wait_for_mission_finished(std::chrono::seconds timeout)

 	REQUIRE(fut.wait_for(timeout) == std::future_status::ready);
 }
-
-std::chrono::milliseconds AutopilotTester::adjust_to_lockstep_speed(std::chrono::milliseconds duration_ms)
-{
-	if (_info == nullptr) {
-		return duration_ms;
-	}
-
-	auto speed_factor = _info->get_speed_factor();
-
-	if (speed_factor.first == Info::Result::Success) {
-		// FIXME: Remove this again:
-		//        Sanitize speed factor to avoid test failures.
-		if (speed_factor.second > 20.0f) {
-			speed_factor.second = 20.0f;
-		}
-
-		return static_cast<std::chrono::milliseconds>(
-			       static_cast<unsigned long>(
-				       std::round(
-					       static_cast<double>(duration_ms.count()) / speed_factor.second)));
-
-	} else {
-		return duration_ms;
-	}
-}
@@ -131,8 +131,6 @@ private:
 	void wait_for_landed_state(Telemetry::LandedState landed_state, std::chrono::seconds timeout);
 	void wait_for_mission_finished(std::chrono::seconds timeout);

-	std::chrono::milliseconds adjust_to_lockstep_speed(std::chrono::milliseconds duration_ms);
-
 	template<typename Rep, typename Period>
 	bool poll_condition_with_timeout(
 		std::function<bool()> fun, std::chrono::duration<Rep, Period> duration)
@@ -184,6 +182,31 @@ private:
 		return true;
 	}

+	template<typename Rep, typename Period>
+	void sleep_for(std::chrono::duration<Rep, Period> duration)
+	{
+		const std::chrono::microseconds duration_us(duration);
+
+		if (_telemetry && _telemetry->attitude_quaternion().timestamp_us != 0) {
+
+			const int64_t start_time_us = _telemetry->attitude_quaternion().timestamp_us;
+
+			while (true) {
+				// Hopefully this is often enough not to have PX4 time out on us.
+				std::this_thread::sleep_for(std::chrono::milliseconds(1));
+
+				const int64_t elapsed_time_us = _telemetry->attitude_quaternion().timestamp_us - start_time_us;
+
+				if (elapsed_time_us > duration_us.count()) {
+					return;
+				}
+			}
+
+		} else {
+			std::this_thread::sleep_for(duration);
+		}
+	}
+
 	mavsdk::Mavsdk _mavsdk{};
 	std::unique_ptr<mavsdk::Action> _action{};
 	std::unique_ptr<mavsdk::Failure> _failure{};
@@ -16,6 +16,16 @@ import process_helper as ph
 from typing import Any, Dict, List, NoReturn, TextIO, Optional
 from types import FrameType

+try:
+    import requests
+except ImportError as e:
+    print("Failed to import requests: " + str(e))
+    print("")
+    print("You may need to install it using:")
+    print("    pip3 install --user requests")
+    print("")
+    sys.exit(1)
+

 def main() -> NoReturn:

@@ -37,6 +47,8 @@ def main() -> NoReturn:
                             "(or multiple cases with wildcard '*')")
    parser.add_argument("--debugger", default="",
                        help="choice from valgrind, callgrind, gdb, lldb")
+    parser.add_argument("--upload", default=False, action='store_true',
+                        help="Upload logs to logs.px4.io")
    parser.add_argument("--verbose", default=False, action='store_true',
                        help="enable more verbose output")
    parser.add_argument("config_file", help="JSON config file to use")
@@ -67,7 +79,8 @@ def main() -> NoReturn:
        args.debugger,
        args.log_dir,
        args.gui,
-        args.verbose
+        args.verbose,
+        args.upload
    )
    signal.signal(signal.SIGINT, tester.sigint_handler)

@@ -124,7 +137,8 @@ class Tester:
                 debugger: str,
                 log_dir: str,
                 gui: bool,
-                 verbose: bool):
+                 verbose: bool,
+                 upload: bool):
        self.config = config
        self.active_runners: List[ph.Runner]
        self.iterations = iterations
@@ -135,6 +149,7 @@ class Tester:
        self.log_dir = log_dir
        self.gui = gui
        self.verbose = verbose
+        self.upload = upload
        self.start_time = datetime.datetime.now()
        self.log_fd: Any[TextIO] = None

@@ -364,6 +379,12 @@ class Tester:
            print("  - {}".format(logfile_path))
            for runner in self.active_runners:
                print("  - {}".format(runner.get_log_filename()))
+
+        if self.upload:
+            ulog_file = self.parse_for_ulog_file(
+                self.get_combined_log(logfile_path))
+            self.upload_log(ulog_file, test['model'], case, is_success)
+
        return is_success

    def start_runners(self,
@@ -459,6 +480,66 @@ class Tester:
                if self.verbose:
                    print(line, end="")

+    def parse_for_ulog_file(self, log: str) -> Optional[str]:
+
+        match = "[logger] Opened full log file: ./"
+        for line in log.splitlines():
+            found = line.find(match)
+            if found != -1:
+                return os.getcwd() \
+                    + "/build/px4_sitl_default/tmp_mavsdk_tests/rootfs/" \
+                    + line[found+len(match):]
+        return None
+
+    def upload_log(self, ulog_path: Optional[str],
+                   model: str, case: str, success: bool) -> None:
+        if not ulog_path:
+            print("    Could not find ulog log file to upload")
+            return
+
+        if not os.getenv('GITHUB_WORKFLOW'):
+            print("    Upload only implemented for GitHub Actions CI")
+            return
+
+        print("    Uploading logfile '{}' ...".format(ulog_path))
+
+        server = "https://logs.px4.io"
+
+        payload = {
+            "type": "flightreport",
+            "description": "SITL integration test with {} for {}: {}"
+            .format(model, case, "passing" if success else "failing"),
+            "feedback":
+                "workflow: {}, ".format(os.getenv("GITHUB_WORKFLOW")) +
+                "run_id: {}, ".format(os.getenv("GITHUB_RUN_ID")) +
+                "repository: {}, ".format(os.getenv("GITHUB_REPOSITORY")) +
+                "ref: {}, ".format(os.getenv("GITHUB_REF")) +
+                "model: {}, ".format(model) +
+                "case: {}, ".format(case) +
+                "pass: {}".format("true" if success else "false"),
+            "email": "",
+            "source": "CI",
+            "videoUrl": "",
+            "rating": "notset",
+            "windSpeed": -1,
+            "public": "true"
+        }
+
+        with open(ulog_path, 'rb') as f:
+            r = requests.post(server + "/upload",
+                              data=payload,
+                              files={'filearg': f},
+                              allow_redirects=False)
+            if r.status_code == 302:  # redirect
+                if 'Location' in r.headers:
+                    plot_url = r.headers['Location']
+                    if len(plot_url) > 0 and plot_url[0] == '/':
+                        plot_url = server + plot_url
+                    print("    Uploaded to: " + plot_url)
+            else:
+                print("    Upload failed with status_code: {}"
+                      .format(r.status_code))
+
    def start_combined_log(self, filename: str) -> None:
        self.log_fd = open(filename, 'w')
Author	SHA1	Message	Date
Daniel Agar	b56ba14910	WIP: mRo ctrl-zero-h7	2021-02-16 21:51:42 -05:00
Daniel Agar	acaf219c0f	boards: mRo pixracer pro disable USART6 (conflicts with SPI6)	2021-02-16 21:48:57 -05:00
Daniel Agar	3ecc2df820	boards: mRo pixracer pro fix incorrect SPI2 sck	2021-02-16 21:26:12 -05:00
Daniel Agar	a416731656	drivers/heater: add logging and minor improvements - new heater_status logging message - run continously at low rate until configured sensor is found - fix px4io fd bugs (fd open/close/ioctl must be same thread)	2021-02-16 15:16:19 -05:00
Daniel Agar	5abf29d93c	commander: auto TAKEOFF to LOITER/MISSION minor fix to avoid failsafe confusion - base on nav_state rather than main_state	2021-02-16 13:02:14 -05:00
Julian Oes	64d0ce43b6	mavsdk_tests: upload to logs.px4.io in test_runner By doing the upload directly inside the mavsdk_test_runner.py we have more context such as the model, case, etc.	2021-02-16 17:07:21 +01:00
Julian Oes	18281eee29	workflows: more meta information to log upload This adds a little bit more information but still lacks the one to one mapping between test and uploaded file.	2021-02-16 17:07:21 +01:00
Julian Oes	2315618b85	mavsdk_tests: use autopilot timestamps to sleep This way we should be able to avoid some of the timeouts happening on the PX4 side if MAVSDK doesn't send setpoints in time.	2021-02-16 17:07:21 +01:00
Julian Oes	6c26387e85	mavsdk_tests: try to prevent auto-disarm We should not spend too much time sending RC init messages because if it takes too long, we might auto-disarm in the meantime.	2021-02-16 17:07:21 +01:00
Julian Oes	c1de27acf7	mavsdk_tests: use more conservative speed_factor	2021-02-16 17:07:21 +01:00
Julian Oes	0ebb8c6981	mavsdk_tests: use microseconds to avoid sleep(0)	2021-02-16 17:07:21 +01:00
Julian Oes	b6b9ce5f11	mavsdk_tests: robustify test by climbing longer	2021-02-16 17:07:21 +01:00
Julian Oes	b79553862e	mavsdk_tests: remove timeout for mission upload Just use what is MAVSDK internal.	2021-02-16 17:07:21 +01:00