msp_osd: Add VTX config, stick commands and arming status

- Adds MSPv1 rx parsing to fetch VTX config
 - Allows to inspect and change VTX channel through CLI
 - Forward MSP_RC for stick commands osd
 - Forward MSP_STATUS for arming status for PIT and LP mode

.github/workflows/build_all_targets.yml

@@ -16,13 +16,11 @@ on:
       - 'release/**'
     paths-ignore:
       - 'docs/**'
-      - '.github/**'
   pull_request:
     branches:
-      - '*'
+      - '**'
     paths-ignore:
       - 'docs/**'
-      - '.github/**'
 
 jobs:
   group_targets:

.github/workflows/checks.yml

@@ -9,7 +9,7 @@ on:
       - '.github/**'
   pull_request:
     branches:
-    - '*'
+      - '**'
     paths-ignore:
       - 'docs/**'
       - '.github/**'

.github/workflows/clang-tidy.yml

@@ -9,7 +9,7 @@ on:
       - '.github/**'
   pull_request:
     branches:
-    - '*'
+      - '**'
     paths-ignore:
       - 'docs/**'
       - '.github/**'

.github/workflows/compile_macos.yml

@@ -9,7 +9,7 @@ on:
       - '.github/**'
   pull_request:
     branches:
-    - '*'
+      - '**'
     paths-ignore:
       - 'docs/**'
       - '.github/**'

.github/workflows/compile_ubuntu.yml

@@ -12,7 +12,7 @@ on:
       - '.github/**'
   pull_request:
     branches:
-      - '*'
+      - '**'
     paths-ignore:
       - 'docs/**'
       - '.github/**'

.github/workflows/dev_container.yml

@@ -15,7 +15,7 @@ on:
       - '.github/**'
   pull_request:
     branches:
-      - '*'
+      - '**'
     paths-ignore:
       - 'docs/**'
       - '.github/**'

.github/workflows/docs_deploy.yml

@@ -9,7 +9,7 @@ on:
       - 'docs/en/**'
   pull_request:
     branches:
-      - '*'
+      - '**'
     paths:
       - 'docs/en/**'
 
@@ -80,7 +80,7 @@ jobs:
       - name: Deploy
         run: |
           git clone --single-branch --branch main --depth 1 https://${{ secrets.PX4BUILTBOT_PERSONAL_ACCESS_TOKEN }}@github.com/PX4/docs.px4.io.git
-          # make it an orphan branch      
+          # make it an orphan branch
           cd docs.px4.io
           CURRENT_DATETIME=$(date +'%Y%m%d_%H_%M')
           git checkout --orphan "${CURRENT_DATETIME}_main"

.github/workflows/ekf_functional_change_indicator.yml

@@ -3,7 +3,7 @@ name: EKF Change Indicator
 on:
   pull_request:
     branches:
-    - '*'
+      - '**'
     paths-ignore:
       - 'docs/**'
       - '.github/**'

.github/workflows/failsafe_sim.yml

@@ -9,7 +9,7 @@ on:
       - '.github/**'
   pull_request:
     branches:
-    - '*'
+      - '**'
     paths-ignore:
       - 'docs/**'
       - '.github/**'

.github/workflows/flash_analysis.yml

@@ -14,7 +14,7 @@ on:
       - '.github/**'
   pull_request:
     branches:
-      - '*'
+      - '**'
     paths-ignore:
       - 'docs/**'
       - '.github/**'

.github/workflows/mavros_mission_tests.yml

@@ -9,7 +9,7 @@ on:
       - '.github/**'
   pull_request:
     branches:
-    - '*'
+      - '**'
     paths-ignore:
       - 'docs/**'
       - '.github/**'

.github/workflows/mavros_offboard_tests.yml

@@ -9,7 +9,7 @@ on:
       - '.github/**'
   pull_request:
     branches:
-    - '*'
+    - '**'
     paths-ignore:
       - 'docs/**'
       - '.github/**'

.github/workflows/nuttx_env_config.yml

@@ -9,7 +9,7 @@ on:
       - '.github/**'
   pull_request:
     branches:
-    - '*'
+      - '**'
     paths-ignore:
       - 'docs/**'
       - '.github/**'

.github/workflows/python_checks.yml

@@ -9,7 +9,7 @@ on:
       - '.github/**'
   pull_request:
     branches:
-    - '*'
+    - '**'
     paths-ignore:
       - 'docs/**'
       - '.github/**'

.github/workflows/ros_integration_tests.yml

@@ -14,7 +14,7 @@ on:
       - '.github/**'
   pull_request:
     branches:
-    - '*'
+      - '**'
     paths-ignore:
       - 'docs/**'
       - '.github/**'

.github/workflows/ros_translation_node.yml

@@ -8,7 +8,7 @@ on:
       - '.github/**'
   pull_request:
     branches:
-    - '*'
+      - '**'
     paths-ignore:
       - 'docs/**'
       - '.github/**'

.github/workflows/sitl_tests.yml

@@ -14,7 +14,7 @@ on:
       - '.github/**'
   pull_request:
     branches:
-    - '*'
+      - '**'
     paths-ignore:
       - 'docs/**'
       - '.github/**'

.vscode/cmake-variants.yaml

@@ -301,6 +301,11 @@ CONFIG:
       buildType: MinSizeRel
       settings:
         CONFIG: holybro_durandal-v1_default
+    holybro_kakuteh7-wing_default:
+      short: holybro_kakuteh7-wing
+      buildType: MinSizeRel
+      settings:
+        CONFIG: holybro_kakuteh7-wing_default
     matek_h743-slim_default:
       short: matek_h743-slim
       buildType: MinSizeRel

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

@@ -127,6 +127,9 @@
 #define SPI6_nRESET_EXTERNAL1       /* PF10 */ (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_2MHz|GPIO_OUTPUT_CLEAR|GPIO_PORTF|GPIO_PIN10)
 #define SPI6_RESET(on_true)          px4_arch_gpiowrite(SPI6_nRESET_EXTERNAL1, !(on_true))
 
+// ADIS16507 hardware reset
+#define GPIO_ADIS16507_RESET(reset) SPI6_RESET(reset)
+
 /* I2C busses */
 
 /* Devices on the onboard buses.

boards/hkust/nxt-dual/default.px4board

@@ -17,6 +17,7 @@ CONFIG_COMMON_DIFFERENTIAL_PRESSURE=y
 CONFIG_COMMON_DISTANCE_SENSOR=y
 CONFIG_DRIVERS_DSHOT=y
 CONFIG_DRIVERS_GPS=y
+CONFIG_DRIVERS_OSD_MSP_OSD=y
 CONFIG_DRIVERS_IMU_BOSCH_BMI088=y
 CONFIG_DRIVERS_IRLOCK=y
 CONFIG_COMMON_LIGHT=y

boards/holybro/kakuteh7-wing/bootloader.px4board

@@ -0,0 +1,3 @@
+CONFIG_BOARD_TOOLCHAIN="arm-none-eabi"
+CONFIG_BOARD_ARCHITECTURE="cortex-m7"
+CONFIG_BOARD_ROMFSROOT=""

boards/holybro/kakuteh7-wing/default.px4board

@@ -0,0 +1,77 @@
+CONFIG_BOARD_TOOLCHAIN="arm-none-eabi"
+CONFIG_BOARD_ARCHITECTURE="cortex-m7"
+CONFIG_BOARD_SERIAL_GPS1="/dev/ttyS0"
+CONFIG_BOARD_SERIAL_GPS2="/dev/ttyS1"
+CONFIG_BOARD_SERIAL_TEL1="/dev/ttyS2"
+CONFIG_BOARD_SERIAL_TEL2="/dev/ttyS3"
+CONFIG_BOARD_SERIAL_TEL3="/dev/ttyS5"
+CONFIG_BOARD_SERIAL_RC="/dev/ttyS4"
+CONFIG_BOARD_CONSTRAINED_FLASH=y
+CONFIG_DRIVERS_ADC_BOARD_ADC=y
+CONFIG_DRIVERS_BAROMETER_BMP280=y
+CONFIG_DRIVERS_BAROMETER_GOERTEK_SPA06=y
+CONFIG_DRIVERS_BATT_SMBUS=y
+CONFIG_DRIVERS_CDCACM_AUTOSTART=y
+CONFIG_COMMON_DIFFERENTIAL_PRESSURE=y
+CONFIG_COMMON_DISTANCE_SENSOR=y
+CONFIG_DRIVERS_DSHOT=y
+CONFIG_DRIVERS_GPS=y
+CONFIG_DRIVERS_IMU_INVENSENSE_ICM42688P=y
+CONFIG_DRIVERS_OSD_ATXXXX=y
+CONFIG_COMMON_LIGHT=y
+CONFIG_COMMON_MAGNETOMETER=y
+CONFIG_DRIVERS_PWM_OUT=y
+CONFIG_DRIVERS_RC_INPUT=y
+CONFIG_COMMON_TELEMETRY=y
+CONFIG_DRIVERS_TONE_ALARM=y
+CONFIG_DRIVERS_UAVCAN=y
+CONFIG_BOARD_UAVCAN_TIMER_OVERRIDE=7
+CONFIG_MODULES_AIRSPEED_SELECTOR=y
+CONFIG_MODULES_BATTERY_STATUS=y
+CONFIG_MODULES_COMMANDER=y
+CONFIG_MODULES_CONTROL_ALLOCATOR=y
+CONFIG_MODULES_DATAMAN=y
+CONFIG_MODULES_EKF2=y
+CONFIG_MODULES_ESC_BATTERY=y
+CONFIG_MODULES_EVENTS=y
+CONFIG_MODULES_FLIGHT_MODE_MANAGER=y
+CONFIG_MODULES_FW_ATT_CONTROL=y
+CONFIG_MODULES_FW_AUTOTUNE_ATTITUDE_CONTROL=y
+CONFIG_MODULES_FW_POS_CONTROL=y
+CONFIG_MODULES_FW_RATE_CONTROL=y
+CONFIG_MODULES_GIMBAL=y
+CONFIG_MODULES_GYRO_CALIBRATION=y
+CONFIG_MODULES_GYRO_FFT=y
+CONFIG_MODULES_LAND_DETECTOR=y
+CONFIG_MODULES_LANDING_TARGET_ESTIMATOR=y
+CONFIG_MODULES_LOAD_MON=y
+CONFIG_MODULES_LOGGER=y
+CONFIG_MODULES_MAG_BIAS_ESTIMATOR=y
+CONFIG_MODULES_MANUAL_CONTROL=y
+CONFIG_MODULES_MAVLINK=y
+CONFIG_MODULES_MC_ATT_CONTROL=y
+CONFIG_MODULES_MC_AUTOTUNE_ATTITUDE_CONTROL=y
+CONFIG_MODULES_MC_HOVER_THRUST_ESTIMATOR=y
+CONFIG_MODULES_MC_POS_CONTROL=y
+CONFIG_MODULES_MC_RATE_CONTROL=y
+CONFIG_MODULES_NAVIGATOR=y
+CONFIG_NUM_MISSION_ITMES_SUPPORTED=1000
+CONFIG_MODULES_RC_UPDATE=y
+CONFIG_MODULES_SENSORS=y
+CONFIG_MODULES_VTOL_ATT_CONTROL=y
+CONFIG_SYSTEMCMDS_BSONDUMP=y
+CONFIG_SYSTEMCMDS_DMESG=y
+CONFIG_SYSTEMCMDS_HARDFAULT_LOG=y
+CONFIG_SYSTEMCMDS_MFT=y
+CONFIG_SYSTEMCMDS_NSHTERM=y
+CONFIG_SYSTEMCMDS_PARAM=y
+CONFIG_SYSTEMCMDS_REBOOT=y
+CONFIG_SYSTEMCMDS_SD_BENCH=y
+CONFIG_SYSTEMCMDS_SYSTEM_TIME=y
+CONFIG_SYSTEMCMDS_TOP=y
+CONFIG_SYSTEMCMDS_TOPIC_LISTENER=y
+CONFIG_SYSTEMCMDS_TUNE_CONTROL=y
+CONFIG_SYSTEMCMDS_UORB=y
+CONFIG_SYSTEMCMDS_USB_CONNECTED=y
+CONFIG_SYSTEMCMDS_VER=y
+CONFIG_SYSTEMCMDS_WORK_QUEUE=y

boards/holybro/kakuteh7-wing/firmware.prototype

@@ -0,0 +1,13 @@
+{
+    "board_id": 1105,
+    "magic": "PX4FWv1",
+    "description": "Firmware for the Holybro KakuteH7-Wing board",
+    "image": "",
+    "build_time": 0,
+    "summary": "KAKUTEH7-WING",
+    "version": "0.1",
+    "image_size": 0,
+    "image_maxsize": 1703936,
+    "git_identity": "",
+    "board_revision": 0
+}

boards/holybro/kakuteh7-wing/init/rc.board_defaults

@@ -0,0 +1,13 @@
+#!/bin/sh
+#
+# board specific defaults
+#------------------------------------------------------------------------------
+
+
+param set-default BAT1_V_DIV 18.1
+param set-default BAT2_V_DIV 18.1
+
+param set-default BAT1_A_PER_V 36.367515152
+param set-default BAT2_A_PER_V 36.367515152
+
+param set-default OSD_ATXXXX_CFG 1

boards/holybro/kakuteh7-wing/init/rc.board_sensors

@@ -0,0 +1,20 @@
+#!/bin/sh
+#
+# PX4 FMUv6C specific board sensors init
+#------------------------------------------------------------------------------
+board_adc start
+
+# Internal SPI bus ICM42688p
+icm42688p -R 2 -s start
+
+# Internal baro
+if ! bmp280 -I -b 4 -q start
+then
+	spa06 -I -b 4 start
+fi
+
+# MAX7456 OSD
+if ! param compare OSD_ATXXXX_CFG 0
+then
+	atxxxx start -s
+fi

boards/holybro/kakuteh7-wing/nuttx-config/Kconfig

@@ -0,0 +1,17 @@
+#
+# For a description of the syntax of this configuration file,
+# see misc/tools/kconfig-language.txt.
+#
+config BOARD_HAS_PROBES
+	bool "Board provides GPIO or other Hardware for signaling to timing analyze."
+	default y
+	---help---
+		This board provides GPIO FMU-CH1-5, CAP1-6 as PROBE_1-11 to provide timing signals from selected drivers.
+
+config BOARD_USE_PROBES
+	bool "Enable the use the board provided FMU-CH1-5, CAP1-6 as PROBE_1-11"
+	default n
+	depends on BOARD_HAS_PROBES
+
+	---help---
+		Select to use GPIO FMU-CH1-5, CAP1-6  to provide timing signals from selected drivers.

boards/holybro/kakuteh7-wing/nuttx-config/bootloader/defconfig

@@ -0,0 +1,91 @@
+#
+# This file is autogenerated: PLEASE DO NOT EDIT IT.
+#
+# You can use "make menuconfig" to make any modifications to the installed .config file.
+# You can then do "make savedefconfig" to generate a new defconfig file that includes your
+# modifications.
+#
+# CONFIG_DEV_CONSOLE is not set
+# CONFIG_DISABLE_PSEUDOFS_OPERATIONS is not set
+# CONFIG_SPI_EXCHANGE is not set
+# CONFIG_STM32H7_SYSCFG is not set
+CONFIG_ARCH="arm"
+CONFIG_ARCH_BOARD_CUSTOM=y
+CONFIG_ARCH_BOARD_CUSTOM_DIR="../../../../boards/holybro/kakuteh7-wing/nuttx-config"
+CONFIG_ARCH_BOARD_CUSTOM_DIR_RELPATH=y
+CONFIG_ARCH_BOARD_CUSTOM_NAME="px4"
+CONFIG_ARCH_CHIP="stm32h7"
+CONFIG_ARCH_CHIP_STM32H743VI=y
+CONFIG_ARCH_CHIP_STM32H7=y
+CONFIG_ARCH_INTERRUPTSTACK=512
+CONFIG_ARMV7M_BASEPRI_WAR=y
+CONFIG_ARMV7M_ICACHE=y
+CONFIG_ARMV7M_MEMCPY=y
+CONFIG_ARMV7M_USEBASEPRI=y
+CONFIG_BOARDCTL=y
+CONFIG_BOARDCTL_RESET=y
+CONFIG_BOARD_ASSERT_RESET_VALUE=0
+CONFIG_BOARD_INITTHREAD_PRIORITY=254
+CONFIG_BOARD_LATE_INITIALIZE=y
+CONFIG_BOARD_LOOPSPERMSEC=95150
+CONFIG_BOARD_RESET_ON_ASSERT=2
+CONFIG_CDCACM=y
+CONFIG_CDCACM_IFLOWCONTROL=y
+CONFIG_CDCACM_PRODUCTID=0x0050
+CONFIG_CDCACM_PRODUCTSTR="PX4 BL KakuteH7-Wing"
+CONFIG_CDCACM_RXBUFSIZE=600
+CONFIG_CDCACM_TXBUFSIZE=12000
+CONFIG_CDCACM_VENDORID=0x3162
+CONFIG_CDCACM_VENDORSTR="Holybro"
+CONFIG_DEBUG_FULLOPT=y
+CONFIG_DEBUG_SYMBOLS=y
+CONFIG_DEBUG_TCBINFO=y
+CONFIG_DEFAULT_SMALL=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_INIT_ENTRYPOINT="bootloader_main"
+CONFIG_INIT_STACKSIZE=3194
+CONFIG_LIBC_FLOATINGPOINT=y
+CONFIG_LIBC_LONG_LONG=y
+CONFIG_LIBC_STRERROR=y
+CONFIG_MEMSET_64BIT=y
+CONFIG_MEMSET_OPTSPEED=y
+CONFIG_PREALLOC_TIMERS=50
+CONFIG_PTHREAD_STACK_MIN=512
+CONFIG_RAM_SIZE=245760
+CONFIG_RAM_START=0x20010000
+CONFIG_RAW_BINARY=y
+CONFIG_SERIAL_TERMIOS=y
+CONFIG_SIG_DEFAULT=y
+CONFIG_SIG_SIGALRM_ACTION=y
+CONFIG_SIG_SIGUSR1_ACTION=y
+CONFIG_SIG_SIGUSR2_ACTION=y
+CONFIG_SPI=y
+CONFIG_STACK_COLORATION=y
+CONFIG_START_DAY=30
+CONFIG_START_MONTH=11
+CONFIG_STDIO_BUFFER_SIZE=32
+CONFIG_STM32H7_BKPSRAM=y
+CONFIG_STM32H7_DMA1=y
+CONFIG_STM32H7_DMA2=y
+CONFIG_STM32H7_OTGFS=y
+CONFIG_STM32H7_PROGMEM=y
+CONFIG_STM32H7_SERIAL_DISABLE_REORDERING=y
+CONFIG_STM32H7_TIM1=y
+CONFIG_STM32H7_USART1=y
+CONFIG_SYSTEMTICK_HOOK=y
+CONFIG_SYSTEM_CDCACM=y
+CONFIG_TASK_NAME_SIZE=24
+CONFIG_TTY_SIGINT=y
+CONFIG_TTY_SIGINT_CHAR=0x03
+CONFIG_TTY_SIGTSTP=y
+CONFIG_USART1_RXBUFSIZE=600
+CONFIG_USART1_TXBUFSIZE=300
+CONFIG_USBDEV=y
+CONFIG_USBDEV_BUSPOWERED=y
+CONFIG_USBDEV_MAXPOWER=500
+CONFIG_USEC_PER_TICK=1000

boards/holybro/kakuteh7-wing/nuttx-config/include/board.h

@@ -0,0 +1,503 @@
+/************************************************************************************
+ *
+ *   Copyright (C) 2016-2019 Gregory Nutt. All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright
+ *    notice, this list of conditions and the following disclaimer.
+ * 2. Redistributions in binary form must reproduce the above copyright
+ *    notice, this list of conditions and the following disclaimer in
+ *    the documentation and/or other materials provided with the
+ *    distribution.
+ * 3. Neither the name NuttX nor the names of its contributors may be
+ *    used to endorse or promote products derived from this software
+ *    without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
+ * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
+ * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *
+ ************************************************************************************/
+#pragma once
+
+/************************************************************************************
+ * Included Files
+ ************************************************************************************/
+
+#include "board_dma_map.h"
+
+#include <nuttx/config.h>
+
+#ifndef __ASSEMBLY__
+# include <stdint.h>
+#endif
+
+#include "stm32_rcc.h"
+#include "stm32_sdmmc.h"
+
+/************************************************************************************
+ * Pre-processor Definitions
+ ************************************************************************************/
+
+/* Clocking *************************************************************************/
+/* The px4_fmu-v6C  board provides the following clock sources:
+ *
+ *   X1: 16 MHz crystal for HSE
+ *
+ * So we have these clock source available within the STM32
+ *
+ *   HSI: 16 MHz RC factory-trimmed
+ *   HSE: 16 MHz crystal for HSE
+ */
+
+#define STM32_BOARD_XTAL        16000000ul
+
+#define STM32_HSI_FREQUENCY     16000000ul
+#define STM32_LSI_FREQUENCY     32000
+#define STM32_HSE_FREQUENCY     STM32_BOARD_XTAL
+#define STM32_LSE_FREQUENCY     32768
+
+/* Main PLL Configuration.
+ *
+ * PLL source is HSE = 16,000,000
+ *
+ * PLL_VCOx = (STM32_HSE_FREQUENCY / PLLM) * PLLN
+ * Subject to:
+ *
+ *     1 <= PLLM <= 63
+ *     4 <= PLLN <= 512
+ *   150 MHz <= PLL_VCOL <= 420MHz
+ *   192 MHz <= PLL_VCOH <= 836MHz
+ *
+ * SYSCLK  = PLL_VCO / PLLP
+ * CPUCLK  = SYSCLK / D1CPRE
+ * Subject to
+ *
+ *   PLLP1   = {2, 4, 6, 8, ..., 128}
+ *   PLLP2,3 = {2, 3, 4, ..., 128}
+ *   CPUCLK <= 480 MHz
+ */
+
+#define STM32_BOARD_USEHSE
+
+#define STM32_PLLCFG_PLLSRC      RCC_PLLCKSELR_PLLSRC_HSE
+
+/* PLL1, wide 4 - 8 MHz input, enable DIVP, DIVQ, DIVR
+ *
+ *   PLL1_VCO = (16,000,000 / 1) * 60 = 960 MHz
+ *
+ *   PLL1P = PLL1_VCO/2  = 960 MHz / 2   = 480 MHz
+ *   PLL1Q = PLL1_VCO/4  = 960 MHz / 4   = 240 MHz
+ *   PLL1R = PLL1_VCO/8  = 960 MHz / 8   = 120 MHz
+ */
+
+#define STM32_PLLCFG_PLL1CFG    (RCC_PLLCFGR_PLL1VCOSEL_WIDE | \
+				 RCC_PLLCFGR_PLL1RGE_4_8_MHZ | \
+				 RCC_PLLCFGR_DIVP1EN | \
+				 RCC_PLLCFGR_DIVQ1EN | \
+				 RCC_PLLCFGR_DIVR1EN)
+#define STM32_PLLCFG_PLL1M       RCC_PLLCKSELR_DIVM1(1)
+#define STM32_PLLCFG_PLL1N       RCC_PLL1DIVR_N1(60)
+#define STM32_PLLCFG_PLL1P       RCC_PLL1DIVR_P1(2)
+#define STM32_PLLCFG_PLL1Q       RCC_PLL1DIVR_Q1(4)
+#define STM32_PLLCFG_PLL1R       RCC_PLL1DIVR_R1(8)
+
+#define STM32_VCO1_FREQUENCY     ((STM32_HSE_FREQUENCY / 1) * 60)
+#define STM32_PLL1P_FREQUENCY    (STM32_VCO1_FREQUENCY / 2)
+#define STM32_PLL1Q_FREQUENCY    (STM32_VCO1_FREQUENCY / 4)
+#define STM32_PLL1R_FREQUENCY    (STM32_VCO1_FREQUENCY / 8)
+
+/* PLL2 */
+
+#define STM32_PLLCFG_PLL2CFG     (RCC_PLLCFGR_PLL2VCOSEL_WIDE | \
+				  RCC_PLLCFGR_PLL2RGE_4_8_MHZ | \
+				  RCC_PLLCFGR_DIVP2EN | \
+				  RCC_PLLCFGR_DIVQ2EN | \
+				  RCC_PLLCFGR_DIVR2EN)
+#define STM32_PLLCFG_PLL2M       RCC_PLLCKSELR_DIVM2(4)
+#define STM32_PLLCFG_PLL2N       RCC_PLL2DIVR_N2(48)
+#define STM32_PLLCFG_PLL2P       RCC_PLL2DIVR_P2(2)
+#define STM32_PLLCFG_PLL2Q       RCC_PLL2DIVR_Q2(2)
+#define STM32_PLLCFG_PLL2R       RCC_PLL2DIVR_R2(2)
+
+#define STM32_VCO2_FREQUENCY     ((STM32_HSE_FREQUENCY / 4) * 48)
+#define STM32_PLL2P_FREQUENCY    (STM32_VCO2_FREQUENCY / 2)
+#define STM32_PLL2Q_FREQUENCY    (STM32_VCO2_FREQUENCY / 2)
+#define STM32_PLL2R_FREQUENCY    (STM32_VCO2_FREQUENCY / 2)
+
+/* PLL3 */
+
+#define STM32_PLLCFG_PLL3CFG    (RCC_PLLCFGR_PLL3VCOSEL_WIDE | \
+				 RCC_PLLCFGR_PLL3RGE_4_8_MHZ | \
+				 RCC_PLLCFGR_DIVQ3EN)
+#define STM32_PLLCFG_PLL3M      RCC_PLLCKSELR_DIVM3(4)
+#define STM32_PLLCFG_PLL3N      RCC_PLL3DIVR_N3(48)
+#define STM32_PLLCFG_PLL3P      RCC_PLL3DIVR_P3(2)
+#define STM32_PLLCFG_PLL3Q      RCC_PLL3DIVR_Q3(4)
+#define STM32_PLLCFG_PLL3R      RCC_PLL3DIVR_R3(2)
+
+#define STM32_VCO3_FREQUENCY    ((STM32_HSE_FREQUENCY / 4) * 48)
+#define STM32_PLL3P_FREQUENCY   (STM32_VCO3_FREQUENCY / 2)
+#define STM32_PLL3Q_FREQUENCY   (STM32_VCO3_FREQUENCY / 4)
+#define STM32_PLL3R_FREQUENCY   (STM32_VCO3_FREQUENCY / 2)
+
+/* SYSCLK = PLL1P = 480MHz
+ * CPUCLK = SYSCLK / 1 = 480 MHz
+ */
+
+#define STM32_RCC_D1CFGR_D1CPRE  (RCC_D1CFGR_D1CPRE_SYSCLK)
+#define STM32_SYSCLK_FREQUENCY   (STM32_PLL1P_FREQUENCY)
+#define STM32_CPUCLK_FREQUENCY   (STM32_SYSCLK_FREQUENCY / 1)
+
+/* Configure Clock Assignments */
+
+/* AHB clock (HCLK) is SYSCLK/2 (240 MHz max)
+ * HCLK1 = HCLK2 = HCLK3 = HCLK4 = 240
+ */
+
+#define STM32_RCC_D1CFGR_HPRE   RCC_D1CFGR_HPRE_SYSCLKd2        /* HCLK  = SYSCLK / 2 */
+#define STM32_ACLK_FREQUENCY    (STM32_CPUCLK_FREQUENCY / 2)    /* ACLK in D1, HCLK3 in D1 */
+#define STM32_HCLK_FREQUENCY    (STM32_CPUCLK_FREQUENCY / 2)    /* HCLK in D2, HCLK4 in D3 */
+#define STM32_BOARD_HCLK        STM32_HCLK_FREQUENCY            /* same as above, to satisfy compiler */
+
+/* APB1 clock (PCLK1) is HCLK/2 (120 MHz) */
+
+#define STM32_RCC_D2CFGR_D2PPRE1  RCC_D2CFGR_D2PPRE1_HCLKd2       /* PCLK1 = HCLK / 2 */
+#define STM32_PCLK1_FREQUENCY     (STM32_HCLK_FREQUENCY/2)
+
+/* APB2 clock (PCLK2) is HCLK/2 (120 MHz) */
+
+#define STM32_RCC_D2CFGR_D2PPRE2  RCC_D2CFGR_D2PPRE2_HCLKd2       /* PCLK2 = HCLK / 2 */
+#define STM32_PCLK2_FREQUENCY     (STM32_HCLK_FREQUENCY/2)
+
+/* APB3 clock (PCLK3) is HCLK/2 (120 MHz) */
+
+#define STM32_RCC_D1CFGR_D1PPRE   RCC_D1CFGR_D1PPRE_HCLKd2        /* PCLK3 = HCLK / 2 */
+#define STM32_PCLK3_FREQUENCY     (STM32_HCLK_FREQUENCY/2)
+
+/* APB4 clock (PCLK4) is HCLK/4 (120 MHz) */
+
+#define STM32_RCC_D3CFGR_D3PPRE   RCC_D3CFGR_D3PPRE_HCLKd2       /* PCLK4 = HCLK / 2 */
+#define STM32_PCLK4_FREQUENCY     (STM32_HCLK_FREQUENCY/2)
+
+/* Timer clock frequencies */
+
+/* Timers driven from APB1 will be twice PCLK1 */
+
+#define STM32_APB1_TIM2_CLKIN   (2*STM32_PCLK1_FREQUENCY)
+#define STM32_APB1_TIM3_CLKIN   (2*STM32_PCLK1_FREQUENCY)
+#define STM32_APB1_TIM4_CLKIN   (2*STM32_PCLK1_FREQUENCY)
+#define STM32_APB1_TIM5_CLKIN   (2*STM32_PCLK1_FREQUENCY)
+#define STM32_APB1_TIM6_CLKIN   (2*STM32_PCLK1_FREQUENCY)
+#define STM32_APB1_TIM7_CLKIN   (2*STM32_PCLK1_FREQUENCY)
+#define STM32_APB1_TIM12_CLKIN  (2*STM32_PCLK1_FREQUENCY)
+#define STM32_APB1_TIM13_CLKIN  (2*STM32_PCLK1_FREQUENCY)
+#define STM32_APB1_TIM14_CLKIN  (2*STM32_PCLK1_FREQUENCY)
+
+/* Timers driven from APB2 will be twice PCLK2 */
+
+#define STM32_APB2_TIM1_CLKIN   (2*STM32_PCLK2_FREQUENCY)
+#define STM32_APB2_TIM8_CLKIN   (2*STM32_PCLK2_FREQUENCY)
+#define STM32_APB2_TIM15_CLKIN  (2*STM32_PCLK2_FREQUENCY)
+#define STM32_APB2_TIM16_CLKIN  (2*STM32_PCLK2_FREQUENCY)
+#define STM32_APB2_TIM17_CLKIN  (2*STM32_PCLK2_FREQUENCY)
+
+/* Kernel Clock Configuration
+ *
+ * Note: look at Table 54 in ST Manual
+ */
+
+/* I2C123 clock source */
+
+#define STM32_RCC_D2CCIP2R_I2C123SRC RCC_D2CCIP2R_I2C123SEL_HSI
+
+/* I2C4 clock source */
+
+#define STM32_RCC_D3CCIPR_I2C4SRC    RCC_D3CCIPR_I2C4SEL_HSI
+
+/* SPI123 clock source */
+
+#define STM32_RCC_D2CCIP1R_SPI123SRC RCC_D2CCIP1R_SPI123SEL_PLL2
+
+/* SPI45 clock source */
+
+#define STM32_RCC_D2CCIP1R_SPI45SRC  RCC_D2CCIP1R_SPI45SEL_PLL2
+
+/* SPI6 clock source */
+
+#define STM32_RCC_D3CCIPR_SPI6SRC    RCC_D3CCIPR_SPI6SEL_PLL2
+
+/* USB 1 and 2 clock source */
+
+#define STM32_RCC_D2CCIP2R_USBSRC    RCC_D2CCIP2R_USBSEL_PLL3
+
+/* UART clock selection */
+/* reset to default to overwrite any changes done by any bootloader */
+
+#define STM32_RCC_D2CCIP2R_USART234578_SEL RCC_D2CCIP2R_USART234578SEL_RCC
+#define STM32_RCC_D2CCIP2R_USART16_SEL     RCC_D2CCIP2R_USART16SEL_RCC
+
+/* ADC 1 2 3 clock source */
+
+#define STM32_RCC_D3CCIPR_ADCSRC     RCC_D3CCIPR_ADCSEL_PLL2
+
+/* FDCAN 1 2 clock source */
+
+#define STM32_RCC_D2CCIP1R_FDCANSEL  RCC_D2CCIP1R_FDCANSEL_HSE   /* FDCAN 1 2 clock source */
+
+#define STM32_FDCANCLK               STM32_HSE_FREQUENCY
+
+/* FLASH wait states
+ *
+ *  ------------ ---------- -----------
+ *  Vcore        MAX ACLK   WAIT STATES
+ *  ------------ ---------- -----------
+ *  1.15-1.26 V     70 MHz    0
+ *  (VOS1 level)   140 MHz    1
+ *                 210 MHz    2
+ *  1.05-1.15 V     55 MHz    0
+ *  (VOS2 level)   110 MHz    1
+ *                 165 MHz    2
+ *                 220 MHz    3
+ *  0.95-1.05 V     45 MHz    0
+ *  (VOS3 level)    90 MHz    1
+ *                 135 MHz    2
+ *                 180 MHz    3
+ *                 225 MHz    4
+ *  ------------ ---------- -----------
+ */
+
+#define BOARD_FLASH_WAITSTATES 2
+
+/* SDMMC definitions ********************************************************/
+
+/* Init 400kHz, freq = PLL1Q/(2*div)  div =  PLL1Q/(2*freq) */
+
+#define STM32_SDMMC_INIT_CLKDIV     (300 << STM32_SDMMC_CLKCR_CLKDIV_SHIFT)
+
+/* 25 MHz Max for now, 25 mHZ = PLL1Q/(2*div), div =  PLL1Q/(2*freq)
+ * div = 4.8 = 240 / 50, So round up to 5 for default speed 24 MB/s
+ */
+
+#if defined(CONFIG_STM32H7_SDMMC_XDMA) || defined(CONFIG_STM32H7_SDMMC_IDMA)
+#  define STM32_SDMMC_MMCXFR_CLKDIV   (5 << STM32_SDMMC_CLKCR_CLKDIV_SHIFT)
+#else
+#  define STM32_SDMMC_MMCXFR_CLKDIV   (100 << STM32_SDMMC_CLKCR_CLKDIV_SHIFT)
+#endif
+#if defined(CONFIG_STM32H7_SDMMC_XDMA) || defined(CONFIG_STM32H7_SDMMC_IDMA)
+#  define STM32_SDMMC_SDXFR_CLKDIV    (5 << STM32_SDMMC_CLKCR_CLKDIV_SHIFT)
+#else
+#  define STM32_SDMMC_SDXFR_CLKDIV    (100 << STM32_SDMMC_CLKCR_CLKDIV_SHIFT)
+#endif
+
+#define STM32_SDMMC_CLKCR_EDGE      STM32_SDMMC_CLKCR_NEGEDGE
+
+/* LED definitions ******************************************************************/
+/* The PX4 FMUV6C board has three, LED_GREEN a Green LED, LED_BLUE a Blue LED and
+ * LED_RED a Red LED, that can be controlled by software.
+ *
+ * If CONFIG_ARCH_LEDS is not defined, then the user can control the LEDs in any way.
+ * The following definitions are used to access individual LEDs.
+ */
+
+/* LED index values for use with board_userled() */
+
+/* LED definitions ******************************************************************/
+/* The px4_fmu-v6c board has three, LED_GREEN a Green LED, LED_BLUE a Blue LED and
+ * LED_RED a Red LED, that can be controlled by software.
+ *
+ * If CONFIG_ARCH_LEDS is not defined, then the user can control the LEDs in any way.
+ * The following definitions are used to access individual LEDs.
+ */
+
+/* LED index values for use with board_userled() */
+
+#define BOARD_LED1        0
+#define BOARD_LED2        1
+#define BOARD_LED3        2
+#define BOARD_NLEDS       3
+
+#define BOARD_LED_RED     BOARD_LED1
+#define BOARD_LED_GREEN   BOARD_LED2
+#define BOARD_LED_BLUE    BOARD_LED3
+
+/* LED bits for use with board_userled_all() */
+
+#define BOARD_LED1_BIT    (1 << BOARD_LED1)
+#define BOARD_LED2_BIT    (1 << BOARD_LED2)
+#define BOARD_LED3_BIT    (1 << BOARD_LED3)
+
+/* If CONFIG_ARCH_LEDS is defined, the usage by the board port is defined in
+ * include/board.h and src/stm32_leds.c. The LEDs are used to encode OS-related
+ * events as follows:
+ *
+ *
+ *   SYMBOL                     Meaning                      LED state
+ *                                                        Red   Green Blue
+ *   ----------------------  --------------------------  ------ ------ ----*/
+
+#define LED_STARTED        0 /* NuttX has been started   OFF    OFF   OFF  */
+#define LED_HEAPALLOCATE   1 /* Heap has been allocated  OFF    OFF   ON   */
+#define LED_IRQSENABLED    2 /* Interrupts enabled       OFF    ON    OFF  */
+#define LED_STACKCREATED   3 /* Idle stack created       OFF    ON    ON   */
+#define LED_INIRQ          4 /* In an interrupt          N/C    N/C   GLOW */
+#define LED_SIGNAL         5 /* In a signal handler      N/C    GLOW  N/C  */
+#define LED_ASSERTION      6 /* An assertion failed      GLOW   N/C   GLOW */
+#define LED_PANIC          7 /* The system has crashed   Blink  OFF   N/C  */
+#define LED_IDLE           8 /* MCU is is sleep mode     ON     OFF   OFF  */
+
+/* Thus if the Green LED is statically on, NuttX has successfully booted and
+ * is, apparently, running normally.  If the Red LED is flashing at
+ * approximately 2Hz, then a fatal error has been detected and the system
+ * has halted.
+ */
+
+/* Alternate function pin selections ************************************************/
+
+#define GPIO_USART1_RX   GPIO_USART1_RX_2    /* PA10 */
+#define GPIO_USART1_TX   GPIO_USART1_TX_3    /* PB6 */
+
+#define GPIO_USART2_RX   GPIO_USART2_RX_2   /* PD6   */
+#define GPIO_USART2_TX   GPIO_USART2_TX_2   /* PD5   */
+
+#define GPIO_USART3_RX   GPIO_USART3_RX_3   /* PD9   */
+#define GPIO_USART3_TX   GPIO_USART3_TX_3   /* PD8   */
+
+#define GPIO_UART5_RX    GPIO_UART5_RX_3    /* PD2  */
+#define GPIO_UART5_TX    GPIO_UART5_TX_1    /* PB13 */
+
+#define GPIO_USART6_RX   GPIO_USART6_RX_1   /* PC7 */
+#define GPIO_USART6_TX   GPIO_USART6_TX_1   /* PC6  */
+
+#define GPIO_UART7_RX    GPIO_UART7_RX_3    /* PE7  */
+#define GPIO_UART7_TX    GPIO_UART7_TX_3    /* PE8  */
+#define GPIO_UART7_RTS   GPIO_UART7_RTS_1   /* PE9  */
+#define GPIO_UART7_CTS   (GPIO_UART7_CTS_1 | GPIO_PULLDOWN) /* PE10 */
+
+#define GPIO_UART8_RX    GPIO_UART8_RX_1    /* PE0 */
+#define GPIO_UART8_TX    GPIO_UART8_TX_1    /* PE1 */
+
+
+/* CAN
+ *
+ * CAN1 is routed to transceiver.
+ */
+#define GPIO_CAN1_RX     GPIO_CAN1_RX_3     /* PD0  */
+#define GPIO_CAN1_TX     GPIO_CAN1_TX_3     /* PD1  */
+
+/* SPI
+ *
+ * SPI1, SPI 3 for sensors
+ * SPI2 for OSD
+ */
+
+#define ADJ_SLEW_RATE(p) (((p) & ~GPIO_SPEED_MASK) | (GPIO_SPEED_2MHz))
+
+#define GPIO_SPI1_MISO   GPIO_SPI1_MISO_1               /* PA6  */
+#define GPIO_SPI1_MOSI   GPIO_SPI1_MOSI_1               /* PA7  */
+#define GPIO_SPI1_SCK    ADJ_SLEW_RATE(GPIO_SPI1_SCK_1) /* PA5  */
+
+#define GPIO_SPI2_MISO   GPIO_SPI2_MISO_2               /* PC2  */
+#define GPIO_SPI2_MOSI   GPIO_SPI2_MOSI_3               /* PC3  */
+#define GPIO_SPI2_SCK    ADJ_SLEW_RATE(GPIO_SPI2_SCK_5) /* PD3  */
+
+#define GPIO_SPI3_MISO   GPIO_SPI3_MISO_2               /* PC11  */
+#define GPIO_SPI3_MOSI   GPIO_SPI3_MOSI_2               /* PC12  */
+#define GPIO_SPI3_SCK    ADJ_SLEW_RATE(GPIO_SPI3_SCK_2) /* PC10  */
+
+
+/*
+ * I2C
+ *
+ */
+
+#define GPIO_I2C1_SCL GPIO_I2C1_SCL_2       /* PB8  */
+#define GPIO_I2C1_SDA GPIO_I2C1_SDA_1       /* PB7  */
+
+#define GPIO_I2C1_SCL_GPIO                  (GPIO_OUTPUT | GPIO_OPENDRAIN |GPIO_SPEED_50MHz | GPIO_OUTPUT_SET | GPIO_PORTB | GPIO_PIN8)
+#define GPIO_I2C1_SDA_GPIO                  (GPIO_OUTPUT | GPIO_OPENDRAIN |GPIO_SPEED_50MHz | GPIO_OUTPUT_SET | GPIO_PORTB | GPIO_PIN7)
+
+#define GPIO_I2C2_SCL GPIO_I2C2_SCL_1       /* PB10 */
+#define GPIO_I2C2_SDA GPIO_I2C2_SDA_1       /* PB11*/
+
+#define GPIO_I2C2_SCL_GPIO                  (GPIO_OUTPUT | GPIO_OPENDRAIN |GPIO_SPEED_50MHz | GPIO_OUTPUT_SET | GPIO_PORTB | GPIO_PIN10)
+#define GPIO_I2C2_SDA_GPIO                  (GPIO_OUTPUT | GPIO_OPENDRAIN |GPIO_SPEED_50MHz | GPIO_OUTPUT_SET | GPIO_PORTB | GPIO_PIN11)
+
+#define GPIO_I2C4_SCL GPIO_I2C4_SCL_1       /* PD12 */
+#define GPIO_I2C4_SDA GPIO_I2C4_SDA_1       /* PD13 */
+
+#define GPIO_I2C4_SCL_GPIO                  (GPIO_OUTPUT | GPIO_OPENDRAIN | GPIO_SPEED_50MHz | GPIO_OUTPUT_SET | GPIO_PORTD | GPIO_PIN12)
+#define GPIO_I2C4_SDA_GPIO                  (GPIO_OUTPUT | GPIO_OPENDRAIN | GPIO_SPEED_50MHz | GPIO_OUTPUT_SET | GPIO_PORTD | GPIO_PIN13)
+
+/* SDMMC2
+ *
+ *      VDD 3.3
+ *      GND
+ *      SDMMC2_CK                           PC1
+ *      SDMMC2_CMD                          PD7
+ *      SDMMC2_D0                           PB14
+ *      SDMMC2_D1                           PB15
+ *      SDMMC2_D2                           PB3
+ *      SDMMC2_D3                           PB4
+ */
+
+#define GPIO_SDMMC2_CK   GPIO_SDMMC2_CK_2  /* PC1  */
+#define GPIO_SDMMC2_CMD  GPIO_SDMMC2_CMD_1 /* PD7  */
+//      GPIO_SDMMC2_D0   No Remap          /* PB14 */
+//      GPIO_SDMMC2_D1   No Remap          /* PB15 */
+#define GPIO_SDMMC2_D2   GPIO_SDMMC2_D2_2  /* PB3  */
+//      GPIO_SDMMC2_D3    No Remap         /* PB4  */
+
+/* USB
+ *
+ *      OTG_FS_DM                           PA11
+ *      OTG_FS_DP                           PA12
+ *      VBUS                                PA9
+ */
+
+
+/* Board provides GPIO or other Hardware for signaling to timing analyzer */
+
+#if defined(CONFIG_BOARD_USE_PROBES)
+# include "stm32_gpio.h"
+# define PROBE_N(n) (1<<((n)-1))
+# define PROBE_1    (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_2MHz|GPIO_OUTPUT_CLEAR|GPIO_PORTA|GPIO_PIN8)   /* PA8  AUX1 */
+# define PROBE_2    (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_2MHz|GPIO_OUTPUT_CLEAR|GPIO_PORTE|GPIO_PIN11)  /* PE11 AUX2 */
+# define PROBE_3    (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_2MHz|GPIO_OUTPUT_CLEAR|GPIO_PORTE|GPIO_PIN13)  /* PE13 AUX3 */
+# define PROBE_4    (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_2MHz|GPIO_OUTPUT_CLEAR|GPIO_PORTE|GPIO_PIN14)  /* PE14 AUX4 */
+# define PROBE_5    (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_2MHz|GPIO_OUTPUT_CLEAR|GPIO_PORTD|GPIO_PIN14)  /* PD14 AUX5 */
+# define PROBE_6    (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_2MHz|GPIO_OUTPUT_CLEAR|GPIO_PORTD|GPIO_PIN15)  /* PD15 AUX6 */
+# define PROBE_7    (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_2MHz|GPIO_OUTPUT_CLEAR|GPIO_PORTA|GPIO_PIN0)   /* PA0  AUX7 */
+# define PROBE_8    (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_2MHz|GPIO_OUTPUT_CLEAR|GPIO_PORTA|GPIO_PIN2)   /* PA1  AUX8 */
+
+# define PROBE_INIT(mask) \
+	do { \
+		if ((mask)& PROBE_N(1)) { stm32_configgpio(PROBE_1); } \
+		if ((mask)& PROBE_N(2)) { stm32_configgpio(PROBE_2); } \
+		if ((mask)& PROBE_N(3)) { stm32_configgpio(PROBE_3); } \
+		if ((mask)& PROBE_N(4)) { stm32_configgpio(PROBE_4); } \
+		if ((mask)& PROBE_N(5)) { stm32_configgpio(PROBE_5); } \
+		if ((mask)& PROBE_N(6)) { stm32_configgpio(PROBE_6); } \
+		if ((mask)& PROBE_N(7)) { stm32_configgpio(PROBE_7); } \
+		if ((mask)& PROBE_N(8)) { stm32_configgpio(PROBE_8); } \
+	} while(0)
+
+# define PROBE(n,s)  do {stm32_gpiowrite(PROBE_##n,(s));}while(0)
+# define PROBE_MARK(n) PROBE(n,false);PROBE(n,true)
+#else
+# define PROBE_INIT(mask)
+# define PROBE(n,s)
+# define PROBE_MARK(n)
+#endif

boards/holybro/kakuteh7-wing/nuttx-config/include/board_dma_map.h

@@ -0,0 +1,81 @@
+/****************************************************************************
+ *
+ *   Copyright (c) 2025 PX4 Development Team. All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright
+ *    notice, this list of conditions and the following disclaimer.
+ * 2. Redistributions in binary form must reproduce the above copyright
+ *    notice, this list of conditions and the following disclaimer in
+ *    the documentation and/or other materials provided with the
+ *    distribution.
+ * 3. Neither the name PX4 nor the names of its contributors may be
+ *    used to endorse or promote products derived from this software
+ *    without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
+ * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
+ * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *
+ ****************************************************************************/
+
+#pragma once
+
+// DMAMUX1 Using at most 8 Channels on DMA1 --------   Assigned
+//                                                     V
+
+#define DMAMAP_SPI1_RX    DMAMAP_DMA12_SPI1RX_0     /* 1 DMA1:37  */
+#define DMAMAP_SPI1_TX    DMAMAP_DMA12_SPI1TX_0     /* 2 DMA1:38  */
+
+#define DMAMAP_SPI3_RX    DMAMAP_DMA12_SPI3RX_0     /* 3 DMA1:39 FRAM */
+#define DMAMAP_SPI3_TX    DMAMAP_DMA12_SPI3TX_0     /* 4 DMA1:40 FRAM */
+
+// TODO
+//#define DMAMAP_USART1_RX  DMAMAP_DMA12_USART1RX_0 /*  DMA1:41  GPS1 */
+//#define DMAMAP_USART1_TX  DMAMAP_DMA12_USART1TX_0 /*  DMA1:42  GPS1 */
+
+//#define DMAMAP_USART2_RX  DMAMAP_DMA12_USART2RX_0 /*  DMA1:43 Telem3 */
+//#define DMAMAP_USART2_TX  DMAMAP_DMA12_USART2TX_0 /*  DMA1:44 Telem3 */
+
+//#define DMAMAP_USART3_RX  DMAMAP_DMA12_USART3RX_0 /*  DMA1:45 DEBUG */
+//#define DMAMAP_USART3_TX  DMAMAP_DMA12_USART3TX_0 /*  DMA1:46 DEBUG */
+
+//#define DMAMAP_UART4_RX   DMAMAP_DMA12_UART4RX_0  /*  DMA1:63 EXT2 */
+//#define DMAMAP_UART4_TX   DMAMAP_DMA12_UART4TX_0  /*  DMA1:64 EXT2 */
+
+//#define DMAMAP_USART6_RX  DMAMAP_DMA12_USART6RX_0   /* 5 DMA1:71 PX4IO */
+//#define DMAMAP_USART6_TX  DMAMAP_DMA12_USART6TX_0   /* 6 DMA1:72 PX4IO */
+
+// Assigned in timer_config.cpp
+
+// TODO
+// Timer 4                                          /* 7 DMA1:32 TIM4UP */
+// Timer 5                                          /* 8 DMA1:50 TIM5UP */
+
+// DMAMUX2 Using at most 8 Channels on DMA2 --------   Assigned
+//                                                     V
+
+// TODO
+#define DMAMAP_USART3_RX  DMAMAP_DMA12_USART3RX_1   /* 1 DMA2:45  */
+#define DMAMAP_USART3_TX  DMAMAP_DMA12_USART3TX_1   /* 2 DMA2:46  */
+
+#define DMAMAP_UART5_RX   DMAMAP_DMA12_UART5RX_1    /* 3 DMA2:65  */
+#define DMAMAP_UART5_TX   DMAMAP_DMA12_UART5TX_1    /* 4 DMA2:66  */
+
+//#define DMAMAP_USART6_RX  DMAMAP_DMA12_USART6RX_1   /* 5 DMA2:71  */
+//#define DMAMAP_USART6_TX  DMAMAP_DMA12_USART6TX_1   /* 6 DMA2:72  */
+
+#define DMAMAP_UART7_RX   DMAMAP_DMA12_UART7RX_1    /* 7 DMA1:79  */
+#define DMAMAP_UART7_TX   DMAMAP_DMA12_UART7TX_1    /* 8 DMA1:80  */

boards/holybro/kakuteh7-wing/nuttx-config/nsh/defconfig

@@ -0,0 +1,270 @@
+#
+# 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_DISABLE_PTHREAD 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_CAT is not set
+# CONFIG_NSH_DISABLE_CD is not set
+# CONFIG_NSH_DISABLE_CP is not set
+# CONFIG_NSH_DISABLE_DATE is not set
+# CONFIG_NSH_DISABLE_DF is not set
+# CONFIG_NSH_DISABLE_ECHO is not set
+# CONFIG_NSH_DISABLE_ENV is not set
+# CONFIG_NSH_DISABLE_EXEC is not set
+# CONFIG_NSH_DISABLE_EXIT is not set
+# CONFIG_NSH_DISABLE_EXPORT is not set
+# CONFIG_NSH_DISABLE_FREE is not set
+# CONFIG_NSH_DISABLE_GET is not set
+# CONFIG_NSH_DISABLE_HELP is not set
+# CONFIG_NSH_DISABLE_ITEF is not set
+# CONFIG_NSH_DISABLE_KILL is not set
+# CONFIG_NSH_DISABLE_LOOPS is not set
+# CONFIG_NSH_DISABLE_LS is not set
+# CONFIG_NSH_DISABLE_MKDIR is not set
+# CONFIG_NSH_DISABLE_MKFATFS is not set
+# CONFIG_NSH_DISABLE_MOUNT is not set
+# CONFIG_NSH_DISABLE_MV is not set
+# CONFIG_NSH_DISABLE_PS is not set
+# CONFIG_NSH_DISABLE_PSSTACKUSAGE is not set
+# CONFIG_NSH_DISABLE_PWD is not set
+# CONFIG_NSH_DISABLE_RM is not set
+# CONFIG_NSH_DISABLE_RMDIR is not set
+# CONFIG_NSH_DISABLE_SEMICOLON is not set
+# CONFIG_NSH_DISABLE_SET is not set
+# CONFIG_NSH_DISABLE_SLEEP is not set
+# CONFIG_NSH_DISABLE_SOURCE is not set
+# CONFIG_NSH_DISABLE_TEST is not set
+# CONFIG_NSH_DISABLE_TIME is not set
+# CONFIG_NSH_DISABLE_UMOUNT is not set
+# CONFIG_NSH_DISABLE_UNSET is not set
+# CONFIG_NSH_DISABLE_USLEEP is not set
+CONFIG_ARCH="arm"
+CONFIG_ARCH_BOARD_CUSTOM=y
+CONFIG_ARCH_BOARD_CUSTOM_DIR="../../../../boards/holybro/kakuteh7-wing/nuttx-config"
+CONFIG_ARCH_BOARD_CUSTOM_DIR_RELPATH=y
+CONFIG_ARCH_BOARD_CUSTOM_NAME="px4"
+CONFIG_ARCH_CHIP="stm32h7"
+CONFIG_ARCH_CHIP_STM32H743VI=y
+CONFIG_ARCH_CHIP_STM32H7=y
+CONFIG_ARCH_INTERRUPTSTACK=768
+CONFIG_ARCH_STACKDUMP=y
+CONFIG_ARMV7M_BASEPRI_WAR=y
+CONFIG_ARMV7M_DCACHE=y
+CONFIG_ARMV7M_DTCM=y
+CONFIG_ARMV7M_ICACHE=y
+CONFIG_ARMV7M_MEMCPY=y
+CONFIG_ARMV7M_USEBASEPRI=y
+CONFIG_ARM_MPU_EARLY_RESET=y
+CONFIG_BOARDCTL_RESET=y
+CONFIG_BOARD_ASSERT_RESET_VALUE=0
+CONFIG_BOARD_CRASHDUMP=y
+CONFIG_BOARD_LOOPSPERMSEC=95751
+CONFIG_BOARD_RESET_ON_ASSERT=2
+CONFIG_BUILTIN=y
+CONFIG_CDCACM=y
+CONFIG_CDCACM_IFLOWCONTROL=y
+CONFIG_CDCACM_PRODUCTID=0x0051
+CONFIG_CDCACM_PRODUCTSTR="PX4 KakuteH7-Wing"
+CONFIG_CDCACM_RXBUFSIZE=600
+CONFIG_CDCACM_TXBUFSIZE=12000
+CONFIG_CDCACM_VENDORID=0x3162
+CONFIG_CDCACM_VENDORSTR="Holybro"
+CONFIG_DEBUG_FULLOPT=y
+CONFIG_DEBUG_HARDFAULT_ALERT=y
+CONFIG_DEBUG_MEMFAULT=y
+CONFIG_DEBUG_SYMBOLS=y
+CONFIG_DEBUG_TCBINFO=y
+CONFIG_DEFAULT_SMALL=y
+CONFIG_DEV_FIFO_SIZE=0
+CONFIG_DEV_PIPE_MAXSIZE=1024
+CONFIG_DEV_PIPE_SIZE=70
+CONFIG_EXPERIMENTAL=y
+CONFIG_FAT_DMAMEMORY=y
+CONFIG_FAT_LCNAMES=y
+CONFIG_FAT_LFN=y
+CONFIG_FAT_LFN_ALIAS_HASH=y
+CONFIG_FDCLONE_STDIO=y
+CONFIG_FS_BINFS=y
+CONFIG_FS_CROMFS=y
+CONFIG_FS_FAT=y
+CONFIG_FS_FATTIME=y
+CONFIG_FS_PROCFS=y
+CONFIG_FS_PROCFS_INCLUDE_PROGMEM=y
+CONFIG_FS_PROCFS_MAX_TASKS=64
+CONFIG_FS_PROCFS_REGISTER=y
+CONFIG_FS_ROMFS=y
+CONFIG_GRAN=y
+CONFIG_GRAN_INTR=y
+CONFIG_HAVE_CXX=y
+CONFIG_HAVE_CXXINITIALIZE=y
+CONFIG_I2C=y
+CONFIG_I2C_RESET=y
+CONFIG_IDLETHREAD_STACKSIZE=750
+CONFIG_INIT_ENTRYPOINT="nsh_main"
+CONFIG_INIT_STACKSIZE=3194
+CONFIG_IOB_NBUFFERS=24
+CONFIG_IOB_NCHAINS=24
+CONFIG_LIBC_FLOATINGPOINT=y
+CONFIG_LIBC_LONG_LONG=y
+CONFIG_LIBC_MAX_EXITFUNS=1
+CONFIG_LIBC_STRERROR=y
+CONFIG_MEMSET_64BIT=y
+CONFIG_MEMSET_OPTSPEED=y
+CONFIG_MMCSD=y
+CONFIG_MMCSD_SDIO=y
+CONFIG_MMCSD_SDIOWAIT_WRCOMPLETE=y
+CONFIG_MM_IOB=y
+CONFIG_MM_REGIONS=4
+CONFIG_MTD=y
+CONFIG_MTD_BYTE_WRITE=y
+CONFIG_MTD_PARTITION=y
+CONFIG_MTD_PROGMEM=y
+CONFIG_MTD_RAMTRON=y
+CONFIG_NAME_MAX=40
+CONFIG_NSH_ARCHINIT=y
+CONFIG_NSH_ARGCAT=y
+CONFIG_NSH_BUILTIN_APPS=y
+CONFIG_NSH_CMDPARMS=y
+CONFIG_NSH_CROMFSETC=y
+CONFIG_NSH_LINELEN=128
+CONFIG_NSH_MAXARGUMENTS=15
+CONFIG_NSH_NESTDEPTH=8
+CONFIG_NSH_QUOTE=y
+CONFIG_NSH_ROMFSETC=y
+CONFIG_NSH_ROMFSSECTSIZE=128
+CONFIG_NSH_STRERROR=y
+CONFIG_NSH_VARS=y
+CONFIG_OTG_ID_GPIO_DISABLE=y
+CONFIG_PIPES=y
+CONFIG_PREALLOC_TIMERS=50
+CONFIG_PRIORITY_INHERITANCE=y
+CONFIG_PTHREAD_MUTEX_ROBUST=y
+CONFIG_PTHREAD_STACK_MIN=512
+CONFIG_RAMTRON_EMULATE_PAGE_SHIFT=5
+CONFIG_RAMTRON_EMULATE_SECTOR_SHIFT=5
+CONFIG_RAMTRON_SETSPEED=y
+CONFIG_RAM_SIZE=245760
+CONFIG_RAM_START=0x20010000
+CONFIG_RAW_BINARY=y
+CONFIG_READLINE_CMD_HISTORY=y
+CONFIG_READLINE_TABCOMPLETION=y
+CONFIG_RTC_DATETIME=y
+CONFIG_SCHED_HPWORK=y
+CONFIG_SCHED_HPWORKPRIORITY=249
+CONFIG_SCHED_HPWORKSTACKSIZE=1280
+CONFIG_SCHED_INSTRUMENTATION=y
+CONFIG_SCHED_INSTRUMENTATION_EXTERNAL=y
+CONFIG_SCHED_INSTRUMENTATION_SWITCH=y
+CONFIG_SCHED_LPWORK=y
+CONFIG_SCHED_LPWORKPRIORITY=50
+CONFIG_SCHED_LPWORKSTACKSIZE=1632
+CONFIG_SCHED_WAITPID=y
+CONFIG_SEM_PREALLOCHOLDERS=32
+CONFIG_SERIAL_IFLOWCONTROL_WATERMARKS=y
+CONFIG_SERIAL_TERMIOS=y
+CONFIG_SIG_DEFAULT=y
+CONFIG_SIG_SIGALRM_ACTION=y
+CONFIG_SIG_SIGUSR1_ACTION=y
+CONFIG_SIG_SIGUSR2_ACTION=y
+CONFIG_SIG_SIGWORK=4
+CONFIG_STACK_COLORATION=y
+CONFIG_START_DAY=30
+CONFIG_START_MONTH=11
+CONFIG_STDIO_BUFFER_SIZE=256
+CONFIG_STM32H7_ADC1=y
+CONFIG_STM32H7_ADC3=y
+CONFIG_STM32H7_BBSRAM=y
+CONFIG_STM32H7_BBSRAM_FILES=5
+CONFIG_STM32H7_BDMA=y
+CONFIG_STM32H7_BKPSRAM=y
+CONFIG_STM32H7_DMA1=y
+CONFIG_STM32H7_DMA2=y
+CONFIG_STM32H7_DMACAPABLE=y
+CONFIG_STM32H7_FLOWCONTROL_BROKEN=y
+CONFIG_STM32H7_I2C1=y
+CONFIG_STM32H7_I2C2=y
+CONFIG_STM32H7_I2C4=y
+CONFIG_STM32H7_I2C_DYNTIMEO=y
+CONFIG_STM32H7_I2C_DYNTIMEO_STARTSTOP=10
+CONFIG_STM32H7_OTGFS=y
+CONFIG_STM32H7_PROGMEM=y
+CONFIG_STM32H7_RTC=y
+CONFIG_STM32H7_RTC_HSECLOCK=y
+CONFIG_STM32H7_RTC_MAGIC_REG=1
+CONFIG_STM32H7_SAVE_CRASHDUMP=y
+CONFIG_STM32H7_SDMMC2=y
+CONFIG_STM32H7_SERIALBRK_BSDCOMPAT=y
+CONFIG_STM32H7_SERIAL_DISABLE_REORDERING=y
+CONFIG_STM32H7_SPI1=y
+CONFIG_STM32H7_SPI1_DMA=y
+CONFIG_STM32H7_SPI1_DMA_BUFFER=1024
+CONFIG_STM32H7_SPI2=y
+CONFIG_STM32H7_SPI3=y
+CONFIG_STM32H7_SPI3_DMA=y
+CONFIG_STM32H7_SPI3_DMA_BUFFER=1024
+CONFIG_STM32H7_TIM12=y
+CONFIG_STM32H7_TIM1=y
+CONFIG_STM32H7_TIM4=y
+CONFIG_STM32H7_TIM5=y
+CONFIG_STM32H7_TIM15=y
+CONFIG_STM32H7_TIM3=y
+CONFIG_STM32H7_TIM2=y
+CONFIG_STM32H7_TIM8=y
+CONFIG_STM32H7_TIM17=y
+CONFIG_STM32H7_UART5=y
+CONFIG_STM32H7_UART7=y
+CONFIG_STM32H7_UART8=y
+CONFIG_STM32H7_USART1=y
+CONFIG_STM32H7_USART2=y
+CONFIG_STM32H7_USART3=y
+CONFIG_STM32H7_USART6=y
+CONFIG_STM32H7_USART_BREAKS=y
+CONFIG_STM32H7_USART_INVERT=y
+CONFIG_STM32H7_USART_SINGLEWIRE=y
+CONFIG_STM32H7_USART_SWAP=y
+CONFIG_SYSTEM_CDCACM=y
+CONFIG_SYSTEM_NSH=y
+CONFIG_TASK_NAME_SIZE=24
+CONFIG_UART5_TXBUFSIZE=3000
+CONFIG_UART7_BAUD=57600
+CONFIG_UART7_IFLOWCONTROL=y
+CONFIG_UART7_OFLOWCONTROL=y
+CONFIG_UART7_RXBUFSIZE=600
+CONFIG_UART7_RXDMA=y
+CONFIG_UART7_TXBUFSIZE=3000
+CONFIG_UART7_TXDMA=y
+CONFIG_UART8_BAUD=57600
+CONFIG_UART8_RXBUFSIZE=600
+CONFIG_UART8_TXBUFSIZE=1500
+CONFIG_UART8_SERIAL_CONSOLE=y
+CONFIG_USART1_BAUD=57600
+CONFIG_USART1_RXBUFSIZE=600
+CONFIG_USART1_TXBUFSIZE=1500
+CONFIG_USART2_BAUD=57600
+CONFIG_USART2_RXBUFSIZE=600
+CONFIG_USART2_TXBUFSIZE=3000
+CONFIG_USART3_BAUD=57600
+CONFIG_USART3_RXBUFSIZE=180
+CONFIG_USART3_RXDMA=y
+CONFIG_USART3_TXBUFSIZE=1500
+CONFIG_USART3_TXDMA=y
+CONFIG_USART6_BAUD=57600
+CONFIG_USART6_RXBUFSIZE=600
+CONFIG_USART6_TXBUFSIZE=1500
+CONFIG_USBDEV=y
+CONFIG_USBDEV_BUSPOWERED=y
+CONFIG_USBDEV_MAXPOWER=500
+CONFIG_USEC_PER_TICK=1000
+CONFIG_WATCHDOG=y
+CONFIG_WQUEUE_NOTIFIER=y

boards/holybro/kakuteh7-wing/nuttx-config/scripts/bootloader_script.ld

@@ -0,0 +1,215 @@
+/****************************************************************************
+ * scripts/script.ld
+ *
+ *   Copyright (C) 2016, 2019 Gregory Nutt. All rights reserved.
+ *   Author: Gregory Nutt <gnutt@nuttx.org>
+ *
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright
+ *    notice, this list of conditions and the following disclaimer.
+ * 2. Redistributions in binary form must reproduce the above copyright
+ *    notice, this list of conditions and the following disclaimer in
+ *    the documentation and/or other materials provided with the
+ *    distribution.
+ * 3. Neither the name NuttX nor the names of its contributors may be
+ *    used to endorse or promote products derived from this software
+ *    without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
+ * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
+ * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *
+ ****************************************************************************/
+
+/* The KakuteH7 uses an STM32H743VI 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 STM32H743VI, two different boot spaces can be selected through
+ * the BOOT pin and the boot base address programmed in the BOOT_ADD0 and
+ * BOOT_ADD1 option bytes:
+ *
+ *   1) BOOT=0: Boot address defined by user option byte BOOT_ADD0[15:0].
+ *      ST programmed value: Flash memory at 0x0800:0000
+ *   2) BOOT=1: Boot address defined by user option byte BOOT_ADD1[15:0].
+ *      ST programmed value: System bootloader at 0x1FF0:0000
+ *
+ * The KakuteH7 has 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
+ * depressed, then the boot will be from 0x1FF0:0000
+ *
+ * The STM32H743VI also has 1024Kb of data SRAM.
+ * SRAM is split up into several blocks and into three power domains:
+ *
+ *   1) TCM SRAMs are dedicated to the Cortex-M7 and are accessible with
+ *      0 wait states by the Cortex-M7 and by MDMA through AHBS slave bus
+ *
+ *      1.1) 128Kb of DTCM-RAM beginning at address 0x2000:0000
+ *
+ *           The DTCM-RAM is organized as 2 x 64Kb DTCM-RAMs on 2 x 32 bit
+ *           DTCM ports. The DTCM-RAM could be used for critical real-time
+ *           data, such as interrupt service routines or stack / heap memory.
+ *           Both DTCM-RAMs can be used in parallel (for load/store operations)
+ *           thanks to the Cortex-M7 dual issue capability.
+ *
+ *      1.2)  64Kb of ITCM-RAM beginning at address 0x0000:0000
+ *
+ *           This RAM is connected to ITCM 64-bit interface designed for
+ *           execution of critical real-times routines by the CPU.
+ *
+ *   2) AXI SRAM (D1 domain) accessible by all system masters except BDMA
+ *      through D1 domain AXI bus matrix
+ *
+ *      2.1) 512Kb of SRAM beginning at address 0x2400:0000
+ *
+ *   3) AHB SRAM (D2 domain) accessible by all system masters except BDMA
+ *      through D2 domain AHB bus matrix
+ *
+ *      3.1) 128Kb of SRAM1 beginning at address 0x3000:0000
+ *      3.2) 128Kb of SRAM2 beginning at address 0x3002:0000
+ *      3.3)  32Kb of SRAM3 beginning at address 0x3004:0000
+ *
+ *      SRAM1 - SRAM3 are one contiguous block: 288Kb at address 0x3000:0000
+ *
+ *   4) AHB SRAM (D3 domain) accessible by most of system masters
+ *      through D3 domain AHB bus matrix
+ *
+ *      4.1)  64Kb of SRAM4 beginning at address 0x3800:0000
+ *      4.1)   4Kb of backup RAM beginning at address 0x3880:0000
+ *
+ * When booting from FLASH, FLASH memory is aliased to address 0x0000:0000
+ * where the code expects to begin execution by jumping to the entry point in
+ * the 0x0800:0000 address range.
+ *
+ * The bootloader uses the first sector of the flash, which is 128K in length.
+ */
+
+MEMORY
+{
+	itcm  (rwx) : ORIGIN = 0x00000000, LENGTH =   64K
+	flash (rx)  : ORIGIN = 0x08000000, LENGTH =  128K
+	dtcm1 (rwx) : ORIGIN = 0x20000000, LENGTH =   64K
+	dtcm2 (rwx) : ORIGIN = 0x20010000, LENGTH =   64K
+	sram  (rwx) : ORIGIN = 0x24000000, LENGTH =  512K
+	sram1 (rwx) : ORIGIN = 0x30000000, LENGTH =  128K
+	sram2 (rwx) : ORIGIN = 0x30020000, LENGTH =  128K
+	sram3 (rwx) : ORIGIN = 0x30040000, LENGTH =   32K
+	sram4 (rwx) : ORIGIN = 0x38000000, LENGTH =   64K
+	bbram (rwx) : ORIGIN = 0x38800000, LENGTH =    4K
+}
+
+OUTPUT_ARCH(arm)
+EXTERN(_vectors)
+ENTRY(_stext)
+
+/*
+ * Ensure that abort() is present in the final object.  The exception handling
+ * code pulled in by libgcc.a requires it (and that code cannot be easily avoided).
+ */
+EXTERN(abort)
+EXTERN(_bootdelay_signature)
+
+SECTIONS
+{
+	.text : {
+		_stext = ABSOLUTE(.);
+		*(.vectors)
+		. = ALIGN(32);
+		/*
+		This signature provides the bootloader with a way to delay booting
+		*/
+		_bootdelay_signature = ABSOLUTE(.);
+		FILL(0xffecc2925d7d05c5)
+		. += 8;
+		*(.text .text.*)
+		*(.fixup)
+		*(.gnu.warning)
+		*(.rodata .rodata.*)
+		*(.gnu.linkonce.t.*)
+		*(.glue_7)
+		*(.glue_7t)
+		*(.got)
+		*(.gcc_except_table)
+		*(.gnu.linkonce.r.*)
+		_etext = ABSOLUTE(.);
+
+	} > flash
+
+	/*
+	 * Init functions (static constructors and the like)
+	 */
+	.init_section : {
+		_sinit = ABSOLUTE(.);
+		KEEP(*(.init_array .init_array.*))
+		_einit = ABSOLUTE(.);
+	} > flash
+
+
+	.ARM.extab : {
+		*(.ARM.extab*)
+	} > flash
+
+	__exidx_start = ABSOLUTE(.);
+	.ARM.exidx : {
+		*(.ARM.exidx*)
+	} > flash
+	__exidx_end = ABSOLUTE(.);
+
+	_eronly = ABSOLUTE(.);
+
+	.data : {
+		_sdata = ABSOLUTE(.);
+		*(.data .data.*)
+		*(.gnu.linkonce.d.*)
+		CONSTRUCTORS
+		_edata = ABSOLUTE(.);
+	} > sram AT > flash
+
+	.bss : {
+		_sbss = ABSOLUTE(.);
+		*(.bss .bss.*)
+		*(.gnu.linkonce.b.*)
+		*(COMMON)
+		. = ALIGN(4);
+		_ebss = ABSOLUTE(.);
+	} > sram
+
+	/* Stabs debugging sections. */
+	.stab 0 : { *(.stab) }
+	.stabstr 0 : { *(.stabstr) }
+	.stab.excl 0 : { *(.stab.excl) }
+	.stab.exclstr 0 : { *(.stab.exclstr) }
+	.stab.index 0 : { *(.stab.index) }
+	.stab.indexstr 0 : { *(.stab.indexstr) }
+	.comment 0 : { *(.comment) }
+	.debug_abbrev 0 : { *(.debug_abbrev) }
+	.debug_info 0 : { *(.debug_info) }
+	.debug_line 0 : { *(.debug_line) }
+	.debug_pubnames 0 : { *(.debug_pubnames) }
+	.debug_aranges 0 : { *(.debug_aranges) }
+}

boards/holybro/kakuteh7-wing/nuttx-config/scripts/script.ld

@@ -0,0 +1,228 @@
+/****************************************************************************
+ * scripts/script.ld
+ *
+ *   Copyright (C) 2016, 2019 Gregory Nutt. All rights reserved.
+ *   Author: Gregory Nutt <gnutt@nuttx.org>
+ *
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright
+ *    notice, this list of conditions and the following disclaimer.
+ * 2. Redistributions in binary form must reproduce the above copyright
+ *    notice, this list of conditions and the following disclaimer in
+ *    the documentation and/or other materials provided with the
+ *    distribution.
+ * 3. Neither the name NuttX nor the names of its contributors may be
+ *    used to endorse or promote products derived from this software
+ *    without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
+ * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
+ * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *
+ ****************************************************************************/
+
+/* The KakuteH7 uses an STM32H743VI 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 STM32H743VI, two different boot spaces can be selected through
+ * the BOOT pin and the boot base address programmed in the BOOT_ADD0 and
+ * BOOT_ADD1 option bytes:
+ *
+ *   1) BOOT=0: Boot address defined by user option byte BOOT_ADD0[15:0].
+ *      ST programmed value: Flash memory at 0x0800:0000
+ *   2) BOOT=1: Boot address defined by user option byte BOOT_ADD1[15:0].
+ *      ST programmed value: System bootloader at 0x1FF0:0000
+ *
+ * The KakuteH7 has 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
+ * depressed, then the boot will be from 0x1FF0:0000
+ *
+ * The STM32H743VI 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 = 1664K /* params in last two sectors */
+
+	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(.);
+
+		/* Pad out last section as the STM32H7 Flash write size is 256 bits. 32 bytes */
+		. = ALIGN(16);
+		FILL(0xffff)
+		. += 16;
+	} > AXI_SRAM AT > FLASH  = 0xffff
+
+	.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_reserve (NOLOAD) :
+	{
+		*(.sram4)
+		. = ALIGN(4);
+		_sram4_heap_start = ABSOLUTE(.);
+	} > SRAM4
+
+	/* Stabs debugging sections. */
+	.stab 0 : { *(.stab) }
+	.stabstr 0 : { *(.stabstr) }
+	.stab.excl 0 : { *(.stab.excl) }
+	.stab.exclstr 0 : { *(.stab.exclstr) }
+	.stab.index 0 : { *(.stab.index) }
+	.stab.indexstr 0 : { *(.stab.indexstr) }
+	.comment 0 : { *(.comment) }
+	.debug_abbrev 0 : { *(.debug_abbrev) }
+	.debug_info 0 : { *(.debug_info) }
+	.debug_line 0 : { *(.debug_line) }
+	.debug_pubnames 0 : { *(.debug_pubnames) }
+	.debug_aranges 0 : { *(.debug_aranges) }
+}

boards/holybro/kakuteh7-wing/src/CMakeLists.txt

@@ -1,6 +1,6 @@
 ############################################################################
 #
-#   Copyright (c) 2021 PX4 Development Team. All rights reserved.
+#   Copyright (c) 2016 PX4 Development Team. All rights reserved.
 #
 # Redistribution and use in source and binary forms, with or without
 # modification, are permitted provided that the following conditions
@@ -30,5 +30,45 @@
 # POSSIBILITY OF SUCH DAMAGE.
 #
 ############################################################################
+if("${PX4_BOARD_LABEL}" STREQUAL  "bootloader")
+	add_compile_definitions(BOOTLOADER)
+	add_library(drivers_board
+		bootloader_main.c
+		init.c
+		usb.c
+		timer_config.cpp
+	)
+	target_link_libraries(drivers_board
+		PRIVATE
+			nuttx_arch # sdio
+			nuttx_drivers # sdio
+			px4_layer #gpio
+			arch_io_pins # iotimer
+			bootloader
+	)
+	target_include_directories(drivers_board PRIVATE ${PX4_SOURCE_DIR}/platforms/nuttx/src/bootloader/common)
 
-add_subdirectory(vcm1193l)
+else()
+	add_library(drivers_board
+		can.c
+		i2c.cpp
+		init.c
+		led.c
+		sdio.c
+		spi.cpp
+		timer_config.cpp
+		usb.c
+	)
+	add_dependencies(drivers_board arch_board_hw_info)
+
+	target_link_libraries(drivers_board
+		PRIVATE
+			arch_io_pins
+			arch_spi
+			arch_board_hw_info
+			drivers__led # drv_led_start
+			nuttx_arch # sdio
+			nuttx_drivers # sdio
+			px4_layer
+	)
+endif()

boards/holybro/kakuteh7-wing/src/board_config.h

@@ -0,0 +1,273 @@
+/****************************************************************************
+ *
+ *   Copyright (c) 2022 PX4 Development Team. All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright
+ *    notice, this list of conditions and the following disclaimer.
+ * 2. Redistributions in binary form must reproduce the above copyright
+ *    notice, this list of conditions and the following disclaimer in
+ *    the documentation and/or other materials provided with the
+ *    distribution.
+ * 3. Neither the name PX4 nor the names of its contributors may be
+ *    used to endorse or promote products derived from this software
+ *    without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
+ * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
+ * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *
+ ****************************************************************************/
+
+/**
+ * @file board_config.h
+ *
+ * PX4FMU-v6c internal definitions
+ */
+
+#pragma once
+
+/****************************************************************************************************
+ * Included Files
+ ****************************************************************************************************/
+
+#include <px4_platform_common/px4_config.h>
+#include <nuttx/compiler.h>
+#include <stdint.h>
+
+
+#include <stm32_gpio.h>
+
+/****************************************************************************************************
+ * Definitions
+ ****************************************************************************************************/
+
+#undef TRACE_PINS
+
+/* PX4FMU GPIOs ***********************************************************************************/
+
+
+/* LEDs are driven with push pull Anodes to 3.3V */
+
+#define GPIO_nLED_RED        /* PC14 */  (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_50MHz|GPIO_OUTPUT_SET|GPIO_PORTC|GPIO_PIN14)
+#define GPIO_nLED_BLUE       /* PC15 */  (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_50MHz|GPIO_OUTPUT_SET|GPIO_PORTC|GPIO_PIN15)
+
+#define BOARD_HAS_CONTROL_STATUS_LEDS      1
+#define BOARD_OVERLOAD_LED     LED_RED
+#define BOARD_ARMED_STATE_LED  LED_BLUE
+
+/*
+ * ADC channels
+ *
+ * These are the channel numbers of the ADCs of the microcontroller that
+ * can be used by the Px4 Firmware in the adc driver
+ */
+
+/* ADC defines to be used in sensors.cpp to read from a particular channel */
+
+#define ADC1_CH(n)                  (n)
+
+/* N.B. there is no offset mapping needed for ADC3 because */
+#define ADC3_CH(n)                  (n)
+
+/* We are only use ADC3 for REV/VER. */
+
+/* Define GPIO pins used as ADC N.B. Channel numbers must match below  */
+
+#define PX4_ADC_GPIO  \
+	/* PC4  */  GPIO_ADC12_INP4, \
+	/* PC5  */  GPIO_ADC12_INP8, \
+	/* PA2  */  GPIO_ADC12_INP14, \
+	/* PA3  */  GPIO_ADC12_INP15, \
+	/* PA4  */  GPIO_ADC12_INP18, \
+	/* PC0  */  GPIO_ADC123_INP10 \
+
+/* Define Channel numbers must match above GPIO pin IN(n)*/
+#define ADC_BATTERY1_CURRENT_CHANNEL            /* PC4 */  ADC1_CH(4)
+#define ADC_BATTERY1_VOLTAGE_CHANNEL            /* PC5 */  ADC1_CH(8)
+#define ADC_BATTERY2_CURRENT_CHANNEL            /* PA2 */  ADC1_CH(14)
+#define ADC_BATTERY2_VOLTAGE_CHANNEL            /* PA3 */  ADC1_CH(15)
+#define ADC_SCALED_V5_CHANNEL                   /* PA4 */  ADC1_CH(18)
+#define ADC_RSSI_IN_CHANNEL                     /* PC0  */  ADC3_CH(10)
+
+#define ADC_CHANNELS \
+	((1 << ADC_BATTERY1_CURRENT_CHANNEL) | \
+	 (1 << ADC_BATTERY1_VOLTAGE_CHANNEL) | \
+	 (1 << ADC_BATTERY2_CURRENT_CHANNEL) | \
+	 (1 << ADC_BATTERY2_VOLTAGE_CHANNEL) | \
+	 (1 << ADC_SCALED_V5_CHANNEL) | \
+	 (1 << ADC_RSSI_IN_CHANNEL       ))
+
+#define SYSTEM_ADC_BASE STM32_ADC1_BASE
+
+#define BOARD_NUMBER_BRICKS 2
+
+#define GPIO_nVDD_BRICK1_VALID          (1) /* Brick 1 Is Chosen */
+#define GPIO_nVDD_BRICK2_VALID          (0) /* Brick 2 Is Chosen  */
+
+/*
+ * PWM
+ */
+#define DIRECT_PWM_OUTPUT_CHANNELS 14
+#define BOARD_NUM_IO_TIMERS 6
+
+/*
+ * UAVCAN
+ */
+#define UAVCAN_NUM_IFACES_RUNTIME  1
+
+#define GPIO_VDD_3V3_SENSORS_EN         /* PB2  */ (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_2MHz|GPIO_OUTPUT_SET|GPIO_PORTB|GPIO_PIN2)
+
+#define GPIO_VTX_9V_EN                  /* PE3  */ (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_2MHz|GPIO_OUTPUT_SET|GPIO_PORTE|GPIO_PIN3)
+
+#define GPIO_CAM_SWITCH                 /* PC13 */ (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_2MHz|GPIO_OUTPUT_CLEAR|GPIO_PORTC|GPIO_PIN13)
+
+/* Define True logic Power Control in arch agnostic form */
+
+#define VDD_3V3_SENSORS_EN(on_true)       px4_arch_gpiowrite(GPIO_VDD_3V3_SENSORS_EN, (on_true))
+
+#define VTX_9V_EN(on_true)                px4_arch_gpiowrite(GPIO_VTX_9V_EN, (on_true))
+
+#define CAM_SWITCH_CAM1                   px4_arch_gpiowrite(GPIO_CAM_SWITCH, false) // low is CAM1
+#define CAM_SWITCH_CAM2                   px4_arch_gpiowrite(GPIO_CAM_SWITCH, true)  // high is CAM2
+
+/* Tone alarm output */
+
+#define TONE_ALARM_TIMER        17  /* Timer 17 */
+#define TONE_ALARM_CHANNEL      1  /* PB9 GPIO_TIM4_CH4OUT_1 */
+
+#define GPIO_BUZZER_1           /* PB9 */ (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_2MHz|GPIO_OUTPUT_CLEAR|GPIO_PORTB|GPIO_PIN9)
+
+#define GPIO_TONE_ALARM_IDLE    GPIO_BUZZER_1
+#define GPIO_TONE_ALARM        GPIO_TIM17_CH1OUT_1
+
+/* USB OTG FS
+ *
+ * PA9  OTG_FS_VBUS VBUS sensing
+ */
+#define GPIO_OTGFS_VBUS         /* PA9 */ (GPIO_INPUT|GPIO_PULLDOWN|GPIO_SPEED_100MHz|GPIO_PORTA|GPIO_PIN9)
+
+/* High-resolution timer */
+#define HRT_TIMER               8  /* use timer8 for the HRT */
+#define HRT_TIMER_CHANNEL       3  /* use capture/compare channel 3 */
+
+/* PWM input driver */
+//#define PWMIN_TIMER                       8
+//#define PWMIN_TIMER_CHANNEL    /* TIM8CH2 */ 2
+//#define GPIO_PWM_IN            /* PC7 */ GPIO_TIM8_CH2IN_1
+
+#define HRT_PPM_CHANNEL         /* TIM8CH2 */  2  /* use capture/compare channel 2 */
+#define GPIO_PPM_IN             /* PC7 */ GPIO_TIM8_CH2IN_1
+
+#define RC_SERIAL_PORT                     "/dev/ttyS4" // USART6
+#define RC_SERIAL_PORT_SHARED_PPM_PIN_GPIO_RX             GPIO_USART6_RX
+
+#define SDIO_SLOTNO                    0  /* Only one slot */
+#define SDIO_MINOR                     0
+
+/* SD card bringup does not work if performed on the IDLE thread because it
+ * will cause waiting.  Use either:
+ *
+ *  CONFIG_BOARDCTL=y, OR
+ *  CONFIG_BOARD_INITIALIZE=y && CONFIG_BOARD_INITTHREAD=y
+ */
+
+#if defined(CONFIG_BOARD_INITIALIZE) && !defined(CONFIG_BOARDCTL) && \
+   !defined(CONFIG_BOARD_INITTHREAD)
+#  warning SDIO initialization cannot be perfomed on the IDLE thread
+#endif
+
+/* By Providing BOARD_ADC_USB_CONNECTED (using the px4_arch abstraction)
+ * this board support the ADC system_power interface, and therefore
+ * provides the true logic GPIO BOARD_ADC_xxxx macros.
+ */
+#define BOARD_ADC_USB_CONNECTED (px4_arch_gpioread(GPIO_OTGFS_VBUS))
+
+
+#define BOARD_ADC_SERVO_VALID     (1)
+
+#define BOARD_ADC_BRICK1_VALID  (1)
+#define BOARD_ADC_BRICK2_VALID  (1)
+
+#define BOARD_ADC_SERVO_VALID   (1)
+
+
+/* This board provides a DMA pool and APIs */
+#define BOARD_DMA_ALLOC_POOL_SIZE 5120
+
+/* This board provides the board_on_reset interface */
+
+#define BOARD_HAS_ON_RESET 1
+
+#define PX4_GPIO_INIT_LIST { \
+		PX4_ADC_GPIO,                     \
+		GPIO_CAN1_TX,                     \
+		GPIO_CAN1_RX,                     \
+		GPIO_VDD_3V3_SENSORS_EN,          \
+		GPIO_TONE_ALARM_IDLE,             \
+		GPIO_PPM_IN,                      \
+		GPIO_VTX_9V_EN,                   \
+		GPIO_CAM_SWITCH,                  \
+	}
+
+#define BOARD_ENABLE_CONSOLE_BUFFER
+
+#define FLASH_BASED_PARAMS
+
+__BEGIN_DECLS
+
+/****************************************************************************************************
+ * Public Types
+ ****************************************************************************************************/
+
+/****************************************************************************************************
+ * Public data
+ ****************************************************************************************************/
+
+#ifndef __ASSEMBLY__
+
+/****************************************************************************************************
+ * Public Functions
+ ****************************************************************************************************/
+
+/****************************************************************************
+ * Name: stm32_sdio_initialize
+ *
+ * Description:
+ *   Initialize SDIO-based MMC/SD card support
+ *
+ ****************************************************************************/
+
+int stm32_sdio_initialize(void);
+
+/****************************************************************************************************
+ * Name: stm32_spiinitialize
+ *
+ * Description:
+ *   Called to configure SPI chip select GPIO pins for the PX4FMU board.
+ *
+ ****************************************************************************************************/
+
+extern void stm32_spiinitialize(void);
+
+extern void stm32_usbinitialize(void);
+
+extern void board_peripheral_reset(int ms);
+
+#include <px4_platform_common/board_common.h>
+
+#endif /* __ASSEMBLY__ */
+
+__END_DECLS

boards/holybro/kakuteh7-wing/src/bootloader_main.c

@@ -0,0 +1,75 @@
+/****************************************************************************
+ *
+ *   Copyright (c) 2019-2021 PX4 Development Team. All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * 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_usbinitialize();
+}
+
+__EXPORT int board_app_initialize(uintptr_t arg)
+{
+	return 0;
+}
+
+void board_late_initialize(void)
+{
+	sercon_main(0, NULL);
+}
+
+extern void sys_tick_handler(void);
+void board_timerhook(void)
+{
+	sys_tick_handler();
+}

boards/holybro/kakuteh7-wing/src/can.c

@@ -0,0 +1,132 @@
+/****************************************************************************
+ *
+ *   Copyright (C) 2012 PX4 Development Team. All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright
+ *    notice, this list of conditions and the following disclaimer.
+ * 2. Redistributions in binary form must reproduce the above copyright
+ *    notice, this list of conditions and the following disclaimer in
+ *    the documentation and/or other materials provided with the
+ *    distribution.
+ * 3. Neither the name PX4 nor the names of its contributors may be
+ *    used to endorse or promote products derived from this software
+ *    without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
+ * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
+ * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *
+ ****************************************************************************/
+
+/**
+ * @file can.c
+ *
+ * Board-specific CAN functions.
+ */
+
+#include <errno.h>
+#include <debug.h>
+
+#include <nuttx/can/can.h>
+#include <arch/board/board.h>
+
+
+#if !defined(CONFIG_CAN)
+__EXPORT
+uint16_t board_get_can_interfaces(void)
+{
+	return 0x1;
+}
+
+#else
+
+#include "chip.h"
+#include "arm_internal.h"
+
+#include "chip.h"
+#include "stm32_can.h"
+#include "board_config.h"
+
+/************************************************************************************
+ * Pre-processor Definitions
+ ************************************************************************************/
+/* Configuration ********************************************************************/
+
+#if defined(CONFIG_STM32_CAN1) && defined(CONFIG_STM32_CAN2)
+#  warning "Both CAN1 and CAN2 are enabled.  Assuming only CAN1."
+#  undef CONFIG_STM32_CAN2
+#endif
+
+#ifdef CONFIG_STM32_CAN1
+#  define CAN_PORT 1
+#else
+#  define CAN_PORT 2
+#endif
+
+/************************************************************************************
+ * Private Functions
+ ************************************************************************************/
+
+/************************************************************************************
+ * Public Functions
+ ************************************************************************************/
+int can_devinit(void);
+
+/************************************************************************************
+ * Name: can_devinit
+ *
+ * Description:
+ *   All STM32 architectures must provide the following interface to work with
+ *   examples/can.
+ *
+ ************************************************************************************/
+
+int can_devinit(void)
+{
+	static bool initialized = false;
+	struct can_dev_s *can;
+	int ret;
+
+	/* Check if we have already initialized */
+
+	if (!initialized) {
+		/* Call stm32_caninitialize() to get an instance of the CAN interface */
+
+		can = stm32_caninitialize(CAN_PORT);
+
+		if (can == NULL) {
+			canerr("ERROR:  Failed to get CAN interface\n");
+			return -ENODEV;
+		}
+
+		/* Register the CAN driver at "/dev/can0" */
+
+		ret = can_register("/dev/can0", can);
+
+		if (ret < 0) {
+			canerr("ERROR: can_register failed: %d\n", ret);
+			return ret;
+		}
+
+		/* Now we are initialized */
+
+		initialized = true;
+	}
+
+	return OK;
+}
+
+#endif

boards/holybro/kakuteh7-wing/src/hw_config.h

@@ -0,0 +1,128 @@
+/*
+ * hw_config.h
+ *
+ *  Created on: May 17, 2015
+ *      Author: david_s5
+ */
+
+#ifndef HW_CONFIG_H_
+#define HW_CONFIG_H_
+
+/****************************************************************************
+ * 10-8--2016:
+ *  To simplify the ripple effect on the tools, we will be using
+ *  /dev/serial/by-id/<asterisk>PX4<asterisk> to locate PX4 devices. Therefore
+ *  moving forward all Bootloaders must contain the prefix "PX4 BL "
+ *  in the USBDEVICESTRING
+ *  This Change will be made in an upcoming BL release
+ ****************************************************************************/
+/*
+ * Define usage to configure a bootloader
+ *
+ *
+ * Constant                example          Usage
+ * APP_LOAD_ADDRESS     0x08004000            - The address in Linker Script, where the app fw is org-ed
+ * BOOTLOADER_DELAY     5000                  - Ms to wait while under USB pwr or bootloader request
+ * BOARD_FMUV2
+ * INTERFACE_USB        1                     - (Optional) Scan and use the USB interface for bootloading
+ * INTERFACE_USART      1                     - (Optional) Scan and use the Serial interface for bootloading
+ * USBDEVICESTRING      "PX4 BL FMU v2.x"     - USB id string
+ * USBPRODUCTID         0x0011                - PID Should match defconfig
+ * BOOT_DELAY_ADDRESS   0x000001a0            - (Optional) From the linker script from Linker Script to get a custom
+ *                                               delay provided by an APP FW
+ * BOARD_TYPE           9                     - Must match .prototype boad_id
+ * _FLASH_KBYTES        (*(uint16_t *)0x1fff7a22) - Run time flash size detection
+ * BOARD_FLASH_SECTORS  ((_FLASH_KBYTES == 0x400) ? 11 : 23) - Run time determine the physical last sector
+ * BOARD_FLASH_SECTORS   11                   - Hard coded zero based last sector
+ * BOARD_FLASH_SIZE     (_FLASH_KBYTES*1024)-   Total Flash size of device, determined at run time.
+ *                         (1024 * 1024)      - Hard coded Total Flash of device - The bootloader and app reserved will be deducted
+ *                                              programmatically
+ *
+ * BOARD_FIRST_FLASH_SECTOR_TO_ERASE  2        - Optional sectors index in the flash_sectors table  (F4 only), to begin erasing.
+ *                                               This is to allow sectors to be reserved for app fw usage. That will NOT be erased
+ *                                               during a FW upgrade.
+ *                                               The default is 0, and selects the first sector to be erased, as the 0th entry in the
+ *                                               flash_sectors table. Which is the second physical sector of FLASH in the device.
+ *                                               The first physical sector of FLASH is used by the bootloader, and is not defined
+ *                                               in the table.
+ *
+ * APP_RESERVATION_SIZE (BOARD_FIRST_FLASH_SECTOR_TO_ERASE * 16 * 1024) - Number of bytes reserved by the APP FW. This number plus
+ *                                                                        BOOTLOADER_RESERVATION_SIZE  will be deducted from
+ *                                                                        BOARD_FLASH_SIZE to determine the size of the App FW
+ *                                                                        and hence the address space of FLASH to erase and program.
+ * USBMFGSTRING            "PX4 AP"            - Optional USB MFG string (default is '3D Robotics' if not defined.)
+ * SERIAL_BREAK_DETECT_DISABLED                -  Optional prevent break selection on Serial port from entering or staying in BL
+ *
+ * * Other defines are somewhat self explanatory.
+ */
+
+/* Boot device selection list*/
+#define USB0_DEV       0x01
+#define SERIAL0_DEV    0x02
+#define SERIAL1_DEV    0x04
+
+#define APP_LOAD_ADDRESS               0x08020000
+#define BOOTLOADER_DELAY               5000
+#define INTERFACE_USB                  1
+#define INTERFACE_USB_CONFIG           "/dev/ttyACM0"
+#define BOARD_VBUS                     MK_GPIO_INPUT(GPIO_OTGFS_VBUS)
+
+//#define USE_VBUS_PULL_DOWN
+#define INTERFACE_USART                1
+#define INTERFACE_USART_CONFIG         "/dev/ttyS0,115200"
+#define BOOT_DELAY_ADDRESS             0x000001a0
+#define BOARD_TYPE                     1105
+#define BOARD_FLASH_SECTORS            (14)
+#define BOARD_FLASH_SIZE               (16 * 128 * 1024)
+#define APP_RESERVATION_SIZE           (2 * 128 * 1024)
+
+#define OSC_FREQ                       16
+
+#define BOARD_PIN_LED_ACTIVITY         GPIO_nLED_BLUE // BLUE
+#define BOARD_PIN_LED_BOOTLOADER       GPIO_nLED_RED // RED
+#define BOARD_LED_ON                   0
+#define BOARD_LED_OFF                  1
+
+#define SERIAL_BREAK_DETECT_DISABLED   1
+
+/*
+ * Uncommenting this allows to force the bootloader through
+ * a PWM output pin. As this can accidentally initialize
+ * an ESC prematurely, it is not recommended. This feature
+ * has not been used and hence defaults now to off.
+ *
+ * # define BOARD_FORCE_BL_PIN_OUT         (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_2MHz|GPIO_OUTPUT_SET|GPIO_PORTE|GPIO_PIN14)
+ * # define BOARD_FORCE_BL_PIN_IN          (GPIO_INPUT|GPIO_PULLUP|GPIO_PORTE|GPIO_PIN11)
+ *
+ * # define BOARD_POWER_PIN_OUT           (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_2MHz|GPIO_OUTPUT_SET|GPIO_PORTA|GPIO_PIN4)
+ * # define BOARD_POWER_ON                 1
+ * # define BOARD_POWER_OFF                0
+ * # undef  BOARD_POWER_PIN_RELEASE        // Leave pin enabling Power - un comment to release (disable power)
+ *
+*/
+
+#if !defined(ARCH_SN_MAX_LENGTH)
+# define ARCH_SN_MAX_LENGTH 12
+#endif
+
+#if !defined(APP_RESERVATION_SIZE)
+#  define APP_RESERVATION_SIZE 0
+#endif
+
+#if !defined(BOARD_FIRST_FLASH_SECTOR_TO_ERASE)
+#  define BOARD_FIRST_FLASH_SECTOR_TO_ERASE 1
+#endif
+
+#if !defined(USB_DATA_ALIGN)
+# define USB_DATA_ALIGN
+#endif
+
+#ifndef BOOT_DEVICES_SELECTION
+#  define BOOT_DEVICES_SELECTION USB0_DEV|SERIAL0_DEV|SERIAL1_DEV
+#endif
+
+#ifndef BOOT_DEVICES_FILTER_ONUSB
+#  define BOOT_DEVICES_FILTER_ONUSB USB0_DEV|SERIAL0_DEV|SERIAL1_DEV
+#endif
+
+#endif /* HW_CONFIG_H_ */

boards/holybro/kakuteh7-wing/src/i2c.cpp

@@ -0,0 +1,43 @@
+/****************************************************************************
+ *
+ *   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>
+
+#include <lib/drivers/device/Device.hpp>
+#include <px4_platform_common/i2c.h>
+
+constexpr px4_i2c_bus_t px4_i2c_buses[I2C_BUS_MAX_BUS_ITEMS] = {
+	initI2CBusExternal(1),
+	initI2CBusExternal(2),
+	initI2CBusInternal(4),
+};

boards/holybro/kakuteh7-wing/src/init.c

@@ -0,0 +1,273 @@
+/****************************************************************************
+ *
+ *   Copyright (c) 2012-2022 PX4 Development Team. All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright
+ *    notice, this list of conditions and the following disclaimer.
+ * 2. Redistributions in binary form must reproduce the above copyright
+ *    notice, this list of conditions and the following disclaimer in
+ *    the documentation and/or other materials provided with the
+ *    distribution.
+ * 3. Neither the name PX4 nor the names of its contributors may be
+ *    used to endorse or promote products derived from this software
+ *    without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
+ * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
+ * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *
+ ****************************************************************************/
+
+/**
+ * @file init.c
+ *
+ * PX4FMU-specific early startup code.  This file implements the
+ * board_app_initialize() function that is called early by nsh during startup.
+ *
+ * Code here is run before the rcS script is invoked; it should start required
+ * subsystems and perform board-specific initialization.
+ */
+
+/****************************************************************************
+ * Included Files
+ ****************************************************************************/
+
+#include "board_config.h"
+
+#include <stdbool.h>
+#include <stdio.h>
+#include <string.h>
+#include <debug.h>
+#include <errno.h>
+#include <syslog.h>
+
+#include <nuttx/config.h>
+#include <nuttx/board.h>
+#include <nuttx/spi/spi.h>
+#include <nuttx/sdio.h>
+#include <nuttx/mmcsd.h>
+#include <nuttx/analog/adc.h>
+#include <nuttx/mm/gran.h>
+#include <chip.h>
+#include <stm32_uart.h>
+#include <arch/board/board.h>
+#include "arm_internal.h"
+
+#include <drivers/drv_hrt.h>
+#include <drivers/drv_board_led.h>
+#include <systemlib/px4_macros.h>
+#include <px4_arch/io_timer.h>
+#include <px4_platform_common/init.h>
+#include <px4_platform/gpio.h>
+#include <px4_platform/board_determine_hw_info.h>
+#include <px4_platform/board_dma_alloc.h>
+
+# if defined(FLASH_BASED_PARAMS)
+#  include <parameters/flashparams/flashfs.h>
+#endif
+
+
+/****************************************************************************
+ * Pre-Processor Definitions
+ ****************************************************************************/
+
+/* Configuration ************************************************************/
+
+/*
+ * Ideally we'd be able to get these from arm_internal.h,
+ * but since we want to be able to disable the NuttX use
+ * of leds for system indication at will and there is no
+ * separate switch, we need to build independent of the
+ * CONFIG_ARCH_LEDS configuration switch.
+ */
+__BEGIN_DECLS
+extern void led_init(void);
+extern void led_on(int led);
+extern void led_off(int led);
+__END_DECLS
+
+
+/************************************************************************************
+ * Name: board_peripheral_reset
+ *
+ * Description:
+ *
+ ************************************************************************************/
+__EXPORT void board_peripheral_reset(int ms)
+{
+	/* off */
+	VTX_9V_EN(false);
+	VDD_3V3_SENSORS_EN(false);
+
+	board_control_spi_sensors_power(false, 0xffff);
+
+	usleep(ms * 1000);
+	syslog(LOG_DEBUG, "reset done, %d ms\n", ms);
+
+	/* re-enable power */
+	board_control_spi_sensors_power(true, 0xffff);
+	VDD_3V3_SENSORS_EN(true);
+	VTX_9V_EN(true);
+
+	CAM_SWITCH_CAM1;
+}
+
+/************************************************************************************
+ * Name: board_on_reset
+ *
+ * Description:
+ * Optionally provided function called on entry to board_system_reset
+ * It should perform any house keeping prior to the rest.
+ *
+ * status - 1 if resetting to boot loader
+ *          0 if just resetting
+ *
+ ************************************************************************************/
+__EXPORT void board_on_reset(int status)
+{
+	for (int i = 0; i < DIRECT_PWM_OUTPUT_CHANNELS; ++i) {
+		px4_arch_configgpio(io_timer_channel_get_gpio_output(i));
+	}
+
+	/*
+	 * On resets invoked from system (not boot) ensure we establish a low
+	 * output state on PWM pins to disarm the ESC and prevent the reset from potentially
+	 * spinning up the motors.
+	 */
+	if (status >= 0) {
+		up_mdelay(100);
+	}
+}
+
+/************************************************************************************
+ * Name: stm32_boardinitialize
+ *
+ * Description:
+ *   All STM32 architectures must provide the following entry point.  This entry point
+ *   is called early in the initialization -- after all memory has been configured
+ *   and mapped but before any devices have been initialized.
+ *
+ ************************************************************************************/
+
+__EXPORT void
+stm32_boardinitialize(void)
+{
+	board_on_reset(-1); /* Reset PWM first thing */
+
+	/* configure LEDs */
+
+	board_autoled_initialize();
+
+	/* configure pins */
+
+	const uint32_t gpio[] = PX4_GPIO_INIT_LIST;
+	px4_gpio_init(gpio, arraySize(gpio));
+
+	/* configure USB interfaces */
+
+	stm32_usbinitialize();
+}
+
+/****************************************************************************
+ * Name: board_app_initialize
+ *
+ * Description:
+ *   Perform application specific initialization.  This function is never
+ *   called directly from application code, but only indirectly via the
+ *   (non-standard) boardctl() interface using the command BOARDIOC_INIT.
+ *
+ * Input Parameters:
+ *   arg - The boardctl() argument is passed to the board_app_initialize()
+ *         implementation without modification.  The argument has no
+ *         meaning to NuttX; the meaning of the argument is a contract
+ *         between the board-specific initalization logic and the the
+ *         matching application logic.  The value cold be such things as a
+ *         mode enumeration value, a set of DIP switch switch settings, a
+ *         pointer to configuration data read from a file or serial FLASH,
+ *         or whatever you would like to do with it.  Every implementation
+ *         should accept zero/NULL as a default configuration.
+ *
+ * Returned Value:
+ *   Zero (OK) is returned on success; a negated errno value is returned on
+ *   any failure to indicate the nature of the failure.
+ *
+ ****************************************************************************/
+
+__EXPORT int board_app_initialize(uintptr_t arg)
+{
+#if !defined(BOOTLOADER)
+
+	/* Need hrt running before using the ADC */
+
+	px4_platform_init();
+
+	stm32_spiinitialize();
+
+	board_spi_reset(10, 0xffff);
+
+	/* configure the DMA allocator */
+
+	if (board_dma_alloc_init() < 0) {
+		syslog(LOG_ERR, "[boot] DMA alloc FAILED\n");
+	}
+
+#  if defined(SERIAL_HAVE_RXDMA)
+	// set up the serial DMA polling at 1ms intervals for received bytes that have not triggered a DMA event.
+	static struct hrt_call serial_dma_call;
+	hrt_call_every(&serial_dma_call, 1000, 1000, (hrt_callout)stm32_serial_dma_poll, NULL);
+#  endif
+
+	/* initial LED state */
+	drv_led_start();
+	led_off(LED_RED);
+	led_off(LED_BLUE);
+
+	if (board_hardfault_init(2, true) != 0) {
+		led_on(LED_RED);
+	}
+
+#  ifdef CONFIG_MMCSD
+	int ret = stm32_sdio_initialize();
+
+	if (ret != OK) {
+		led_on(LED_RED);
+	}
+
+#  endif /* CONFIG_MMCSD */
+
+#if defined(FLASH_BASED_PARAMS)
+	static sector_descriptor_t params_sector_map[] = {
+		{14, 128 * 1024, 0x081C0000},
+		{15, 128 * 1024, 0x081E0000},
+		{0, 0, 0},
+	};
+
+	/* Initialize the flashfs layer to use heap allocated memory */
+	int result = parameter_flashfs_init(params_sector_map, NULL, 0);
+
+	if (result != OK) {
+		syslog(LOG_ERR, "[boot] FAILED to init params in FLASH %d\n", result);
+		led_on(LED_AMBER);
+	}
+
+#endif
+
+	/* Configure the HW based on the manifest */
+
+	px4_platform_configure();
+#endif
+	return OK;
+}

boards/holybro/kakuteh7-wing/src/led.c

@@ -0,0 +1,235 @@
+/****************************************************************************
+ *
+ *   Copyright (c) 2013 PX4 Development Team. All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright
+ *    notice, this list of conditions and the following disclaimer.
+ * 2. Redistributions in binary form must reproduce the above copyright
+ *    notice, this list of conditions and the following disclaimer in
+ *    the documentation and/or other materials provided with the
+ *    distribution.
+ * 3. Neither the name PX4 nor the names of its contributors may be
+ *    used to endorse or promote products derived from this software
+ *    without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
+ * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
+ * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *
+ ****************************************************************************/
+
+/**
+ * @file led.c
+ *
+ * PX4FMU LED backend.
+ */
+
+#include <px4_platform_common/px4_config.h>
+
+#include <stdbool.h>
+
+#include "chip.h"
+#include "stm32_gpio.h"
+#include "board_config.h"
+
+#include <nuttx/board.h>
+#include <arch/board/board.h>
+
+/*
+ * Ideally we'd be able to get these from arm_internal.h,
+ * but since we want to be able to disable the NuttX use
+ * of leds for system indication at will and there is no
+ * separate switch, we need to build independent of the
+ * CONFIG_ARCH_LEDS configuration switch.
+ */
+__BEGIN_DECLS
+extern void led_init(void);
+extern void led_on(int led);
+extern void led_off(int led);
+extern void led_toggle(int led);
+__END_DECLS
+
+#ifdef CONFIG_ARCH_LEDS
+static bool nuttx_owns_leds = true;
+//                                B  R  S  G
+//                                0  1  2  3
+static const uint8_t xlatpx4[] = {1, 2, 4, 0};
+#  define xlat(p) xlatpx4[(p)]
+static uint32_t g_ledmap[] = {
+	GPIO_nLED_GREEN,   // Indexed by BOARD_LED_GREEN
+	GPIO_nLED_BLUE,    // Indexed by BOARD_LED_BLUE
+	GPIO_nLED_RED,     // Indexed by BOARD_LED_RED
+	GPIO_nSAFETY_SWITCH_LED_OUT,  // Indexed by LED_SAFETY by xlatpx4
+};
+
+#else
+
+#  define xlat(p) (p)
+static uint32_t g_ledmap[] = {
+	GPIO_nLED_BLUE,                     // Indexed by LED_BLUE
+	GPIO_nLED_RED,                      // Indexed by LED_RED, LED_AMBER
+	0,                                  // Indexed by LED_SAFETY (defaulted to an input)
+	0,                                  // Indexed by LED_GREEN
+};
+
+#endif
+
+__EXPORT void led_init(void)
+{
+	for (size_t l = 0; l < (sizeof(g_ledmap) / sizeof(g_ledmap[0])); l++) {
+		if (g_ledmap[l] != 0) {
+			stm32_configgpio(g_ledmap[l]);
+		}
+	}
+}
+
+static void phy_set_led(int led, bool state)
+{
+	/* Drive Low to switch on */
+
+	if (g_ledmap[led] != 0) {
+		stm32_gpiowrite(g_ledmap[led], !state);
+	}
+}
+
+static bool phy_get_led(int led)
+{
+	/* If Low it is on */
+	if (g_ledmap[led] != 0) {
+		return !stm32_gpioread(g_ledmap[led]);
+	}
+
+	return false;
+}
+
+__EXPORT void led_on(int led)
+{
+	phy_set_led(xlat(led), true);
+}
+
+__EXPORT void led_off(int led)
+{
+	phy_set_led(xlat(led), false);
+}
+
+__EXPORT void led_toggle(int led)
+{
+	phy_set_led(xlat(led), !phy_get_led(xlat(led)));
+}
+
+#ifdef CONFIG_ARCH_LEDS
+/****************************************************************************
+ * Public Functions
+ ****************************************************************************/
+
+/****************************************************************************
+ * Name: board_autoled_initialize
+ ****************************************************************************/
+
+void board_autoled_initialize(void)
+{
+	led_init();
+}
+
+/****************************************************************************
+ * Name: board_autoled_on
+ ****************************************************************************/
+
+void board_autoled_on(int led)
+{
+	if (!nuttx_owns_leds) {
+		return;
+	}
+
+	switch (led) {
+	default:
+		break;
+
+	case LED_HEAPALLOCATE:
+		phy_set_led(BOARD_LED_BLUE, true);
+		break;
+
+	case LED_IRQSENABLED:
+		phy_set_led(BOARD_LED_BLUE, false);
+		phy_set_led(BOARD_LED_GREEN, true);
+		break;
+
+	case LED_STACKCREATED:
+		phy_set_led(BOARD_LED_GREEN, true);
+		phy_set_led(BOARD_LED_BLUE, true);
+		break;
+
+	case LED_INIRQ:
+		phy_set_led(BOARD_LED_BLUE, true);
+		break;
+
+	case LED_SIGNAL:
+		phy_set_led(BOARD_LED_GREEN, true);
+		break;
+
+	case LED_ASSERTION:
+		phy_set_led(BOARD_LED_RED, true);
+		phy_set_led(BOARD_LED_BLUE, true);
+		break;
+
+	case LED_PANIC:
+		phy_set_led(BOARD_LED_RED, true);
+		break;
+
+	case LED_IDLE : /* IDLE */
+		phy_set_led(BOARD_LED_RED, true);
+		break;
+	}
+}
+
+/****************************************************************************
+ * Name: board_autoled_off
+ ****************************************************************************/
+
+void board_autoled_off(int led)
+{
+	if (!nuttx_owns_leds) {
+		return;
+	}
+
+	switch (led) {
+	default:
+		break;
+
+	case LED_SIGNAL:
+		phy_set_led(BOARD_LED_GREEN, false);
+		break;
+
+	case LED_INIRQ:
+		phy_set_led(BOARD_LED_BLUE, false);
+		break;
+
+	case LED_ASSERTION:
+		phy_set_led(BOARD_LED_RED, false);
+		phy_set_led(BOARD_LED_BLUE, false);
+		break;
+
+	case LED_PANIC:
+		phy_set_led(BOARD_LED_RED, false);
+		break;
+
+	case LED_IDLE : /* IDLE */
+		phy_set_led(BOARD_LED_RED, false);
+		break;
+	}
+}
+
+#endif /* CONFIG_ARCH_LEDS */

boards/holybro/kakuteh7-wing/src/sdio.c

@@ -0,0 +1,177 @@
+/****************************************************************************
+ *
+ *   Copyright (C) 2014, 2016 Gregory Nutt. All rights reserved.
+ *   Author: Gregory Nutt <gnutt@nuttx.org>
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright
+ *    notice, this list of conditions and the following disclaimer.
+ * 2. Redistributions in binary form must reproduce the above copyright
+ *    notice, this list of conditions and the following disclaimer in
+ *    the documentation and/or other materials provided with the
+ *    distribution.
+ * 3. Neither the name NuttX nor the names of its contributors may be
+ *    used to endorse or promote products derived from this software
+ *    without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
+ * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
+ * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *
+ ****************************************************************************/
+
+/****************************************************************************
+ * Included Files
+ ****************************************************************************/
+
+#include <nuttx/config.h>
+#include <board_config.h>
+
+#include <stdbool.h>
+#include <stdio.h>
+#include <debug.h>
+#include <errno.h>
+
+#include <nuttx/sdio.h>
+#include <nuttx/mmcsd.h>
+
+#include "chip.h"
+#include "board_config.h"
+#include "stm32_gpio.h"
+#include "stm32_sdmmc.h"
+
+#ifdef CONFIG_MMCSD
+
+
+/****************************************************************************
+ * Pre-processor Definitions
+ ****************************************************************************/
+
+/* Card detections requires card support and a card detection GPIO */
+
+#define HAVE_NCD   1
+#if !defined(GPIO_SDMMC1_NCD)
+#  undef HAVE_NCD
+#endif
+
+/****************************************************************************
+ * Private Data
+ ****************************************************************************/
+
+static FAR struct sdio_dev_s *sdio_dev;
+#ifdef HAVE_NCD
+static bool g_sd_inserted = 0xff; /* Impossible value */
+#endif
+
+/****************************************************************************
+ * Private Functions
+ ****************************************************************************/
+
+/****************************************************************************
+ * Name: stm32_ncd_interrupt
+ *
+ * Description:
+ *   Card detect interrupt handler.
+ *
+ ****************************************************************************/
+
+#ifdef HAVE_NCD
+static int stm32_ncd_interrupt(int irq, FAR void *context)
+{
+	bool present;
+
+	present = !stm32_gpioread(GPIO_SDMMC1_NCD);
+
+	if (sdio_dev && present != g_sd_inserted) {
+		sdio_mediachange(sdio_dev, present);
+		g_sd_inserted = present;
+	}
+
+	return OK;
+}
+#endif
+
+/****************************************************************************
+ * Public Functions
+ ****************************************************************************/
+
+/****************************************************************************
+ * Name: stm32_sdio_initialize
+ *
+ * Description:
+ *   Initialize SDIO-based MMC/SD card support
+ *
+ ****************************************************************************/
+
+int stm32_sdio_initialize(void)
+{
+	int ret;
+
+#ifdef HAVE_NCD
+	/* Card detect */
+
+	bool cd_status;
+
+	/* Configure the card detect GPIO */
+
+	stm32_configgpio(GPIO_SDMMC1_NCD);
+
+	/* Register an interrupt handler for the card detect pin */
+
+	stm32_gpiosetevent(GPIO_SDMMC1_NCD, true, true, true, stm32_ncd_interrupt);
+#endif
+
+	/* Mount the SDIO-based MMC/SD block driver */
+	/* First, get an instance of the SDIO interface */
+
+	finfo("Initializing SDIO slot %d\n", SDIO_SLOTNO);
+
+	sdio_dev = sdio_initialize(SDIO_SLOTNO);
+
+	if (!sdio_dev) {
+		syslog(LOG_ERR, "[boot] Failed to initialize SDIO slot %d\n", SDIO_SLOTNO);
+		return -ENODEV;
+	}
+
+	/* Now bind the SDIO interface to the MMC/SD driver */
+
+	finfo("Bind SDIO to the MMC/SD driver, minor=%d\n", SDIO_MINOR);
+
+	ret = mmcsd_slotinitialize(SDIO_MINOR, sdio_dev);
+
+	if (ret != OK) {
+		syslog(LOG_ERR, "[boot] Failed to bind SDIO to the MMC/SD driver: %d\n", ret);
+		return ret;
+	}
+
+	finfo("Successfully bound SDIO to the MMC/SD driver\n");
+
+#ifdef HAVE_NCD
+	/* Use SD card detect pin to check if a card is g_sd_inserted */
+
+	cd_status = !stm32_gpioread(GPIO_SDMMC1_NCD);
+	finfo("Card detect : %d\n", cd_status);
+
+	sdio_mediachange(sdio_dev, cd_status);
+#else
+	/* Assume that the SD card is inserted.  What choice do we have? */
+
+	sdio_mediachange(sdio_dev, true);
+#endif
+
+	return OK;
+}
+
+#endif /* CONFIG_MMCSD */

boards/holybro/kakuteh7-wing/src/spi.cpp

@@ -0,0 +1,52 @@
+/****************************************************************************
+ *
+ *   Copyright (C) 2020, 2022 PX4 Development Team. All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright
+ *    notice, this list of conditions and the following disclaimer.
+ * 2. Redistributions in binary form must reproduce the above copyright
+ *    notice, this list of conditions and the following disclaimer in
+ *    the documentation and/or other materials provided with the
+ *    distribution.
+ * 3. Neither the name PX4 nor the names of its contributors may be
+ *    used to endorse or promote products derived from this software
+ *    without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
+ * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
+ * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *
+ ****************************************************************************/
+
+#include <px4_arch/spi_hw_description.h>
+#include <drivers/drv_sensor.h>
+#include <nuttx/spi/spi.h>
+
+constexpr px4_spi_bus_t px4_spi_buses[SPI_BUS_MAX_BUS_ITEMS] = {
+
+	initSPIBus(SPI::Bus::SPI1, {
+		initSPIDevice(DRV_ACC_DEVTYPE_BMI088, SPI::CS{GPIO::PortC, GPIO::Pin8}),
+		initSPIDevice(DRV_GYR_DEVTYPE_BMI088, SPI::CS{GPIO::PortC, GPIO::Pin9})
+	}),
+	initSPIBus(SPI::Bus::SPI2, {
+		initSPIDevice(DRV_OSD_DEVTYPE_ATXXXX, SPI::CS{GPIO::PortB, GPIO::Pin12}),
+	}),
+	initSPIBus(SPI::Bus::SPI3, {
+		initSPIDevice(DRV_IMU_DEVTYPE_ICM42688P, SPI::CS{GPIO::PortE, GPIO::Pin12}),
+	}),
+};
+
+static constexpr bool unused = validateSPIConfig(px4_spi_buses);

boards/holybro/kakuteh7-wing/src/timer_config.cpp

@@ -0,0 +1,64 @@
+/****************************************************************************
+ *
+ *   Copyright (C) 2012 PX4 Development Team. All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright
+ *    notice, this list of conditions and the following disclaimer.
+ * 2. Redistributions in binary form must reproduce the above copyright
+ *    notice, this list of conditions and the following disclaimer in
+ *    the documentation and/or other materials provided with the
+ *    distribution.
+ * 3. Neither the name PX4 nor the names of its contributors may be
+ *    used to endorse or promote products derived from this software
+ *    without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
+ * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
+ * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *
+ ****************************************************************************/
+
+#include <px4_arch/io_timer_hw_description.h>
+
+
+constexpr io_timers_t io_timers[MAX_IO_TIMERS] = {
+	initIOTimer(Timer::Timer1, DMA{DMA::Index1}),
+	initIOTimer(Timer::Timer4, DMA{DMA::Index1}),
+	initIOTimer(Timer::Timer5, DMA{DMA::Index1}),
+	initIOTimer(Timer::Timer15),
+	initIOTimer(Timer::Timer3),
+	initIOTimer(Timer::Timer2),
+};
+
+constexpr timer_io_channels_t timer_io_channels[MAX_TIMER_IO_CHANNELS] = {
+	initIOTimerChannel(io_timers, {Timer::Timer1, Timer::Channel1}, {GPIO::PortA, GPIO::Pin8}),
+	initIOTimerChannel(io_timers, {Timer::Timer1, Timer::Channel2}, {GPIO::PortE, GPIO::Pin11}),
+	initIOTimerChannel(io_timers, {Timer::Timer1, Timer::Channel3}, {GPIO::PortE, GPIO::Pin13}),
+	initIOTimerChannel(io_timers, {Timer::Timer1, Timer::Channel4}, {GPIO::PortE, GPIO::Pin14}),
+	initIOTimerChannel(io_timers, {Timer::Timer4, Timer::Channel3}, {GPIO::PortD, GPIO::Pin14}),
+	initIOTimerChannel(io_timers, {Timer::Timer4, Timer::Channel4}, {GPIO::PortD, GPIO::Pin15}),
+	initIOTimerChannel(io_timers, {Timer::Timer5, Timer::Channel1}, {GPIO::PortA, GPIO::Pin0}),
+	initIOTimerChannel(io_timers, {Timer::Timer5, Timer::Channel2}, {GPIO::PortA, GPIO::Pin1}),
+	initIOTimerChannel(io_timers, {Timer::Timer15, Timer::Channel1}, {GPIO::PortE, GPIO::Pin5}),
+	initIOTimerChannel(io_timers, {Timer::Timer15, Timer::Channel2}, {GPIO::PortE, GPIO::Pin6}),
+	initIOTimerChannel(io_timers, {Timer::Timer3, Timer::Channel2}, {GPIO::PortB, GPIO::Pin5}),
+	initIOTimerChannel(io_timers, {Timer::Timer3, Timer::Channel3}, {GPIO::PortB, GPIO::Pin0}),
+	initIOTimerChannel(io_timers, {Timer::Timer3, Timer::Channel4}, {GPIO::PortB, GPIO::Pin1}),
+	initIOTimerChannel(io_timers, {Timer::Timer2, Timer::Channel1}, {GPIO::PortA, GPIO::Pin15}),
+};
+
+constexpr io_timers_channel_mapping_t io_timers_channel_mapping =
+	initIOTimerChannelMapping(io_timers, timer_io_channels);

boards/holybro/kakuteh7-wing/src/usb.c

@@ -0,0 +1,105 @@
+/****************************************************************************
+ *
+ *   Copyright (C) 2016 PX4 Development Team. All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright
+ *    notice, this list of conditions and the following disclaimer.
+ * 2. Redistributions in binary form must reproduce the above copyright
+ *    notice, this list of conditions and the following disclaimer in
+ *    the documentation and/or other materials provided with the
+ *    distribution.
+ * 3. Neither the name PX4 nor the names of its contributors may be
+ *    used to endorse or promote products derived from this software
+ *    without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
+ * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
+ * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *
+ ****************************************************************************/
+
+/**
+ * @file usb.c
+ *
+ * Board-specific USB functions.
+ */
+
+/************************************************************************************
+ * Included Files
+ ************************************************************************************/
+
+#include <px4_platform_common/px4_config.h>
+
+#include <sys/types.h>
+#include <stdint.h>
+#include <stdbool.h>
+#include <debug.h>
+
+#include <nuttx/usb/usbdev.h>
+#include <nuttx/usb/usbdev_trace.h>
+
+#include <arm_internal.h>
+#include <chip.h>
+#include <stm32_gpio.h>
+#include <stm32_otg.h>
+#include "board_config.h"
+
+/************************************************************************************
+ * Definitions
+ ************************************************************************************/
+
+/************************************************************************************
+ * Private Functions
+ ************************************************************************************/
+
+/************************************************************************************
+ * Public Functions
+ ************************************************************************************/
+
+/************************************************************************************
+ * Name: stm32_usbinitialize
+ *
+ * Description:
+ *   Called to setup USB-related GPIO pins for the PX4FMU board.
+ *
+ ************************************************************************************/
+
+__EXPORT void stm32_usbinitialize(void)
+{
+	/* The OTG FS has an internal soft pull-up */
+
+	/* Configure the OTG FS VBUS sensing GPIO, Power On, and Overcurrent GPIOs */
+
+#ifdef CONFIG_STM32H7_OTGFS
+	stm32_configgpio(GPIO_OTGFS_VBUS);
+#endif
+}
+
+/************************************************************************************
+ * Name:  stm32_usbsuspend
+ *
+ * Description:
+ *   Board logic must provide the stm32_usbsuspend logic if the USBDEV driver is
+ *   used.  This function is called whenever the USB enters or leaves suspend mode.
+ *   This is an opportunity for the board logic to shutdown clocks, power, etc.
+ *   while the USB is suspended.
+ *
+ ************************************************************************************/
+
+__EXPORT void stm32_usbsuspend(FAR struct usbdev_s *dev, bool resume)
+{
+	uinfo("resume: %d\n", resume);
+}

docs/en/SUMMARY.md

@@ -181,6 +181,7 @@
       - [Holybro Kakute H7v2](flight_controller/kakuteh7v2.md)
       - [Holybro Kakute H7mini](flight_controller/kakuteh7mini.md)
       - [Holybro Kakute H7](flight_controller/kakuteh7.md)
+      - [Holybro Kakute H7 Wing](flight_controller/kakuteh7-wing.md)
       - [Holybro Durandal](flight_controller/durandal.md)
         - [Wiring Quickstart](assembly/quick_start_durandal.md)
       - [Holybro Pix32 v5](flight_controller/holybro_pix32_v5.md)

docs/en/_sidebar.md

@@ -182,6 +182,7 @@
       - [Holybro Kakute H7v2](/flight_controller/kakuteh7v2.md)
       - [Holybro Kakute H7mini](/flight_controller/kakuteh7mini.md)
       - [Holybro Kakute H7](/flight_controller/kakuteh7.md)
+      - [Holybro Kakute H7 Wing](flight_controller/kakuteh7-wing.md)
       - [Holybro Durandal](/flight_controller/durandal.md)
         - [Wiring Quickstart](/assembly/quick_start_durandal.md)
       - [Holybro Pix32 v5](/flight_controller/holybro_pix32_v5.md)

docs/en/flight_controller/kakuteh7-wing.md

@@ -0,0 +1,86 @@
+# Holybro Kakute H7 V2
+
+:::warning
+PX4 does not manufacture this (or any) autopilot.
+Contact the [manufacturer](https://holybro.com/) for hardware support or compliance issues.
+:::
+
+The [Holybro Kakute H743 Wing](https://holybro.com/products/kakute-h743-wing) is a fully featured flight controller specifically aimed at fixed-wing and VTOL applications. It has the STM32 H743 Processor running at 480 MHz and CAN Bus support, along with dual camera support & switch, ON/OFF Pit Switch, 5V, 6V/8V, 9V/12 BEC, and plug-and-play GPS, CAN, I2C ports.
+
+
+::: info
+This flight controller is [manufacturer supported](../flight_controller/autopilot_manufacturer_supported.md).
+:::
+
+## Where to Buy
+
+The board can be bought from one of the following shops (for example):
+
+- [Holybro](https://holybro.com/products/kakute-h743-wing)
+
+
+## Connectors and Pins
+
+| Pin              | Function                          | PX4 default                |
+| ---------------- | --------------------------------- | -------------------------- |
+| GPS 1            | USART1 and I2C1                   | GPS1                       |
+| R2, T2           | USART2 RX and TX                  | GPS2                       |
+| R3, T3           | USART3 RX and TX                  | TELEM1                     |
+| R5, T5           | USART5 RX and TX                  | TELEM2                     |
+| R6, T6           | USART6 RX and TX                  | RC (PPM, SBUS, etc.) input |
+| R7, T7, RTS, CTS | UART7 RX and TX with flow control | TELEM3                     |
+| R8, T8           | UART8 RX and TX                   | Console                    |
+| Buz-, Buz+       | Piezo buzzer                      |                            |
+| M1 to M14        | Motor signal outputs              |                            |
+
+<a id="bootloader"></a>
+
+## PX4 Bootloader Update
+
+The board comes pre-installed with [Betaflight](https://github.com/betaflight/betaflight/wiki).
+Before the PX4 firmware can be installed, the _PX4 bootloader_ must be flashed.
+Download the [holybro_kakuteh7-wing.hex](https://github.com/PX4/PX4-Autopilot/raw/main/docs/assets/flight_controller/kakuteh7-wing/holybro_kakuteh7-wing_bootloader.hex) bootloader binary and read [this page](../advanced_config/bootloader_update_from_betaflight.md) for flashing instructions.
+
+## Building Firmware
+
+To [build PX4](../dev_setup/building_px4.md) for this target:
+
+```
+make holybro_kakuteh7-wing_default
+```
+
+## Installing PX4 Firmware
+
+::: info
+KakuteH7-wing is supported with PX4 master & PX4 v1.16 or newer..
+Prior to that release you will need to manually build and install the firmware.
+:::
+
+Firmware can be manually installed in any of the normal ways:
+
+- Build and upload the source:
+
+  ```
+  make holybro_kakuteh7-wing_default upload
+  ```
+
+- [Load the firmware](../config/firmware.md) using _QGroundControl_.
+  You can use either pre-built firmware or your own custom firmware.
+
+## Serial Port Mapping
+
+| UART   | Device     | Port                  | Default function |
+| ------ | ---------- | --------------------- | ---------------- |
+| USART1 | /dev/ttyS0 | GPS 1                 | GPS1             |
+| USART2 | /dev/ttyS1 | R2, T2                | GPS2             |
+| USART3 | /dev/ttyS2 | R3, T3                | TELEM1           |
+| UART5  | /dev/ttyS3 | R5, T5                | TELEM2           |
+| USART6 | /dev/ttyS4 | R6, (T6)              | RC input         |
+| UART7  | /dev/ttyS5 | R7, T7, RTS, CTS      | TELEM3           |
+| UART8  | /dev/ttyS6 | R8, T8                | Console          |
+
+## Debug Port
+
+### System Console
+
+UART8 RX and TX are configured for use as the [System Console](../debug/system_console.md).

msg/versioned/VehicleStatus.msg

@@ -89,9 +89,9 @@ uint8 HIL_STATE_ON = 1
 
 # Current vehicle locomotion method. A vehicle can have different methods (e.g. VTOL transitions from RW to FW method)
 uint8 vehicle_type
-uint8 VEHICLE_TYPE_ROTARY_WING = 0
-uint8 VEHICLE_TYPE_FIXED_WING = 1
-uint8 VEHICLE_TYPE_ROVER = 2
+uint8 VEHICLE_TYPE_ROTARY_WING = 1
+uint8 VEHICLE_TYPE_FIXED_WING = 2
+uint8 VEHICLE_TYPE_ROVER = 3
 
 uint8 FAILSAFE_DEFER_STATE_DISABLED = 0
 uint8 FAILSAFE_DEFER_STATE_ENABLED = 1

platforms/nuttx/src/px4/stm/stm32_common/hrt/hrt.c

@@ -232,7 +232,6 @@
 /*
  * Specific registers and bits used by HRT sub-functions
  */
-/* FIXME! There is an interaction in the CCMR registers that prevents using Chan 1 as the timer and chan 2 as the PPM*/
 #if HRT_TIMER_CHANNEL == 1
 # define rCCR_HRT	rCCR1			/* compare register for HRT */
 # define DIER_HRT	GTIM_DIER_CC1IE		/* interrupt enable for HRT */
@@ -306,13 +305,12 @@ int hrt_ioctl(unsigned int cmd, unsigned long arg);
 #  define GTIM_CCER_CC4NP 0
 #  define PPM_EDGE_FLIP
 # endif
-/* FIXME! There is an interaction in the CCMR registers that prevents using Chan 1 as the timer and chan 2 as the PPM*/
 # if HRT_PPM_CHANNEL == 1
 #  define rCCR_PPM	rCCR1			/* capture register for PPM */
 #  define DIER_PPM	GTIM_DIER_CC1IE		/* capture interrupt (non-DMA mode) */
 #  define SR_INT_PPM	GTIM_SR_CC1IF		/* capture interrupt (non-DMA mode) */
 #  define SR_OVF_PPM	GTIM_SR_CC1OF		/* capture overflow (non-DMA mode) */
-#  define CCMR1_PPM	1			/* not on TI1/TI2 */
+#  define CCMR1_PPM	(1 << 0)		/* not on TI1/TI2 */
 #  define CCMR2_PPM	0			/* on TI3, not on TI4 */
 #  define CCER_PPM	(GTIM_CCER_CC1E | GTIM_CCER_CC1P | GTIM_CCER_CC1NP) /* CC1, both edges */
 #  define CCER_PPM_FLIP	GTIM_CCER_CC1P
@@ -321,7 +319,7 @@ int hrt_ioctl(unsigned int cmd, unsigned long arg);
 #  define DIER_PPM	GTIM_DIER_CC2IE		/* capture interrupt (non-DMA mode) */
 #  define SR_INT_PPM	GTIM_SR_CC2IF		/* capture interrupt (non-DMA mode) */
 #  define SR_OVF_PPM	GTIM_SR_CC2OF		/* capture overflow (non-DMA mode) */
-#  define CCMR1_PPM	2			/* not on TI1/TI2 */
+#  define CCMR1_PPM	(1 << 8)		/* not on TI1/TI2 */
 #  define CCMR2_PPM	0			/* on TI3, not on TI4 */
 #  define CCER_PPM	(GTIM_CCER_CC2E | GTIM_CCER_CC2P | GTIM_CCER_CC2NP) /* CC2, both edges */
 #  define CCER_PPM_FLIP	GTIM_CCER_CC2P
@@ -331,7 +329,7 @@ int hrt_ioctl(unsigned int cmd, unsigned long arg);
 #  define SR_INT_PPM	GTIM_SR_CC3IF		/* capture interrupt (non-DMA mode) */
 #  define SR_OVF_PPM	GTIM_SR_CC3OF		/* capture overflow (non-DMA mode) */
 #  define CCMR1_PPM	0			/* not on TI1/TI2 */
-#  define CCMR2_PPM	1			/* on TI3, not on TI4 */
+#  define CCMR2_PPM	(1 << 0)		/* on TI3, not on TI4 */
 #  define CCER_PPM	(GTIM_CCER_CC3E | GTIM_CCER_CC3P | GTIM_CCER_CC3NP) /* CC3, both edges */
 #  define CCER_PPM_FLIP	GTIM_CCER_CC3P
 # elif HRT_PPM_CHANNEL == 4
@@ -340,7 +338,7 @@ int hrt_ioctl(unsigned int cmd, unsigned long arg);
 #  define SR_INT_PPM	GTIM_SR_CC4IF		/* capture interrupt (non-DMA mode) */
 #  define SR_OVF_PPM	GTIM_SR_CC4OF		/* capture overflow (non-DMA mode) */
 #  define CCMR1_PPM	0			/* not on TI1/TI2 */
-#  define CCMR2_PPM	2			/* on TI3, not on TI4 */
+#  define CCMR2_PPM	(1 << 8)		/* on TI3, not on TI4 */
 #  define CCER_PPM	(GTIM_CCER_CC4E | GTIM_CCER_CC4P | GTIM_CCER_CC4NP) /* CC4, both edges */
 #  define CCER_PPM_FLIP	GTIM_CCER_CC4P
 # else

src/drivers/imu/analog_devices/adis16507/ADIS16507.cpp

@@ -55,9 +55,10 @@ ADIS16507::ADIS16507(const I2CSPIDriverConfig &config) :
 ADIS16507::~ADIS16507()
 {
 	perf_free(_reset_perf);
-	perf_free(_bad_register_perf);
 	perf_free(_bad_transfer_perf);
-	perf_free(_perf_crc_bad);
+	perf_free(_bad_status_perf);
+	perf_free(_bad_checksum_perf);
+	perf_free(_bad_data_cntr_perf);
 	perf_free(_drdy_missed_perf);
 }
 
@@ -66,20 +67,20 @@ int ADIS16507::init()
 	int ret = SPI::init();
 
 	if (ret != PX4_OK) {
-		DEVICE_DEBUG("SPI::init failed (%i)", ret);
+		PX4_ERR("SPI::init failed (%i)", ret);
 		return ret;
 	}
 
-	return Reset() ? 0 : -1;
+	_state = STATE::RESET;
+	ScheduleNow();
+	return PX4_OK;
 }
 
-bool ADIS16507::Reset()
+void ADIS16507::Reset()
 {
 	_state = STATE::RESET;
 	DataReadyInterruptDisable();
 	ScheduleClear();
-	ScheduleNow();
-	return true;
 }
 
 void ADIS16507::exit_and_cleanup()
@@ -88,96 +89,145 @@ void ADIS16507::exit_and_cleanup()
 	I2CSPIDriverBase::exit_and_cleanup();
 }
 
-void ADIS16507::print_status()
-{
-	I2CSPIDriverBase::print_status();
-
-	perf_print_counter(_reset_perf);
-	perf_print_counter(_bad_register_perf);
-	perf_print_counter(_bad_transfer_perf);
-	perf_print_counter(_perf_crc_bad);
-	perf_print_counter(_drdy_missed_perf);
-}
-
 int ADIS16507::probe()
 {
 	// Power-On Start-Up Time 310 ms
 	if (hrt_absolute_time() < 310_ms) {
-		PX4_WARN("required Power-On Start-Up Time 310 ms");
+		PX4_WARN("Required Power-On Start-Up Time 310 ms");
 	}
 
-	const uint16_t PROD_ID = RegisterRead(Register::PROD_ID);
+	const uint16_t id = RegisterRead(Register::PROD_ID);
 
-	if (PROD_ID != Product_identification) {
-		DEVICE_DEBUG("unexpected PROD_ID 0x%02x", PROD_ID);
+	if (id != Register::PROD_ID_EXPECTED) {
+		PX4_ERR("Unexpected PROD_ID 0x%02x", id);
 		return PX4_ERROR;
 	}
 
-	const uint16_t SERIAL_NUM = RegisterRead(Register::SERIAL_NUM);
-	const uint16_t FIRM_REV = RegisterRead(Register::FIRM_REV);
-	const uint16_t FIRM_DM = RegisterRead(Register::FIRM_DM);
-	const uint16_t FIRM_Y = RegisterRead(Register::FIRM_Y);
+	const uint16_t serial = RegisterRead(Register::SERIAL_NUM);
+	const uint16_t rev = RegisterRead(Register::FIRM_REV);
+	const uint16_t daymonth = RegisterRead(Register::FIRM_DM);
+	const uint16_t year = RegisterRead(Register::FIRM_Y);
 
-	PX4_INFO("Serial Number: 0x%X, Firmware revision: 0x%X Date: Y %X DM %X", SERIAL_NUM, FIRM_REV, FIRM_Y, FIRM_DM);
+	PX4_INFO("Serial Number: 0x%X, Firmware revision: 0x%X Date: Y %X DM %X", serial, rev, year, daymonth);
 
 	return PX4_OK;
 }
 
+bool ADIS16507::Configure()
+{
+	struct {
+		uint16_t reg;
+		uint16_t val;
+	} defaults[] = {
+		// Default 0x00C1
+		// - Change Data Ready polarity to active low
+		{ Register::MSC_CTRL, 0x00C0 },
+	};
+
+	// Write default register configuration
+	for (const auto &r : defaults) {
+		RegisterWrite(r.reg, r.val);
+	}
+
+	// Wait for changes to apply
+	px4_usleep(SPI_STALL_PERIOD);
+
+	// Check that all are configured
+	for (const auto &r : defaults) {
+		if (!RegisterCheck(r.reg, r.val)) {
+			return false;
+		}
+	}
+
+	// Accelerometer only has a single measurement range and scale
+	_px4_accel.set_range(392);
+	_px4_accel.set_scale(392.f / 32'000.f); // 32,000 -> 392 m/sec^2
+
+	// Check gyroscope measurement range
+	const uint16_t rang_mdl = RegisterRead(Register::RANG_MDL);
+
+	// sanity check RANG_MDL [1:0] Reserved, binary value = 11
+	if (rang_mdl & (Bit1 | Bit0)) {
+		const uint16_t gyro_range = (rang_mdl & (Bit3 | Bit2)) >> 2;
+
+		if (gyro_range == 0b11) {
+			PX4_DEBUG("Gyro Range ±2000°/sec");
+			// 11 = ±2000°/sec (ADIS16507-3BMLZ)
+			_px4_gyro.set_range(math::radians(2000.f));
+			_px4_gyro.set_scale(math::radians(1.f / 10.f)); // scaling 10 LSB/°/sec -> rad/s per LSB
+
+		} else if (gyro_range == 0b01) {
+			PX4_DEBUG("Gyro Range ±500°/sec");
+			// 01 = ±500°/sec (ADIS16507-2BMLZ)
+			_px4_gyro.set_range(math::radians(500.f));
+			_px4_gyro.set_scale(math::radians(1.f / 40.f)); // scaling 40 LSB/°/sec -> rad/s per LSB
+
+		} else if (gyro_range == 0b00) {
+			PX4_DEBUG("Gyro Range ±125°/sec");
+			// 00 = ±125°/sec (ADIS16507-1BMLZ)
+			_px4_gyro.set_range(math::radians(125.f));
+			_px4_gyro.set_scale(math::radians(1.f / 160.f)); // scaling 160 LSB/°/sec -> rad/s per LSB
+		}
+	}
+
+	return true;
+}
+
 void ADIS16507::RunImpl()
 {
 	const hrt_abstime now = hrt_absolute_time();
 
 	switch (_state) {
 	case STATE::RESET:
+		PX4_DEBUG("Resetting");
 		perf_count(_reset_perf);
-		// GLOB_CMD: software reset
-		RegisterWrite(Register::GLOB_CMD, GLOB_CMD_BIT::Software_reset);
-		_reset_timestamp = now;
+
+		RegisterWrite(Register::GLOB_CMD, Register::GLOB_CMD_BIT::Software_reset);
+
+#ifdef GPIO_ADIS16507_RESET
+		PX4_DEBUG("Hardware reset");
+		GPIO_ADIS16507_RESET(1);
+		px4_usleep(15); // Minimum 10us
+		GPIO_ADIS16507_RESET(0);
+#endif
 		_failure_count = 0;
 		_state = STATE::WAIT_FOR_RESET;
-		ScheduleDelayed(255_ms); // 255 ms Reset Recovery Time
+		ScheduleDelayed(350_ms); // 255 ms Reset Recovery Time
 		break;
 
 	case STATE::WAIT_FOR_RESET:
 
 		if (_self_test_passed) {
-			if ((RegisterRead(Register::PROD_ID) == Product_identification)) {
-				// if reset succeeded then configure
+			if (RegisterRead(Register::PROD_ID) == Register::PROD_ID_EXPECTED) {
 				_state = STATE::CONFIGURE;
+				PX4_DEBUG("Reset complete, configuring");
 				ScheduleNow();
 
 			} else {
-				// RESET not complete
-				if (hrt_elapsed_time(&_reset_timestamp) > 1000_ms) {
-					PX4_DEBUG("Reset failed, retrying");
-					_state = STATE::RESET;
-					ScheduleDelayed(100_ms);
-
-				} else {
-					PX4_DEBUG("Reset not complete, check again in 100 ms");
-					ScheduleDelayed(100_ms);
-				}
+				PX4_DEBUG("Reset failed, retrying");
+				_state = STATE::RESET;
+				ScheduleDelayed(100_ms);
 			}
 
 		} else {
-			RegisterWrite(Register::GLOB_CMD, GLOB_CMD_BIT::Sensor_self_test);
+			PX4_DEBUG("Running self test");
+			RegisterWrite(Register::GLOB_CMD, Register::GLOB_CMD_BIT::Sensor_self_test);
 			_state = STATE::SELF_TEST_CHECK;
-			ScheduleDelayed(14_ms); // Self Test Time
+			ScheduleDelayed(50_ms); // Self Test Time 24ms typical
 		}
 
 		break;
 
 	case STATE::SELF_TEST_CHECK: {
-			// read DIAG_STAT to check result
-			const uint16_t DIAG_STAT = RegisterRead(Register::DIAG_STAT);
+			const uint16_t diag_stat = RegisterRead(Register::DIAG_STAT);
 
-			if (DIAG_STAT != 0) {
-				PX4_ERR("self test failed, resetting. DIAG_STAT: %#X", DIAG_STAT);
-				_state = STATE::RESET;
-				ScheduleDelayed(3_s);
+			if (diag_stat != 0) {
+				PX4_ERR("Self test failed");
+				PrintErrorFlags(diag_stat);
+				ScheduleDelayed(350_ms);
 
 			} else {
-				PX4_DEBUG("self test passed");
+				PX4_DEBUG("Self test passed");
 				_self_test_passed = true;
 				_state = STATE::RESET;
 				ScheduleNow();
@@ -187,30 +237,27 @@ void ADIS16507::RunImpl()
 
 	case STATE::CONFIGURE:
 		if (Configure()) {
-			// if configure succeeded then start reading
 			_state = STATE::READ;
 
 			if (DataReadyInterruptConfigure()) {
+				PX4_DEBUG("Using data ready interrupt");
 				_data_ready_interrupt_enabled = true;
 
 				// backup schedule as a watchdog timeout
 				ScheduleDelayed(100_ms);
+				// Data ready should reschedule this almost immediately
+				return;
 
 			} else {
+				PX4_DEBUG("Not using data ready interrupt");
 				_data_ready_interrupt_enabled = false;
 				ScheduleOnInterval(SAMPLE_INTERVAL_US, SAMPLE_INTERVAL_US);
 			}
 
 		} else {
 			// CONFIGURE not complete
-			if (hrt_elapsed_time(&_reset_timestamp) > 1000_ms) {
-				PX4_DEBUG("Configure failed, resetting");
-				_state = STATE::RESET;
-
-			} else {
-				PX4_DEBUG("Configure failed, retrying");
-			}
-
+			PX4_WARN("Configure failed, resetting");
+			_state = STATE::RESET;
 			ScheduleDelayed(100_ms);
 		}
 
@@ -234,143 +281,104 @@ void ADIS16507::RunImpl()
 				ScheduleDelayed(SAMPLE_INTERVAL_US * 2);
 			}
 
-			bool success = false;
-
-			// TODO: review and test BURST_SEL = 1
-			//  16-Bit Burst Mode with BURST_SEL = 1
-			//  In 16-bit burst mode with BURST_SEL = 1, a burst contains
-			//  calibrated delta angle and delta velocity data in 16-bit format.
-
+			// struct __attribute__((packed)) BurstRead {
 			struct BurstRead {
 				uint16_t cmd;
-				uint16_t DIAG_STAT;
-				int16_t X_GYRO_OUT;
-				int16_t Y_GYRO_OUT;
-				int16_t Z_GYRO_OUT;
-				int16_t X_ACCL_OUT;
-				int16_t Y_ACCL_OUT;
-				int16_t Z_ACCL_OUT;
-				int16_t TEMP_OUT;
-				uint16_t DATA_CNTR;
+				uint16_t diag_stat;
+				int16_t x_gyro_out;
+				int16_t y_gyro_out;
+				int16_t z_gyro_out;
+				int16_t x_accl_out;
+				int16_t y_accl_out;
+				int16_t z_accl_out;
+				int16_t temp_out;
+				uint16_t data_cntr;
 				uint16_t checksum;
 			} buffer{};
 
 			// ADIS16507 burst report should be 176 bits
 			static_assert(sizeof(BurstRead) == (176 / 8), "ADIS16507 report not 176 bits");
 
-			buffer.cmd = static_cast<uint16_t>(Register::GLOB_CMD) << 8;
+			// Pg 20 of Datasheet
+			// 16-Bit Burst Mode with BURST_SEL = 0
+			buffer.cmd = BURST_READ_CMD;
 			set_frequency(SPI_SPEED_BURST);
 
-			if (transferhword((uint16_t *)&buffer, (uint16_t *)&buffer, sizeof(buffer) / sizeof(uint16_t)) == PX4_OK) {
-
-				// Calculate checksum and compare
-
-				// Checksum = DIAG_STAT, Bits[15:8] + DIAG_STAT, Bits[7:0] +
-				//  X_GYRO_OUT, Bits[15:8] + X_GYRO_OUT, Bits[7:0] +
-				//  Y_GYRO_OUT, Bits[15:8] + Y_GYRO_OUT, Bits[7:0] +
-				//  Z_GYRO_OUT, Bits[15:8] + Z_GYRO_OUT, Bits[7:0] +
-				//  X_ACCL_OUT, Bits[15:8] + X_ACCL_OUT, Bits[7:0] +
-				//  Y_ACCL_OUT, Bits[15:8] + Y_ACCL_OUT, Bits[7:0] +
-				//  Z_ACCL_OUT, Bits[15:8] + Z_ACCL_OUT, Bits[7:0] +
-				//  TEMP_OUT, Bits[15:8] + TEMP_OUT, Bits[7:0] +
-				//  DATA_CNTR, Bits[15:8] + DATA_CNTR, Bits[7:0]
-				uint8_t *checksum_helper = (uint8_t *)&buffer.DIAG_STAT;
-
-				uint16_t checksum = 0;
-
-				for (int i = 0; i < 18; i++) {
-					checksum += checksum_helper[i];
-				}
-
-				if (buffer.checksum != checksum) {
-					//PX4_DEBUG("adis_report.checksum: %X vs calculated: %X", buffer.checksum, checksum);
-					perf_count(_bad_transfer_perf);
-					perf_count(_perf_crc_bad);
-				}
-
-				if (buffer.DIAG_STAT != DIAG_STAT_BIT::Data_path_overrun) {
-					// Data path overrun. A 1 indicates that one of the
-					// data paths have experienced an overrun condition.
-					// If this occurs, initiate a reset,
-
-					//Reset();
-					//return;
-				}
-
-				// Check all Status/Error Flag Indicators (DIAG_STAT)
-				if (buffer.DIAG_STAT != 0) {
-					perf_count(_bad_transfer_perf);
-				}
-
-				// temperature 1 LSB = 0.1°C
-				const float temperature = buffer.TEMP_OUT * 0.1f;
-				_px4_accel.set_temperature(temperature);
-				_px4_gyro.set_temperature(temperature);
-
-
-				int16_t accel_x = buffer.X_ACCL_OUT;
-				int16_t accel_y = buffer.Y_ACCL_OUT;
-				int16_t accel_z = buffer.Z_ACCL_OUT;
-
-				// sensor's frame is +x forward, +y left, +z up
-				//  flip y & z to publish right handed with z down (x forward, y right, z down)
-				accel_y = (accel_y == INT16_MIN) ? INT16_MAX : -accel_y;
-				accel_z = (accel_z == INT16_MIN) ? INT16_MAX : -accel_z;
-
-
-				// TODO:
-				// Group Delay with No Filtering: Accelerometer 1.57 ms
-				const uint64_t accel_group_delay_us = 1'570;
-				_px4_accel.update(timestamp_sample - accel_group_delay_us, accel_x, accel_y, accel_z);
-
-
-				int16_t gyro_x = buffer.X_GYRO_OUT;
-				int16_t gyro_y = buffer.Y_GYRO_OUT;
-				int16_t gyro_z = buffer.Z_GYRO_OUT;
-				// sensor's frame is +x forward, +y left, +z up
-				//  flip y & z to publish right handed with z down (x forward, y right, z down)
-				gyro_y = (gyro_y == INT16_MIN) ? INT16_MAX : -gyro_y;
-				gyro_z = (gyro_z == INT16_MIN) ? INT16_MAX : -gyro_z;
-
-				// TODO:
-				// Group Delay with No Filtering:
-				//  Gyroscope (X-Axis) 1.51 ms
-				//  Gyroscope (Y-Axis) 1.51 ms
-				//  Gyroscope (Z-Axis) 1.29 ms
-				const uint64_t gyro_group_delay_us = (1'510 + 1'510 + 1'290) / 3;
-				_px4_gyro.update(timestamp_sample - gyro_group_delay_us, gyro_x, gyro_y, gyro_z);
-
-				success = true;
-
-				if (_failure_count > 0) {
-					_failure_count--;
-				}
-
-			} else {
+			if (transferhword((uint16_t *)&buffer, (uint16_t *)&buffer, sizeof(buffer) / sizeof(uint16_t)) != PX4_OK) {
 				perf_count(_bad_transfer_perf);
-			}
-
-			if (!success) {
 				_failure_count++;
 
-				// full reset if things are failing consistently
 				if (_failure_count > 10) {
+					PX4_DEBUG("Consecutive failures!");
 					Reset();
-					return;
 				}
+
+				// Don't publish on a bad transfer
+				return;
 			}
 
-			if (!success || hrt_elapsed_time(&_last_config_check_timestamp) > 100_ms) {
-				// check configuration registers periodically or immediately following any failure
-				if (RegisterCheck(_register_cfg[_checked_register])) {
-					_last_config_check_timestamp = now;
-					_checked_register = (_checked_register + 1) % size_register_cfg;
+			if (buffer.data_cntr == _last_data_cntr) {
+				// Don't publish if data counter is not incrementing
+				perf_count(_bad_data_cntr_perf);
+				_last_data_cntr = buffer.data_cntr;
+				_failure_count++;
 
-				} else {
-					// register check failed, force reset
-					perf_count(_bad_register_perf);
+				if (_failure_count > 10) {
+					PX4_DEBUG("Consecutive failures!");
 					Reset();
 				}
+
+				return;
+			}
+
+			_last_data_cntr = buffer.data_cntr;
+
+			uint16_t checksum = 0;
+			uint8_t *checksum_helper = (uint8_t *)&buffer.diag_stat;
+
+			for (int i = 0; i < 18; i++) {
+				checksum += checksum_helper[i];
+			}
+
+			if (buffer.checksum != checksum) {
+				perf_count(_bad_checksum_perf);
+				// Don't publish if checksum fails
+				return;
+			}
+
+			// Check all Status/Error Flag Indicators (DIAG_STAT)
+			if (buffer.diag_stat != 0) {
+				perf_count(_bad_status_perf);
+				PX4_DEBUG("Error: DIAG_STAT: 0x%02x", buffer.diag_stat);
+				PrintErrorFlags(buffer.diag_stat);
+				return;
+			}
+
+			const float temperature = buffer.temp_out * 0.1f; // 1 LSB = 0.1°C
+			_px4_accel.set_temperature(temperature);
+			_px4_gyro.set_temperature(temperature);
+
+			// sensor frame is FLU, publish as FRD
+			float accel_x = buffer.x_accl_out;
+			float accel_y = -1.f * buffer.y_accl_out;
+			float accel_z = -1.f * buffer.z_accl_out;
+			float gyro_x = buffer.x_gyro_out;
+			float gyro_y = -1.f * buffer.y_gyro_out;
+			float gyro_z = -1.f * buffer.z_gyro_out;
+
+			// Group Delay with No Filtering: Accelerometer 1.57 ms
+			const uint64_t accel_group_delay_us = 1'570;
+			_px4_accel.update(timestamp_sample - accel_group_delay_us, accel_x, accel_y, accel_z);
+
+			// Group Delay with No Filtering:
+			//  Gyroscope (X-Axis) 1.51 ms
+			//  Gyroscope (Y-Axis) 1.51 ms
+			//  Gyroscope (Z-Axis) 1.29 ms
+			const uint64_t gyro_group_delay_us = (1'510 + 1'510 + 1'290) / 3;
+			_px4_gyro.update(timestamp_sample - gyro_group_delay_us, gyro_x, gyro_y, gyro_z);
+
+			if (_failure_count > 0) {
+				_failure_count--;
 			}
 		}
 
@@ -378,52 +386,49 @@ void ADIS16507::RunImpl()
 	}
 }
 
-bool ADIS16507::Configure()
+void ADIS16507::PrintErrorFlags(uint16_t flags)
 {
-	// first set and clear all configured register bits
-	for (const auto &reg_cfg : _register_cfg) {
-		RegisterSetAndClearBits(reg_cfg.reg, reg_cfg.set_bits, reg_cfg.clear_bits);
+	if (flags & (1 << 10)) {
+		PX4_DEBUG("Accelerometer failure");
 	}
 
-	// now check that all are configured
-	bool success = true;
-
-	for (const auto &reg_cfg : _register_cfg) {
-		if (!RegisterCheck(reg_cfg)) {
-			success = false;
-		}
+	if (flags & (1 << 9)) {
+		PX4_DEBUG("Gyro 2 failure");
 	}
 
-	// accel: ±392 m/sec^2
-	_px4_accel.set_range(392);
-	_px4_accel.set_scale(392.f / 32'000.f); // 32,000 -> 392 m/sec^2
-
-	// Gyroscope measurement range
-	// Range Identifier (RANG_MDL)
-	const uint16_t RANG_MDL = RegisterRead(Register::RANG_MDL);
-
-	// sanity check RANG_MDL [1:0] Reserved, binary value = 11
-	if (RANG_MDL & (Bit1 | Bit0)) {
-		const uint16_t gyro_range = (RANG_MDL & (Bit3 | Bit2)) >> 2;
-
-		if (gyro_range == 0b11) {
-			// 11 = ±2000°/sec (ADIS16507-3BMLZ)
-			_px4_gyro.set_range(math::radians(2000.f));
-			_px4_gyro.set_scale(math::radians(1.f / 10.f)); // scaling 10 LSB/°/sec -> rad/s per LSB
-
-		} else if (gyro_range == 0b01) {
-			// 01 = ±500°/sec (ADIS16507-2BMLZ)
-			_px4_gyro.set_range(math::radians(500.f));
-			_px4_gyro.set_scale(math::radians(1.f / 40.f)); // scaling 40 LSB/°/sec -> rad/s per LSB
-
-		} else if (gyro_range == 0b00) {
-			// 00 = ±125°/sec (ADIS16507-1BMLZ)
-			_px4_gyro.set_range(math::radians(500.f));
-			_px4_gyro.set_scale(math::radians(1.f / 40.f)); // scaling 40 LSB/°/sec -> rad/s per LSB
-		}
+	if (flags & (1 << 8)) {
+		PX4_DEBUG("Gyro 1 failure");
 	}
 
-	return success;
+	if (flags & (1 << 7)) {
+		PX4_DEBUG("Clock error");
+	}
+
+	if (flags & (1 << 6)) {
+		PX4_DEBUG("Memory failure");
+	}
+
+	if (flags & (1 << 5)) {
+		PX4_DEBUG("Sensor failure");
+	}
+
+	if (flags & (1 << 4)) {
+		PX4_DEBUG("Standby mode (VDD < 2.8V)");
+	}
+
+	if (flags & (1 << 3)) {
+		PX4_DEBUG("SPI communication error");
+	}
+
+	if (flags & (1 << 2)) {
+		PX4_DEBUG("Flash memory update failure");
+	}
+
+	if (flags & (1 << 1)) {
+		PX4_DEBUG("Data path overrun");
+	}
+
+	// Bit 0 and 15:11 are reserved and not printed
 }
 
 int ADIS16507::DataReadyInterruptCallback(int irq, void *context, void *arg)
@@ -457,26 +462,7 @@ bool ADIS16507::DataReadyInterruptDisable()
 	return px4_arch_gpiosetevent(_drdy_gpio, false, false, false, nullptr, nullptr) == 0;
 }
 
-bool ADIS16507::RegisterCheck(const register_config_t &reg_cfg)
-{
-	bool success = true;
-
-	const uint16_t reg_value = RegisterRead(reg_cfg.reg);
-
-	if (reg_cfg.set_bits && ((reg_value & reg_cfg.set_bits) != reg_cfg.set_bits)) {
-		PX4_DEBUG("0x%02hhX: 0x%02hhX (0x%02hhX not set)", (uint8_t)reg_cfg.reg, reg_value, reg_cfg.set_bits);
-		success = false;
-	}
-
-	if (reg_cfg.clear_bits && ((reg_value & reg_cfg.clear_bits) != 0)) {
-		PX4_DEBUG("0x%02hhX: 0x%02hhX (0x%02hhX not cleared)", (uint8_t)reg_cfg.reg, reg_value, reg_cfg.clear_bits);
-		success = false;
-	}
-
-	return success;
-}
-
-uint16_t ADIS16507::RegisterRead(Register reg)
+uint16_t ADIS16507::RegisterRead(uint16_t reg)
 {
 	set_frequency(SPI_SPEED);
 
@@ -490,26 +476,39 @@ uint16_t ADIS16507::RegisterRead(Register reg)
 	return cmd[0];
 }
 
-void ADIS16507::RegisterWrite(Register reg, uint16_t value)
+void ADIS16507::RegisterWrite(uint16_t reg, uint16_t val)
 {
 	set_frequency(SPI_SPEED);
 
 	uint16_t cmd[2];
-	cmd[0] = (((static_cast<uint16_t>(reg))     | DIR_WRITE) << 8) | ((0x00FF & value));
-	cmd[1] = (((static_cast<uint16_t>(reg) + 1) | DIR_WRITE) << 8) | ((0xFF00 & value) >> 8);
+	cmd[0] = (((static_cast<uint16_t>(reg))     | DIR_WRITE) << 8) | ((0x00FF & val));
+	cmd[1] = (((static_cast<uint16_t>(reg) + 1) | DIR_WRITE) << 8) | ((0xFF00 & val) >> 8);
 
 	transferhword(cmd, nullptr, 1);
 	px4_udelay(SPI_STALL_PERIOD);
 	transferhword(cmd + 1, nullptr, 1);
 }
 
-void ADIS16507::RegisterSetAndClearBits(Register reg, uint16_t setbits, uint16_t clearbits)
+bool ADIS16507::RegisterCheck(uint16_t reg, uint16_t val)
 {
-	const uint16_t orig_val = RegisterRead(reg);
+	const uint16_t actual = RegisterRead(reg);
 
-	uint16_t val = (orig_val & ~clearbits) | setbits;
-
-	if (orig_val != val) {
-		RegisterWrite(reg, val);
+	if (actual != val) {
+		PX4_WARN("register 0x%02hhX: 0x%02hhX (should be 0x%02hhX)", reg, actual, val);
+		return false;
 	}
+
+	return true;
+}
+
+void ADIS16507::print_status()
+{
+	I2CSPIDriverBase::print_status();
+
+	perf_print_counter(_reset_perf);
+	perf_print_counter(_bad_transfer_perf);
+	perf_print_counter(_bad_status_perf);
+	perf_print_counter(_bad_checksum_perf);
+	perf_print_counter(_bad_data_cntr_perf);
+	perf_print_counter(_drdy_missed_perf);
 }

src/drivers/imu/analog_devices/adis16507/ADIS16507.hpp

@@ -70,15 +70,9 @@ public:
 private:
 	void exit_and_cleanup() override;
 
-	struct register_config_t {
-		Register reg;
-		uint16_t set_bits{0};
-		uint16_t clear_bits{0};
-	};
-
 	int probe() override;
 
-	bool Reset();
+	void Reset();
 
 	bool Configure();
 
@@ -87,11 +81,11 @@ private:
 	bool DataReadyInterruptConfigure();
 	bool DataReadyInterruptDisable();
 
-	bool RegisterCheck(const register_config_t &reg_cfg);
+	uint16_t RegisterRead(uint16_t reg);
+	void RegisterWrite(uint16_t reg, uint16_t val);
+	bool RegisterCheck(uint16_t reg, uint16_t val);
 
-	uint16_t RegisterRead(Register reg);
-	void RegisterWrite(Register reg, uint16_t value);
-	void RegisterSetAndClearBits(Register reg, uint16_t setbits, uint16_t clearbits);
+	void PrintErrorFlags(uint16_t flags);
 
 	const spi_drdy_gpio_t _drdy_gpio;
 
@@ -99,15 +93,16 @@ private:
 	PX4Gyroscope _px4_gyro;
 
 	perf_counter_t _reset_perf{perf_alloc(PC_COUNT, MODULE_NAME": reset")};
-	perf_counter_t _bad_register_perf{perf_alloc(PC_COUNT, MODULE_NAME": bad register")};
 	perf_counter_t _bad_transfer_perf{perf_alloc(PC_COUNT, MODULE_NAME": bad transfer")};
-	perf_counter_t _perf_crc_bad{perf_counter_t(perf_alloc(PC_COUNT, MODULE_NAME": CRC16 bad"))};
+	perf_counter_t _bad_status_perf{perf_alloc(PC_COUNT, MODULE_NAME": bad status")};
+	perf_counter_t _bad_checksum_perf{perf_counter_t(perf_alloc(PC_COUNT, MODULE_NAME": bad checksum"))};
+	perf_counter_t _bad_data_cntr_perf{perf_counter_t(perf_alloc(PC_COUNT, MODULE_NAME": bad data count"))};
 	perf_counter_t _drdy_missed_perf{nullptr};
 
-	hrt_abstime _reset_timestamp{0};
-	hrt_abstime _last_config_check_timestamp{0};
 	int _failure_count{0};
 
+	uint16_t _last_data_cntr{65535};
+
 	px4::atomic<hrt_abstime> _drdy_timestamp_sample{0};
 	bool _data_ready_interrupt_enabled{false};
 
@@ -120,11 +115,4 @@ private:
 		CONFIGURE,
 		READ,
 	} _state{STATE::RESET};
-
-	uint8_t _checked_register{0};
-	static constexpr uint8_t size_register_cfg{1};
-	register_config_t _register_cfg[size_register_cfg] {
-		// Register              | Set bits, Clear bits
-		{ Register::MSC_CTRL,    0, MSC_CTRL_BIT::DR_polarity },
-	};
 };

src/drivers/imu/analog_devices/adis16507/Analog_Devices_ADIS16507_registers.hpp

@@ -64,35 +64,31 @@ namespace Analog_Devices_ADIS16507
 {
 static constexpr uint32_t SPI_SPEED = 2 * 1000 * 1000;       // 2 MHz SPI serial interface
 static constexpr uint32_t SPI_SPEED_BURST = 1 * 1000 * 1000; // 1 MHz SPI serial interface for burst read
-
 static constexpr uint32_t SPI_STALL_PERIOD = 16; // 16 us Stall period between data
 
-static constexpr uint16_t DIR_WRITE = 0x80;
-
-static constexpr uint16_t Product_identification = 0x407B;
-
 static constexpr uint32_t SAMPLE_INTERVAL_US = 500; // 2000 Hz
 
-enum class Register : uint16_t {
-	DIAG_STAT      = 0x02,
+static constexpr uint16_t DIR_WRITE = 0x80;
 
-	FILT_CTRL      = 0x5C,
+static constexpr uint16_t BURST_READ_CMD = 0x6800;
 
-	RANG_MDL       = 0x5E,
 
-	MSC_CTRL       = 0x60,
+namespace Register
+{
+static constexpr uint16_t DIAG_STAT = 0x02;
+static constexpr uint16_t FILT_CTRL = 0x5C;
+static constexpr uint16_t RANG_MDL = 0x5E;
+static constexpr uint16_t MSC_CTRL = 0x60;
+static constexpr uint16_t GLOB_CMD = 0x68;
+static constexpr uint16_t FIRM_REV = 0x6C; // Identification, firmware revision
+static constexpr uint16_t FIRM_DM = 0x6E; // Identification, date code, day and month
+static constexpr uint16_t FIRM_Y = 0x70; // Identification, date code, year
+static constexpr uint16_t PROD_ID = 0x72; // Identification, part number
+static constexpr uint16_t SERIAL_NUM = 0x74; // Identification, serial number
+static constexpr uint16_t FLSHCNT_LOW = 0x7C; // Output, flash memory write cycle counter, lower word
+static constexpr uint16_t FLSHCNT_HIGH = 0x7E; // Output, flash memory write cycle counter, upper word
 
-	GLOB_CMD       = 0x68,
-
-	FIRM_REV       = 0x6C, // Identification, firmware revision
-	FIRM_DM        = 0x6E, // Identification, date code, day and month
-	FIRM_Y         = 0x70, // Identification, date code, year
-	PROD_ID        = 0x72, // Identification, part number
-	SERIAL_NUM     = 0x74, // Identification, serial number
-
-	FLSHCNT_LOW    = 0x7C, // Output, flash memory write cycle counter, lower word
-	FLSHCNT_HIGH   = 0x7E, // Output, flash memory write cycle counter, upper word
-};
+static constexpr uint16_t PROD_ID_EXPECTED = 0x407B;
 
 // DIAG_STAT
 enum DIAG_STAT_BIT : uint16_t {
@@ -116,20 +112,17 @@ enum FILT_CTRL_BIT : uint16_t {
 // MSC_CTRL
 enum MSC_CTRL_BIT : uint16_t {
 	BURST32     = Bit9, // 32-bit burst enable bit
-
 	GYRO_COMP   = Bit7, // Linear acceleration compensation for gyroscopes
-
 	DR_polarity = Bit0, // 1 = active high when data is valid
 };
 
 // GLOB_CMD
 enum GLOB_CMD_BIT : uint16_t {
 	Software_reset    = Bit7,
-
 	Flash_memory_test = Bit4,
-
 	Sensor_self_test  = Bit2,
 };
 
+} // namespace Register
 
 } // namespace Analog_Devices_ADIS16507

src/drivers/magnetometer/Kconfig

@@ -12,7 +12,6 @@ menu "Magnetometer"
         select DRIVERS_MAGNETOMETER_LIS3MDL
         select DRIVERS_MAGNETOMETER_LSM303AGR
         select DRIVERS_MAGNETOMETER_RM3100
-        select DRIVERS_MAGNETOMETER_VTRANTECH_VCM1193L
         select DRIVERS_MAGNETOMETER_MEMSIC_MMC5983MA
         select DRIVERS_MAGNETOMETER_ST_IIS2MDC
         ---help---

src/drivers/osd/msp_osd/MessageDisplay/MessageDisplay.cpp

@@ -56,6 +56,13 @@ void MessageDisplay::set(const MessageDisplayType mode, const char *string)
 		if (strcmp(flight_mode_msg, string) != 0) {
 			flight_mode_msg[MSG_BUFFER_SIZE - 1] = '\0';
 			strncpy(flight_mode_msg, string, MSG_BUFFER_SIZE - 1);
+
+			for (int i = 0; i < (MSG_BUFFER_SIZE - 1); ++i) {
+				if (flight_mode_msg[i] >= 'a' && flight_mode_msg[i] <= 'z') {
+					flight_mode_msg[i] = flight_mode_msg[i] - 32; // toupper
+				}
+			}
+
 			updated_ = true;
 		}
 

src/drivers/osd/msp_osd/MspV1.cpp

@@ -39,6 +39,7 @@
 #include <float.h>
 #include <string.h>
 #include <math.h>
+#include <stdio.h>
 
 #include <drivers/drv_pwm_output.h>
 #include <drivers/drv_hrt.h>
@@ -62,7 +63,7 @@ struct msp_message_descriptor_t {
 	uint8_t message_size;
 };
 
-#define MSP_DESCRIPTOR_COUNT 11
+#define MSP_DESCRIPTOR_COUNT 12
 const msp_message_descriptor_t msp_message_descriptors[MSP_DESCRIPTOR_COUNT] = {
 	{MSP_OSD_CONFIG, true, sizeof(msp_osd_config_t)},
 	{MSP_NAME, true, sizeof(msp_name_t)},
@@ -75,10 +76,9 @@ const msp_message_descriptor_t msp_message_descriptors[MSP_DESCRIPTOR_COUNT] = {
 	{MSP_COMP_GPS, true, sizeof(msp_comp_gps_t)},
 	{MSP_ESC_SENSOR_DATA, true, sizeof(msp_esc_sensor_data_dji_t)},
 	{MSP_MOTOR_TELEMETRY, true, sizeof(msp_motor_telemetry_t)},
+	{MSP_RC, true, sizeof(msp_rc_t)},
 };
 
-#define MSP_FRAME_START_SIZE 5
-#define MSP_CRC_SIZE 1
 bool MspV1::Send(const uint8_t message_id, const void *payload)
 {
 	uint32_t payload_size = 0;
@@ -107,7 +107,7 @@ bool MspV1::Send(const uint8_t message_id, const void *payload)
 
 	packet[0] = '$';
 	packet[1] = 'M';
-	packet[2] = '<';
+	packet[2] = '>';
 	packet[3] = payload_size;
 	packet[4] = message_id;
 
@@ -124,3 +124,101 @@ bool MspV1::Send(const uint8_t message_id, const void *payload)
 	int packet_size =  MSP_FRAME_START_SIZE + payload_size + MSP_CRC_SIZE;
 	return  write(_fd, packet, packet_size) == packet_size;
 }
+
+bool MspV1::Send(const uint8_t message_id, const void *payload, uint32_t payload_size)
+{
+	uint8_t packet[MSP_FRAME_START_SIZE + payload_size + MSP_CRC_SIZE];
+	uint8_t crc;
+
+	packet[0] = '$';
+	packet[1] = 'M';
+	packet[2] = '>';
+	packet[3] = payload_size;
+	packet[4] = message_id;
+
+	crc = payload_size ^ message_id;
+
+	memcpy(packet + MSP_FRAME_START_SIZE, payload, payload_size);
+
+	for (uint32_t i = 0; i < payload_size; i ++) {
+		crc ^= packet[MSP_FRAME_START_SIZE + i];
+	}
+
+	packet[MSP_FRAME_START_SIZE + payload_size] = crc;
+
+	int packet_size =  MSP_FRAME_START_SIZE + payload_size + MSP_CRC_SIZE;
+	return  write(_fd, packet, packet_size) == packet_size;
+}
+
+
+int MspV1::Receive(uint8_t *payload, uint8_t *message_id)
+{
+	uint8_t payload_size;
+	uint8_t crc;
+	uint8_t calc_crc;
+	int ret;
+
+	while (!has_header) {
+		int bytes_available = 0;
+
+		if (ioctl(_fd, FIONREAD, &bytes_available) < 0) {
+			return -EIO;
+		}
+
+		if (bytes_available < 5) {
+			return -EWOULDBLOCK;
+		}
+
+		while (bytes_available > 4) {
+			if ((ret = read(_fd, header, 1)) != 1) {
+				return ret;
+			}
+
+			bytes_available--;
+
+			if (header[0] == '$') {
+				break;
+			}
+
+		}
+
+		if (header[0] != '$') {
+			return -EWOULDBLOCK;
+		}
+
+		if ((ret = read(_fd, &header[1], 4)) != 4) {
+			return ret;
+		}
+
+		if (header[0] == '$' && header[1] == 'M' && header[2] == '<') {
+			has_header = true;
+		}
+	}
+
+	payload_size = header[3];
+	*message_id = header[4];
+
+	if ((ret = read(_fd, payload, payload_size + MSP_CRC_SIZE)) != payload_size + MSP_CRC_SIZE) {
+		if (ret != -EWOULDBLOCK) {
+			has_header = false;
+		}
+
+		return ret;
+	}
+
+	has_header = false;
+
+	crc = payload[payload_size];
+
+	calc_crc = payload_size ^ header[4];
+
+	for (int i = 0; i < payload_size; i++) {
+		calc_crc ^= payload[i];
+	}
+
+	if (calc_crc != crc) {
+		return -EINVAL;
+	}
+
+	return payload_size;
+}

src/drivers/osd/msp_osd/MspV1.hpp

@@ -33,13 +33,20 @@
 
 #pragma once
 
+#define MSP_FRAME_START_SIZE 5
+#define MSP_CRC_SIZE 1
+
 class MspV1
 {
 public:
 	MspV1(int fd);
 	int GetMessageSize(int message_type);
 	bool Send(const uint8_t message_id, const void *payload);
+	bool Send(const uint8_t message_id, const void *payload, uint32_t payload_size);
+	int Receive(uint8_t *payload, uint8_t *message_id);
 
 private:
 	int _fd{-1};
+	uint8_t header[MSP_FRAME_START_SIZE + MSP_CRC_SIZE];
+	bool has_header{false};
 };

src/drivers/osd/msp_osd/module.yaml

@@ -97,3 +97,14 @@ parameters:
             min: 100
             max: 10000
             default: 500
+
+        # RC Stick input
+        OSD_RC_STICK:
+            description:
+                short: OSD RC Stick commands
+                long: |
+                    Forward RC stick input to VTX when disarmed
+            type: int32
+            min: 0
+            max: 1
+            default: 1

src/drivers/osd/msp_osd/msp_defines.h

@@ -48,6 +48,8 @@
 #define MSP_ARMING_CONFIG         61
 #define MSP_RX_MAP                64 // get channel map (also returns number of channels total)
 #define MSP_LOOP_TIME             73 // FC cycle time i.e looptime parameter
+#define MSP_GET_VTX_CONFIG        88
+#define MSP_SET_VTX_CONFIG        89
 #define MSP_STATUS               101
 #define MSP_RAW_IMU              102
 #define MSP_SERVO                103
@@ -75,10 +77,12 @@
 #define MSP_SET_PID              202 // set P I D coeff
 
 // commands
-#define MSP_SET_HEAD             211 // define a new heading hold direction
-#define MSP_SET_RAW_RC           200 // 8 rc chan
-#define MSP_SET_RAW_GPS          201 // fix, numsat, lat, lon, alt, speed
-#define MSP_SET_WP               209 // sets a given WP (WP#, lat, lon, alt, flags)
+#define MSP_SET_HEAD                211 // define a new heading hold direction
+#define MSP_SET_RAW_RC              200 // 8 rc chan
+#define MSP_SET_RAW_GPS             201 // fix, numsat, lat, lon, alt, speed
+#define MSP_SET_WP                  209 // sets a given WP (WP#, lat, lon, alt, flags)
+#define MSP_SET_VTXTABLE_BAND       227
+#define MSP_SET_VTXTABLE_POWERLEVEL 228
 
 // bits of getActiveModes() return value
 #define MSP_MODE_ARM          0
@@ -111,6 +115,7 @@
 #define MSP_MODE_TURNASSIST  27
 #define MSP_MODE_NAVLAUNCH   28
 #define MSP_MODE_AUTOTRIM    29
+#define MSP_CMD_DISPLAYPORT 182
 
 struct msp_esc_sensor_data_t {
 	uint8_t motor_count;
@@ -288,6 +293,25 @@ struct msp_attitude_t {
 	int16_t yaw;
 } __attribute__((packed));
 
+struct msp_rendor_pitch_t {
+	uint8_t subCommand = 0x03; // 0x03 subcommand write string. fixed
+	uint8_t screenYPosition;
+	uint8_t screenXPosition;
+	uint8_t iconAttrs = 0x00;
+	uint8_t iconIndex = 0x15; //PITCH icon
+
+	char str[6]; // -00.0
+} __attribute__((packed));
+
+struct msp_rendor_roll_t {
+	uint8_t subCommand = 0x03; // 0x03 subcommand write string. fixed
+	uint8_t screenYPosition;
+	uint8_t screenXPosition;
+	uint8_t iconAttrs = 0x00;
+	uint8_t iconIndex = 0x14; //ROLL icon
+
+	char str[6]; // -00.0
+} __attribute__((packed));
 
 // MSP_ALTITUDE reply
 struct msp_altitude_t {
@@ -297,6 +321,17 @@ struct msp_altitude_t {
 } __attribute__((packed));
 
 
+struct msp_rendor_altitude_t {
+	uint8_t subCommand = 0x03; // 0x03 subcommand write string. fixed
+	uint8_t screenYPosition;
+	uint8_t screenXPosition;
+	uint8_t iconAttrs = 0x00;
+	uint8_t iconIndex = 0x7F; //ALT icon
+
+	char str[8]; // -0000.0 // 9999.9 meter
+} __attribute__((packed));
+
+
 // MSP_SONAR_ALTITUDE reply
 struct msp_sonar_altitude_t {
 	int32_t altitude;
@@ -311,6 +346,16 @@ struct msp_analog_t {
 	int16_t  amperage; // send amperage in 0.01 A steps, range is -320A to 320A
 } __attribute__((packed));
 
+struct msp_rendor_rssi_t {
+	uint8_t subCommand = 0x03; // 0x03 subcommand write string. fixed
+	uint8_t screenYPosition;
+	uint8_t screenXPosition;
+	uint8_t iconAttrs = 0x00;
+	uint8_t iconIndex = 0x01; //RSSI icon
+
+	char str[4]; // 100%
+} __attribute__((packed));
+
 
 // MSP_ARMING_CONFIG reply
 struct msp_arming_config_t {
@@ -392,6 +437,38 @@ struct msp_raw_gps_t {
 	uint16_t hdop;
 } __attribute__((packed));
 
+struct msp_rendor_latitude_t {
+	uint8_t subCommand = 0x03; // 0x03 subcommand write string. fixed
+	uint8_t screenYPosition;
+	uint8_t screenXPosition;
+	uint8_t iconAttrs = 0x00;
+	uint8_t iconIndex = 0x89; //LAT icon
+
+	char str[11]; // -00.0000000
+} __attribute__((packed));
+
+
+struct msp_rendor_longitude_t {
+	uint8_t subCommand = 0x03; // 0x03 subcommand write string. fixed
+	uint8_t screenYPosition;
+	uint8_t screenXPosition;
+	uint8_t iconAttrs = 0x00;
+	uint8_t iconIndex = 0x98; //LON icon
+
+	char str[12]; // -000.0000000
+} __attribute__((packed));
+
+struct msp_rendor_satellites_used_t {
+	uint8_t subCommand = 0x03; // 0x03 subcommand write string. fixed
+	uint8_t screenYPosition;
+	uint8_t screenXPosition;
+	uint8_t iconAttrs = 0x00;
+	uint8_t iconIndex = 0x1E; //satellites icon
+	uint8_t iconIndex2 = 0x1F; //satellites icon
+
+	char str[3]; // 99
+} __attribute__((packed));
+
 
 // MSP_COMP_GPS reply
 struct msp_comp_gps_t {
@@ -400,6 +477,16 @@ struct msp_comp_gps_t {
 	uint8_t  heartbeat;       // toggles 0 and 1 for each change
 } __attribute__((packed));
 
+struct msp_rendor_distanceToHome_t {
+	uint8_t subCommand = 0x03; // 0x03 subcommand write string. fixed
+	uint8_t screenYPosition;
+	uint8_t screenXPosition;
+	uint8_t iconAttrs = 0x00; //
+	uint8_t iconIndex = 0x71; //distanceToHome icon
+
+	char str[6]; // 65536
+} __attribute__((packed));
+
 
 // values for msp_nav_status_t.mode
 #define MSP_NAV_STATUS_MODE_NONE   0
@@ -788,6 +875,15 @@ struct msp_battery_state_t {
 	uint16_t batteryVoltage;
 } __attribute__((packed));
 
+struct msp_rendor_battery_state_t {
+	uint8_t subCommand; // 0x03 write string. fixed
+	uint8_t screenYPosition;
+	uint8_t screenXPosition;
+	uint8_t iconAttrs;
+	uint8_t iconIndex;
+	char str[5];
+} __attribute__((packed));
+
 // MSP_STATUS reply customized for BF/DJI
 struct msp_status_BF_t {
 	uint16_t task_delta_time;
@@ -803,6 +899,58 @@ struct msp_status_BF_t {
 	uint8_t  extra_flags;
 } __attribute__((packed));
 
+struct msp_set_vtx_config_t {
+	uint16_t new_freq; //  if setting frequency then full uint16 is the frequency in MHz (ie. 5800)
+	//if setting band channel than band is high 8 bits and channel is low 8 bits
+	uint8_t power_level;
+	uint8_t pit_mode; // 0 = off, 1 = on
+	uint8_t low_power_disarm;
+	uint16_t pit_freq;
+	uint8_t user_band;
+	uint8_t user_channel;
+	uint16_t user_freq; // in MHz, 0 if using band & channel
+	uint8_t band_count;
+	uint8_t channel_count;
+	uint8_t power_count;
+	uint8_t clear_vtxtable; // Bool
+} __attribute__((packed));
+
+struct msp_get_vtx_config_t {
+	uint8_t vtx_type;
+	uint8_t band;
+	uint8_t channel;
+	uint8_t power_index;
+	uint8_t pit_mode; // 0 = off, 1 = on
+	uint16_t freq; // in MHz, 0 if using band & channel
+	uint8_t device_ready;
+	uint8_t low_power_disarm;
+} __attribute__((packed));
+
+struct msp_set_vtxtable_powerlevel_t {
+	uint8_t index;
+	uint16_t power_value;
+	uint8_t power_label_length;
+	uint8_t power_label_name[3];
+} __attribute__((packed));
+
+#define VTX_TABLE_BAND_NAME_LENGTH 8
+#define VTXDEV_MSP 5
+
+//29 bytes
+
+struct msp_set_vtxtable_band_t {
+	uint8_t band;
+	uint8_t band_name_length;
+	uint8_t band_label_name[VTX_TABLE_BAND_NAME_LENGTH];
+	uint8_t band_letter;
+	uint8_t is_factory_band;
+	uint8_t channel_count;
+	uint16_t frequency[8];
+} __attribute__((packed));
+
+
+
+
 ////ArduPlane
 enum arduPlaneModes_e {
 	MANUAL        = 0,

src/drivers/osd/msp_osd/msp_osd.cpp

@@ -47,6 +47,7 @@
 #include <unistd.h>
 #include <termios.h>
 #include <string.h>
+#include <ctype.h>
 
 #include <px4_platform_common/getopt.h>
 #include <px4_platform_common/log.h>
@@ -111,6 +112,21 @@ const uint16_t osd_current_draw_pos = 2103;
 
 const uint16_t osd_numerical_vario_pos = LOCATION_HIDDEN;
 
+#define OSD_GRID_COL_MAX (59) // From betaflight-configurator OSD tab
+#define OSD_GRID_ROW_MAX (21) // From betaflight-configurator OSD tab
+
+typedef enum {
+	MSP_DP_HEARTBEAT = 0,         // Release the display after clearing and updating
+	MSP_DP_RELEASE = 1,         // Release the display after clearing and updating
+	MSP_DP_CLEAR_SCREEN = 2,    // Clear the display
+	MSP_DP_WRITE_STRING = 3,    // Write a string at given coordinates
+	MSP_DP_DRAW_SCREEN = 4,     // Trigger a screen draw
+	MSP_DP_OPTIONS = 5,         // Not used by Betaflight. Reserved by Ardupilot and INAV
+	MSP_DP_SYS = 6,             // Display system element displayportSystemElement_e at given coordinates
+	MSP_DP_COUNT,
+} displayportMspSubCommand;
+
+
 MspOsd::MspOsd(const char *device) :
 	ModuleParams(nullptr),
 	ScheduledWorkItem(MODULE_NAME, px4::wq_configurations::lp_default)
@@ -225,6 +241,7 @@ void MspOsd::SendConfig()
 	_msp.Send(MSP_OSD_CONFIG, &msp_osd_config);
 }
 
+// extract it to MSPOSD_BF_Run() and MSPOSD_DJIFPV_Run() for compatibility?
 void MspOsd::Run()
 {
 	if (should_exit()) {
@@ -265,11 +282,34 @@ void MspOsd::Run()
 		_is_initialized = true;
 	}
 
+	if (change_channel) {
+		msp_get_vtx_config_t vtx_set_config{0};
+		vtx_set_config.low_power_disarm = vtx_config.low_power_disarm;
+		vtx_set_config.pit_mode = vtx_config.pit_mode;
+		vtx_set_config.vtx_type = VTXDEV_MSP;
+		vtx_set_config.band = vtx_config.user_band;
+		vtx_set_config.channel = vtx_config.user_channel;
+		this->Send(MSP_GET_VTX_CONFIG, &vtx_set_config, sizeof(msp_get_vtx_config_t));
+		change_channel = false;
+	}
+
+	this->Receive();
+
+	if (!has_vtx_config) {
+		this->Send(MSP_GET_VTX_CONFIG, nullptr, 0);
+	}
+
 	// avoid premature pessimization; if skip processing if we're effectively disabled
 	if (_param_osd_symbols.get() == 0) {
 		return;
 	}
 
+	uint8_t subcmd = MSP_DP_HEARTBEAT;
+	this->Send(MSP_CMD_DISPLAYPORT, &subcmd, 1);
+
+	subcmd = MSP_DP_CLEAR_SCREEN;
+	this->Send(MSP_CMD_DISPLAYPORT, &subcmd, 1);
+
 	// update display message
 	{
 		vehicle_status_s vehicle_status{};
@@ -280,43 +320,39 @@ void MspOsd::Run()
 
 		log_message_s log_message{};
 		_log_message_sub.copy(&log_message);
-
+		// TODO re-wirte this function?
 		const auto display_message = msp_osd::construct_display_message(
 						     vehicle_status,
 						     vehicle_attitude,
 						     log_message,
 						     _param_osd_log_level.get(),
 						     _display);
-		this->Send(MSP_NAME, &display_message);
+
+		char msg[sizeof(msp_name_t) + 5] = {0};
+		int index = 0;
+		msg[index++] = MSP_DP_WRITE_STRING;
+		msg[index++] = 0x02; // row position
+		msg[index++] = 0x14; // colum position
+		msg[index++] = 0;		// Icon attr
+		msg[index++] = 0x03; // Icon index >
+		memcpy(&msg[index++], &display_message, sizeof(msp_name_t));
+		this->Send(MSP_CMD_DISPLAYPORT, &msg, sizeof(msg));
 	}
 
 	// MSP_FC_VARIANT
 	{
 		const auto msg = msp_osd::construct_FC_VARIANT();
-		this->Send(MSP_FC_VARIANT, &msg);
-	}
-
-	// MSP_STATUS
-	{
-		vehicle_status_s vehicle_status{};
-		_vehicle_status_sub.copy(&vehicle_status);
-
-		const auto msg = msp_osd::construct_STATUS(vehicle_status);
-		this->Send(MSP_STATUS, &msg);
+		this->Send(MSP_FC_VARIANT, &msg, sizeof(msg));
 	}
 
 	// MSP_ANALOG
 	{
-		battery_status_s battery_status{};
-		_battery_status_sub.copy(&battery_status);
-
-		input_rc_s input_rc{};
-		_input_rc_sub.copy(&input_rc);
-
-		const auto msg = msp_osd::construct_ANALOG(
-					 battery_status,
-					 input_rc);
-		this->Send(MSP_ANALOG, &msg);
+		if (enabled(SymbolIndex::RSSI_VALUE)) {
+			input_rc_s input_rc{};
+			_input_rc_sub.copy(&input_rc);
+			const auto msg = msp_osd::construct_rendor_RSSI(input_rc);
+			this->Send(MSP_CMD_DISPLAYPORT, &msg, sizeof(msp_rendor_rssi_t));
+		}
 	}
 
 	// MSP_BATTERY_STATE
@@ -324,8 +360,12 @@ void MspOsd::Run()
 		battery_status_s battery_status{};
 		_battery_status_sub.copy(&battery_status);
 
-		const auto msg = msp_osd::construct_BATTERY_STATE(battery_status);
-		this->Send(MSP_BATTERY_STATE, &msg);
+		const auto msg_original = msp_osd::construct_BATTERY_STATE(battery_status);
+		this->Send(MSP_BATTERY_STATE, &msg_original);
+
+		const auto msg = msp_osd::construct_rendor_BATTERY_STATE(battery_status);
+		this->Send(MSP_CMD_DISPLAYPORT, &msg, sizeof(msp_rendor_battery_state_t));
+
 	}
 
 	// MSP_RAW_GPS
@@ -333,32 +373,35 @@ void MspOsd::Run()
 		sensor_gps_s vehicle_gps_position{};
 		_vehicle_gps_position_sub.copy(&vehicle_gps_position);
 
-		airspeed_validated_s airspeed_validated{};
-		_airspeed_validated_sub.copy(&airspeed_validated);
+		if (enabled(SymbolIndex::GPS_LAT)) {
+			const auto msg = msp_osd::construct_rendor_GPS_LAT(vehicle_gps_position);
+			this->Send(MSP_CMD_DISPLAYPORT, &msg, sizeof(msp_rendor_latitude_t));
+		}
 
-		const auto msg = msp_osd::construct_RAW_GPS(
-					 vehicle_gps_position,
-					 airspeed_validated);
-		this->Send(MSP_RAW_GPS, &msg);
+		if (enabled(SymbolIndex::GPS_LON)) {
+			const auto msg = msp_osd::construct_rendor_GPS_LON(vehicle_gps_position);
+			this->Send(MSP_CMD_DISPLAYPORT, &msg, sizeof(msp_rendor_longitude_t));
+		}
+
+		if (enabled(SymbolIndex::GPS_SATS)) {
+			const auto msg = msp_osd::construct_rendor_GPS_NUM(vehicle_gps_position);
+			this->Send(MSP_CMD_DISPLAYPORT, &msg, sizeof(msp_rendor_satellites_used_t));
+		}
 	}
 
 	// MSP_COMP_GPS
 	{
-		// update heartbeat
-		_heartbeat = !_heartbeat;
-
 		home_position_s home_position{};
 		_home_position_sub.copy(&home_position);
 
 		vehicle_global_position_s vehicle_global_position{};
 		_vehicle_global_position_sub.copy(&vehicle_global_position);
 
-		// construct and send message
-		const auto msg = msp_osd::construct_COMP_GPS(
-					 home_position,
-					 vehicle_global_position,
-					 _heartbeat);
-		this->Send(MSP_COMP_GPS, &msg);
+		if (enabled(SymbolIndex::HOME_DIST)) {
+			const auto msg =  msp_osd::construct_rendor_distanceToHome(home_position, vehicle_global_position);
+
+			this->Send(MSP_CMD_DISPLAYPORT, &msg, sizeof(msp_rendor_distanceToHome_t));
+		}
 	}
 
 	// MSP_ATTITUDE
@@ -366,10 +409,17 @@ void MspOsd::Run()
 		vehicle_attitude_s vehicle_attitude{};
 		_vehicle_attitude_sub.copy(&vehicle_attitude);
 
-		const auto msg = msp_osd::construct_ATTITUDE(vehicle_attitude);
-		this->Send(MSP_ATTITUDE, &msg);
+		{
+			const auto msg = msp_osd::construct_rendor_PITCH(vehicle_attitude);
+			this->Send(MSP_CMD_DISPLAYPORT, &msg, sizeof(msp_rendor_pitch_t));
+		}
+		{
+			const auto msg = msp_osd::construct_rendor_ROLL(vehicle_attitude);
+			this->Send(MSP_CMD_DISPLAYPORT, &msg, sizeof(msp_rendor_roll_t));
+		}
 	}
 
+
 	// MSP_ALTITUDE
 	{
 		sensor_gps_s vehicle_gps_position{};
@@ -378,19 +428,45 @@ void MspOsd::Run()
 		vehicle_local_position_s vehicle_local_position{};
 		_vehicle_local_position_sub.copy(&vehicle_local_position);
 
-		// construct and send message
-		const auto msg = msp_osd::construct_ALTITUDE(vehicle_gps_position, vehicle_local_position);
-		this->Send(MSP_ALTITUDE, &msg);
+		if (enabled(SymbolIndex::ALTITUDE)) {
+			const auto msg = msp_osd::construct_Rendor_ALTITUDE(vehicle_gps_position, vehicle_local_position);
+
+			this->Send(MSP_CMD_DISPLAYPORT, &msg, sizeof(msp_altitude_t));
+		}
 	}
 
 	// MSP_MOTOR_TELEMETRY
 	{
-		const auto msg = msp_osd::construct_ESC_SENSOR_DATA();
-		this->Send(MSP_ESC_SENSOR_DATA, &msg);
+
 	}
 
-	// send full configuration
-	SendConfig();
+	// MSP_RC
+	{
+		if (_param_osd_rc_stick.get() == 1) {
+			vehicle_status_s vehicle_status{};
+			_vehicle_status_sub.copy(&vehicle_status);
+
+			if (vehicle_status.arming_state != vehicle_status_s::ARMING_STATE_ARMED) {
+				input_rc_s input_rc{};
+				_input_rc_sub.copy(&input_rc);
+				const auto msg = msp_osd::construct_MSP_RC(input_rc);
+				this->Send(MSP_RC, &msg, sizeof(msp_rc_t));
+			}
+		}
+
+	}
+
+	// MSP_STATUS
+	{
+		vehicle_status_s vehicle_status{};
+		_vehicle_status_sub.copy(&vehicle_status);
+
+		const auto msg = msp_osd::construct_MSP_STATUS(vehicle_status);
+		this->Send(MSP_STATUS, &msg, sizeof(msp_status_t));
+	}
+
+	subcmd = MSP_DP_DRAW_SCREEN;
+	this->Send(MSP_CMD_DISPLAYPORT, &subcmd, 1);
 }
 
 void MspOsd::Send(const unsigned int message_type, const void *payload)
@@ -402,6 +478,67 @@ void MspOsd::Send(const unsigned int message_type, const void *payload)
 		_performance_data.unsuccessful_sends++;
 	}
 }
+void MspOsd::Send(const unsigned int message_type, const void *payload, int32_t payload_size)
+{
+	if (_msp.Send(message_type, payload, payload_size)) {
+		_performance_data.successful_sends++;
+
+	} else {
+		_performance_data.unsuccessful_sends++;
+	}
+}
+
+void MspOsd::Receive()
+{
+	uint8_t packet[255];
+	uint8_t message_id;
+	int ret;
+
+	while ((ret = _msp.Receive(packet, &message_id)) != -EWOULDBLOCK) {
+		if (ret >= 0) {
+			switch (message_id) {
+
+			case MSP_SET_VTX_CONFIG: {
+					if (ret == 0xF) {
+						memcpy((void *)&vtx_config, packet, sizeof(vtx_config));
+						has_vtx_config = true;
+					}
+
+					break;
+				}
+
+			case MSP_SET_VTXTABLE_BAND: {
+					msp_set_vtxtable_band_t *band_info = (msp_set_vtxtable_band_t *)&packet[0];
+
+					// Only supported fixed name lenght and < 8 channels for now
+					if (band_info->band <= BAND_COUNT && band_info->band_name_length == 8 && band_info->channel_count <= 8) {
+						memcpy((void *)&vtx_bands[band_info->band - 1], packet, sizeof(msp_set_vtxtable_band_t));
+
+						if (has_vtx_config && band_info->band == vtx_config.band_count) {
+							has_vtx_bands = true;
+						}
+					}
+
+					break;
+				}
+
+			case MSP_SET_VTXTABLE_POWERLEVEL: {
+					if ((packet[0] - 1) < POWER_LEVEL_COUNT) {
+						memcpy((void *)&power_levels[packet[0] - 1], packet, sizeof(msp_set_vtxtable_powerlevel_t));
+						has_power_config = true;
+					}
+
+					break;
+				}
+
+			default:
+				break;
+
+			}
+		}
+	}
+
+}
 
 void MspOsd::parameters_update()
 {
@@ -483,11 +620,102 @@ int MspOsd::print_status()
 	_display.get(msg, hrt_absolute_time());
 	PX4_INFO("Current message: \n\t%s", msg);
 
+	if (has_vtx_config) {
+		PX4_INFO("=== VTX Configuration ===");
+
+		if (has_vtx_bands) {
+			PX4_INFO("Channel: %c%u", vtx_bands[vtx_config.user_band - 1].band_letter, vtx_config.user_channel);
+
+		} else {
+			PX4_INFO("Band: %u", vtx_config.user_band);
+			PX4_INFO("Channel: %u", vtx_config.user_channel);
+		}
+
+		PX4_INFO("Frequency: %u MHz", vtx_config.user_freq);
+
+		if (has_power_config && (vtx_config.power_level - 1) < POWER_LEVEL_COUNT) {
+			PX4_INFO("Transmit power: %.*s mW", power_levels[vtx_config.power_level - 1].power_label_length,
+				 power_levels[vtx_config.power_level - 1].power_label_name);
+
+		} else {
+			PX4_INFO("Power Level: %u/%u", vtx_config.power_level,  vtx_config.power_count);
+
+		}
+
+		PX4_INFO("PIT Mode: %s", vtx_config.pit_mode ? "On" : "Off");
+
+		const char *disarm_modes[] = {
+			"Off",
+			"Always",
+			"Until First Arm"
+		};
+
+		if (vtx_config.low_power_disarm < 3) {
+			PX4_INFO("Low Power Disarm: %s", disarm_modes[vtx_config.low_power_disarm]);
+
+		} else {
+			PX4_INFO("Low Power Disarm: Unknown (%u)", vtx_config.low_power_disarm);
+		}
+
+		PX4_INFO("PIT Frequency: %u MHz", vtx_config.pit_freq);
+
+	} else {
+		PX4_INFO("No VTX Configuration available, can't do channel switching");
+	}
+
 	return 0;
 }
 
+int MspOsd::set_channel(char *new_channel)
+{
+	char band_letter = toupper(new_channel[0]);
+
+	if (!has_vtx_bands) {
+		return -2;
+	}
+
+	for (int i = 0; i < BAND_COUNT; i++) {
+		if (vtx_bands[i].band != 0) {
+			if (band_letter == toupper(vtx_bands[i].band_letter)) {
+				int channel = atoi(&new_channel[1]);
+
+				if (channel > 0 && channel <= vtx_config.channel_count && vtx_bands[i].frequency[channel - 1] != 0) {
+					vtx_config.user_band = vtx_bands[i].band;
+					vtx_config.user_channel = channel;
+					vtx_config.user_freq = vtx_bands[i].frequency[channel - 1];
+					change_channel = true;
+					return 0;
+				}
+
+			}
+		}
+	}
+
+	return -1;
+}
+
 int MspOsd::custom_command(int argc, char *argv[])
 {
+	if (argc > 0 && strcmp("channel", argv[0]) == 0) {
+		if (argc == 1) {
+			PX4_INFO("Please provide a channel");
+
+		} else if (is_running() && _object.load()) {
+			MspOsd *object = _object.load();
+			int ret = object->set_channel(argv[1]);
+
+			if (ret == -1) {
+				PX4_INFO("Channel not found");
+
+			} else if (ret == -2) {
+				PX4_INFO("No VTX Channel table available");
+			}
+
+		} else {
+			PX4_INFO("not running");
+		}
+	}
+
 	return 0;
 }
 
@@ -513,6 +741,7 @@ $ msp_osd
 
 	PRINT_MODULE_USAGE_NAME("msp_osd", "driver");
 	PRINT_MODULE_USAGE_DEFAULT_COMMANDS();
+	PRINT_MODULE_USAGE_COMMAND_DESCR("channel", "Change VTX channel");
 
 	return 0;
 }

src/drivers/osd/msp_osd/msp_osd.hpp

@@ -64,6 +64,9 @@ using namespace time_literals;
 // location to "hide" unused display elements
 #define LOCATION_HIDDEN 234;
 
+#define POWER_LEVEL_COUNT 5
+#define BAND_COUNT 7
+
 struct PerformanceData {
 	bool initialization_problems{false};
 	long unsigned int successful_sends{0};
@@ -118,11 +121,17 @@ public:
 	/** @see ModuleBase::print_status() */
 	int print_status() override;
 
+	int set_channel(char *new_channel);
+
 private:
 	void Run() override;
 
 	// update a single display element in the display
 	void Send(const unsigned int message_type, const void *payload);
+	void Send(const unsigned int message_type, const void *payload, int32_t payload_size);
+
+	// receive vtx data
+	void Receive();
 
 	// send full configuration to MSP (triggers the actual update)
 	void SendConfig();
@@ -165,10 +174,19 @@ private:
 		(ParamInt<px4::params::OSD_CH_HEIGHT>) _param_osd_ch_height,
 		(ParamInt<px4::params::OSD_SCROLL_RATE>) _param_osd_scroll_rate,
 		(ParamInt<px4::params::OSD_DWELL_TIME>) _param_osd_dwell_time,
-		(ParamInt<px4::params::OSD_LOG_LEVEL>) _param_osd_log_level
+		(ParamInt<px4::params::OSD_LOG_LEVEL>) _param_osd_log_level,
+		(ParamInt<px4::params::OSD_RC_STICK>) _param_osd_rc_stick
 	)
 
 	// metadata
 	char _device[64] {};
 	PerformanceData _performance_data{};
+
+	msp_set_vtx_config_t vtx_config;
+	msp_set_vtxtable_powerlevel_t power_levels[POWER_LEVEL_COUNT];
+	msp_set_vtxtable_band_t vtx_bands[BAND_COUNT] {};
+	bool has_vtx_config {false};
+	bool has_power_config {false};
+	bool has_vtx_bands {false};
+	bool change_channel {false};
 };

src/drivers/osd/msp_osd/uorb_to_msp.cpp

@@ -51,6 +51,16 @@ using namespace time_literals;
 
 namespace msp_osd
 {
+typedef enum {
+	MSP_DP_HEARTBEAT = 0,         // Release the display after clearing and updating
+	MSP_DP_RELEASE = 1,         // Release the display after clearing and updating
+	MSP_DP_CLEAR_SCREEN = 2,    // Clear the display
+	MSP_DP_WRITE_STRING = 3,    // Write a string at given coordinates
+	MSP_DP_DRAW_SCREEN = 4,     // Trigger a screen draw
+	MSP_DP_OPTIONS = 5,         // Not used by Betaflight. Reserved by Ardupilot and INAV
+	MSP_DP_SYS = 6,             // Display system element displayportSystemElement_e at given coordinates
+	MSP_DP_COUNT,
+} displayportMspSubCommand;
 
 msp_name_t construct_display_message(const vehicle_status_s &vehicle_status,
 				     const vehicle_attitude_s &vehicle_attitude,
@@ -198,6 +208,18 @@ msp_analog_t construct_ANALOG(const battery_status_s &battery_status, const inpu
 	return analog;
 }
 
+msp_rendor_rssi_t construct_rendor_RSSI(const input_rc_s &input_rc)
+{
+	msp_rendor_rssi_t rssi;
+	rssi.screenYPosition = 0x02;
+	rssi.screenXPosition = 0x02;
+
+	snprintf(&rssi.str[0], sizeof(rssi.str), "%3d", input_rc.link_quality);
+	rssi.str[3] = '%';
+
+	return rssi;
+}
+
 msp_battery_state_t construct_BATTERY_STATE(const battery_status_s &battery_status)
 {
 	// initialize result
@@ -222,6 +244,36 @@ msp_battery_state_t construct_BATTERY_STATE(const battery_status_s &battery_stat
 	return battery_state;
 }
 
+msp_rendor_battery_state_t construct_rendor_BATTERY_STATE(const battery_status_s &battery_status)
+{
+	// initialize result
+	msp_rendor_battery_state_t battery_state = {0};
+
+	battery_state.subCommand = MSP_DP_WRITE_STRING; // 3 write string. fixed
+	battery_state.screenYPosition = 0x04;
+	battery_state.screenXPosition = 0x02;
+	battery_state.iconAttrs = 0x00;
+
+	float sigle_cell_v = battery_status.voltage_v / battery_status.cell_count;
+
+	if (sigle_cell_v > 4.0f) {
+		battery_state.iconIndex = 0x91; // Full battery Icon
+
+	} else if ((sigle_cell_v <= 4.0f) && (sigle_cell_v > 3.5f)) {
+		battery_state.iconIndex = 0x93; // Half battery Icon
+
+	} else if ((sigle_cell_v <= 3.5f) && (sigle_cell_v > 3.2f)) {
+		battery_state.iconIndex = 0x95; // Empty battery Icon
+
+	} else {
+		battery_state.iconIndex = 0x96; // Dead battery Icon
+	}
+
+	snprintf(&battery_state.str[0], sizeof(battery_state.str), "%.1fV", (double)sigle_cell_v);
+	return battery_state;
+}
+
+
 msp_raw_gps_t construct_RAW_GPS(const sensor_gps_s &vehicle_gps_position,
 				const airspeed_validated_s &airspeed_validated)
 {
@@ -279,6 +331,54 @@ msp_raw_gps_t construct_RAW_GPS(const sensor_gps_s &vehicle_gps_position,
 	return raw_gps;
 }
 
+msp_rendor_latitude_t construct_rendor_GPS_LAT(const sensor_gps_s &vehicle_gps_position)
+{
+	msp_rendor_latitude_t lat;
+
+	lat.screenYPosition = 0x0A;
+	lat.screenXPosition = 0x29;
+
+	if (vehicle_gps_position.fix_type >= 2) {
+		snprintf(&lat.str[0], sizeof(lat.str), "%.6f", vehicle_gps_position.latitude_deg);
+
+	} else {
+		snprintf(&lat.str[0], sizeof(lat.str), "%.6f", 0.0);
+	}
+
+	return lat;
+}
+
+msp_rendor_longitude_t construct_rendor_GPS_LON(const sensor_gps_s &vehicle_gps_position)
+{
+	msp_rendor_longitude_t lon;
+
+	lon.screenYPosition = 0x09;
+	lon.screenXPosition = 0x29;
+
+	if (vehicle_gps_position.fix_type >= 2) {
+		snprintf(&lon.str[0], sizeof(lon.str), "%.6f", vehicle_gps_position.longitude_deg);
+
+	} else {
+		snprintf(&lon.str[0], sizeof(lon.str), "%.6f", -0.0);
+	}
+
+	return lon;
+}
+
+msp_rendor_satellites_used_t construct_rendor_GPS_NUM(const sensor_gps_s &vehicle_gps_position)
+{
+	msp_rendor_satellites_used_t num;
+
+	num.screenYPosition = 0x08;
+	num.screenXPosition = 0x29;
+
+	memset(&num.str[0], 0, sizeof(num.str));
+	snprintf(&num.str[0], sizeof(num.str), "%d", vehicle_gps_position.satellites_used);
+
+	return num;
+}
+
+
 msp_comp_gps_t construct_COMP_GPS(const home_position_s &home_position,
 				  const vehicle_global_position_s &vehicle_global_position,
 				  const bool heartbeat)
@@ -315,6 +415,34 @@ msp_comp_gps_t construct_COMP_GPS(const home_position_s &home_position,
 	return comp_gps;
 }
 
+msp_rendor_distanceToHome_t construct_rendor_distanceToHome(const home_position_s &home_position,
+		const vehicle_global_position_s &vehicle_global_position)
+{
+	msp_rendor_distanceToHome_t distance;
+
+	distance.screenYPosition = 0x08;
+	distance.screenXPosition = 0x02;
+
+	int16_t dist_i = 0;
+
+	if (home_position.valid_hpos
+	    && home_position.valid_lpos
+	    && (hrt_elapsed_time(&vehicle_global_position.timestamp) < 1_s)) {
+
+		float distance_to_home = get_distance_to_next_waypoint(vehicle_global_position.lat,
+					 vehicle_global_position.lon,
+					 home_position.lat, home_position.lon);
+
+		dist_i = (int16_t)distance_to_home; // meters
+
+	}
+
+	memset(&distance.str[0], 0, sizeof(distance.str));
+	snprintf(&distance.str[0], sizeof(distance.str), "%d", dist_i); // 65536
+
+	return distance;
+}
+
 msp_attitude_t construct_ATTITUDE(const vehicle_attitude_s &vehicle_attitude)
 {
 	// initialize results
@@ -339,6 +467,45 @@ msp_attitude_t construct_ATTITUDE(const vehicle_attitude_s &vehicle_attitude)
 	return attitude;
 }
 
+msp_rendor_pitch_t  construct_rendor_PITCH(const vehicle_attitude_s &vehicle_attitude)
+{
+	// initialize results
+	msp_rendor_pitch_t pit;
+
+	pit.screenYPosition = 0x0D;
+	pit.screenXPosition = 0x29;
+
+	// convert from quaternion to RPY
+	matrix::Eulerf euler_attitude(matrix::Quatf(vehicle_attitude.q));
+	double pitch_deg = (double)math::degrees(euler_attitude.theta());
+	// attitude.roll = math::degrees(euler_attitude.phi()) * 10;
+
+	memset(&pit.str[0], 0, sizeof(pit.str));
+	snprintf(&pit.str[0], sizeof(pit.str), "%.1f", pitch_deg);
+
+	return pit;
+}
+
+msp_rendor_roll_t  construct_rendor_ROLL(const vehicle_attitude_s &vehicle_attitude)
+{
+	// initialize results
+	msp_rendor_roll_t roll;
+
+	roll.screenYPosition = 0x0E;
+	roll.screenXPosition = 0x29;
+
+	// convert from quaternion to RPY
+	matrix::Eulerf euler_attitude(matrix::Quatf(vehicle_attitude.q));
+	// double pitch = (double)math::degrees(euler_attitude.theta());
+	double roll_deg = (double)math::degrees(euler_attitude.phi());
+
+	memset(&roll.str[0], 0, sizeof(roll.str));
+	snprintf(&roll.str[0], sizeof(roll.str), "%.1f", roll_deg);
+
+	return roll;
+}
+
+
 msp_altitude_t construct_ALTITUDE(const sensor_gps_s &vehicle_gps_position,
 				  const vehicle_local_position_s &vehicle_local_position)
 {
@@ -362,6 +529,29 @@ msp_altitude_t construct_ALTITUDE(const sensor_gps_s &vehicle_gps_position,
 	return altitude;
 }
 
+msp_rendor_altitude_t construct_Rendor_ALTITUDE(const sensor_gps_s &vehicle_gps_position,
+		const vehicle_local_position_s &vehicle_local_position)
+{
+	msp_rendor_altitude_t altitude;
+
+	altitude.screenYPosition = 0x06;
+	altitude.screenXPosition = 0x02;
+
+	double alt;
+
+	if (vehicle_gps_position.fix_type >= 2) {
+		alt = vehicle_gps_position.altitude_msl_m;
+
+	} else {
+		alt = (double)(vehicle_local_position.z * -1.0f);
+	}
+
+	memset(&altitude.str[0], 0, sizeof(altitude.str));
+	snprintf(&altitude.str[0], sizeof(altitude.str), "%.1f", alt);
+
+	return altitude;
+}
+
 msp_esc_sensor_data_dji_t construct_ESC_SENSOR_DATA()
 {
 	// initialize result
@@ -373,4 +563,30 @@ msp_esc_sensor_data_dji_t construct_ESC_SENSOR_DATA()
 	return esc_sensor_data;
 }
 
+msp_rc_t construct_MSP_RC(const input_rc_s &input_rc)
+{
+	// initialize result
+	msp_rc_t rc;
+
+	rc.channelValue[0] = input_rc.values[0]; // roll
+	rc.channelValue[1] = input_rc.values[1]; // pitch
+	rc.channelValue[2] = input_rc.values[3]; // yaw
+	rc.channelValue[3] = input_rc.values[2]; // Throttle
+	return rc;
+}
+
+msp_status_t construct_MSP_STATUS(const vehicle_status_s &vehicle_status)
+{
+	// initialize result
+	msp_status_t status{0};
+
+	if (vehicle_status.arming_state == vehicle_status_s::ARMING_STATE_ARMED) {
+		status.flightModeFlags |= (1 << MSP_MODE_ARM);
+	}
+
+	return status;
+}
+
+
+
 } // namespace msp_osd

src/drivers/osd/msp_osd/uorb_to_msp.hpp

@@ -84,9 +84,13 @@ msp_status_BF_t construct_STATUS(const vehicle_status_s &vehicle_status);
 // construct an MSP_ANALOG struct
 msp_analog_t construct_ANALOG(const battery_status_s &battery_status, const input_rc_s &input_rc);
 
+msp_rendor_rssi_t construct_rendor_RSSI(const input_rc_s &input_rc);
+
 // construct an MSP_BATTERY_STATE struct
 msp_battery_state_t construct_BATTERY_STATE(const battery_status_s &battery_status);
 
+msp_rendor_battery_state_t construct_rendor_BATTERY_STATE(const battery_status_s &battery_status);
+
 // construct an MSP_RAW_GPS struct
 msp_raw_gps_t construct_RAW_GPS(const sensor_gps_s &vehicle_gps_position,
 				const airspeed_validated_s &airspeed_validated);
@@ -96,14 +100,36 @@ msp_comp_gps_t construct_COMP_GPS(const home_position_s &home_position,
 				  const vehicle_global_position_s &vehicle_global_position,
 				  const bool heartbeat);
 
+msp_rendor_latitude_t construct_rendor_GPS_LAT(const sensor_gps_s &vehicle_gps_position);
+
+msp_rendor_longitude_t construct_rendor_GPS_LON(const sensor_gps_s &vehicle_gps_position);
+
+msp_rendor_satellites_used_t construct_rendor_GPS_NUM(const sensor_gps_s &vehicle_gps_position);
+
 // construct an MSP_ATTITUDE struct
 msp_attitude_t construct_ATTITUDE(const vehicle_attitude_s &vehicle_attitude);
 
+msp_rendor_pitch_t  construct_rendor_PITCH(const vehicle_attitude_s &vehicle_attitude);
+
+msp_rendor_roll_t  construct_rendor_ROLL(const vehicle_attitude_s &vehicle_attitude);
+
 // construct an MSP_ALTITUDE struct
 msp_altitude_t construct_ALTITUDE(const sensor_gps_s &vehicle_gps_position,
 				  const vehicle_local_position_s &vehicle_local_position);
 
+msp_rendor_altitude_t construct_Rendor_ALTITUDE(const sensor_gps_s &vehicle_gps_position,
+		const vehicle_local_position_s &vehicle_local_position);
+
+msp_rendor_distanceToHome_t construct_rendor_distanceToHome(const home_position_s &home_position,
+		const vehicle_global_position_s &vehicle_global_position);
+
 // construct an MSP_ESC_SENSOR_DATA struct
 msp_esc_sensor_data_dji_t construct_ESC_SENSOR_DATA();
 
+// construct an MSP_RC struct
+msp_rc_t construct_MSP_RC(const input_rc_s &input_rc);
+
+// construct an MSP_STATUS struct
+msp_status_t construct_MSP_STATUS(const vehicle_status_s &vehicle_status);
+
 } // namespace msp_osd

src/drivers/power_monitor/ina226/ina226_params.c

@@ -50,6 +50,7 @@ PARAM_DEFINE_INT32(SENS_EN_INA226, 0);
  * @max 65535
  * @decimal 1
  * @increment 1
+ * @reboot_required true
 */
 PARAM_DEFINE_INT32(INA226_CONFIG, 18139);
 
@@ -61,6 +62,7 @@ PARAM_DEFINE_INT32(INA226_CONFIG, 18139);
  * @max 200.0
  * @decimal 2
  * @increment 0.1
+ * @reboot_required true
  */
 PARAM_DEFINE_FLOAT(INA226_CURRENT, 164.0f);
 
@@ -72,5 +74,6 @@ PARAM_DEFINE_FLOAT(INA226_CURRENT, 164.0f);
  * @max 0.1
  * @decimal 10
  * @increment .000000001
+ * @reboot_required true
  */
 PARAM_DEFINE_FLOAT(INA226_SHUNT, 0.0005f);

src/drivers/power_monitor/ina228/ina228.cpp

@@ -59,8 +59,6 @@ INA228::INA228(const I2CSPIDriverConfig &config, int battery_index) :
 		_max_current = fvalue;
 	}
 
-	_range = _max_current > (MAX_CURRENT - 1.0f) ? INA228_ADCRANGE_HIGH : INA228_ADCRANGE_LOW;
-
 	fvalue = INA228_SHUNT;
 	_rshunt = fvalue;
 	ph = param_find("INA228_SHUNT");
@@ -69,6 +67,21 @@ INA228::INA228(const I2CSPIDriverConfig &config, int battery_index) :
 		_rshunt = fvalue;
 	}
 
+	// According to page 8.2.2.1, page 36/48 of the INA228 interface datasheet (Rev. A),
+	// the requirement is: R_SHUNT < V_SENSE_MAX / I_MAX
+	// therefore: R_SHUNT * I_MAX < V_SENSE_MAX
+	// and so if V_SENSE_MAX is bigger, we need to use the bigger ADC range to avoid
+	// the device from capping the measured current.
+
+	const float v_sense_max = _rshunt * _max_current;
+
+	if (v_sense_max > INA228_ADCRANGE_LOW_V_SENSE) {
+		_range = INA228_ADCRANGE_HIGH;
+
+	} else {
+		_range = INA228_ADCRANGE_LOW;
+	}
+
 	ph = param_find("INA228_CONFIG");
 	int32_t value = INA228_ADCCONFIG;
 	_config = (uint16_t)value;

src/drivers/power_monitor/ina228/ina228.h

@@ -293,6 +293,8 @@ using namespace time_literals;
 #define INA228_VSCALE                        1.95e-04f  /* LSB of voltage is 195.3125 uV/LSB */
 #define INA228_TSCALE                        7.8125e-03f /* LSB of temperature is 7.8125 mDegC/LSB */
 
+#define INA228_ADCRANGE_LOW_V_SENSE          0.04096f // ± 40.96 mV
+
 #define swap16(w)                            __builtin_bswap16((w))
 #define swap32(d)                            __builtin_bswap32((d))
 #define swap64(q)                            __builtin_bswap64((q))

src/drivers/power_monitor/ina228/ina228_params.c

@@ -50,6 +50,7 @@ PARAM_DEFINE_INT32(SENS_EN_INA228, 0);
  * @max 65535
  * @decimal 1
  * @increment 1
+ * @reboot_required true
 */
 PARAM_DEFINE_INT32(INA228_CONFIG, 63779);
 
@@ -61,6 +62,7 @@ PARAM_DEFINE_INT32(INA228_CONFIG, 63779);
  * @max 327.68
  * @decimal 2
  * @increment 0.1
+ * @reboot_required true
  */
 PARAM_DEFINE_FLOAT(INA228_CURRENT, 327.68f);
 
@@ -72,5 +74,6 @@ PARAM_DEFINE_FLOAT(INA228_CURRENT, 327.68f);
  * @max 0.1
  * @decimal 10
  * @increment .000000001
+ * @reboot_required true
  */
 PARAM_DEFINE_FLOAT(INA228_SHUNT, 0.0005f);

src/drivers/power_monitor/ina238/ina238.cpp

@@ -55,8 +55,6 @@ INA238::INA238(const I2CSPIDriverConfig &config, int battery_index) :
 		_max_current = fvalue;
 	}
 
-	_range = _max_current > (DEFAULT_MAX_CURRENT - 1.0f) ? INA238_ADCRANGE_HIGH : INA238_ADCRANGE_LOW;
-
 	fvalue = DEFAULT_SHUNT;
 	_rshunt = fvalue;
 	ph = param_find("INA238_SHUNT");
@@ -65,6 +63,21 @@ INA238::INA238(const I2CSPIDriverConfig &config, int battery_index) :
 		_rshunt = fvalue;
 	}
 
+	// According to page 8.2.2.1, page 33/48 of the INA238 interface datasheet (Rev. A),
+	// the requirement is: R_SHUNT < V_SENSE_MAX / I_MAX
+	// therefore: R_SHUNT * I_MAX < V_SENSE_MAX
+	// and so if V_SENSE_MAX is bigger, we need to use the bigger ADC range to avoid
+	// the device from capping the measured current.
+
+	const float v_sense_max = _rshunt * _max_current;
+
+	if (v_sense_max > INA238_ADCRANGE_LOW_V_SENSE) {
+		_range = INA238_ADCRANGE_HIGH;
+
+	} else {
+		_range = INA238_ADCRANGE_LOW;
+	}
+
 	_current_lsb = _max_current / INA238_DN_MAX;
 	_shunt_calibration = static_cast<uint16_t>(INA238_CONST * _current_lsb * _rshunt);
 
@@ -124,7 +137,15 @@ int INA238::init()
 		return ret;
 	}
 
-	return Reset();
+	ret = Reset();
+
+	if (ret) {
+		return ret;
+	}
+
+	start();
+
+	return 0;
 }
 
 int INA238::force_init()

src/drivers/power_monitor/ina238/ina238.h

@@ -75,6 +75,8 @@ using namespace ina238;
 #define INA238_VSCALE                        3.125e-03f  /* LSB of voltage is 3.1255 mV/LSB */
 #define INA238_TSCALE                        7.8125e-03f /* LSB of temperature is 7.8125 mDegC/LSB */
 
+#define INA238_ADCRANGE_LOW_V_SENSE          0.04096f // ± 40.96 mV
+
 #define DEFAULT_MAX_CURRENT                  327.68f    /* Amps */
 #define DEFAULT_SHUNT                        0.0003f   /* Shunt is 300 uOhm */
 

src/drivers/power_monitor/ina238/ina238_params.c

@@ -50,6 +50,7 @@ PARAM_DEFINE_INT32(SENS_EN_INA238, 0);
  * @max 327.68
  * @decimal 2
  * @increment 0.1
+ * @reboot_required true
  */
 PARAM_DEFINE_FLOAT(INA238_CURRENT, 327.68f);
 
@@ -61,5 +62,6 @@ PARAM_DEFINE_FLOAT(INA238_CURRENT, 327.68f);
  * @max 0.1
  * @decimal 10
  * @increment .000000001
+ * @reboot_required true
  */
 PARAM_DEFINE_FLOAT(INA238_SHUNT, 0.0003f);

src/drivers/rc_input/RCInput.cpp

@@ -108,6 +108,18 @@ RCInput::init()
 #ifdef GPIO_PPM_IN
 	// disable CPPM input by mapping it away from the timer capture input
 	px4_arch_unconfiggpio(GPIO_PPM_IN);
+
+#ifdef RC_SERIAL_PORT_SHARED_PPM_PIN_GPIO_RX
+
+	// If we use the same STM32 pin for PPM input as well as serial input, we
+	// need to configure the serial port, as long as we're actually using that
+	// serial device.
+	if (strcmp(_device, RC_SERIAL_PORT) == 0) {
+		px4_arch_configgpio(RC_SERIAL_PORT_SHARED_PPM_PIN_GPIO_RX);
+	}
+
+#endif // RC_SERIAL_PORT_SHARED_PPM_PIN_GPIO_RX
+
 #endif // GPIO_PPM_IN
 
 	rc_io_invert(false);
@@ -661,6 +673,15 @@ void RCInput::Run()
 #ifdef HRT_PPM_CHANNEL
 			if (_rc_scan_begin == 0) {
 				_rc_scan_begin = cycle_timestamp;
+
+#ifdef RC_SERIAL_PORT_SHARED_PPM_PIN_GPIO_RX
+
+				if (strcmp(_device, RC_SERIAL_PORT) == 0) {
+					px4_arch_unconfiggpio(RC_SERIAL_PORT_SHARED_PPM_PIN_GPIO_RX);
+				}
+
+#endif // RC_SERIAL_PORT_SHARED_PPM_PIN_GPIO_RX
+
 				// Configure timer input pin for CPPM
 				px4_arch_configgpio(GPIO_PPM_IN);
 
@@ -684,6 +705,15 @@ void RCInput::Run()
 			} else {
 				// disable CPPM input by mapping it away from the timer capture input
 				px4_arch_unconfiggpio(GPIO_PPM_IN);
+
+#ifdef RC_SERIAL_PORT_SHARED_PPM_PIN_GPIO_RX
+
+				if (strcmp(_device, RC_SERIAL_PORT) == 0) {
+					px4_arch_configgpio(RC_SERIAL_PORT_SHARED_PPM_PIN_GPIO_RX);
+				}
+
+#endif // RC_SERIAL_PORT_SHARED_PPM_PIN_GPIO_RX
+
 				// Scan the next protocol
 				set_rc_scan_state(RC_SCAN_CRSF);
 			}

src/drivers/uavcan/actuators/esc.cpp

@@ -40,6 +40,7 @@
 #include "esc.hpp"
 #include <systemlib/err.h>
 #include <drivers/drv_hrt.h>
+#include <lib/atmosphere/atmosphere.h>
 
 #define MOTOR_BIT(x) (1<<(x))
 
@@ -140,7 +141,7 @@ UavcanEscController::esc_status_sub_cb(const uavcan::ReceivedDataStructure<uavca
 		ref.esc_address = msg.getSrcNodeID().get();
 		ref.esc_voltage     = msg.voltage;
 		ref.esc_current     = msg.current;
-		ref.esc_temperature = msg.temperature;
+		ref.esc_temperature = msg.temperature + atmosphere::kAbsoluteNullCelsius; // Kelvin to Celsius
 		ref.esc_rpm         = msg.rpm;
 		ref.esc_errorcount  = msg.error_count;
 

src/modules/commander/esc_calibration.cpp

@@ -63,19 +63,13 @@ bool check_battery_disconnected(orb_advert_t *mavlink_log_pub)
 
 	const bool recent_battery_measurement = hrt_absolute_time() < (battery_status_sub.get().timestamp + 1_s);
 
-	if (!recent_battery_measurement) {
-		// We have to send this message for now because "battery unavailable" gets ignored by QGC
+	if (recent_battery_measurement && battery_status_sub.get().connected) {
 		calibration_log_critical(mavlink_log_pub, CAL_QGC_FAILED_MSG, "Disconnect battery and try again");
 		return false;
-	}
 
-	// Make sure battery is reported to be disconnected
-	if (recent_battery_measurement && !battery_status_sub.get().connected) {
+	} else {
 		return true;
 	}
-
-	calibration_log_critical(mavlink_log_pub, CAL_QGC_FAILED_MSG, "Disconnect battery and try again");
-	return false;
 }
 
 static void set_motor_actuators(uORB::Publication<actuator_test_s> &publisher, float value, bool release_control)

src/modules/control_allocator/ControlAllocator.cpp

@@ -95,11 +95,6 @@ ControlAllocator::init()
 		return false;
 	}
 
-	if (!_vehicle_thrust_setpoint_sub.registerCallback()) {
-		PX4_ERR("callback registration failed");
-		return false;
-	}
-
 #ifndef ENABLE_LOCKSTEP_SCHEDULER // Backup schedule would interfere with lockstep
 	ScheduleDelayed(50_ms);
 #endif
@@ -309,7 +304,6 @@ ControlAllocator::Run()
 {
 	if (should_exit()) {
 		_vehicle_torque_setpoint_sub.unregisterCallback();
-		_vehicle_thrust_setpoint_sub.unregisterCallback();
 		exit_and_cleanup();
 		return;
 	}
@@ -396,15 +390,8 @@ ControlAllocator::Run()
 
 	}
 
-	// Also run allocator on thrust setpoint changes if the torque setpoint
-	// has not been updated for more than 5ms
 	if (_vehicle_thrust_setpoint_sub.update(&vehicle_thrust_setpoint)) {
 		_thrust_sp = matrix::Vector3f(vehicle_thrust_setpoint.xyz);
-
-		if (dt > 0.005f) {
-			do_update = true;
-			_timestamp_sample = vehicle_thrust_setpoint.timestamp_sample;
-		}
 	}
 
 	if (do_update) {

src/modules/control_allocator/ControlAllocator.hpp

@@ -175,7 +175,7 @@ private:
 
 	// Inputs
 	uORB::SubscriptionCallbackWorkItem _vehicle_torque_setpoint_sub{this, ORB_ID(vehicle_torque_setpoint)};  /**< vehicle torque setpoint subscription */
-	uORB::SubscriptionCallbackWorkItem _vehicle_thrust_setpoint_sub{this, ORB_ID(vehicle_thrust_setpoint)};	 /**< vehicle thrust setpoint subscription */
+	uORB::Subscription _vehicle_thrust_setpoint_sub{ORB_ID(vehicle_thrust_setpoint)};	 /**< vehicle thrust setpoint subscription */
 
 	uORB::Subscription _vehicle_torque_setpoint1_sub{ORB_ID(vehicle_torque_setpoint), 1};  /**< vehicle torque setpoint subscription (2. instance) */
 	uORB::Subscription _vehicle_thrust_setpoint1_sub{ORB_ID(vehicle_thrust_setpoint), 1};	 /**< vehicle thrust setpoint subscription (2. instance) */