fixup! Update submodule nuttx

uavcan_v1: Add Publishers dir to includes
uavcan_v1: SocketCAN set 'can_fd' based on CONFIG
2026-08-17 19:30:34 +08:00 · 2022-03-18 10:56:44 -07:00 · 2022-03-18 09:03:48 -07:00 · 2022-03-18 09:03:06 -07:00 · 2022-03-18 09:02:00 -07:00 · 2022-03-17 17:01:54 -07:00
339 changed files with 11738 additions and 7986 deletions
@@ -861,7 +861,7 @@ void printTopics() {
  sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-*` --cmd "listener cpuload" || true'
  sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-*` --cmd "listener distance_sensor" || true'
  sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-*` --cmd "listener ekf2_timestamps" || true'
-  sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-*` --cmd "listener ekf_gps_drift" || true'
+  sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-*` --cmd "listener estimator_gps_status" || true'
  sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-*` --cmd "listener esc_status" || true'
  sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-*` --cmd "listener estimator_attitude" || true'
  sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-*` --cmd "listener estimator_baro_bias" || true'
@@ -24,8 +24,8 @@
 	branch = master
 [submodule "platforms/nuttx/NuttX/nuttx"]
 	path = platforms/nuttx/NuttX/nuttx
-	url = https://github.com/PX4/NuttX.git
-	branch = px4_firmware_nuttx-10.1.0+
+	url = https://github.com/JacobCrabill/incubator-nuttx.git
+	branch = dev-h7-socketcan
 [submodule "platforms/nuttx/NuttX/apps"]
 	path = platforms/nuttx/NuttX/apps
 	url = https://github.com/PX4/NuttX-apps.git
@@ -10,7 +10,7 @@
 # param set-default SYS_CTRL_ALLOC 1
 param set-default CA_AIRFRAME 2

-param set-default CA_ROTOR_COUNT 4
+param set-default CA_ROTOR_COUNT 5
 param set-default CA_ROTOR0_PX 0.1515
 param set-default CA_ROTOR0_PY 0.245
 param set-default CA_ROTOR0_KM 0.05
@@ -23,6 +23,9 @@ param set-default CA_ROTOR2_KM -0.05
 param set-default CA_ROTOR3_PX -0.1515
 param set-default CA_ROTOR3_PY 0.1875
 param set-default CA_ROTOR3_KM -0.05
+param set-default CA_ROTOR4_AX 1.0
+param set-default CA_ROTOR4_AZ 0.0
+param set-default CA_ROTOR4_PX 0.2

 param set-default CA_SV_CS_COUNT 3
 param set-default CA_SV_CS0_TYPE 1
@@ -39,6 +39,26 @@ param set-default FW_RR_P 0.3

 param set-default RWTO_TKOFF 1

+param set-default CA_AIRFRAME 1
+param set-default CA_ROTOR_COUNT 1
+param set-default CA_ROTOR0_PX 0.3
+param set-default CA_SV_CS_COUNT 4
+param set-default CA_SV_CS0_TRQ_R 0.5
+param set-default CA_SV_CS0_TYPE 2
+param set-default CA_SV_CS1_TRQ_P 1.0
+param set-default CA_SV_CS1_TYPE 3
+param set-default CA_SV_CS2_TRQ_Y 1.0
+param set-default CA_SV_CS2_TYPE 4
+param set-default CA_SV_CS3_TYPE 10
+
+param set-default HIL_ACT_REV 2
+param set-default HIL_ACT_FUNC1 201
+param set-default HIL_ACT_FUNC2 202
+param set-default HIL_ACT_FUNC3 203
+param set-default HIL_ACT_FUNC4 101
+param set-default HIL_ACT_FUNC5 204
+param set-default HIL_ACT_FUNC6 400
+
 param set SYS_HITL 1

 # disable some checks to allow to fly
@@ -15,6 +15,23 @@ set MIXER quad_x

 param set SYS_HITL 1

+param set-default CA_ROTOR_COUNT 4
+param set-default CA_ROTOR0_PX 0.15
+param set-default CA_ROTOR0_PY 0.15
+param set-default CA_ROTOR1_PX -0.15
+param set-default CA_ROTOR1_PY -0.15
+param set-default CA_ROTOR2_PX 0.15
+param set-default CA_ROTOR2_PY -0.15
+param set-default CA_ROTOR2_KM -0.05
+param set-default CA_ROTOR3_PX -0.15
+param set-default CA_ROTOR3_PY 0.15
+param set-default CA_ROTOR3_KM -0.05
+
+param set-default HIL_ACT_FUNC1 101
+param set-default HIL_ACT_FUNC2 102
+param set-default HIL_ACT_FUNC3 103
+param set-default HIL_ACT_FUNC4 104
+
 # disable some checks to allow to fly
 # - with usb
 param set-default CBRK_USB_CHK 197848
@@ -61,6 +61,37 @@ param set-default VT_MOT_ID 1234
 param set-default VT_FW_MOT_OFFID 1234
 param set-default VT_TYPE 2

+param set-default CA_AIRFRAME 2
+param set-default CA_ROTOR_COUNT 5
+param set-default CA_ROTOR0_PX 0.15
+param set-default CA_ROTOR0_PY 0.15
+param set-default CA_ROTOR1_PX -0.15
+param set-default CA_ROTOR1_PY -0.15
+param set-default CA_ROTOR2_PX 0.15
+param set-default CA_ROTOR2_PY -0.15
+param set-default CA_ROTOR2_KM -0.05
+param set-default CA_ROTOR3_PX -0.15
+param set-default CA_ROTOR3_PY 0.15
+param set-default CA_ROTOR3_KM -0.05
+param set-default CA_ROTOR4_AX 1.0
+param set-default CA_ROTOR4_AZ 0.0
+param set-default CA_ROTOR4_PX 0.2
+
+param set-default CA_SV_CS_COUNT 3
+param set-default CA_SV_CS1_TRQ_R 0.5
+param set-default CA_SV_CS1_TYPE 2
+param set-default CA_SV_CS2_TRQ_P 1.0
+param set-default CA_SV_CS2_TYPE 3
+
+param set-default HIL_ACT_FUNC1 101
+param set-default HIL_ACT_FUNC2 102
+param set-default HIL_ACT_FUNC3 103
+param set-default HIL_ACT_FUNC4 104
+param set-default HIL_ACT_FUNC5 105
+param set-default HIL_ACT_FUNC6 201
+param set-default HIL_ACT_FUNC7 202
+param set-default HIL_ACT_FUNC8 203
+

 param set SYS_HITL 1

@@ -17,6 +17,23 @@ set PWM_OUT 1234

 param set UAVCAN_ENABLE 0

+param set-default CA_ROTOR_COUNT 4
+param set-default CA_ROTOR0_PX 0.15
+param set-default CA_ROTOR0_PY 0.15
+param set-default CA_ROTOR1_PX -0.15
+param set-default CA_ROTOR1_PY -0.15
+param set-default CA_ROTOR2_PX 0.15
+param set-default CA_ROTOR2_PY -0.15
+param set-default CA_ROTOR2_KM -0.05
+param set-default CA_ROTOR3_PX -0.15
+param set-default CA_ROTOR3_PY 0.15
+param set-default CA_ROTOR3_KM -0.05
+
+param set-default HIL_ACT_FUNC1 101
+param set-default HIL_ACT_FUNC2 102
+param set-default HIL_ACT_FUNC3 103
+param set-default HIL_ACT_FUNC4 104
+
 # set SYS_HITL to 2 to start the SIH and avoid sensors startup
 param set SYS_HITL 2

@@ -17,6 +17,26 @@ set PWM_OUT 1234

 param set UAVCAN_ENABLE 0

+param set-default CA_AIRFRAME 1
+param set-default CA_ROTOR_COUNT 1
+param set-default CA_ROTOR0_PX 0.3
+param set-default CA_SV_CS_COUNT 4
+param set-default CA_SV_CS0_TRQ_R 0.5
+param set-default CA_SV_CS0_TYPE 2
+param set-default CA_SV_CS1_TRQ_P 1.0
+param set-default CA_SV_CS1_TYPE 3
+param set-default CA_SV_CS2_TRQ_Y 1.0
+param set-default CA_SV_CS2_TYPE 4
+param set-default CA_SV_CS3_TYPE 10
+
+param set-default HIL_ACT_REV 2
+param set-default HIL_ACT_FUNC1 201
+param set-default HIL_ACT_FUNC2 202
+param set-default HIL_ACT_FUNC3 203
+param set-default HIL_ACT_FUNC4 101
+param set-default HIL_ACT_FUNC5 204
+param set-default HIL_ACT_FUNC6 400
+
 # set SYS_HITL to 2 to start the SIH and avoid sensors startup
 param set-default SYS_HITL 2

@@ -26,6 +26,26 @@ param set-default VT_FW_DIFTHR_SC 0.3
 param set-default MPC_MAN_Y_MAX 60
 param set-default MC_PITCH_P 5

+param set-default CA_AIRFRAME 4
+param set-default CA_ROTOR_COUNT 2
+param set-default CA_ROTOR0_KM -0.05
+param set-default CA_ROTOR0_PY 0.2
+param set-default CA_ROTOR1_KM -0.05
+param set-default CA_ROTOR1_PY -0.2
+param set-default CA_SV_CS_COUNT 2
+param set-default CA_SV_CS0_TRQ_P 0.3
+param set-default CA_SV_CS0_TRQ_Y 0.3
+param set-default CA_SV_CS0_TYPE 5
+param set-default CA_SV_CS1_TRQ_P 0.3
+param set-default CA_SV_CS1_TRQ_Y -0.3
+param set-default CA_SV_CS1_TYPE 6
+
+param set-default HIL_ACT_FUNC1 101
+param set-default HIL_ACT_FUNC2 102
+param set-default HIL_ACT_FUNC5 202
+param set-default HIL_ACT_FUNC6 201
+param set-default HIL_ACT_REV 32
+
 param set-default MAV_TYPE 19
 set MAV_TYPE 19
 set MIXER vtol_tailsitter_duo_sat
@@ -25,7 +25,7 @@
 . ${R}etc/init.d/rc.vtol_defaults

 param set-default CA_AIRFRAME 2
-param set-default CA_ROTOR_COUNT 4
+param set-default CA_ROTOR_COUNT 5
 param set-default CA_ROTOR0_PX 0.15
 param set-default CA_ROTOR0_PY 0.15
 param set-default CA_ROTOR1_PX -0.15
@@ -36,6 +36,9 @@ param set-default CA_ROTOR2_KM -0.05
 param set-default CA_ROTOR3_PX -0.15
 param set-default CA_ROTOR3_PY 0.15
 param set-default CA_ROTOR3_KM -0.05
+param set-default CA_ROTOR4_AX 1.0
+param set-default CA_ROTOR4_AZ 0.0
+param set-default CA_ROTOR4_PX 0.2
 param set-default CA_SV_CS_COUNT 4
 param set-default CA_SV_CS0_TYPE 1
 param set-default CA_SV_CS0_TRQ_R -0.5
@@ -164,6 +164,8 @@ param set-default RC_MAP_AUX2       5
 param set-default RC_MAP_AUX3       10
 param set-default RC_MAP_AUX4       8
 param set-default RC_MAP_FLTMODE    6
+param set-default RC_MAP_RETURN_SW  7
+
 param set-default RC1_TRIM          1000


@@ -155,6 +155,13 @@ then
 	irlock start -X
 fi

+# PCF8583 counter (RPM sensor)
+if param compare -s SENS_EN_PCF8583 1
+then
+	pcf8583 start -X
+	pcf8583 start -X -a 0x51
+fi
+
 # probe for optional external I2C devices
 if param compare SENS_EXT_I2C_PRB 1
 then
@@ -330,9 +330,11 @@ def get_mixers(yaml_config, output_functions, verbose):
        option = select_param + '==' + str(type_index)
        mixer_config = {
                'option': option,
+                'help-url': 'https://docs.px4.io/master/en/config/actuators.html',
            }
-        if 'type' in current_type:
-            mixer_config['type'] = current_type['type']
+        for optional in ['type', 'title']:
+            if optional in current_type:
+                mixer_config[optional] = current_type[optional]
        actuators = []
        for actuator_conf in current_type['actuators']:
            actuator = {
@@ -7,7 +7,8 @@ import os
 import math
 import matplotlib.pyplot as plt
 import numpy as np
-import scipy as sp
+
+from scipy.signal import medfilt

 from pyulog import *

@@ -65,7 +66,7 @@ def resampleWithDeltaX(x,y):
    return resampledX,resampledY

 def median_filter(data):
-    return sp.signal.medfilt(data, 31)
+    return medfilt(data, 31)

 parser = argparse.ArgumentParser(description='Reads in IMU data from a static thermal calibration test and performs a curve fit of gyro, accel and baro bias vs temperature')
 parser.add_argument('filename', metavar='file.ulg', help='ULog input file')
@@ -16,7 +16,7 @@ class ModuleDocumentation(object):
    valid_categories = ['driver', 'estimator', 'controller', 'system',
                        'communication', 'command', 'template', 'simulation', 'autotune']
    valid_subcategories = ['', 'distance_sensor', 'imu', 'airspeed_sensor',
-                           'magnetometer', 'baro', 'optical_flow']
+                           'magnetometer', 'baro', 'optical_flow', 'rpm_sensor']

    max_line_length = 80 # wrap lines that are longer than this

@@ -192,7 +192,7 @@ elif [ "$program" == "ignition" ] && [ -z "$no_sim" ]; then
 		ignition_headless=""
 	fi
 	source "$src_path/Tools/setup_ignition.bash" "${src_path}" "${build_path}"
-	ign gazebo ${verbose} ${ignition_headless} -r "${src_path}/Tools/simulation-ignition/worlds/${model}.world"&
+	ign gazebo --force-version 5 ${verbose} ${ignition_headless} -r "${src_path}/Tools/simulation-ignition/worlds/${model}.world"&
 elif [ "$program" == "flightgear" ] && [ -z "$no_sim" ]; then
 	echo "FG setup"
 	cd "${src_path}/Tools/flightgear_bridge/"
@@ -0,0 +1,245 @@
+#
+# This file is autogenerated: PLEASE DO NOT EDIT IT.
+#
+# You can use "make menuconfig" to make any modifications to the installed .config file.
+# You can then do "make savedefconfig" to generate a new defconfig file that includes your
+# modifications.
+#
+# CONFIG_DISABLE_ENVIRON is not set
+# CONFIG_DISABLE_PSEUDOFS_OPERATIONS is not set
+# CONFIG_MMCSD_HAVE_CARDDETECT is not set
+# CONFIG_MMCSD_HAVE_WRITEPROTECT is not set
+# CONFIG_MMCSD_MMCSUPPORT is not set
+# CONFIG_MMCSD_SPI is not set
+# CONFIG_NET_CAN_CANFD is not set
+# CONFIG_NET_ETHERNET is not set
+# CONFIG_NET_IPv4 is not set
+# CONFIG_NSH_DISABLEBG is not set
+# CONFIG_NSH_DISABLESCRIPT is not set
+# CONFIG_NSH_DISABLE_DF is not set
+# CONFIG_NSH_DISABLE_EXEC is not set
+# CONFIG_NSH_DISABLE_EXIT is not set
+# CONFIG_NSH_DISABLE_GET is not set
+# CONFIG_NSH_DISABLE_ITEF is not set
+# CONFIG_NSH_DISABLE_LOOPS is not set
+# CONFIG_NSH_DISABLE_MKFATFS is not set
+# CONFIG_NSH_DISABLE_REBOOT is not set
+# CONFIG_NSH_DISABLE_SEMICOLON is not set
+# CONFIG_NSH_DISABLE_SHUTDOWN is not set
+# CONFIG_NSH_DISABLE_TIME is not set
+CONFIG_ARCH="arm"
+CONFIG_ARCH_BOARD_CUSTOM=y
+CONFIG_ARCH_BOARD_CUSTOM_DIR="../../../../boards/cubepilot/cubeorange/nuttx-config"
+CONFIG_ARCH_BOARD_CUSTOM_DIR_RELPATH=y
+CONFIG_ARCH_BOARD_CUSTOM_NAME="px4"
+CONFIG_ARCH_CHIP="stm32h7"
+CONFIG_ARCH_CHIP_STM32H743ZI=y
+CONFIG_ARCH_CHIP_STM32H7=y
+CONFIG_ARCH_INTERRUPTSTACK=512
+CONFIG_ARCH_STACKDUMP=y
+CONFIG_ARMV7M_BASEPRI_WAR=y
+CONFIG_ARMV7M_DCACHE=y
+CONFIG_ARMV7M_DTCM=y
+CONFIG_ARMV7M_ICACHE=y
+CONFIG_ARMV7M_MEMCPY=y
+CONFIG_ARMV7M_USEBASEPRI=y
+CONFIG_ARM_MPU_EARLY_RESET=y
+CONFIG_BOARDCTL_RESET=y
+CONFIG_BOARD_CRASHDUMP=y
+CONFIG_BOARD_LOOPSPERMSEC=79954
+CONFIG_BOARD_RESET_ON_ASSERT=2
+CONFIG_BUILTIN=y
+CONFIG_C99_BOOL8=y
+CONFIG_CANUTILS_CANDUMP=y
+CONFIG_CANUTILS_CANSEND=y
+CONFIG_CDCACM=y
+CONFIG_CDCACM_IFLOWCONTROL=y
+CONFIG_CDCACM_PRODUCTID=0x1016
+CONFIG_CDCACM_PRODUCTSTR="CubeOrange"
+CONFIG_CDCACM_RXBUFSIZE=600
+CONFIG_CDCACM_TXBUFSIZE=12000
+CONFIG_CDCACM_VENDORID=0x2DAE
+CONFIG_CDCACM_VENDORSTR="CubePilot"
+CONFIG_DEBUG_FULLOPT=y
+CONFIG_DEBUG_HARDFAULT_ALERT=y
+CONFIG_DEBUG_SYMBOLS=y
+CONFIG_DEFAULT_SMALL=y
+CONFIG_DEV_FIFO_SIZE=0
+CONFIG_DEV_PIPE_MAXSIZE=1024
+CONFIG_DEV_PIPE_SIZE=70
+CONFIG_EXAMPLES_CALIB_UDELAY=y
+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_LIBC_LONG_LONG=y
+CONFIG_LIBC_STRERROR=y
+CONFIG_MEMSET_64BIT=y
+CONFIG_MEMSET_OPTSPEED=y
+CONFIG_MMCSD=y
+CONFIG_MMCSD_SDIO=y
+CONFIG_MMCSD_SDIOWAIT_WRCOMPLETE=y
+CONFIG_MM_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_NET=y
+CONFIG_NETDEV_CAN_BITRATE_IOCTL=y
+CONFIG_NETDEV_IFINDEX=y
+CONFIG_NETDEV_PHY_IOCTL=y
+CONFIG_NET_CAN=y
+CONFIG_NET_CAN_RAW_TX_DEADLINE=y
+CONFIG_NET_CAN_SOCK_OPTS=y
+CONFIG_NET_TIMESTAMP=y
+CONFIG_NSH_ARCHINIT=y
+CONFIG_NSH_ARGCAT=y
+CONFIG_NSH_BUILTIN_APPS=y
+CONFIG_NSH_CMDPARMS=y
+CONFIG_NSH_CROMFSETC=y
+CONFIG_NSH_DISABLE_TELNETD=y
+CONFIG_NSH_LINELEN=128
+CONFIG_NSH_MAXARGUMENTS=15
+CONFIG_NSH_NESTDEPTH=8
+CONFIG_NSH_QUOTE=y
+CONFIG_NSH_ROMFSETC=y
+CONFIG_NSH_ROMFSSECTSIZE=128
+CONFIG_NSH_STRERROR=y
+CONFIG_NSH_VARS=y
+CONFIG_OTG_ID_GPIO_DISABLE=y
+CONFIG_PIPES=y
+CONFIG_PREALLOC_TIMERS=50
+CONFIG_PRIORITY_INHERITANCE=y
+CONFIG_PTHREAD_MUTEX_ROBUST=y
+CONFIG_PTHREAD_STACK_MIN=512
+CONFIG_RAMTRON_SETSPEED=y
+CONFIG_RAM_SIZE=245760
+CONFIG_RAM_START=0x20010000
+CONFIG_RAW_BINARY=y
+CONFIG_READLINE_CMD_HISTORY=y
+CONFIG_READLINE_TABCOMPLETION=y
+CONFIG_RTC_DATETIME=y
+CONFIG_SCHED_ATEXIT=y
+CONFIG_SCHED_HPWORK=y
+CONFIG_SCHED_HPWORKPRIORITY=249
+CONFIG_SCHED_HPWORKSTACKSIZE=1280
+CONFIG_SCHED_INSTRUMENTATION=y
+CONFIG_SCHED_INSTRUMENTATION_EXTERNAL=y
+CONFIG_SCHED_LPWORK=y
+CONFIG_SCHED_LPWORKPRIORITY=50
+CONFIG_SCHED_LPWORKSTACKSIZE=1632
+CONFIG_SCHED_WAITPID=y
+CONFIG_SDCLONE_DISABLE=y
+CONFIG_SDMMC1_SDIO_PULLUP=y
+CONFIG_SEM_NNESTPRIO=8
+CONFIG_SEM_PREALLOCHOLDERS=0
+CONFIG_SERIAL_IFLOWCONTROL_WATERMARKS=y
+CONFIG_SERIAL_TERMIOS=y
+CONFIG_SIG_DEFAULT=y
+CONFIG_SIG_SIGALRM_ACTION=y
+CONFIG_SIG_SIGUSR1_ACTION=y
+CONFIG_SIG_SIGUSR2_ACTION=y
+CONFIG_SIG_SIGWORK=4
+CONFIG_STACK_COLORATION=y
+CONFIG_START_DAY=30
+CONFIG_START_MONTH=11
+CONFIG_STDIO_BUFFER_SIZE=256
+CONFIG_STM32H7_ADC1=y
+CONFIG_STM32H7_ADC3=y
+CONFIG_STM32H7_BBSRAM=y
+CONFIG_STM32H7_BBSRAM_FILES=5
+CONFIG_STM32H7_BKPSRAM=y
+CONFIG_STM32H7_DMA1=y
+CONFIG_STM32H7_DMA2=y
+CONFIG_STM32H7_DMACAPABLE=y
+CONFIG_STM32H7_FDCAN1=y
+CONFIG_STM32H7_FLOWCONTROL_BROKEN=y
+CONFIG_STM32H7_I2C1=y
+CONFIG_STM32H7_I2C2=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_SDMMC1=y
+CONFIG_STM32H7_SERIALBRK_BSDCOMPAT=y
+CONFIG_STM32H7_SERIAL_DISABLE_REORDERING=y
+CONFIG_STM32H7_SPI1=y
+CONFIG_STM32H7_SPI1_DMA=y
+CONFIG_STM32H7_SPI1_DMA_BUFFER=1024
+CONFIG_STM32H7_SPI2=y
+CONFIG_STM32H7_SPI4=y
+CONFIG_STM32H7_SPI4_DMA=y
+CONFIG_STM32H7_SPI4_DMA_BUFFER=1024
+CONFIG_STM32H7_SPI_DMA=y
+CONFIG_STM32H7_TIM1=y
+CONFIG_STM32H7_TIM3=y
+CONFIG_STM32H7_TIM4=y
+CONFIG_STM32H7_UART4=y
+CONFIG_STM32H7_UART7=y
+CONFIG_STM32H7_UART8=y
+CONFIG_STM32H7_USART2=y
+CONFIG_STM32H7_USART3=y
+CONFIG_STM32H7_USART6=y
+CONFIG_STM32H7_USART_BREAKS=y
+CONFIG_STM32H7_USART_INVERT=y
+CONFIG_STM32H7_USART_SINGLEWIRE=y
+CONFIG_STM32H7_USART_SWAP=y
+CONFIG_SYSTEM_CDCACM=y
+CONFIG_SYSTEM_NSH=y
+CONFIG_TASK_NAME_SIZE=24
+CONFIG_TTY_SIGINT=y
+CONFIG_TTY_SIGTSTP=y
+CONFIG_UART4_BAUD=57600
+CONFIG_UART4_RXBUFSIZE=600
+CONFIG_UART4_TXBUFSIZE=1500
+CONFIG_UART7_BAUD=57600
+CONFIG_UART7_RXBUFSIZE=600
+CONFIG_UART7_SERIAL_CONSOLE=y
+CONFIG_UART7_TXBUFSIZE=1500
+CONFIG_UART8_BAUD=57600
+CONFIG_UART8_RXBUFSIZE=600
+CONFIG_UART8_TXBUFSIZE=1500
+CONFIG_USART2_BAUD=57600
+CONFIG_USART2_IFLOWCONTROL=y
+CONFIG_USART2_OFLOWCONTROL=y
+CONFIG_USART2_RXBUFSIZE=600
+CONFIG_USART2_TXBUFSIZE=1500
+CONFIG_USART3_BAUD=57600
+CONFIG_USART3_IFLOWCONTROL=y
+CONFIG_USART3_OFLOWCONTROL=y
+CONFIG_USART3_RXBUFSIZE=600
+CONFIG_USART3_TXBUFSIZE=3000
+CONFIG_USART6_BAUD=57600
+CONFIG_USART6_RXBUFSIZE=600
+CONFIG_USART6_TXBUFSIZE=1500
+CONFIG_USBDEV=y
+CONFIG_USBDEV_BUSPOWERED=y
+CONFIG_USBDEV_MAXPOWER=500
+CONFIG_USEC_PER_TICK=1000
+CONFIG_USERMAIN_STACKSIZE=2944
+CONFIG_USER_ENTRYPOINT="nsh_main"
+CONFIG_WATCHDOG=y
@@ -0,0 +1,22 @@
+CONFIG_DRIVERS_ADC_ADS1115=n
+CONFIG_DRIVERS_IMU_ANALOG_DEVICES_ADIS16448=n
+CONFIG_DRIVERS_IRLOCK=n
+CONFIG_DRIVERS_PCA9685=n
+CONFIG_DRIVERS_PCA9685_PWM_OUT=n
+CONFIG_DRIVERS_RPM=n
+CONFIG_DRIVERS_UAVCAN=n
+CONFIG_MODULES_ATTITUDE_ESTIMATOR_Q=n
+CONFIG_MODULES_LOCAL_POSITION_ESTIMATOR=n
+CONFIG_MODULES_ROVER_POS_CONTROL=n
+CONFIG_MODULES_TEMPERATURE_COMPENSATION=n
+CONFIG_DRIVERS_UAVCAN_V1=y
+CONFIG_UAVCAN_V1_BMS_SUBSCRIBER=y
+CONFIG_UAVCAN_V1_ESC_CONTROLLER=y
+CONFIG_UAVCAN_V1_ESC_SUBSCRIBER=y
+CONFIG_UAVCAN_V1_GNSS_PUBLISHER=y
+CONFIG_UAVCAN_V1_GNSS_SUBSCRIBER_0=y
+CONFIG_UAVCAN_V1_GNSS_SUBSCRIBER_1=y
+CONFIG_UAVCAN_V1_READINESS_PUBLISHER=y
+CONFIG_UAVCAN_V1_UORB_ACTUATOR_OUTPUTS_PUBLISHER=y
+CONFIG_UAVCAN_V1_UORB_SENSOR_GPS_PUBLISHER=y
+CONFIG_UAVCAN_V1_UORB_SENSOR_GPS_SUBSCRIBER=y
@@ -15,7 +15,6 @@ CONFIG_DRIVERS_OSD=y
 CONFIG_DRIVERS_PWM_OUT=y
 CONFIG_DRIVERS_RC_INPUT=y
 CONFIG_DRIVERS_TELEMETRY_FRSKY_TELEMETRY=y
-CONFIG_DRIVERS_TONE_ALARM=y
 CONFIG_MODULES_ATTITUDE_ESTIMATOR_Q=y
 CONFIG_MODULES_BATTERY_STATUS=y
 CONFIG_MODULES_COMMANDER=y
@@ -27,7 +26,6 @@ CONFIG_MODULES_LOGGER=y
 CONFIG_MODULES_MANUAL_CONTROL=y
 CONFIG_MODULES_MAVLINK=y
 CONFIG_MODULES_MC_ATT_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
@@ -90,6 +90,7 @@ CONFIG_IDLETHREAD_STACKSIZE=750
 CONFIG_LIBC_FLOATINGPOINT=y
 CONFIG_LIBC_LONG_LONG=y
 CONFIG_LIBC_STRERROR=y
+CONFIG_LIBC_STRERROR_SHORT=y
 CONFIG_MEMSET_64BIT=y
 CONFIG_MEMSET_OPTSPEED=y
 CONFIG_MMCSD=y
@@ -133,6 +133,8 @@

 #define GPIO_RSSI_IN                       /* PC5  */ (GPIO_INPUT|GPIO_PULLDOWN|GPIO_PORTC|GPIO_PIN5)

+#define GPIO_RF_SWITCH                     /* PE13  */ (GPIO_OUTPUT|GPIO_PULLDOWN|GPIO_PORTE|GPIO_PIN13)
+
 /* Power switch controls ******************************************************/

 #define SDIO_SLOTNO                    0  /* Only one slot */
@@ -175,6 +177,7 @@
 		PX4_ADC_GPIO,                     \
 		GPIO_TONE_ALARM_IDLE,             \
 		GPIO_RSSI_IN,                     \
+		GPIO_RF_SWITCH,                   \
 	}

 #define BOARD_ENABLE_CONSOLE_BUFFER
@@ -160,6 +160,9 @@ stm32_boardinitialize(void)

 	board_control_spi_sensors_power_configgpio();

+	/* Turn bluetooth off by default (no mavlink support yet) */
+	px4_arch_gpiowrite(GPIO_RF_SWITCH, 0);
+
 	/* configure USB interfaces */

 	stm32_usbinitialize();
@@ -284,17 +284,17 @@

 #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
+/* 20 MHz Max for now - more reliable on some boards than 25 MHz
+ * 20 MHz = PLL1Q/(2*div), div =  PLL1Q/(2*freq), div = 6 = 240 / 40
 */

 #if defined(CONFIG_STM32H7_SDMMC_XDMA) || defined(CONFIG_STM32H7_SDMMC_IDMA)
-#  define STM32_SDMMC_MMCXFR_CLKDIV   (5 << STM32_SDMMC_CLKCR_CLKDIV_SHIFT)
+#  define STM32_SDMMC_MMCXFR_CLKDIV   (6 << 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)
+#  define STM32_SDMMC_SDXFR_CLKDIV    (6 << STM32_SDMMC_CLKCR_CLKDIV_SHIFT)
 #else
 #  define STM32_SDMMC_SDXFR_CLKDIV    (100 << STM32_SDMMC_CLKCR_CLKDIV_SHIFT)
 #endif
@@ -111,8 +111,8 @@

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

 #define BOARD_HAS_PWM  DIRECT_PWM_OUTPUT_CHANNELS

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

 constexpr timer_io_channels_t timer_io_channels[MAX_TIMER_IO_CHANNELS] = {
@@ -49,10 +49,10 @@ constexpr timer_io_channels_t timer_io_channels[MAX_TIMER_IO_CHANNELS] = {
 	initIOTimerChannel(io_timers, {Timer::Timer5, Timer::Channel4}, {GPIO::PortA, GPIO::Pin3}),
 	initIOTimerChannel(io_timers, {Timer::Timer4, Timer::Channel1}, {GPIO::PortD, GPIO::Pin12}),
 	initIOTimerChannel(io_timers, {Timer::Timer4, Timer::Channel2}, {GPIO::PortD, GPIO::Pin13}),
+	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::Timer4, Timer::Channel3}, {GPIO::PortD, GPIO::Pin14}),
 	// initIOTimerChannel(io_timers, {Timer::Timer4, Timer::Channel4}, {GPIO::PortD, GPIO::Pin15}),
-	// initIOTimerChannel(io_timers, {Timer::Timer15, Timer::Channel1}, {GPIO::PortE, GPIO::Pin5}),
-	// initIOTimerChannel(io_timers, {Timer::Timer15, Timer::Channel2}, {GPIO::PortE, GPIO::Pin6}),
 };

 constexpr io_timers_channel_mapping_t io_timers_channel_mapping =
@@ -0,0 +1,249 @@
+#
+# This file is autogenerated: PLEASE DO NOT EDIT IT.
+#
+# You can use "make menuconfig" to make any modifications to the installed .config file.
+# You can then do "make savedefconfig" to generate a new defconfig file that includes your
+# modifications.
+#
+# CONFIG_DISABLE_ENVIRON is not set
+# CONFIG_DISABLE_PSEUDOFS_OPERATIONS is not set
+# CONFIG_MMCSD_HAVE_CARDDETECT is not set
+# CONFIG_MMCSD_HAVE_WRITEPROTECT is not set
+# CONFIG_MMCSD_MMCSUPPORT is not set
+# CONFIG_MMCSD_SPI is not set
+# CONFIG_NET_CAN_CANFD is not set
+# CONFIG_NET_ETHERNET is not set
+# CONFIG_NET_IPv4 is not set
+# CONFIG_NSH_DISABLEBG is not set
+# CONFIG_NSH_DISABLESCRIPT is not set
+# CONFIG_NSH_DISABLE_DF is not set
+# CONFIG_NSH_DISABLE_EXEC is not set
+# CONFIG_NSH_DISABLE_EXIT is not set
+# CONFIG_NSH_DISABLE_GET is not set
+# CONFIG_NSH_DISABLE_ITEF is not set
+# CONFIG_NSH_DISABLE_LOOPS is not set
+# CONFIG_NSH_DISABLE_MKFATFS is not set
+# CONFIG_NSH_DISABLE_SEMICOLON is not set
+# CONFIG_NSH_DISABLE_TIME is not set
+CONFIG_ARCH="arm"
+CONFIG_ARCH_BOARD_CUSTOM=y
+CONFIG_ARCH_BOARD_CUSTOM_DIR="../../../../boards/mro/ctrl-zero-h7-oem/nuttx-config"
+CONFIG_ARCH_BOARD_CUSTOM_DIR_RELPATH=y
+CONFIG_ARCH_BOARD_CUSTOM_NAME="px4"
+CONFIG_ARCH_CHIP="stm32h7"
+CONFIG_ARCH_CHIP_STM32H743II=y
+CONFIG_ARCH_CHIP_STM32H7=y
+CONFIG_ARCH_INTERRUPTSTACK=512
+CONFIG_ARCH_STACKDUMP=y
+CONFIG_ARMV7M_BASEPRI_WAR=y
+CONFIG_ARMV7M_DCACHE=y
+CONFIG_ARMV7M_DTCM=y
+CONFIG_ARMV7M_ICACHE=y
+CONFIG_ARMV7M_MEMCPY=y
+CONFIG_ARMV7M_USEBASEPRI=y
+CONFIG_ARM_MPU_EARLY_RESET=y
+CONFIG_BOARDCTL_RESET=y
+CONFIG_BOARD_CRASHDUMP=y
+CONFIG_BOARD_LOOPSPERMSEC=95150
+CONFIG_BOARD_RESET_ON_ASSERT=2
+CONFIG_BUILTIN=y
+CONFIG_C99_BOOL8=y
+CONFIG_CDCACM=y
+CONFIG_CDCACM_IFLOWCONTROL=y
+CONFIG_CDCACM_PRODUCTID=0x1024
+CONFIG_CDCACM_PRODUCTSTR="mRoControlZeroH7 OEM"
+CONFIG_CDCACM_RXBUFSIZE=600
+CONFIG_CDCACM_TXBUFSIZE=12000
+CONFIG_CDCACM_VENDORID=0x26ac
+CONFIG_CDCACM_VENDORSTR="mRo"
+CONFIG_CLOCK_MONOTONIC=y
+CONFIG_DEBUG_FULLOPT=y
+CONFIG_DEBUG_HARDFAULT_ALERT=y
+CONFIG_DEBUG_MEMFAULT=y
+CONFIG_DEBUG_SYMBOLS=y
+CONFIG_DEFAULT_SMALL=y
+CONFIG_DEV_FIFO_SIZE=0
+CONFIG_DEV_PIPE_MAXSIZE=1024
+CONFIG_DEV_PIPE_SIZE=70
+CONFIG_DISABLE_MQUEUE=y
+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_LIBC_FLOATINGPOINT=y
+CONFIG_LIBC_LONG_LONG=y
+CONFIG_LIBC_STRERROR=y
+CONFIG_MEMSET_64BIT=y
+CONFIG_MEMSET_OPTSPEED=y
+CONFIG_MMCSD=y
+CONFIG_MMCSD_SDIO=y
+CONFIG_MMCSD_SDIOWAIT_WRCOMPLETE=y
+CONFIG_MM_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_NET=y
+CONFIG_NETDEV_CAN_BITRATE_IOCTL=y
+CONFIG_NETDEV_IFINDEX=y
+CONFIG_NETDEV_PHY_IOCTL=y
+CONFIG_NET_CAN=y
+CONFIG_NET_CAN_RAW_TX_DEADLINE=y
+CONFIG_NET_CAN_SOCK_OPTS=y
+CONFIG_NET_TIMESTAMP=y
+CONFIG_NSH_ARCHINIT=y
+CONFIG_NSH_ARGCAT=y
+CONFIG_NSH_BUILTIN_APPS=y
+CONFIG_NSH_CMDPARMS=y
+CONFIG_NSH_CROMFSETC=y
+CONFIG_NSH_DISABLE_IFCONFIG=y
+CONFIG_NSH_DISABLE_IFUPDOWN=y
+CONFIG_NSH_DISABLE_TELNETD=y
+CONFIG_NSH_LINELEN=128
+CONFIG_NSH_MAXARGUMENTS=15
+CONFIG_NSH_NESTDEPTH=8
+CONFIG_NSH_QUOTE=y
+CONFIG_NSH_ROMFSETC=y
+CONFIG_NSH_ROMFSSECTSIZE=128
+CONFIG_NSH_STRERROR=y
+CONFIG_NSH_VARS=y
+CONFIG_OTG_ID_GPIO_DISABLE=y
+CONFIG_PIPES=y
+CONFIG_PREALLOC_TIMERS=50
+CONFIG_PRIORITY_INHERITANCE=y
+CONFIG_PTHREAD_MUTEX_ROBUST=y
+CONFIG_PTHREAD_STACK_MIN=512
+CONFIG_RAMTRON_SETSPEED=y
+CONFIG_RAM_SIZE=245760
+CONFIG_RAM_START=0x20010000
+CONFIG_RAW_BINARY=y
+CONFIG_READLINE_CMD_HISTORY=y
+CONFIG_READLINE_TABCOMPLETION=y
+CONFIG_RTC_DATETIME=y
+CONFIG_SCHED_ATEXIT=y
+CONFIG_SCHED_HPWORK=y
+CONFIG_SCHED_HPWORKPRIORITY=249
+CONFIG_SCHED_HPWORKSTACKSIZE=1280
+CONFIG_SCHED_INSTRUMENTATION=y
+CONFIG_SCHED_INSTRUMENTATION_EXTERNAL=y
+CONFIG_SCHED_LPWORK=y
+CONFIG_SCHED_LPWORKPRIORITY=50
+CONFIG_SCHED_LPWORKSTACKSIZE=1632
+CONFIG_SCHED_WAITPID=y
+CONFIG_SDCLONE_DISABLE=y
+CONFIG_SDMMC1_SDIO_PULLUP=y
+CONFIG_SEM_NNESTPRIO=8
+CONFIG_SEM_PREALLOCHOLDERS=0
+CONFIG_SERIAL_IFLOWCONTROL_WATERMARKS=y
+CONFIG_SERIAL_TERMIOS=y
+CONFIG_SIG_DEFAULT=y
+CONFIG_SIG_SIGALRM_ACTION=y
+CONFIG_SIG_SIGUSR1_ACTION=y
+CONFIG_SIG_SIGUSR2_ACTION=y
+CONFIG_SIG_SIGWORK=4
+CONFIG_STACK_COLORATION=y
+CONFIG_START_DAY=30
+CONFIG_START_MONTH=11
+CONFIG_STDIO_BUFFER_SIZE=256
+CONFIG_STM32H7_ADC1=y
+CONFIG_STM32H7_ADC3=y
+CONFIG_STM32H7_BBSRAM=y
+CONFIG_STM32H7_BBSRAM_FILES=5
+CONFIG_STM32H7_BDMA=y
+CONFIG_STM32H7_BKPSRAM=y
+CONFIG_STM32H7_DMA1=y
+CONFIG_STM32H7_DMA2=y
+CONFIG_STM32H7_DMACAPABLE=y
+CONFIG_STM32H7_FDCAN1=y
+CONFIG_STM32H7_FLOWCONTROL_BROKEN=y
+CONFIG_STM32H7_I2C1=y
+CONFIG_STM32H7_I2C3=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_SDMMC1=y
+CONFIG_STM32H7_SERIALBRK_BSDCOMPAT=y
+CONFIG_STM32H7_SERIAL_DISABLE_REORDERING=y
+CONFIG_STM32H7_SPI1=y
+CONFIG_STM32H7_SPI1_DMA=y
+CONFIG_STM32H7_SPI1_DMA_BUFFER=1024
+CONFIG_STM32H7_SPI2=y
+CONFIG_STM32H7_SPI5=y
+CONFIG_STM32H7_SPI5_DMA=y
+CONFIG_STM32H7_SPI5_DMA_BUFFER=1024
+CONFIG_STM32H7_SPI_DMA=y
+CONFIG_STM32H7_TIM1=y
+CONFIG_STM32H7_TIM2=y
+CONFIG_STM32H7_TIM3=y
+CONFIG_STM32H7_TIM4=y
+CONFIG_STM32H7_TIM8=y
+CONFIG_STM32H7_UART4=y
+CONFIG_STM32H7_UART7=y
+CONFIG_STM32H7_UART8=y
+CONFIG_STM32H7_USART2=y
+CONFIG_STM32H7_USART3=y
+CONFIG_STM32H7_USART6=y
+CONFIG_STM32H7_USART_BREAKS=y
+CONFIG_STM32H7_USART_INVERT=y
+CONFIG_STM32H7_USART_SINGLEWIRE=y
+CONFIG_STM32H7_USART_SWAP=y
+CONFIG_SYSTEM_CDCACM=y
+CONFIG_SYSTEM_NSH=y
+CONFIG_TASK_NAME_SIZE=24
+CONFIG_TTY_SIGINT=y
+CONFIG_TTY_SIGTSTP=y
+CONFIG_UART4_BAUD=57600
+CONFIG_UART4_RXBUFSIZE=600
+CONFIG_UART4_TXBUFSIZE=1500
+CONFIG_UART7_BAUD=57600
+CONFIG_UART7_RXBUFSIZE=600
+CONFIG_UART7_TXBUFSIZE=1500
+CONFIG_UART8_BAUD=57600
+CONFIG_UART8_RXBUFSIZE=600
+CONFIG_UART8_SERIAL_CONSOLE=y
+CONFIG_UART8_TXBUFSIZE=1500
+CONFIG_USART2_BAUD=57600
+CONFIG_USART2_IFLOWCONTROL=y
+CONFIG_USART2_OFLOWCONTROL=y
+CONFIG_USART2_RXBUFSIZE=600
+CONFIG_USART2_TXBUFSIZE=1500
+CONFIG_USART3_BAUD=57600
+CONFIG_USART3_IFLOWCONTROL=y
+CONFIG_USART3_OFLOWCONTROL=y
+CONFIG_USART3_RXBUFSIZE=600
+CONFIG_USART3_TXBUFSIZE=3000
+CONFIG_USART6_BAUD=57600
+CONFIG_USART6_RXBUFSIZE=600
+CONFIG_USART6_TXBUFSIZE=1500
+CONFIG_USBDEV=y
+CONFIG_USBDEV_BUSPOWERED=y
+CONFIG_USBDEV_MAXPOWER=500
+CONFIG_USEC_PER_TICK=1000
+CONFIG_USERMAIN_STACKSIZE=2944
+CONFIG_USER_ENTRYPOINT="nsh_main"
@@ -0,0 +1,13 @@
+CONFIG_DRIVERS_UAVCAN=n
+CONFIG_DRIVERS_UAVCAN_V1=y
+CONFIG_UAVCAN_V1_BMS_SUBSCRIBER=y
+CONFIG_UAVCAN_V1_CLIENT=y
+CONFIG_UAVCAN_V1_ESC_CONTROLLER=y
+CONFIG_UAVCAN_V1_ESC_SUBSCRIBER=y
+CONFIG_UAVCAN_V1_GNSS_PUBLISHER=y
+CONFIG_UAVCAN_V1_GNSS_SUBSCRIBER_0=y
+CONFIG_UAVCAN_V1_GNSS_SUBSCRIBER_1=y
+CONFIG_UAVCAN_V1_READINESS_PUBLISHER=y
+CONFIG_UAVCAN_V1_UORB_ACTUATOR_OUTPUTS_PUBLISHER=y
+CONFIG_UAVCAN_V1_UORB_SENSOR_GPS_PUBLISHER=y
+CONFIG_UAVCAN_V1_UORB_SENSOR_GPS_SUBSCRIBER=y
@@ -228,6 +228,8 @@
 #define GPIO_nARMED          /* GPIO_SD_B1_01 GPIO3_IO1 */ (GPIO_PORT3 | GPIO_PIN1 | GPIO_OUTPUT | GPIO_OUTPUT_ZERO | nARMED_OUTPUT_IOMUX)

 #define BOARD_INDICATE_EXTERNAL_LOCKOUT_STATE(enabled)  px4_arch_configgpio((enabled) ? GPIO_nARMED : GPIO_nARMED_INIT)
+#define BOARD_GET_EXTERNAL_LOCKOUT_STATE() px4_arch_gpioread(GPIO_nARMED)
+

 /* PWM
 */
@@ -22,7 +22,6 @@ CONFIG_MODULES_COMMANDER=y
 CONFIG_MODULES_CONTROL_ALLOCATOR=y
 CONFIG_MODULES_DATAMAN=y
 CONFIG_MODULES_EKF2=y
-CONFIG_MODULES_EVENTS=y
 CONFIG_MODULES_FLIGHT_MODE_MANAGER=y
 CONFIG_MODULES_LAND_DETECTOR=y
 CONFIG_MODULES_LOGGER=y
@@ -34,7 +33,7 @@ CONFIG_MODULES_MC_RATE_CONTROL=y
 CONFIG_MODULES_NAVIGATOR=y
 CONFIG_MODULES_RC_UPDATE=y
 CONFIG_MODULES_SENSORS=y
-CONFIG_SYSTEMCMDS_DMESG=y
+CONFIG_SYSTEMCMDS_DMESG=n
 CONFIG_SYSTEMCMDS_HARDFAULT_LOG=y
 CONFIG_SYSTEMCMDS_MIXER=y
 CONFIG_SYSTEMCMDS_PARAM=y
@@ -0,0 +1,3 @@
+CONFIG_DRIVERS_IMU_INVENSENSE_ICM20602=n
+CONFIG_DRIVERS_IMU_INVENSENSE_MPU6000=n
+CONFIG_DRIVERS_IMU_INVENSENSE_ICM20608G=y
@@ -8,7 +8,11 @@ board_adc start
 if ! mpu6000 -R 6 -s start
 then
 	# some boards such as the Hobbywing XRotor F4 G2 use the ICM-20602
-	icm20602 -s -R 6 start
+	if ! icm20602 -s -R 6 start
+	then
+		# some clones of Omnibus F4 use the ICM-20608G (such as F4 PDB for Martian II FPV Drone)
+		icm20608g -s -R 6 start
+	fi
 fi

 bmp280 -s start
@@ -39,6 +39,7 @@ constexpr px4_spi_bus_t px4_spi_buses[SPI_BUS_MAX_BUS_ITEMS] = {
 	initSPIBus(SPI::Bus::SPI1, {
 		initSPIDevice(DRV_IMU_DEVTYPE_MPU6000, SPI::CS{GPIO::PortA, GPIO::Pin4}),
 		initSPIDevice(DRV_IMU_DEVTYPE_ICM20602, SPI::CS{GPIO::PortA, GPIO::Pin4}),
+		initSPIDevice(DRV_IMU_DEVTYPE_ICM20608G, SPI::CS{GPIO::PortA, GPIO::Pin4}),
 	}),
 	initSPIBus(SPI::Bus::SPI2, {
 		initSPIDevice(SPIDEV_MMCSD(0), SPI::CS{GPIO::PortB, GPIO::Pin12})
@@ -9,7 +9,6 @@ CONFIG_BOARD_SERIAL_TEL2="/dev/ttyS2"
 CONFIG_BOARD_SERIAL_TEL4="/dev/ttyS6"
 CONFIG_DRIVERS_ADC_BOARD_ADC=y
 CONFIG_DRIVERS_BAROMETER_MS5611=y
-CONFIG_DRIVERS_DIFFERENTIAL_PRESSURE_MS4525=y
 CONFIG_DRIVERS_GPS=y
 CONFIG_DRIVERS_IMU_INVENSENSE_MPU6000=y
 CONFIG_DRIVERS_IMU_L3GD20=y
@@ -21,6 +20,7 @@ CONFIG_DRIVERS_PX4IO=y
 CONFIG_DRIVERS_TONE_ALARM=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_FLIGHT_MODE_MANAGER=y
@@ -32,7 +32,6 @@ CONFIG_MODULES_MC_ATT_CONTROL=y
 CONFIG_MODULES_MC_HOVER_THRUST_ESTIMATOR=y
 CONFIG_MODULES_MC_POS_CONTROL=y
 CONFIG_MODULES_MC_RATE_CONTROL=y
-CONFIG_MODULES_CONTROL_ALLOCATOR=y
 CONFIG_MODULES_NAVIGATOR=y
 CONFIG_MODULES_RC_UPDATE=y
 CONFIG_MODULES_SENSORS=y
@@ -12,7 +12,6 @@ CONFIG_MODULES_CAMERA_FEEDBACK=y
 CONFIG_MODULES_LOAD_MON=y
 CONFIG_MODULES_ROVER_POS_CONTROL=y
 CONFIG_MODULES_TEMPERATURE_COMPENSATION=y
-CONFIG_SYSTEMCMDS_I2CDETECT=y
 CONFIG_SYSTEMCMDS_PERF=y
 CONFIG_SYSTEMCMDS_UORB=y
 CONFIG_SYSTEMCMDS_USB_CONNECTED=y
@@ -212,6 +212,7 @@
 #define GPIO_nARMED          /* PI0 */  (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_2MHz|GPIO_OUTPUT_CLEAR|GPIO_PORTI|GPIO_PIN0)

 #define BOARD_INDICATE_EXTERNAL_LOCKOUT_STATE(enabled)  px4_arch_configgpio((enabled) ? GPIO_nARMED : GPIO_nARMED_INIT)
+#define BOARD_GET_EXTERNAL_LOCKOUT_STATE() px4_arch_gpioread(GPIO_nARMED)

 /* PWM
 */
@@ -14,13 +14,16 @@ CONFIG_DRIVERS_PCA9685=n
 CONFIG_DRIVERS_PCA9685_PWM_OUT=n
 CONFIG_DRIVERS_POWER_MONITOR_INA226=n
 CONFIG_DRIVERS_PWM_INPUT=n
+CONFIG_DRIVERS_PWM_OUT_SIM=n
 CONFIG_DRIVERS_ROBOCLAW=n
 CONFIG_DRIVERS_RPM=n
 CONFIG_DRIVERS_SMART_BATTERY_BATMON=n
 CONFIG_DRIVERS_UAVCAN=n
+CONFIG_EXAMPLES_FAKE_GPS=n
 CONFIG_MODULES_ATTITUDE_ESTIMATOR_Q=n
 CONFIG_MODULES_CAMERA_FEEDBACK=n
 CONFIG_MODULES_ESC_BATTERY=n
+CONFIG_MODULES_GIMBAL=n
 CONFIG_MODULES_GYRO_CALIBRATION=n
 CONFIG_MODULES_GYRO_FFT=n
 CONFIG_MODULES_LANDING_TARGET_ESTIMATOR=n
@@ -28,6 +31,6 @@ CONFIG_MODULES_LOCAL_POSITION_ESTIMATOR=n
 CONFIG_MODULES_ROVER_POS_CONTROL=n
 CONFIG_MODULES_SIH=n
 CONFIG_MODULES_TEMPERATURE_COMPENSATION=n
-CONFIG_MODULES_GIMBAL=n
+CONFIG_SYSTEMCMDS_SERIAL_TEST=n
 CONFIG_BOARD_CONSTRAINED_FLASH=y
 CONFIG_BOARD_TESTING=y
@@ -213,6 +213,7 @@

 #if !defined(TRACE_PINS)
 #  define BOARD_INDICATE_EXTERNAL_LOCKOUT_STATE(enabled)  px4_arch_configgpio((enabled) ? GPIO_nARMED : GPIO_nARMED_INIT)
+#  define BOARD_GET_EXTERNAL_LOCKOUT_STATE() px4_arch_gpioread(GPIO_nARMED)
 #endif
 /* PWM
 */
@@ -72,19 +72,22 @@ 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_USART3=y
+CONFIG_STM32H7_UART7=y
 CONFIG_SYSTEMTICK_HOOK=y
 CONFIG_SYSTEM_CDCACM=y
 CONFIG_TASK_NAME_SIZE=24
 CONFIG_TTY_SIGINT=y
 CONFIG_TTY_SIGINT_CHAR=0x03
 CONFIG_TTY_SIGTSTP=y
-CONFIG_USART3_RXBUFSIZE=600
-CONFIG_USART3_TXBUFSIZE=300
+CONFIG_UART7_RXBUFSIZE=512
+CONFIG_UART7_RXDMA=y
+CONFIG_UART7_TXBUFSIZE=512
+CONFIG_UART7_TXDMA=y
 CONFIG_USBDEV=y
 CONFIG_USBDEV_BUSPOWERED=y
 CONFIG_USBDEV_MAXPOWER=500
@@ -33,19 +33,55 @@

 #pragma once

-// DMAMUX1
-#define DMAMAP_SPI1_RX    DMAMAP_DMA12_SPI1RX_0 /* DMA1:37 */
-#define DMAMAP_SPI1_TX    DMAMAP_DMA12_SPI1TX_0 /* DMA1:38 */
+// DMAMUX1 Using at most 8 Channels on DMA1 --------   Assigned
+//                                                     V

-#define DMAMAP_SPI2_RX    DMAMAP_DMA12_SPI2RX_0 /* DMA1:39 */
-#define DMAMAP_SPI2_TX    DMAMAP_DMA12_SPI2TX_0 /* DMA1:40 */
+#define DMAMAP_SPI1_RX    DMAMAP_DMA12_SPI1RX_0     /* 1 DMA1:37 ICM-20649 */
+#define DMAMAP_SPI1_TX    DMAMAP_DMA12_SPI1TX_0     /* 2 DMA1:38 ICM-20649 */

-#define DMAMAP_USART6_RX   DMAMAP_DMA12_USART6RX_0 /* DMA1:71 */
-#define DMAMAP_USART6_TX   DMAMAP_DMA12_USART6TX_0 /* DMA1:72 */
+#define DMAMAP_SPI2_RX    DMAMAP_DMA12_SPI2RX_0     /* 3 DMA1:39 ICM-42688-P */
+#define DMAMAP_SPI2_TX    DMAMAP_DMA12_SPI2TX_0     /* 4 DMA1:40 ICM-42688-P */

-// DMAMUX2
-#define DMAMAP_SPI3_RX    DMAMAP_DMA12_SPI3RX_1 /* DMA2:61 */
-#define DMAMAP_SPI3_TX    DMAMAP_DMA12_SPI3TX_1 /* DMA2:62 */
+//#define DMAMAP_USART1_RX  DMAMAP_DMA12_USART1RX_0 /*  DMA1:41  GPS1 */
+//#define DMAMAP_USART1_TX  DMAMAP_DMA12_USART1TX_0 /*  DMA1:42  GPS1 */

-#define DMAMAP_SPI6_RX    DMAMAP_BDMA_SPI6_RX /* BDMA:11 */
-#define DMAMAP_SPI6_TX    DMAMAP_BDMA_SPI6_TX /* BDMA:12 */
+//#define DMAMAP_USART2_RX  DMAMAP_DMA12_USART2RX_0 /*  DMA1:43 Telem3 */
+//#define DMAMAP_USART2_TX  DMAMAP_DMA12_USART2TX_0 /*  DMA1:44 Telem3 */
+
+//#define DMAMAP_USART3_RX  DMAMAP_DMA12_USART3RX_0 /*  DMA1:45 DEBUG */
+//#define DMAMAP_USART3_TX  DMAMAP_DMA12_USART3TX_0 /*  DMA1:46 DEBUG */
+
+//#define DMAMAP_UART4_RX   DMAMAP_DMA12_UART4RX_0  /*  DMA1:63 EXT2 */
+//#define DMAMAP_UART4_TX   DMAMAP_DMA12_UART4TX_0  /*  DMA1:64 EXT2 */
+
+#define DMAMAP_USART6_RX  DMAMAP_DMA12_USART6RX_0   /* 5 DMA1:71 PX4IO */
+#define DMAMAP_USART6_TX  DMAMAP_DMA12_USART6TX_0   /* 6 DMA1:72 PX4IO */
+
+// Assigned in timer_config.cpp
+
+// Timer 4                                          /* 7 DMA1:32 TIM4UP */
+// Timer 5                                          /* 8 DMA1:50 TIM5UP */
+
+// DMAMUX2 Using at most 8 Channels on DMA2 --------   Assigned
+//                                                     V
+
+#define DMAMAP_SPI3_RX    DMAMAP_DMA12_SPI3RX_1     /* 1 DMA2:61 BMI088 */
+#define DMAMAP_SPI3_TX    DMAMAP_DMA12_SPI3TX_1     /* 2 DMA2:62 BMI088 */
+
+#define DMAMAP_USART3_RX  DMAMAP_DMA12_USART3RX_1   /* 3 DMA2:45 DEBUG */
+#define DMAMAP_USART3_TX  DMAMAP_DMA12_USART3TX_1   /* 4 DMA2:46 DEBUG */
+
+#define DMAMAP_UART5_RX   DMAMAP_DMA12_UART5RX_1    /* 5 DMA2:65 TELEM2 */
+#define DMAMAP_UART5_TX   DMAMAP_DMA12_UART5TX_1    /* 6 DMA2:66  TELEM2 */
+
+#define DMAMAP_UART7_RX   DMAMAP_DMA12_UART7RX_1    /* 7 DMA1:79  TELEM1 */
+#define DMAMAP_UART7_TX   DMAMAP_DMA12_UART7TX_1    /* 8 DMA1:80  TELEM1 */
+
+//#define DMAMAP_UART8_RX   DMAMAP_DMA12_UART8RX_1  /*  DMA1:81  GPS2 */
+//#define DMAMAP_UART8_TX   DMAMAP_DMA12_UART8TX_1  /*  DMA1:82  GPS2 */
+
+// DMAMUX2 Using at most 8 Channels on BDMA --------   Assigned
+//                                                     V
+
+#define DMAMAP_SPI6_RX    DMAMAP_BDMA_SPI6_RX       /* 1 BDMA:11 SPI J11 */
+#define DMAMAP_SPI6_TX    DMAMAP_BDMA_SPI6_TX       /* 2 BDMA:12 SPI J11 */
@@ -257,11 +257,16 @@ CONFIG_UART4_RXBUFSIZE=600
 CONFIG_UART4_TXBUFSIZE=1500
 CONFIG_UART5_IFLOWCONTROL=y
 CONFIG_UART5_OFLOWCONTROL=y
+CONFIG_UART5_RXDMA=y
+CONFIG_UART5_TXBUFSIZE=3000
+CONFIG_UART5_TXDMA=y
 CONFIG_UART7_BAUD=57600
 CONFIG_UART7_IFLOWCONTROL=y
 CONFIG_UART7_OFLOWCONTROL=y
 CONFIG_UART7_RXBUFSIZE=600
+CONFIG_UART7_RXDMA=y
 CONFIG_UART7_TXBUFSIZE=3000
+CONFIG_UART7_TXDMA=y
 CONFIG_UART8_BAUD=57600
 CONFIG_UART8_RXBUFSIZE=600
 CONFIG_UART8_TXBUFSIZE=1500
@@ -275,8 +280,10 @@ CONFIG_USART2_RXBUFSIZE=600
 CONFIG_USART2_TXBUFSIZE=3000
 CONFIG_USART3_BAUD=57600
 CONFIG_USART3_RXBUFSIZE=180
+CONFIG_USART3_RXDMA=y
 CONFIG_USART3_SERIAL_CONSOLE=y
 CONFIG_USART3_TXBUFSIZE=1500
+CONFIG_USART3_TXDMA=y
 CONFIG_USART6_BAUD=57600
 CONFIG_USART6_RXBUFSIZE=600
 CONFIG_USART6_TXBUFSIZE=1500
@@ -229,6 +229,7 @@
 #define GPIO_nARMED_INIT     /* PE6 */  (GPIO_INPUT|GPIO_PULLUP|GPIO_PORTE|GPIO_PIN6)
 #define GPIO_nARMED          /* PE6 */  (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_2MHz|GPIO_OUTPUT_CLEAR|GPIO_PORTE|GPIO_PIN6)
 #define BOARD_INDICATE_EXTERNAL_LOCKOUT_STATE(enabled)  px4_arch_configgpio((enabled) ? GPIO_nARMED : GPIO_nARMED_INIT)
+#define BOARD_GET_EXTERNAL_LOCKOUT_STATE() px4_arch_gpioread(GPIO_nARMED)
 #endif


@@ -13,7 +13,7 @@ if(REPLAY_FILE)

 	message(STATUS "Building without lockstep for replay")
 	set(ENABLE_LOCKSTEP_SCHEDULER no)
-elif(CMAKE_BUILD_TYPE STREQUAL FuzzTesting)
+elseif(CMAKE_BUILD_TYPE STREQUAL FuzzTesting)
 	set(ENABLE_LOCKSTEP_SCHEDULER no)
 else()
 	set(ENABLE_LOCKSTEP_SCHEDULER yes)
@@ -8,6 +8,7 @@ CONFIG_COMMON_DISTANCE_SENSOR=y
 CONFIG_DRIVERS_DSHOT=y
 CONFIG_DRIVERS_GPS=y
 CONFIG_DRIVERS_IMU_INVENSENSE_ICM20602=y
+CONFIG_DRIVERS_IMU_INVENSENSE_ICM42688P=y
 CONFIG_DRIVERS_IMU_INVENSENSE_MPU6000=y
 CONFIG_COMMON_LIGHT=y
 CONFIG_COMMON_MAGNETOMETER=y
@@ -21,9 +22,11 @@ CONFIG_DRIVERS_TONE_ALARM=y
 CONFIG_MODULES_ATTITUDE_ESTIMATOR_Q=y
 CONFIG_MODULES_BATTERY_STATUS=y
 CONFIG_MODULES_COMMANDER=y
+CONFIG_MODULES_CONTROL_ALLOCATOR=y
 CONFIG_MODULES_DATAMAN=y
 CONFIG_MODULES_EVENTS=y
 CONFIG_MODULES_FLIGHT_MODE_MANAGER=y
+CONFIG_MODULES_GIMBAL=y
 CONFIG_MODULES_LAND_DETECTOR=y
 CONFIG_MODULES_LOAD_MON=y
 CONFIG_MODULES_LOGGER=y
@@ -35,11 +38,9 @@ CONFIG_MODULES_MC_AUTOTUNE_ATTITUDE_CONTROL=y
 CONFIG_MODULES_MC_HOVER_THRUST_ESTIMATOR=y
 CONFIG_MODULES_MC_POS_CONTROL=y
 CONFIG_MODULES_MC_RATE_CONTROL=y
-CONFIG_MODULES_CONTROL_ALLOCATOR=y
 CONFIG_MODULES_NAVIGATOR=y
 CONFIG_MODULES_RC_UPDATE=y
 CONFIG_MODULES_SENSORS=y
-CONFIG_MODULES_GIMBAL=y
 CONFIG_SYSTEMCMDS_DMESG=y
 CONFIG_SYSTEMCMDS_DUMPFILE=y
 CONFIG_SYSTEMCMDS_HARDFAULT_LOG=y
@@ -1,17 +1,23 @@
 #!/bin/sh
 #
-# SP Racing H7 EXTREME specific board sensors init
+# board specific sensors init
 #------------------------------------------------------------------------------
+
 board_adc start

-# Internal SPI bus ICM-20602
-#mpu6000 -s -b 2 -R 11 -T 20602 start # SPI 2
-#mpu6000 -s -b 3 -R 10 -T 20602 start # SPI 3
-icm20602 -s -b 2 -R 5 start # SPI 2
-icm20602 -s -b 3 -R 4 start # SPI 3
+# Internal SPI bus ICM-20602 or ICM-42688-P
+
+# SPI2
+if ! icm20602 -s -b 2 -R 5 start
+then
+	icm42688p -s -b 2 -R 5 start
+fi
+
+# SPI3
+if ! icm20602 -s -b 3 -R 4 start
+then
+	icm42688p -s -b 3 -R 4 start
+fi

 # Internal I2C bus
 bmp388 -I start
-
-
-
@@ -142,8 +142,8 @@ SECTIONS

 		*modules__mc_att_control.a:*(.text* .rodata*)
 		*modules__mc_rate_control.a:*(.text* .rodata*)
-		*modules__rc_update.a:*(.text* .rodata*)
 		*drivers__imu__invensense__icm20602.a:*(.text* .rodata*)
+		*drivers__imu__invensense__icm42688p.a:*(.text* .rodata*)
 		*drivers__device.a:*(.text* .rodata*)
 		*drivers__pwm_out.a:*(.text* .rodata*)
 		*vehicle_angular_velocity.a:*(.text* .rodata*)
@@ -163,6 +163,7 @@ SECTIONS
 		*modules__flight_mode_manager.a:*(.text* .rodata*)
 		*modules__mc_pos_control.a:*(.text* .rodata*)
 		*modules__mc_hover_thrust_estimator.a:*(.text* .rodata*)
+		*modules__rc_update.a:*(.text* .rodata*)
 		*modules__sensors.a:*(.text* .rodata*)
 		*libc.a:*(.text* .rodata*)
 		*modules__commander.a:*(.text* .rodata*)
@@ -64,7 +64,7 @@ set(msg_files
 	differential_pressure.msg
 	distance_sensor.msg
 	ekf2_timestamps.msg
-	ekf_gps_drift.msg
+	estimator_gps_status.msg
 	esc_report.msg
 	esc_status.msg
 	estimator_baro_bias.msg
@@ -3,3 +3,6 @@ uint8 NUM_ACTUATOR_OUTPUTS		= 16
 uint8 NUM_ACTUATOR_OUTPUT_GROUPS	= 4	# for sanity checking
 uint32 noutputs				# valid outputs
 float32[16] output				# output data, in natural output units
+
+# actuator_outputs_sim is used for SITL, HITL & SIH (with an output range of [-1, 1])
+# TOPICS actuator_outputs actuator_outputs_sim
@@ -16,7 +16,8 @@ uint8 MAIN_STATE_AUTO_LAND          = 11
 uint8 MAIN_STATE_AUTO_FOLLOW_TARGET = 12
 uint8 MAIN_STATE_AUTO_PRECLAND      = 13
 uint8 MAIN_STATE_ORBIT              = 14
-uint8 MAIN_STATE_MAX                = 15
+uint8 MAIN_STATE_AUTO_VTOL_TAKEOFF  = 15
+uint8 MAIN_STATE_MAX                = 16

 uint8 main_state

@@ -1,6 +0,0 @@
-uint64 timestamp		# time since system start (microseconds)
-float32 hpos_drift_rate		# Horizontal position rate magnitude checked using EKF2_REQ_HDRIFT (m/s)
-float32 vpos_drift_rate		# Vertical position rate magnitude checked using EKF2_REQ_VDRIFT (m/s)
-float32 hspd			# Filtered horizontal velocity magnitude checked using EKF2_REQ_HDRIFT (m/s)
-
-bool blocked			# true when drift calculation is blocked due to IMU movement check
@@ -0,0 +1,19 @@
+uint64 timestamp                          # time since system start (microseconds)
+uint64 timestamp_sample                   # the timestamp of the raw data (microseconds)
+
+bool checks_passed
+
+bool check_fail_gps_fix          # 0 : insufficient fix type (no 3D solution)
+bool check_fail_min_sat_count    # 1 : minimum required sat count fail
+bool check_fail_max_pdop         # 2 : maximum allowed PDOP fail
+bool check_fail_max_horz_err     # 3 : maximum allowed horizontal position error fail
+bool check_fail_max_vert_err     # 4 : maximum allowed vertical position error fail
+bool check_fail_max_spd_err      # 5 : maximum allowed speed error fail
+bool check_fail_max_horz_drift   # 6 : maximum allowed horizontal position drift fail - requires stationary vehicle
+bool check_fail_max_vert_drift   # 7 : maximum allowed vertical position drift fail - requires stationary vehicle
+bool check_fail_max_horz_spd_err # 8 : maximum allowed horizontal speed fail - requires stationary vehicle
+bool check_fail_max_vert_spd_err # 9 : maximum allowed vertical velocity discrepancy fail
+
+float32 position_drift_rate_horizontal_m_s # Horizontal position rate magnitude (m/s)
+float32 position_drift_rate_vertical_m_s   # Vertical position rate magnitude (m/s)
+float32 filtered_horizontal_speed_m_s      # Filtered horizontal velocity magnitude (m/s)
@@ -33,6 +33,8 @@ bool cs_synthetic_mag_z       # 25 - true when we are using a synthesized measur
 bool cs_vehicle_at_rest       # 26 - true when the vehicle is at rest
 bool cs_gps_yaw_fault         # 27 - true when the GNSS heading has been declared faulty and is no longer being used
 bool cs_rng_fault             # 28 - true when the range finder has been declared faulty and is no longer being used
+bool cs_inertial_dead_reckoning # 29 - true if we are no longer fusing measurements that constrain horizontal velocity drift
+bool cs_wind_dead_reckoning     # 30 - true if we are navigationg reliant on wind relative measurements

 # fault status
 uint32 fault_status_changes   # number of filter fault status (fs) changes
@@ -35,7 +35,7 @@ uint32 rx_packet_drop_count             # number of packet drops
 float32 rx_message_lost_rate


-uint64 HEARTBEAT_TIMEOUT_US = 1500000       # Heartbeat timeout 1.5 seconds
+uint64 HEARTBEAT_TIMEOUT_US = 2500000       # Heartbeat timeout (tolerate missing 1 + jitter)

 # Heartbeats per type
 bool heartbeat_type_antenna_tracker         # MAV_TYPE_ANTENNA_TRACKER
@@ -33,7 +33,7 @@ uint32 silence       # in us
 uint8 volume         # value between 0-100 if supported by backend

 uint8 VOLUME_LEVEL_MIN = 0
-uint8 VOLUME_LEVEL_DEFAULT = 40
+uint8 VOLUME_LEVEL_DEFAULT = 20
 uint8 VOLUME_LEVEL_MAX = 100

 uint8 ORB_QUEUE_LENGTH = 4
@@ -46,7 +46,8 @@ uint8 NAVIGATION_STATE_AUTO_LAND = 18		# Land
 uint8 NAVIGATION_STATE_AUTO_FOLLOW_TARGET = 19	# Auto Follow
 uint8 NAVIGATION_STATE_AUTO_PRECLAND = 20	# Precision land with landing target
 uint8 NAVIGATION_STATE_ORBIT = 21       # Orbit in a circle
-uint8 NAVIGATION_STATE_MAX = 22
+uint8 NAVIGATION_STATE_AUTO_VTOL_TAKEOFF = 22   # Takeoff, transition, establish loiter
+uint8 NAVIGATION_STATE_MAX = 23

 uint8 VEHICLE_TYPE_UNKNOWN = 0
 uint8 VEHICLE_TYPE_ROTARY_WING = 1
@@ -13,12 +13,19 @@ bool condition_local_altitude_valid
 bool condition_local_position_valid		# set to true by the commander app if the quality of the local position estimate is good enough to use for navigation
 bool condition_local_velocity_valid		# set to true by the commander app if the quality of the local horizontal velocity data is good enough to use for navigation
 bool condition_global_position_valid		# set to true by the commander app if the quality of the global position estimate is good enough to use for navigation
+bool condition_gps_position_valid
 bool condition_home_position_valid		# indicates a valid home position (a valid home position is not always a valid launch)
 bool condition_power_input_valid                # set if input power is valid
 bool condition_battery_healthy                # set if battery is available and not low
 bool condition_escs_error		      # set to true if one or more ESCs reporting esc_status are offline
 bool condition_escs_failure		      # set to true if one or more ESCs reporting esc_status has a failure

+bool position_reliant_on_gps
+bool position_reliant_on_optical_flow
+bool position_reliant_on_vision_position
+
+bool dead_reckoning
+
 bool circuit_breaker_engaged_power_check
 bool circuit_breaker_engaged_airspd_check
 bool circuit_breaker_engaged_enginefailure_check
@@ -34,7 +34,9 @@
 # this includes the generated topics directory
 include_directories(${CMAKE_CURRENT_BINARY_DIR})

-px4_add_library(uORB
+set(SRCS)
+
+set(SRCS_COMMON
 	ORBSet.hpp
 	Publication.hpp
 	PublicationMulti.hpp
@@ -47,18 +49,49 @@ px4_add_library(uORB
 	uORB.h
 	uORBCommon.hpp
 	uORBCommunicator.hpp
-	uORBDeviceMaster.cpp
-	uORBDeviceMaster.hpp
-	uORBDeviceNode.cpp
-	uORBDeviceNode.hpp
-	uORBManager.cpp
 	uORBManager.hpp
 	uORBUtils.cpp
 	uORBUtils.hpp
-)
+	uORBDeviceMaster.hpp
+	uORBDeviceNode.hpp
+	)

+set(SRCS_KERNEL
+	uORBDeviceMaster.cpp
+	uORBDeviceNode.cpp
+	uORBManager.cpp
+	)
+
+set(SRCS_USER
+	uORBManagerUsr.cpp
+	)
+
+if (NOT DEFINED CONFIG_BUILD_FLAT AND "${PX4_PLATFORM}" MATCHES "nuttx")
+	# Kernel side library in nuttx kernel/protected build
+	px4_add_library(uORB_kernel
+		${SRCS_COMMON}
+		${SRCS_KERNEL}
+		)
+	target_link_libraries(uORB_kernel PRIVATE cdev uorb_msgs)
+	target_compile_options(uORB_kernel PRIVATE ${MAX_CUSTOM_OPT_LEVEL} -D__KERNEL__)
+
+	# User side library in nuttx kernel/protected build
+	px4_add_library(uORB
+		${SRCS_COMMON}
+		${SRCS_USER}
+		)
+else()
+
+	# Library for all other targets (flat build, posix...)
+	px4_add_library(uORB
+		${SRCS_COMMON}
+		${SRCS_KERNEL}
+		)
+	target_link_libraries(uORB PRIVATE cdev)
+endif()
+
+target_link_libraries(uORB PRIVATE uorb_msgs)
 target_compile_options(uORB PRIVATE ${MAX_CUSTOM_OPT_LEVEL})
-target_link_libraries(uORB PRIVATE cdev uorb_msgs)

 if(PX4_TESTING)
 	add_subdirectory(uORB_tests)
@@ -136,19 +136,40 @@ public:
 	/**
 	 * Check if there is a new update.
 	 */
-	bool updated() { return advertised() && Manager::updates_available(_node, _last_generation); }
+	bool updated()
+	{
+		if (!valid()) {
+			subscribe();
+		}
+
+		return valid() ? Manager::updates_available(_node, _last_generation) : false;
+	}

 	/**
 	 * Update the struct
 	 * @param dst The uORB message struct we are updating.
 	 */
-	bool update(void *dst) { return updated() && Manager::orb_data_copy(_node, dst, _last_generation); }
+	bool update(void *dst)
+	{
+		if (!valid()) {
+			subscribe();
+		}
+
+		return valid() ? Manager::orb_data_copy(_node, dst, _last_generation, true) : false;
+	}

 	/**
 	 * Copy the struct
 	 * @param dst The uORB message struct we are updating.
 	 */
-	bool copy(void *dst) { return advertised() && Manager::orb_data_copy(_node, dst, _last_generation); }
+	bool copy(void *dst)
+	{
+		if (!valid()) {
+			subscribe();
+		}
+
+		return valid() ? Manager::orb_data_copy(_node, dst, _last_generation, false) : false;
+	}

 	/**
 	 * Change subscription instance
@@ -41,10 +41,15 @@
 #include "uORBManager.hpp"
 #include "uORBCommon.hpp"

+
 #include <lib/drivers/device/Device.hpp>
 #include <matrix/Quaternion.hpp>
 #include <mathlib/mathlib.h>

+#ifdef __PX4_NUTTX
+#include <sys/boardctl.h>
+#endif
+
 static uORB::DeviceMaster *g_dev = nullptr;

 int uorb_start(void)
@@ -60,6 +65,7 @@ int uorb_start(void)
 		return -ENOMEM;
 	}

+#if !defined(__PX4_NUTTX) || defined(CONFIG_BUILD_FLAT) || defined(__KERNEL__)
 	/* create the driver */
 	g_dev = uORB::Manager::get_instance()->get_device_master();

@@ -67,11 +73,15 @@ int uorb_start(void)
 		return -errno;
 	}

+#endif
+
 	return OK;
 }

 int uorb_status(void)
 {
+#if !defined(__PX4_NUTTX) || defined(CONFIG_BUILD_FLAT) || defined(__KERNEL__)
+
 	if (g_dev != nullptr) {
 		g_dev->printStatistics();

@@ -79,11 +89,16 @@ int uorb_status(void)
 		PX4_INFO("uorb is not running");
 	}

+#else
+	boardctl(ORBIOCDEVMASTERCMD, ORB_DEVMASTER_STATUS);
+#endif
 	return OK;
 }

 int uorb_top(char **topic_filter, int num_filters)
 {
+#if !defined(__PX4_NUTTX) || defined(CONFIG_BUILD_FLAT) || defined(__KERNEL__)
+
 	if (g_dev != nullptr) {
 		g_dev->showTop(topic_filter, num_filters);

@@ -91,6 +106,9 @@ int uorb_top(char **topic_filter, int num_filters)
 		PX4_INFO("uorb is not running");
 	}

+#else
+	boardctl(ORBIOCDEVMASTERCMD, ORB_DEVMASTER_TOP);
+#endif
 	return OK;
 }

@@ -40,6 +40,10 @@
 #include <px4_platform_common/posix.h>
 #include <px4_platform_common/tasks.h>

+#if defined(__PX4_NUTTX) && !defined(CONFIG_BUILD_FLAT) && defined(__KERNEL__)
+#include <px4_platform/board_ctrl.h>
+#endif
+
 #include "uORBDeviceNode.hpp"
 #include "uORBUtils.hpp"
 #include "uORBManager.hpp"
@@ -52,6 +56,9 @@ bool uORB::Manager::initialize()
 		_Instance = new uORB::Manager();
 	}

+#if defined(__PX4_NUTTX) && !defined(CONFIG_BUILD_FLAT) && defined(__KERNEL__)
+	px4_register_boardct_ioctl(_ORBIOCDEVBASE, orb_ioctl);
+#endif
 	return _Instance != nullptr;
 }

@@ -103,6 +110,124 @@ uORB::DeviceMaster *uORB::Manager::get_device_master()
 	return _device_master;
 }

+#if defined(__PX4_NUTTX) && !defined(CONFIG_BUILD_FLAT) && defined(__KERNEL__)
+int	uORB::Manager::orb_ioctl(unsigned int cmd, unsigned long arg)
+{
+	int ret = PX4_OK;
+
+	switch (cmd) {
+	case ORBIOCDEVEXISTS: {
+			orbiocdevexists_t *data = (orbiocdevexists_t *)arg;
+
+			if (data->check_advertised) {
+				data->ret = uORB::Manager::orb_exists(get_orb_meta(data->orb_id), data->instance);
+
+			} else {
+				data->ret = uORB::Manager::orb_device_node_exists(data->orb_id, data->instance) ? PX4_OK : PX4_ERROR;
+			}
+		}
+		break;
+
+	case ORBIOCDEVADVERTISE: {
+			orbiocdevadvertise_t *data = (orbiocdevadvertise_t *)arg;
+			uORB::DeviceMaster *dev =  uORB::Manager::get_instance()->get_device_master();
+
+			if (dev) {
+				data->ret = dev->advertise(data->meta, data->is_advertiser, data->instance);
+
+			} else {
+				data->ret = PX4_ERROR;
+			}
+		}
+		break;
+
+	case ORBIOCDEVUNADVERTISE: {
+			orbiocdevunadvertise_t *data = (orbiocdevunadvertise_t *)arg;
+			data->ret = uORB::Manager::orb_unadvertise(data->handle);
+		}
+		break;
+
+	case ORBIOCDEVPUBLISH: {
+			orbiocdevpublish_t *data = (orbiocdevpublish_t *)arg;
+			data->ret = uORB::Manager::orb_publish(data->meta, data->handle, data->data);
+		}
+		break;
+
+	case ORBIOCDEVADDSUBSCRIBER: {
+			orbiocdevaddsubscriber_t *data = (orbiocdevaddsubscriber_t *)arg;
+			data->handle = uORB::Manager::orb_add_internal_subscriber(data->orb_id, data->instance, data->initial_generation);
+		}
+		break;
+
+	case ORBIOCDEVREMSUBSCRIBER: {
+			uORB::Manager::orb_remove_internal_subscriber(reinterpret_cast<void *>(arg));
+		}
+		break;
+
+	case ORBIOCDEVQUEUESIZE: {
+			orbiocdevqueuesize_t *data = (orbiocdevqueuesize_t *)arg;
+			data->size = uORB::Manager::orb_get_queue_size(data->handle);
+		}
+		break;
+
+	case ORBIOCDEVDATACOPY: {
+			orbiocdevdatacopy_t *data = (orbiocdevdatacopy_t *)arg;
+			data->ret = uORB::Manager::orb_data_copy(data->handle, data->dst, data->generation, data->only_if_updated);
+		}
+		break;
+
+	case ORBIOCDEVREGCALLBACK: {
+			orbiocdevregcallback_t *data = (orbiocdevregcallback_t *)arg;
+			data->registered = uORB::Manager::register_callback(data->handle, data->callback_sub);
+		}
+		break;
+
+	case ORBIOCDEVUNREGCALLBACK: {
+			orbiocdevunregcallback_t *data = (orbiocdevunregcallback_t *)arg;
+			uORB::Manager::unregister_callback(data->handle, data->callback_sub);
+		}
+		break;
+
+	case ORBIOCDEVGETINSTANCE: {
+			orbiocdevgetinstance_t *data = (orbiocdevgetinstance_t *)arg;
+			data->instance = uORB::Manager::orb_get_instance(data->handle);
+		}
+		break;
+
+	case ORBIOCDEVMASTERCMD: {
+			uORB::DeviceMaster *dev = uORB::Manager::get_instance()->get_device_master();
+
+			if (dev) {
+				if (arg == ORB_DEVMASTER_TOP) {
+					dev->showTop(nullptr, 0);
+
+				} else {
+					dev->printStatistics();
+				}
+			}
+		}
+		break;
+
+	case ORBIOCDEVUPDATESAVAIL: {
+			orbiocdevupdatesavail_t *data = (orbiocdevupdatesavail_t *)arg;
+			data->ret = updates_available(data->handle, data->last_generation);
+		}
+		break;
+
+	case ORBIOCDEVISADVERTISED: {
+			orbiocdevisadvertised_t *data = (orbiocdevisadvertised_t *)arg;
+			data->ret = is_advertised(data->handle);
+		}
+		break;
+
+	default:
+		ret = -ENOTTY;
+	}
+
+	return ret;
+}
+#endif
+
 int uORB::Manager::orb_exists(const struct orb_metadata *meta, int instance)
 {
 	int ret = PX4_ERROR;
@@ -354,8 +479,16 @@ void uORB::Manager::orb_remove_internal_subscriber(void *node_handle)

 uint8_t uORB::Manager::orb_get_queue_size(const void *node_handle) { return static_cast<const DeviceNode *>(node_handle)->get_queue_size(); }

-bool uORB::Manager::orb_data_copy(void *node_handle, void *dst, unsigned &generation)
+bool uORB::Manager::orb_data_copy(void *node_handle, void *dst, unsigned &generation, bool only_if_updated)
 {
+	if (!is_advertised(node_handle)) {
+		return false;
+	}
+
+	if (only_if_updated && !static_cast<const uORB::DeviceNode *>(node_handle)->updates_available(generation)) {
+		return false;
+	}
+
 	return static_cast<DeviceNode *>(node_handle)->copy(dst, generation);
 }

@@ -380,6 +513,20 @@ uint8_t uORB::Manager::orb_get_instance(const void *node_handle)
 	return -1;
 }

+/* These are optimized by inlining in NuttX Flat build */
+#if !defined(CONFIG_BUILD_FLAT)
+unsigned uORB::Manager::updates_available(const void *node_handle, unsigned last_generation)
+{
+	return is_advertised(node_handle) ? static_cast<const uORB::DeviceNode *>(node_handle)->updates_available(
+		       last_generation) : 0;
+}
+
+bool uORB::Manager::is_advertised(const void *node_handle)
+{
+	return static_cast<const uORB::DeviceNode *>(node_handle)->is_advertised();
+}
+#endif
+
 int uORB::Manager::node_open(const struct orb_metadata *meta, bool advertiser, int *instance)
 {
 	char path[orb_maxpath];
@@ -38,6 +38,7 @@
 #include "uORBCommon.hpp"
 #include "uORBDeviceMaster.hpp"

+#include <uORB/topics/uORBTopics.hpp> // For ORB_ID enum
 #include <stdint.h>

 #ifdef __PX4_NUTTX
@@ -58,6 +59,107 @@ class Manager;
 class SubscriptionCallback;
 }

+
+/*
+ * IOCTLs for manager to access device nodes using
+ * a handle
+ */
+
+#define _ORBIOCDEV(_n) (_PX4_IOC(_ORBIOCDEVBASE, _n))
+#define ORBIOCDEVEXISTS	_ORBIOCDEV(30)
+typedef struct orbiocdevexists {
+	const ORB_ID orb_id;
+	const uint8_t instance;
+	const bool check_advertised;
+	int ret;
+} orbiocdevexists_t;
+
+#define ORBIOCDEVADVERTISE	_ORBIOCDEV(31)
+typedef struct orbiocadvertise {
+	const struct orb_metadata *meta;
+	bool is_advertiser;
+	int *instance;
+	int ret;
+} orbiocdevadvertise_t;
+
+#define ORBIOCDEVUNADVERTISE	_ORBIOCDEV(32)
+typedef struct orbiocunadvertise {
+	void *handle;
+	int ret;
+} orbiocdevunadvertise_t;
+
+#define ORBIOCDEVPUBLISH	_ORBIOCDEV(33)
+typedef struct orbiocpublish {
+	const struct orb_metadata *meta;
+	orb_advert_t handle;
+	const void *data;
+	int ret;
+} orbiocdevpublish_t;
+
+#define ORBIOCDEVADDSUBSCRIBER	_ORBIOCDEV(34)
+typedef struct {
+	const ORB_ID orb_id;
+	const uint8_t instance;
+	unsigned *initial_generation;
+	void *handle;
+} orbiocdevaddsubscriber_t;
+
+#define ORBIOCDEVREMSUBSCRIBER	_ORBIOCDEV(35)
+
+#define ORBIOCDEVQUEUESIZE	_ORBIOCDEV(36)
+typedef struct {
+	const void *handle;
+	uint8_t size;
+} orbiocdevqueuesize_t;
+
+#define ORBIOCDEVDATACOPY	_ORBIOCDEV(37)
+typedef struct {
+	void *handle;
+	void *dst;
+	unsigned generation;
+	bool only_if_updated;
+	bool ret;
+} orbiocdevdatacopy_t;
+
+#define ORBIOCDEVREGCALLBACK	_ORBIOCDEV(38)
+typedef struct {
+	void *handle;
+	class uORB::SubscriptionCallback *callback_sub;
+	bool registered;
+} orbiocdevregcallback_t;
+
+#define ORBIOCDEVUNREGCALLBACK	_ORBIOCDEV(39)
+typedef struct {
+	void *handle;
+	class uORB::SubscriptionCallback *callback_sub;
+} orbiocdevunregcallback_t;
+
+#define ORBIOCDEVGETINSTANCE	_ORBIOCDEV(40)
+typedef struct {
+	const void *handle;
+	uint8_t instance;
+} orbiocdevgetinstance_t;
+
+#define ORBIOCDEVUPDATESAVAIL	_ORBIOCDEV(41)
+typedef struct {
+	const void *handle;
+	unsigned last_generation;
+	unsigned ret;
+} orbiocdevupdatesavail_t;
+
+#define ORBIOCDEVISADVERTISED	_ORBIOCDEV(42)
+typedef struct {
+	const void *handle;
+	bool ret;
+} orbiocdevisadvertised_t;
+
+typedef enum {
+	ORB_DEVMASTER_STATUS = 0,
+	ORB_DEVMASTER_TOP = 1
+} orbiocdevmastercmd_t;
+#define ORBIOCDEVMASTERCMD	_ORBIOCDEV(45)
+
+
 /**
 * This is implemented as a singleton.  This class manages creating the
 * uORB nodes for each uORB topics and also implements the behavor of the
@@ -98,6 +200,10 @@ public:
 	 */
 	uORB::DeviceMaster *get_device_master();

+#if defined(__PX4_NUTTX) && !defined(CONFIG_BUILD_FLAT) && defined(__KERNEL__)
+	static int orb_ioctl(unsigned int cmd, unsigned long arg);
+#endif
+
 	// ==== uORB interface methods ====
 	/**
 	 * Advertise as the publisher of a topic.
@@ -345,7 +451,7 @@ public:

 	static uint8_t orb_get_queue_size(const void *node_handle);

-	static bool orb_data_copy(void *node_handle, void *dst, unsigned &generation);
+	static bool orb_data_copy(void *node_handle, void *dst, unsigned &generation, bool only_if_updated);

 	static bool register_callback(void *node_handle, SubscriptionCallback *callback_sub);

@@ -353,10 +459,18 @@ public:

 	static uint8_t orb_get_instance(const void *node_handle);

-	static unsigned updates_available(const void *node_handle, unsigned last_generation) { return static_cast<const DeviceNode *>(node_handle)->updates_available(last_generation); }
+#if defined(CONFIG_BUILD_FLAT)
+	/* These are optimized by inlining in NuttX Flat build */
+	static unsigned updates_available(const void *node_handle, unsigned last_generation) { return is_advertised(node_handle) ? static_cast<const DeviceNode *>(node_handle)->updates_available(last_generation) : 0; }

 	static bool is_advertised(const void *node_handle) { return static_cast<const DeviceNode *>(node_handle)->is_advertised(); }

+#else
+	static unsigned updates_available(const void *node_handle, unsigned last_generation);
+
+	static bool is_advertised(const void *node_handle);
+#endif
+
 #ifdef ORB_COMMUNICATOR
 	/**
 	 * Method to set the uORBCommunicator::IChannel instance.
@@ -0,0 +1,349 @@
+/****************************************************************************
+ *
+ *   Copyright (c) 2012-2015 PX4 Development Team. All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright
+ *    notice, this list of conditions and the following disclaimer.
+ * 2. Redistributions in binary form must reproduce the above copyright
+ *    notice, this list of conditions and the following disclaimer in
+ *    the documentation and/or other materials provided with the
+ *    distribution.
+ * 3. Neither the name PX4 nor the names of its contributors may be
+ *    used to endorse or promote products derived from this software
+ *    without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
+ * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
+ * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *
+ ****************************************************************************/
+
+#include <sys/types.h>
+#include <sys/stat.h>
+#include <stdarg.h>
+#include <fcntl.h>
+
+#include <px4_platform_common/px4_config.h>
+#include <px4_platform_common/posix.h>
+#include <px4_platform_common/tasks.h>
+
+#include <sys/boardctl.h>
+
+#include "uORBDeviceNode.hpp"
+#include "uORBUtils.hpp"
+#include "uORBManager.hpp"
+
+uORB::Manager *uORB::Manager::_Instance = nullptr;
+
+bool uORB::Manager::initialize()
+{
+	if (_Instance == nullptr) {
+		_Instance = new uORB::Manager();
+	}
+
+	return _Instance != nullptr;
+}
+
+bool uORB::Manager::terminate()
+{
+	if (_Instance != nullptr) {
+		delete _Instance;
+		_Instance = nullptr;
+		return true;
+	}
+
+	return false;
+}
+
+uORB::Manager::Manager()
+{
+}
+
+uORB::Manager::~Manager()
+{
+}
+
+int uORB::Manager::orb_exists(const struct orb_metadata *meta, int instance)
+{
+	// instance valid range: [0, ORB_MULTI_MAX_INSTANCES)
+	if ((instance < 0) || (instance > (ORB_MULTI_MAX_INSTANCES - 1))) {
+		return PX4_ERROR;
+	}
+
+	orbiocdevexists_t data = {static_cast<ORB_ID>(meta->o_id), static_cast<uint8_t>(instance), true, PX4_ERROR};
+	boardctl(ORBIOCDEVEXISTS, reinterpret_cast<unsigned long>(&data));
+
+	return data.ret;
+}
+
+orb_advert_t uORB::Manager::orb_advertise_multi(const struct orb_metadata *meta, const void *data, int *instance,
+		unsigned int queue_size)
+{
+	/* open the node as an advertiser */
+	int fd = node_open(meta, true, instance);
+
+	if (fd == PX4_ERROR) {
+		PX4_ERR("%s advertise failed (%i)", meta->o_name, errno);
+		return nullptr;
+	}
+
+	/* Set the queue size. This must be done before the first publication; thus it fails if
+	 * this is not the first advertiser.
+	 */
+	int result = px4_ioctl(fd, ORBIOCSETQUEUESIZE, (unsigned long)queue_size);
+
+	if (result < 0 && queue_size > 1) {
+		PX4_WARN("orb_advertise_multi: failed to set queue size");
+	}
+
+	/* get the advertiser handle and close the node */
+	orb_advert_t advertiser;
+
+	result = px4_ioctl(fd, ORBIOCGADVERTISER, (unsigned long)&advertiser);
+	px4_close(fd);
+
+	if (result == PX4_ERROR) {
+		PX4_WARN("px4_ioctl ORBIOCGADVERTISER failed. fd = %d", fd);
+		return nullptr;
+	}
+
+	/* the advertiser may perform an initial publish to initialise the object */
+	if (data != nullptr) {
+		result = orb_publish(meta, advertiser, data);
+
+		if (result == PX4_ERROR) {
+			PX4_ERR("orb_publish failed %s", meta->o_name);
+			return nullptr;
+		}
+	}
+
+	return advertiser;
+}
+
+int uORB::Manager::orb_unadvertise(orb_advert_t handle)
+{
+	orbiocdevunadvertise_t data = {handle, PX4_ERROR};
+	boardctl(ORBIOCDEVUNADVERTISE, reinterpret_cast<unsigned long>(&data));
+
+	return data.ret;
+}
+
+int uORB::Manager::orb_subscribe(const struct orb_metadata *meta)
+{
+	return node_open(meta, false);
+}
+
+int uORB::Manager::orb_subscribe_multi(const struct orb_metadata *meta, unsigned instance)
+{
+	int inst = instance;
+	return node_open(meta, false, &inst);
+}
+
+int uORB::Manager::orb_unsubscribe(int fd)
+{
+	return px4_close(fd);
+}
+
+int uORB::Manager::orb_publish(const struct orb_metadata *meta, orb_advert_t handle, const void *data)
+{
+	orbiocdevpublish_t d = {meta, handle, data, PX4_ERROR};
+	boardctl(ORBIOCDEVPUBLISH, reinterpret_cast<unsigned long>(&d));
+
+	return d.ret;
+}
+
+int uORB::Manager::orb_copy(const struct orb_metadata *meta, int handle, void *buffer)
+{
+	int ret;
+
+	ret = px4_read(handle, buffer, meta->o_size);
+
+	if (ret < 0) {
+		return PX4_ERROR;
+	}
+
+	if (ret != (int)meta->o_size) {
+		errno = EIO;
+		return PX4_ERROR;
+	}
+
+	return PX4_OK;
+}
+
+int uORB::Manager::orb_check(int handle, bool *updated)
+{
+	/* Set to false here so that if `px4_ioctl` fails to false. */
+	*updated = false;
+	return px4_ioctl(handle, ORBIOCUPDATED, (unsigned long)(uintptr_t)updated);
+}
+
+int uORB::Manager::orb_set_interval(int handle, unsigned interval)
+{
+	return px4_ioctl(handle, ORBIOCSETINTERVAL, interval * 1000);
+}
+
+int uORB::Manager::orb_get_interval(int handle, unsigned *interval)
+{
+	int ret = px4_ioctl(handle, ORBIOCGETINTERVAL, (unsigned long)interval);
+	*interval /= 1000;
+	return ret;
+}
+
+int uORB::Manager::node_open(const struct orb_metadata *meta, bool advertiser, int *instance)
+{
+	char path[orb_maxpath];
+	int fd = -1;
+	int ret = PX4_ERROR;
+
+	/*
+	 * If meta is null, the object was not defined, i.e. it is not
+	 * known to the system.  We can't advertise/subscribe such a thing.
+	 */
+	if (nullptr == meta) {
+		errno = ENOENT;
+		return PX4_ERROR;
+	}
+
+	/* if we have an instance and are an advertiser, we will generate a new node and set the instance,
+	 * so we do not need to open here */
+	if (!instance || !advertiser) {
+		/*
+		 * Generate the path to the node and try to open it.
+		 */
+		ret = uORB::Utils::node_mkpath(path, meta, instance);
+
+		if (ret != OK) {
+			errno = -ret;
+			return PX4_ERROR;
+		}
+
+		/* open the path as either the advertiser or the subscriber */
+		fd = px4_open(path, advertiser ? PX4_F_WRONLY : PX4_F_RDONLY);
+
+	} else {
+		*instance = 0;
+	}
+
+	/* we may need to advertise the node... */
+	if (fd < 0) {
+		ret = PX4_ERROR;
+
+		orbiocdevadvertise_t data = {meta, advertiser, instance, PX4_ERROR};
+		boardctl(ORBIOCDEVADVERTISE, (unsigned long)&data);
+		ret = data.ret;
+
+		/* it's OK if it already exists */
+		if ((ret != PX4_OK) && (EEXIST == errno)) {
+			ret = PX4_OK;
+		}
+
+		if (ret == PX4_OK) {
+			/* update the path, as it might have been updated during the node advertise call */
+			ret = uORB::Utils::node_mkpath(path, meta, instance);
+
+			/* on success, try to open again */
+			if (ret == PX4_OK) {
+				fd = px4_open(path, (advertiser) ? PX4_F_WRONLY : PX4_F_RDONLY);
+
+			} else {
+				errno = -ret;
+				return PX4_ERROR;
+			}
+		}
+	}
+
+	if (fd < 0) {
+		errno = EIO;
+		return PX4_ERROR;
+	}
+
+	/* everything has been OK, we can return the handle now */
+	return fd;
+}
+
+bool uORB::Manager::orb_device_node_exists(ORB_ID orb_id, uint8_t instance)
+{
+	orbiocdevexists_t data = {orb_id, instance, false, 0};
+	boardctl(ORBIOCDEVEXISTS, reinterpret_cast<unsigned long>(&data));
+
+	return data.ret == PX4_OK ? true : false;
+}
+
+void *uORB::Manager::orb_add_internal_subscriber(ORB_ID orb_id, uint8_t instance, unsigned *initial_generation)
+{
+	orbiocdevaddsubscriber_t data = {orb_id, instance, initial_generation, nullptr};
+	boardctl(ORBIOCDEVADDSUBSCRIBER, reinterpret_cast<unsigned long>(&data));
+
+	return data.handle;
+}
+
+void uORB::Manager::orb_remove_internal_subscriber(void *node_handle)
+{
+	boardctl(ORBIOCDEVREMSUBSCRIBER, reinterpret_cast<unsigned long>(node_handle));
+}
+
+uint8_t uORB::Manager::orb_get_queue_size(const void *node_handle)
+{
+	orbiocdevqueuesize_t data = {node_handle, 0};
+	boardctl(ORBIOCDEVQUEUESIZE, reinterpret_cast<unsigned long>(&data));
+
+	return data.size;
+}
+
+bool uORB::Manager::orb_data_copy(void *node_handle, void *dst, unsigned &generation, bool only_if_updated)
+{
+	orbiocdevdatacopy_t data = {node_handle, dst, generation, only_if_updated, false};
+	boardctl(ORBIOCDEVDATACOPY, reinterpret_cast<unsigned long>(&data));
+	generation = data.generation;
+
+	return data.ret;
+}
+
+bool uORB::Manager::register_callback(void *node_handle, SubscriptionCallback *callback_sub)
+{
+	orbiocdevregcallback_t data = {node_handle, callback_sub, false};
+	boardctl(ORBIOCDEVREGCALLBACK, reinterpret_cast<unsigned long>(&data));
+
+	return data.registered;
+}
+
+void uORB::Manager::unregister_callback(void *node_handle, SubscriptionCallback *callback_sub)
+{
+	orbiocdevunregcallback_t data = {node_handle, callback_sub};
+	boardctl(ORBIOCDEVUNREGCALLBACK, reinterpret_cast<unsigned long>(&data));
+}
+
+uint8_t uORB::Manager::orb_get_instance(const void *node_handle)
+{
+	orbiocdevgetinstance_t data = {node_handle, 0};
+	boardctl(ORBIOCDEVGETINSTANCE, reinterpret_cast<unsigned long>(&data));
+
+	return data.instance;
+}
+
+unsigned uORB::Manager::updates_available(const void *node_handle, unsigned last_generation)
+{
+	orbiocdevupdatesavail_t data = {node_handle, last_generation, 0};
+	boardctl(ORBIOCDEVUPDATESAVAIL, reinterpret_cast<unsigned long>(&data));
+	return data.ret;
+}
+
+bool uORB::Manager::is_advertised(const void *node_handle)
+{
+	orbiocdevisadvertised_t data = {node_handle, false};
+	boardctl(ORBIOCDEVISADVERTISED, reinterpret_cast<unsigned long>(&data));
+	return data.ret;
+}
@@ -21,6 +21,24 @@
                "set print pretty",
            ]
        },
+        {
+            "name": "stlink (@PX4_BOARD@)",
+            "device": "@DEBUG_DEVICE@",
+            "svdFile": "@DEBUG_SVD_FILE_PATH@",
+            "executable": "${command:cmake.launchTargetPath}",
+            "request": "launch",
+            "type": "cortex-debug",
+            "servertype": "stutil",
+            "cwd": "${workspaceFolder}",
+            "internalConsoleOptions": "openOnSessionStart",
+            "preLaunchCommands": [
+                "source ${workspaceFolder}/platforms/nuttx/Debug/PX4",
+                "source ${workspaceFolder}/platforms/nuttx/Debug/NuttX",
+                "source ${workspaceFolder}/platforms/nuttx/Debug/ARMv7M",
+                "set mem inaccessible-by-default off",
+                "set print pretty",
+            ]
+        },
        {
            "name": "blackmagic (@PX4_BOARD@)",
            "device": "@DEBUG_DEVICE@",
@@ -89,6 +89,39 @@ if(CONFIG_NSH_LIBRARY)
 	endforeach()
 endif()

+if (NOT CONFIG_BUILD_FLAT)
+	set(KERNEL_BUILTIN_DIR ${CMAKE_CURRENT_BINARY_DIR}/kernel_builtin)
+	set(kernel_builtin_apps_string)
+	set(kernel_builtin_apps_proxy_string)
+	set(kernel_builtin_apps_decl_string)
+
+	list(SORT kernel_module_libraries)
+	foreach(module ${kernel_module_libraries})
+		get_target_property(MAIN ${module} MAIN)
+		get_target_property(STACK_MAIN ${module} STACK_MAIN)
+		get_target_property(PRIORITY ${module} PRIORITY)
+
+		if(MAIN)
+			set(kernel_builtin_apps_string "${kernel_builtin_apps_string}{ \"${MAIN}\", ${PRIORITY}, ${STACK_MAIN}, ${MAIN}_main },\n")
+			set(kernel_builtin_apps_proxy_string "${kernel_builtin_apps_proxy_string}{ \"${MAIN}\", ${PRIORITY}, ${STACK_MAIN}, launch_kmod_main },\n")
+			set(kernel_builtin_apps_decl_string "${kernel_builtin_apps_decl_string}int ${MAIN}_main(int argc, char *argv[]);\n")
+		endif()
+	endforeach()
+
+	configure_file(${CMAKE_CURRENT_SOURCE_DIR}/px4_kernel.bdat.in ${CMAKE_CURRENT_BINARY_DIR}/px4_kernel.bdat)
+	configure_file(${CMAKE_CURRENT_SOURCE_DIR}/px4_kernel.pdat.in ${CMAKE_CURRENT_BINARY_DIR}/px4_kernel.pdat)
+
+	add_custom_command(OUTPUT ${KERNEL_BUILTIN_DIR}/kernel_builtin_list.h ${KERNEL_BUILTIN_DIR}/kernel_builtin_proto.h
+		WORKING_DIRECTORY  ${KERNEL_BUILTIN_DIR}
+		COMMAND ${CMAKE_COMMAND} -E remove -f kernel_builtin_list.h kernel_builtin_proto.h
+		COMMAND ${CMAKE_COMMAND} -E copy_if_different ${CMAKE_CURRENT_BINARY_DIR}/px4_kernel.bdat kernel_builtin_list.h
+		COMMAND ${CMAKE_COMMAND} -E copy_if_different ${CMAKE_CURRENT_BINARY_DIR}/px4_kernel.pdat kernel_builtin_proto.h
+	)
+
+	add_custom_target(px4_kernel_builtin_list_target DEPENDS ${KERNEL_BUILTIN_DIR}/kernel_builtin_list.h ${KERNEL_BUILTIN_DIR}/kernel_builtin_proto.h)
+
+endif() # NOT CONFIG_BUILD_FLAT
+
 configure_file(${CMAKE_CURRENT_SOURCE_DIR}/px4.bdat.in ${CMAKE_CURRENT_BINARY_DIR}/px4.bdat)
 configure_file(${CMAKE_CURRENT_SOURCE_DIR}/px4.pdat.in ${CMAKE_CURRENT_BINARY_DIR}/px4.pdat)

@@ -1 +1,2 @@
@builtin_apps_string@
+@kernel_builtin_apps_proxy_string@
@@ -0,0 +1 @@
+@kernel_builtin_apps_string@
@@ -0,0 +1 @@
+@kernel_builtin_apps_decl_string@
@@ -80,69 +80,69 @@
 // RESET    finalise flash programming, reset chip and starts application
 //

-#define BL_PROTOCOL_VERSION     5   // The revision of the bootloader protocol
+#define BL_PROTOCOL_VERSION         5   // The revision of the bootloader protocol
 //* Next revision needs to update

 // protocol bytes
-#define PROTO_INSYNC        0x12    // 'in sync' byte sent before status
-#define PROTO_EOC         0x20    // end of command
+#define PROTO_INSYNC                0x12    // 'in sync' byte sent before status
+#define PROTO_EOC                   0x20    // end of command

 // Reply bytes
-#define PROTO_OK          0x10    // INSYNC/OK      - 'ok' response
-#define PROTO_FAILED        0x11    // INSYNC/FAILED  - 'fail' response
-#define PROTO_INVALID       0x13  // INSYNC/INVALID - 'invalid' response for bad commands
-#define PROTO_BAD_SILICON_REV     0x14  // On the F4 series there is an issue with < Rev 3 silicon
-#define PROTO_RESERVED_0X15     0x15  // Reserved
+#define PROTO_OK                    0x10    // INSYNC/OK      - 'ok' response
+#define PROTO_FAILED                0x11    // INSYNC/FAILED  - 'fail' response
+#define PROTO_INVALID               0x13    // INSYNC/INVALID - 'invalid' response for bad commands
+#define PROTO_BAD_SILICON_REV       0x14    // On the F4 series there is an issue with < Rev 3 silicon
+#define PROTO_RESERVED_0X15         0x15    // Reserved

 // see https://pixhawk.org/help/errata
 // Command bytes
-#define PROTO_GET_SYNC        0x21    // NOP for re-establishing sync
-#define PROTO_GET_DEVICE      0x22    // get device ID bytes
-#define PROTO_CHIP_ERASE      0x23    // erase program area and reset program address
-#define PROTO_PROG_MULTI      0x27    // write bytes at program address and increment
-#define PROTO_GET_CRC       0x29  // compute & return a CRC
-#define PROTO_GET_OTP       0x2a  // read a byte from OTP at the given address
-#define PROTO_GET_SN        0x2b    // read a word from UDID area ( Serial)  at the given address
-#define PROTO_GET_CHIP        0x2c    // read chip version (MCU IDCODE)
-#define PROTO_SET_DELAY       0x2d    // set minimum boot delay
-#define PROTO_GET_CHIP_DES      0x2e    // read chip version In ASCII
-#define PROTO_BOOT          0x30    // boot the application
-#define PROTO_DEBUG         0x31    // emit debug information - format not defined
-#define PROTO_SET_BAUD        0x33    // set baud rate on uart
+#define PROTO_GET_SYNC              0x21    // NOP for re-establishing sync
+#define PROTO_GET_DEVICE            0x22    // get device ID bytes
+#define PROTO_CHIP_ERASE            0x23    // erase program area and reset program address
+#define PROTO_PROG_MULTI            0x27    // write bytes at program address and increment
+#define PROTO_GET_CRC               0x29    // compute & return a CRC
+#define PROTO_GET_OTP               0x2a    // read a byte from OTP at the given address
+#define PROTO_GET_SN                0x2b    // read a word from UDID area ( Serial)  at the given address
+#define PROTO_GET_CHIP              0x2c    // read chip version (MCU IDCODE)
+#define PROTO_SET_DELAY             0x2d    // set minimum boot delay
+#define PROTO_GET_CHIP_DES          0x2e    // read chip version In ASCII
+#define PROTO_BOOT                  0x30    // boot the application
+#define PROTO_DEBUG                 0x31    // emit debug information - format not defined
+#define PROTO_SET_BAUD              0x33    // set baud rate on uart

-#define PROTO_RESERVED_0X36     0x36  // Reserved
-#define PROTO_RESERVED_0X37     0x37  // Reserved
-#define PROTO_RESERVED_0X38     0x38  // Reserved
-#define PROTO_RESERVED_0X39     0x39  // Reserved
+#define PROTO_RESERVED_0X36         0x36  // Reserved
+#define PROTO_RESERVED_0X37         0x37  // Reserved
+#define PROTO_RESERVED_0X38         0x38  // Reserved
+#define PROTO_RESERVED_0X39         0x39  // Reserved

-#define PROTO_PROG_MULTI_MAX    64  // maximum PROG_MULTI size
-#define PROTO_READ_MULTI_MAX    255 // size of the size field
+#define PROTO_PROG_MULTI_MAX        64  // maximum PROG_MULTI size
+#define PROTO_READ_MULTI_MAX        255 // size of the size field

 /* argument values for PROTO_GET_DEVICE */
-#define PROTO_DEVICE_BL_REV 1 // bootloader revision
-#define PROTO_DEVICE_BOARD_ID 2 // board ID
-#define PROTO_DEVICE_BOARD_REV  3 // board revision
-#define PROTO_DEVICE_FW_SIZE  4 // size of flashable area
-#define PROTO_DEVICE_VEC_AREA 5 // contents of reserved vectors 7-10
+#define PROTO_DEVICE_BL_REV         1 // bootloader revision
+#define PROTO_DEVICE_BOARD_ID       2 // board ID
+#define PROTO_DEVICE_BOARD_REV      3 // board revision
+#define PROTO_DEVICE_FW_SIZE        4 // size of flashable area
+#define PROTO_DEVICE_VEC_AREA       5 // contents of reserved vectors 7-10

-#define STATE_PROTO_OK          0x10    // INSYNC/OK      - 'ok' response
-#define STATE_PROTO_FAILED        0x11    // INSYNC/FAILED  - 'fail' response
-#define STATE_PROTO_INVALID       0x13  // INSYNC/INVALID - 'invalid' response for bad commands
-#define STATE_PROTO_BAD_SILICON_REV     0x14  // On the F4 series there is an issue with < Rev 3 silicon
-#define STATE_PROTO_RESERVED_0X15     0x15  // Reserved
+#define STATE_PROTO_OK              0x10    // INSYNC/OK      - 'ok' response
+#define STATE_PROTO_FAILED          0x11    // INSYNC/FAILED  - 'fail' response
+#define STATE_PROTO_INVALID         0x13  // INSYNC/INVALID - 'invalid' response for bad commands
+#define STATE_PROTO_BAD_SILICON_REV 0x14  // On the F4 series there is an issue with < Rev 3 silicon
+#define STATE_PROTO_RESERVED_0X15   0x15  // Reserved


 // State
-#define STATE_PROTO_GET_SYNC      0x1     // Have Seen NOP for re-establishing sync
-#define STATE_PROTO_GET_DEVICE    0x2     // Have Seen get device ID bytes
-#define STATE_PROTO_CHIP_ERASE    0x4     // Have Seen erase program area and reset program address
-#define STATE_PROTO_PROG_MULTI    0x8     // Have Seen write bytes at program address and increment
-#define STATE_PROTO_GET_CRC       0x10    // Have Seen compute & return a CRC
-#define STATE_PROTO_GET_OTP       0x20    // Have Seen read a byte from OTP at the given address
-#define STATE_PROTO_GET_SN        0x40    // Have Seen read a word from UDID area ( Serial)  at the given address
-#define STATE_PROTO_GET_CHIP      0x80    // Have Seen read chip version (MCU IDCODE)
-#define STATE_PROTO_GET_CHIP_DES  0x100   // Have Seen read chip version In ASCII
-#define STATE_PROTO_BOOT          0x200   // Have Seen boot the application
+#define STATE_PROTO_GET_SYNC        0x1     // Have Seen NOP for re-establishing sync
+#define STATE_PROTO_GET_DEVICE      0x2     // Have Seen get device ID bytes
+#define STATE_PROTO_CHIP_ERASE      0x4     // Have Seen erase program area and reset program address
+#define STATE_PROTO_PROG_MULTI      0x8     // Have Seen write bytes at program address and increment
+#define STATE_PROTO_GET_CRC         0x10    // Have Seen compute & return a CRC
+#define STATE_PROTO_GET_OTP         0x20    // Have Seen read a byte from OTP at the given address
+#define STATE_PROTO_GET_SN          0x40    // Have Seen read a word from UDID area ( Serial)  at the given address
+#define STATE_PROTO_GET_CHIP        0x80    // Have Seen read chip version (MCU IDCODE)
+#define STATE_PROTO_GET_CHIP_DES    0x100   // Have Seen read chip version In ASCII
+#define STATE_PROTO_BOOT            0x200   // Have Seen boot the application

 #if defined(TARGET_HW_PX4_PIO_V1)
 #define STATE_ALLOWS_ERASE        (STATE_PROTO_GET_SYNC)
@@ -0,0 +1,148 @@
+/****************************************************************************
+ *
+ *   Copyright (C) 2020 Technology Innovation Institute. 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_ctrl.c
+ *
+ * Provide a kernel-userspace boardctl_ioctl interface
+ */
+
+#include <px4_platform_common/px4_config.h>
+#include <px4_platform_common/defines.h>
+#include <px4_platform/board_ctrl.h>
+#include "board_config.h"
+
+#include <nuttx/lib/builtin.h>
+#include <NuttX/kernel_builtin/kernel_builtin_proto.h>
+
+FAR const struct builtin_s g_kernel_builtins[] = {
+#include <NuttX/kernel_builtin/kernel_builtin_list.h>
+};
+
+const int g_n_kernel_builtins = sizeof(g_kernel_builtins) / sizeof(struct builtin_s);
+
+static struct {
+	ioctl_ptr_t ioctl_func;
+} ioctl_ptrs[MAX_IOCTL_PTRS];
+
+/************************************************************************************
+ * Name: px4_register_boardct_ioctl
+ *
+ * Description:
+ *   an interface function for kernel services to register an ioct handler for user side
+ *
+ ************************************************************************************/
+
+
+int px4_register_boardct_ioctl(unsigned base, ioctl_ptr_t func)
+{
+	unsigned i = IOCTL_BASE_TO_IDX(base);
+	int ret = PX4_ERROR;
+
+	if (i < MAX_IOCTL_PTRS) {
+		ioctl_ptrs[i].ioctl_func = func;
+		ret = PX4_OK;
+	}
+
+	return ret;
+}
+
+/************************************************************************************
+ * Name: board_ioctl
+ *
+ * Description:
+ *   implements board_ioctl for userspace-kernel interface
+ *
+ ************************************************************************************/
+
+int board_ioctl(unsigned int cmd, uintptr_t arg)
+{
+	unsigned i = IOCTL_BASE_TO_IDX(cmd);
+	int ret = -EINVAL;
+
+	if (i < MAX_IOCTL_PTRS && ioctl_ptrs[i].ioctl_func) {
+		ret = ioctl_ptrs[i].ioctl_func(cmd, arg);
+	}
+
+	return ret;
+}
+
+/************************************************************************************
+ * Name: launch_kernel_builtin
+ *
+ * Description:
+ *   launches a kernel-side build-in module
+ *
+ ************************************************************************************/
+
+int launch_kernel_builtin(unsigned int cmd, unsigned long arg)
+{
+	builtinioclaunch_t *kcmd = (builtinioclaunch_t *)arg;
+	int argc = kcmd->argc;
+	char **argv = kcmd->argv;
+	int i;
+	FAR const struct builtin_s *builtin = NULL;
+
+	for (i = 0; i < g_n_kernel_builtins; i++) {
+		if (!strcmp(g_kernel_builtins[i].name, argv[0])) {
+			builtin = &g_kernel_builtins[i];
+			break;
+		}
+	}
+
+	if (builtin) {
+		/* This is running in the userspace thread, created by nsh, and
+		   called via boardctl. Call the main directly */
+		kcmd->ret = builtin->main(argc, argv);
+		return OK;
+	}
+
+	return ENOENT;
+}
+
+/************************************************************************************
+ * Name: kernel_ioctl_initialize
+ *
+ * Description:
+ *   initializes the kernel-side ioctl interface
+ *
+ ************************************************************************************/
+
+void kernel_ioctl_initialize(void)
+{
+	for (int i = 0; i < MAX_IOCTL_PTRS; i++) {
+		ioctl_ptrs[i].ioctl_func = NULL;
+	}
+
+	px4_register_boardct_ioctl(_BUILTINIOCBASE, launch_kernel_builtin);
+}
@@ -87,9 +87,26 @@ static void mavlink_usb_check(void *arg)
 	uORB::SubscriptionData<actuator_armed_s> actuator_armed_sub{ORB_ID(actuator_armed)};

 	const bool armed = actuator_armed_sub.get().armed;
-	const bool vbus_present = (board_read_VBUS_state() == PX4_OK);
+	bool vbus_present = (board_read_VBUS_state() == PX4_OK);
+	bool locked_out = false;

-	if (!armed) {
+	// If the hardware support RESET lockout that has nArmed ANDed with VBUS
+	// vbus_sense may drop during a param save which uses
+	// BOARD_INDICATE_EXTERNAL_LOCKOUT_STATE to prevent external resets
+	// while writing the params.  If we are not armed and nARMRED is low
+	// we are in such a lock out so ignore changes on VBUS_SENSE during this
+	// time.
+#if defined(BOARD_GET_EXTERNAL_LOCKOUT_STATE)
+	locked_out = BOARD_GET_EXTERNAL_LOCKOUT_STATE() == 0;
+
+	if (locked_out) {
+		vbus_present = vbus_present_prev;
+	}
+
+#endif
+
+
+	if (!armed && !locked_out) {
 		switch (usb_auto_start_state) {
 		case UsbAutoStartState::disconnected:
 			if (vbus_present && vbus_present_prev) {
@@ -0,0 +1,75 @@
+/****************************************************************************
+ *
+ *   Copyright (C) 2020 Technology Innovation Institute. All rights reserved.
+ *                 Author: Jukka Laitinen <jukkax@ssrc.tii.ae>
+ *
+ * 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
+
+/* Encode the px4 boardctl ioctls in the following way:
+ * the highest 4-bits identifies the boardctl's used by this if
+ * the next 4-bits identifies the module which handles the ioctl
+ * the low byte identiefies the actual IOCTL within the module
+ */
+
+#define _BOARDCTLIOCBASE		(0x4000)
+#define IOCTL_IDX_TO_BASE(x)	((((x) & 0xF) << 8) | _BOARDCTLIOCBASE)
+#define IOCTL_BASE_TO_IDX(x)	(((x) & 0x0F00) >> 8)
+
+#define _ORBIOCDEVBASE				IOCTL_IDX_TO_BASE(0)
+#define _HRTIOCBASE				IOCTL_IDX_TO_BASE(1)
+#define _CRYPTOIOCBASE			IOCTL_IDX_TO_BASE(2)
+#define _BUILTINIOCBASE			IOCTL_IDX_TO_BASE(3)
+#define MAX_IOCTL_PTRS 4
+
+/* The BUILTINIOCLAUNCH IOCTL is used to launch kernel side modules
+ * from the user side code
+ */
+
+#define BUILTINIOCLAUNCH               (_PX4_IOC(_BUILTINIOCBASE, 1))
+
+typedef struct builtinioclaunch {
+	int argc;
+	char **argv;
+	int ret;
+} builtinioclaunch_t;
+
+typedef int (*ioctl_ptr_t)(unsigned int, unsigned long);
+
+__BEGIN_DECLS
+
+/* Function to initialize or reset the interface */
+void kernel_ioctl_initialize(void);
+
+/* Function to register a px4 boardctl handler */
+int px4_register_boardct_ioctl(unsigned base, ioctl_ptr_t func);
+
+__END_DECLS
@@ -56,11 +56,21 @@
 #include <px4_platform_common/crypto.h>
 #endif

+#if !defined(CONFIG_BUILD_FLAT)
+#include <px4_platform/board_ctrl.h>
+#endif
+
 extern void cdcacm_init(void);

 int px4_platform_init()
 {

+#if !defined(CONFIG_BUILD_FLAT)
+	/* initialize userspace-kernelspace call gate interface */
+
+	kernel_ioctl_initialize();
+#endif
+
 	int ret = px4_console_buffer_init();

 	if (ret < 0) {
@@ -8,6 +8,7 @@ add_library(px4_layer
 	cdc_acm_check.cpp
 	${PX4_SOURCE_DIR}/platforms/posix/src/px4/common/print_load.cpp
 	${PX4_SOURCE_DIR}/platforms/posix/src/px4/common/cpuload.cpp
+	usr_hrt.cpp
 )

 target_link_libraries(px4_layer
@@ -18,6 +19,13 @@ target_link_libraries(px4_layer
 		nuttx_mm
 )

+# Build the interface library between user and kernel side
+add_library(px4_board_ctrl
+		board_ctrl.c
+)
+
+add_dependencies(px4_board_ctrl nuttx_context px4_kernel_builtin_list_target)
+
 # Build the kernel side px4_kernel_layer

 add_library(px4_kernel_layer
@@ -0,0 +1,221 @@
+/****************************************************************************
+ *
+ *   Copyright (c) 2020 PX4 Development Team. All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright
+ *    notice, this list of conditions and the following disclaimer.
+ * 2. Redistributions in binary form must reproduce the above copyright
+ *    notice, this list of conditions and the following disclaimer in
+ *    the documentation and/or other materials provided with the
+ *    distribution.
+ * 3. Neither the name PX4 nor the names of its contributors may be
+ *    used to endorse or promote products derived from this software
+ *    without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
+ * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
+ * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *
+ ****************************************************************************/
+
+/**
+ * @file usr_hrt.c
+ *
+ * Userspace High-resolution timer callouts and timekeeping.
+ *
+ * This can be used with nuttx userspace
+ *
+ */
+
+#include <px4_platform_common/px4_config.h>
+#include <px4_platform_common/defines.h>
+#include <px4_platform_common/posix.h>
+#include <px4_platform_common/shutdown.h>
+
+#include <sys/ioctl.h>
+#include <sys/types.h>
+#include <stdlib.h>
+#include <stdbool.h>
+#include <fcntl.h>
+#include <sched.h>
+#include <errno.h>
+
+#include <assert.h>
+#include <debug.h>
+
+#include <board_config.h>
+#include <drivers/drv_hrt.h>
+#include <sys/boardctl.h>
+
+static px4_task_t g_usr_hrt_task = -1;
+
+/**
+ * Fetch a never-wrapping absolute time value in microseconds from
+ * some arbitrary epoch shortly after system start.
+ */
+hrt_abstime
+hrt_absolute_time(void)
+{
+	hrt_abstime abstime = 0;
+	boardctl(HRT_ABSOLUTE_TIME, (uintptr_t)&abstime);
+	return abstime;
+}
+
+/**
+ * Store the absolute time in an interrupt-safe fashion
+ */
+void
+hrt_store_absolute_time(volatile hrt_abstime *t)
+{
+	irqstate_t flags = px4_enter_critical_section();
+	*t = hrt_absolute_time();
+	px4_leave_critical_section(flags);
+}
+
+/**
+ * Event dispatcher thread
+ */
+int
+event_thread(int argc, char *argv[])
+{
+	struct hrt_call *entry = NULL;
+
+	while (1) {
+		/* Wait for hrt tick */
+		boardctl(HRT_WAITEVENT, (uintptr_t)&entry);
+
+		/* HRT event received, dispatch */
+		if (entry) {
+			entry->usr_callout(entry->usr_arg);
+		}
+	}
+
+	return 0;
+}
+
+/**
+ * Request stop.
+ */
+bool hrt_request_stop()
+{
+	px4_task_delete(g_usr_hrt_task);
+	return true;
+}
+
+/**
+ * Initialise the high-resolution timing module.
+ */
+void
+hrt_init(void)
+{
+	px4_register_shutdown_hook(hrt_request_stop);
+	g_usr_hrt_task = px4_task_spawn_cmd("usr_hrt", SCHED_DEFAULT, SCHED_PRIORITY_MAX, 1000, event_thread, NULL);
+}
+
+/**
+ * Call callout(arg) after interval has elapsed.
+ */
+void
+hrt_call_after(struct hrt_call *entry, hrt_abstime delay, hrt_callout callout, void *arg)
+{
+	hrt_boardctl_t ioc_parm;
+	ioc_parm.entry = entry;
+	ioc_parm.time = delay;
+	ioc_parm.callout = callout;
+	ioc_parm.arg = arg;
+	entry->usr_callout = callout;
+	entry->usr_arg = arg;
+
+	boardctl(HRT_CALL_AFTER, (uintptr_t)&ioc_parm);
+}
+
+/**
+ * Call callout(arg) at calltime.
+ */
+void
+hrt_call_at(struct hrt_call *entry, hrt_abstime calltime, hrt_callout callout, void *arg)
+{
+	hrt_boardctl_t ioc_parm;
+	ioc_parm.entry = entry;
+	ioc_parm.time = calltime;
+	ioc_parm.interval = 0;
+	ioc_parm.callout = callout;
+	ioc_parm.arg = arg;
+	entry->usr_callout = callout;
+	entry->usr_arg = arg;
+
+	boardctl(HRT_CALL_AT, (uintptr_t)&ioc_parm);
+}
+
+/**
+ * Call callout(arg) every period.
+ */
+void
+hrt_call_every(struct hrt_call *entry, hrt_abstime delay, hrt_abstime interval, hrt_callout callout, void *arg)
+{
+	hrt_boardctl_t ioc_parm;
+	ioc_parm.entry = entry;
+	ioc_parm.time = delay;
+	ioc_parm.interval = interval;
+	ioc_parm.callout = callout;
+	ioc_parm.arg = arg;
+	entry->usr_callout = callout;
+	entry->usr_arg = arg;
+
+	boardctl(HRT_CALL_EVERY, (uintptr_t)&ioc_parm);
+}
+
+/**
+ * Remove the entry from the callout list.
+ */
+void
+hrt_cancel(struct hrt_call *entry)
+{
+	boardctl(HRT_CANCEL, (uintptr_t)entry);
+}
+
+void
+hrt_call_init(struct hrt_call *entry)
+{
+	memset(entry, 0, sizeof(*entry));
+}
+
+/**
+ * If this returns true, the call has been invoked and removed from the callout list.
+ *
+ * Always returns false for repeating callouts.
+ */
+bool
+hrt_called(struct hrt_call *entry)
+{
+	return (entry->deadline == 0);
+}
+
+latency_info_t
+get_latency(uint16_t bucket_idx, uint16_t counter_idx)
+{
+	latency_boardctl_t latency_ioc;
+	latency_ioc.bucket_idx = bucket_idx;
+	latency_ioc.counter_idx = counter_idx;
+	latency_ioc.latency = {0, 0};
+	boardctl(HRT_GET_LATENCY, (uintptr_t)&latency_ioc);
+	return latency_ioc.latency;
+}
+
+void reset_latency_counters()
+{
+	boardctl(HRT_RESET_LATENCY, NULL);
+}
@@ -561,31 +561,6 @@ hrt_absolute_time(void)
 	return abstime;
 }

-/**
- * Convert a timespec to absolute time
- */
-hrt_abstime
-ts_to_abstime(const struct timespec *ts)
-{
-	hrt_abstime	result;
-
-	result = (hrt_abstime)(ts->tv_sec) * 1000000;
-	result += ts->tv_nsec / 1000;
-
-	return result;
-}
-
-/**
- * Convert absolute time to a timespec.
- */
-void
-abstime_to_ts(struct timespec *ts, hrt_abstime abstime)
-{
-	ts->tv_sec = abstime / 1000000;
-	abstime -= ts->tv_sec * 1000000;
-	ts->tv_nsec = abstime * 1000;
-}
-
 /**
 * Store the absolute time in an interrupt-safe fashion
 */
@@ -573,31 +573,6 @@ hrt_absolute_time(void)
 	return abstime;
 }

-/**
- * Convert a timespec to absolute time
- */
-hrt_abstime
-ts_to_abstime(const struct timespec *ts)
-{
-	hrt_abstime	result;
-
-	result = (hrt_abstime)(ts->tv_sec) * 1000000;
-	result += ts->tv_nsec / 1000;
-
-	return result;
-}
-
-/**
- * Convert absolute time to a timespec.
- */
-void
-abstime_to_ts(struct timespec *ts, hrt_abstime abstime)
-{
-	ts->tv_sec = abstime / 1000000;
-	abstime -= ts->tv_sec * 1000000;
-	ts->tv_nsec = abstime * 1000;
-}
-
 /**
 * Store the absolute time in an interrupt-safe fashion
 */
@@ -609,31 +609,6 @@ hrt_absolute_time(void)
 	return abstime;
 }

-/**
- * Convert a timespec to absolute time
- */
-hrt_abstime
-ts_to_abstime(const struct timespec *ts)
-{
-	hrt_abstime	result;
-
-	result = (hrt_abstime)(ts->tv_sec) * 1000000;
-	result += ts->tv_nsec / 1000;
-
-	return result;
-}
-
-/**
- * Convert absolute time to a timespec.
- */
-void
-abstime_to_ts(struct timespec *ts, hrt_abstime abstime)
-{
-	ts->tv_sec = abstime / 1000000;
-	abstime -= ts->tv_sec * 1000000;
-	ts->tv_nsec = abstime * 1000;
-}
-
 /**
 * Store the absolute time in an interrupt-safe fashion
 */
@@ -497,43 +497,6 @@ hrt_abstime hrt_absolute_time(void)
 	return ((uint64_t) hi << 32) | lo;
 }

-/**
- * Convert a timespec to absolute time
- */
-hrt_abstime ts_to_abstime(const struct timespec *ts)
-{
-	hrt_abstime	result;
-
-	result = (hrt_abstime)(ts->tv_sec) * 1000000;
-	result += ts->tv_nsec / 1000;
-
-	return result;
-}
-
-/**
- * Convert absolute time to a timespec.
- */
-void abstime_to_ts(struct timespec *ts, hrt_abstime abstime)
-{
-	ts->tv_sec = abstime / 1000000;
-	abstime -= ts->tv_sec * 1000000;
-	ts->tv_nsec = abstime * 1000;
-}
-
-/**
- * Compare a time value with the current time as atomic operation
- */
-hrt_abstime hrt_elapsed_time_atomic(const volatile hrt_abstime *then)
-{
-	irqstate_t flags = px4_enter_critical_section();
-
-	hrt_abstime delta = hrt_absolute_time() - *then;
-
-	px4_leave_critical_section(flags);
-
-	return delta;
-}
-
 /**
 * Store the absolute time in an interrupt-safe fashion
 */
@@ -72,6 +72,33 @@
 #  define hrtinfo(x...)
 #endif

+#if !defined(CONFIG_BUILD_FLAT)
+#include <px4_platform_common/defines.h>
+#include <px4_platform/board_ctrl.h>
+#include <px4_platform_common/sem.h>
+
+#define HRT_ENTRY_QUEUE_MAX_SIZE 3
+static px4_sem_t g_wait_sem;
+static struct hrt_call *next_hrt_entry[HRT_ENTRY_QUEUE_MAX_SIZE];
+static int hrt_entry_queued = 0;
+static bool suppress_entry_queue_error = false;
+static bool hrt_entry_queue_error = false;
+
+void hrt_usr_call(void *arg)
+{
+	// This is called from hrt interrupt
+	if (hrt_entry_queued < HRT_ENTRY_QUEUE_MAX_SIZE) {
+		next_hrt_entry[hrt_entry_queued++] = (struct hrt_call *)arg;
+
+	} else {
+		hrt_entry_queue_error = true;
+	}
+
+	px4_sem_post(&g_wait_sem);
+}
+
+#endif
+
 #ifdef HRT_TIMER

 /* HRT configuration */
@@ -275,6 +302,8 @@ static void		hrt_call_enter(struct hrt_call *entry);
 static void		hrt_call_reschedule(void);
 static void		hrt_call_invoke(void);

+
+int hrt_ioctl(unsigned int cmd, unsigned long arg);
 /*
 * Specific registers and bits used by PPM sub-functions
 */
@@ -694,31 +723,6 @@ hrt_absolute_time(void)
 	return abstime;
 }

-/**
- * Convert a timespec to absolute time
- */
-hrt_abstime
-ts_to_abstime(const struct timespec *ts)
-{
-	hrt_abstime	result;
-
-	result = (hrt_abstime)(ts->tv_sec) * 1000000;
-	result += ts->tv_nsec / 1000;
-
-	return result;
-}
-
-/**
- * Convert absolute time to a timespec.
- */
-void
-abstime_to_ts(struct timespec *ts, hrt_abstime abstime)
-{
-	ts->tv_sec = abstime / 1000000;
-	abstime -= ts->tv_sec * 1000000;
-	ts->tv_nsec = abstime * 1000;
-}
-
 /**
 * Store the absolute time in an interrupt-safe fashion
 */
@@ -743,6 +747,16 @@ hrt_init(void)
 	/* configure the PPM input pin */
 	px4_arch_configgpio(GPIO_PPM_IN);
 #endif
+
+#if !defined(CONFIG_BUILD_FLAT)
+	/* Create a semaphore for handling hrt driver callbacks */
+	px4_sem_init(&g_wait_sem, 0, 0);
+	/* this is a signalling semaphore */
+	px4_sem_setprotocol(&g_wait_sem, SEM_PRIO_NONE);
+
+	/* register ioctl callbacks */
+	px4_register_boardct_ioctl(_HRTIOCBASE, hrt_ioctl);
+#endif
 }

 /**
@@ -1003,6 +1017,81 @@ void reset_latency_counters(void)
 		latency_counters[i] = 0;
 	}
 }
+
+/* board_ioctl interface for user-space hrt driver */
+int
+hrt_ioctl(unsigned int cmd, unsigned long arg)
+{
+	hrt_boardctl_t *h = (hrt_boardctl_t *)arg;
+
+	switch (cmd) {
+	case HRT_WAITEVENT: {
+			irqstate_t flags;
+			px4_sem_wait(&g_wait_sem);
+			/* Atomically update the pointer to user side hrt entry */
+			flags = px4_enter_critical_section();
+
+			/* This should be always true, but check it anyway */
+			if (hrt_entry_queued > 0) {
+				*(struct hrt_call **)arg = next_hrt_entry[--hrt_entry_queued];
+				next_hrt_entry[hrt_entry_queued] = NULL;
+
+			} else {
+				hrt_entry_queue_error = true;
+			}
+
+			px4_leave_critical_section(flags);
+
+			/* Warn once for entry queue being full */
+			if (hrt_entry_queue_error && !suppress_entry_queue_error) {
+				PX4_ERR("HRT entry error, queue size now %d", hrt_entry_queued);
+				suppress_entry_queue_error = true;
+			}
+		}
+		break;
+
+	case HRT_ABSOLUTE_TIME:
+		*(hrt_abstime *)arg = hrt_absolute_time();
+		break;
+
+	case HRT_CALL_AFTER:
+		hrt_call_after(h->entry, h->time, (hrt_callout)hrt_usr_call, h->entry);
+		break;
+
+	case HRT_CALL_AT:
+		hrt_call_at(h->entry, h->time, (hrt_callout)hrt_usr_call, h->entry);
+		break;
+
+	case HRT_CALL_EVERY:
+		hrt_call_every(h->entry, h->time, h->interval, (hrt_callout)hrt_usr_call, h->entry);
+		break;
+
+	case HRT_CANCEL:
+		if (h && h->entry) {
+			hrt_cancel(h->entry);
+
+		} else {
+			PX4_ERR("HRT_CANCEL called with NULL entry");
+		}
+
+		break;
+
+	case HRT_GET_LATENCY: {
+			latency_boardctl_t *latency = (latency_boardctl_t *)arg;
+			latency->latency = get_latency(latency->bucket_idx, latency->counter_idx);
+		}
+		break;
+
+	case HRT_RESET_LATENCY:
+		reset_latency_counters();
+		break;
+
+	default:
+		return -EINVAL;
+	}
+
+	return OK;
+}
 #endif

 #endif /* HRT_TIMER */
@@ -91,6 +91,9 @@ enum Timer {
 	Timer12,
 	Timer13,
 	Timer14,
+#ifdef STM32_TIM15_BASE
+	Timer15
+#endif
 };
 enum Channel {
 	Channel1 = 1,
@@ -153,6 +156,11 @@ static inline constexpr uint32_t timerBaseRegister(Timer::Timer timer)
 	case Timer::Timer14: return STM32_TIM14_BASE;
 #endif

+#ifdef STM32_TIM15_BASE
+
+	case Timer::Timer15: return STM32_TIM15_BASE;
+#endif
+
 	default: break;
 	}

@@ -644,7 +644,12 @@ int io_timer_init_timer(unsigned timer, io_timer_channel_mode_t mode)
 		rCCER(timer) = 0;
 		rDCR(timer) = 0;

-		if ((io_timers[timer].base == STM32_TIM1_BASE) || (io_timers[timer].base == STM32_TIM8_BASE)) {
+		if ((io_timers[timer].base == STM32_TIM1_BASE)
+		    || (io_timers[timer].base == STM32_TIM8_BASE)
+#ifdef STM32_TIM15_BASE
+		    || (io_timers[timer].base == STM32_TIM15_BASE)
+#endif
+		   ) {

 			/* master output enable = on */

@@ -85,6 +85,7 @@ static inline constexpr uint32_t getTimerUpdateDMAMap(Timer::Timer timer, const
 	case Timer::Timer12:
 	case Timer::Timer13:
 	case Timer::Timer14:
+	case Timer::Timer15:
 		break;
 	}

@@ -68,6 +68,10 @@ static inline constexpr timer_io_channels_t initIOTimerGPIOInOut(Timer::TimerCha
 		gpio_af = GPIO_AF3;
 		break;

+	case Timer::Timer15:
+		gpio_af = GPIO_AF4;
+		break;
+
 	case Timer::Timer13:
 	case Timer::Timer14:
 		gpio_af = GPIO_AF9;
@@ -259,6 +263,17 @@ static inline constexpr io_timers_t initIOTimer(Timer::Timer timer, DMA dshot_dm
 		ret.vectorno =  STM32_IRQ_TIM14;
 #ifdef CONFIG_STM32_TIM14
 		nuttx_config_timer_enabled = true;
+#endif
+		break;
+
+	case Timer::Timer15:
+		ret.base = STM32_TIM15_BASE;
+		ret.clock_register = STM32_RCC_APB2ENR;
+		ret.clock_bit = RCC_APB2ENR_TIM15EN;
+		ret.clock_freq = STM32_APB2_TIM15_CLKIN;
+		ret.vectorno =  STM32_IRQ_TIM15;
+#ifdef CONFIG_STM32_TIM15
+		nuttx_config_timer_enabled = true;
 #endif
 		break;
 	}
@@ -144,19 +144,6 @@ hrt_abstime hrt_absolute_time()
 #endif // defined(ENABLE_LOCKSTEP_SCHEDULER)
 }

-/*
- * Convert a timespec to absolute time.
- */
-hrt_abstime ts_to_abstime(const struct timespec *ts)
-{
-	hrt_abstime	result;
-
-	result = (hrt_abstime)(ts->tv_sec) * 1000000;
-	result += ts->tv_nsec / 1000;
-
-	return result;
-}
-
 /*
 * Store the absolute time in an interrupt-safe fashion.
 *
@@ -484,13 +471,6 @@ hrt_call_invoke()
 	hrt_unlock();
 }

-void abstime_to_ts(struct timespec *ts, hrt_abstime abstime)
-{
-	ts->tv_sec = abstime / 1000000;
-	abstime -= ts->tv_sec * 1000000;
-	ts->tv_nsec = abstime * 1000;
-}
-
 int px4_clock_gettime(clockid_t clk_id, struct timespec *tp)
 {
 	if (clk_id == CLOCK_MONOTONIC) {
@@ -47,6 +47,10 @@
 #include <px4_platform_common/time.h>
 #include <queue.h>

+#if defined(__PX4_NUTTX) && !defined(CONFIG_BUILD_FLAT)
+#include <px4_platform/board_ctrl.h>
+#endif
+
 __BEGIN_DECLS

 /**
@@ -76,6 +80,10 @@ typedef struct hrt_call {
 	hrt_abstime		period;
 	hrt_callout		callout;
 	void			*arg;
+#if defined(__PX4_NUTTX) && !defined(CONFIG_BUILD_FLAT)
+	hrt_callout		usr_callout;
+	void			*usr_arg;
+#endif
 } *hrt_call_t;


@@ -89,6 +97,35 @@ typedef struct latency_info {
 	uint32_t                counter;
 } latency_info_t;

+#if defined(__PX4_NUTTX) && !defined(CONFIG_BUILD_FLAT)
+
+typedef struct hrt_boardctl {
+	hrt_call_t		entry;
+	hrt_abstime		time; /* delay or calltime */
+	hrt_abstime		interval;
+	hrt_callout		callout;
+	void			*arg;
+} hrt_boardctl_t;
+
+typedef struct latency_boardctl {
+	uint16_t		bucket_idx;
+	uint16_t		counter_idx;
+	latency_info_t	latency;
+} latency_boardctl_t;
+
+#define _HRTIOC(_n)		(_PX4_IOC(_HRTIOCBASE, _n))
+
+#define HRT_WAITEVENT		_HRTIOC(1)
+#define HRT_ABSOLUTE_TIME	_HRTIOC(2)
+#define HRT_CALL_AFTER		_HRTIOC(3)
+#define HRT_CALL_AT		_HRTIOC(4)
+#define HRT_CALL_EVERY		_HRTIOC(5)
+#define HRT_CANCEL		_HRTIOC(6)
+#define HRT_GET_LATENCY		_HRTIOC(7)
+#define HRT_RESET_LATENCY	_HRTIOC(8)
+
+#endif
+
 /**
 * Get absolute time in [us] (does not wrap).
 */
@@ -97,12 +134,25 @@ __EXPORT extern hrt_abstime hrt_absolute_time(void);
 /**
 * Convert a timespec to absolute time.
 */
-__EXPORT extern hrt_abstime ts_to_abstime(const struct timespec *ts);
+static inline hrt_abstime ts_to_abstime(const struct timespec *ts)
+{
+	hrt_abstime	result;
+
+	result = (hrt_abstime)(ts->tv_sec) * 1000000;
+	result += ts->tv_nsec / 1000;
+
+	return result;
+}

 /**
 * Convert absolute time to a timespec.
 */
-__EXPORT extern void	abstime_to_ts(struct timespec *ts, hrt_abstime abstime);
+static inline void abstime_to_ts(struct timespec *ts, hrt_abstime abstime)
+{
+	ts->tv_sec = abstime / 1000000;
+	abstime -= ts->tv_sec * 1000000;
+	ts->tv_nsec = abstime * 1000;
+}

 /**
 * Compute the delta between a timestamp taken in the past
@@ -478,7 +478,7 @@ bool DShot::updateOutputs(bool stop_motors, uint16_t outputs[MAX_ACTUATORS],
 		_current_command.clear();
 	}

-	if (stop_motors || num_control_groups_updated > 0) {
+	if (stop_motors || num_control_groups_updated > 0 || _mixing_output.useDynamicMixing()) {
 		up_dshot_trigger();
 	}

@@ -2,4 +2,11 @@ menuconfig DRIVERS_GPS
 	bool "gps"
 	default n
 	---help---
-		Enable support for gps
+		Enable support for gps
+
+menuconfig USER_GPS
+	bool "gps running as userspace module"
+	default y
+	depends on BOARD_PROTECTED && DRIVERS_GPS
+	---help---
+		Put gps in userspace memory
@@ -117,7 +117,7 @@ public:
 		Count
 	};

-	GPS(const char *path, gps_driver_mode_t mode, GPSHelper::Interface interface, bool enable_sat_info, Instance instance,
+	GPS(const char *path, gps_driver_mode_t mode, GPSHelper::Interface interface, Instance instance,
 	    unsigned configured_baudrate);
 	~GPS() override;

@@ -279,8 +279,8 @@ px4::atomic<GPS *> GPS::_secondary_instance{nullptr};
 extern "C" __EXPORT int gps_main(int argc, char *argv[]);


-GPS::GPS(const char *path, gps_driver_mode_t mode, GPSHelper::Interface interface, bool enable_sat_info,
-	 Instance instance, unsigned configured_baudrate) :
+GPS::GPS(const char *path, gps_driver_mode_t mode, GPSHelper::Interface interface, Instance instance,
+	 unsigned configured_baudrate) :
 	Device(MODULE_NAME),
 	_configured_baudrate(configured_baudrate),
 	_mode(mode),
@@ -295,6 +295,9 @@ GPS::GPS(const char *path, gps_driver_mode_t mode, GPSHelper::Interface interfac
 	_report_gps_pos.heading = NAN;
 	_report_gps_pos.heading_offset = NAN;

+	int32_t enable_sat_info = 0;
+	param_get(param_find("GPS_SAT_INFO"), &enable_sat_info);
+
 	/* create satellite info data object if requested */
 	if (enable_sat_info) {
 		_sat_info = new GPS_Sat_Info();
@@ -1254,8 +1257,6 @@ $ gps reset warm
 	PRINT_MODULE_USAGE_PARAM_STRING('e', nullptr, "<file:dev>", "Optional secondary GPS device", true);
 	PRINT_MODULE_USAGE_PARAM_INT('g', 0, 0, 3000000, "Baudrate (secondary GPS, can also be p:<param_name>)", true);

-	PRINT_MODULE_USAGE_PARAM_FLAG('s', "Enable publication of satellite info", true);
-
 	PRINT_MODULE_USAGE_PARAM_STRING('i', "uart", "spi|uart", "GPS interface", true);
 	PRINT_MODULE_USAGE_PARAM_STRING('j', "uart", "spi|uart", "secondary GPS interface", true);
 	PRINT_MODULE_USAGE_PARAM_STRING('p', nullptr, "ubx|mtk|ash|eml|fem|nmea", "GPS Protocol (default=auto select)", true);
@@ -1324,7 +1325,6 @@ GPS *GPS::instantiate(int argc, char *argv[], Instance instance)
 	const char *device_name_secondary = nullptr;
 	int baudrate_main = 0;
 	int baudrate_secondary = 0;
-	bool enable_sat_info = false;
 	GPSHelper::Interface interface = GPSHelper::Interface::UART;
 	GPSHelper::Interface interface_secondary = GPSHelper::Interface::UART;
 	gps_driver_mode_t mode = gps_driver_mode_t::None;
@@ -1334,7 +1334,7 @@ GPS *GPS::instantiate(int argc, char *argv[], Instance instance)
 	int ch;
 	const char *myoptarg = nullptr;

-	while ((ch = px4_getopt(argc, argv, "b:d:e:g:si:j:p:", &myoptind, &myoptarg)) != EOF) {
+	while ((ch = px4_getopt(argc, argv, "b:d:e:g:i:j:p:", &myoptind, &myoptarg)) != EOF) {
 		switch (ch) {
 		case 'b':
 			if (px4_get_parameter_value(myoptarg, baudrate_main) != 0) {
@@ -1357,10 +1357,6 @@ GPS *GPS::instantiate(int argc, char *argv[], Instance instance)
 			device_name_secondary = myoptarg;
 			break;

-		case 's':
-			enable_sat_info = true;
-			break;
-
 		case 'i':
 			if (!strcmp(myoptarg, "spi")) {
 				interface = GPSHelper::Interface::SPI;
@@ -1430,7 +1426,7 @@ GPS *GPS::instantiate(int argc, char *argv[], Instance instance)
 	GPS *gps = nullptr;
 	if (instance == Instance::Main) {
 		if (device_name && (access(device_name, R_OK|W_OK) == 0)) {
-			gps = new GPS(device_name, mode, interface, enable_sat_info, instance, baudrate_main);
+			gps = new GPS(device_name, mode, interface, instance, baudrate_main);

 		} else {
 			PX4_ERR("invalid device (-d) %s", device_name ? device_name  : "");
@@ -1453,7 +1449,7 @@ GPS *GPS::instantiate(int argc, char *argv[], Instance instance)
 		}
 	} else { // secondary instance
 		if (device_name_secondary && (access(device_name_secondary, R_OK|W_OK) == 0)) {
-			gps = new GPS(device_name_secondary, mode, interface_secondary, enable_sat_info, instance, baudrate_secondary);
+			gps = new GPS(device_name_secondary, mode, interface_secondary, instance, baudrate_secondary);

 		} else {
 			PX4_ERR("invalid secondary device (-g) %s", device_name_secondary ? device_name_secondary : "");
@@ -69,6 +69,18 @@ PARAM_DEFINE_INT32(GPS_DUMP_COMM, 0);
 */
 PARAM_DEFINE_INT32(GPS_UBX_DYNMODEL, 7);

+/**
+ * Enable sat info (if available)
+ *
+ * Enable publication of satellite info (ORB_ID(satellite_info)) if possible.
+ * Not available on MTK.
+ *
+ * @boolean
+ * @reboot_required true
+ * @group GPS
+ */
+PARAM_DEFINE_INT32(GPS_SAT_INFO, 0);
+
 /**
 * u-blox GPS Mode
 *
@@ -377,7 +377,7 @@ bool ADIS16470::Configure()

 	_px4_accel.set_scale(CONSTANTS_ONE_G / 2048.f);
 	_px4_accel.set_range(40.f * CONSTANTS_ONE_G);
-	_px4_gyro.set_scale(math::radians(2000.f / 32768.f));
+	_px4_gyro.set_scale(math::radians(1.f / 0.1f)); // 1 LSB = 0.1°/sec
 	_px4_gyro.set_range(math::radians(2000.f));

 	_px4_accel.set_scale(1.25f * CONSTANTS_ONE_G / 1000.0f); // accel 1.25 mg/LSB
@@ -0,0 +1,5 @@
+menuconfig DRIVERS_IMU_INVENSENSE_ICM42670P
+	bool "icm42670p"
+	default n
+	---help---
+		Enable support for icm42670p
@@ -1,6 +1,6 @@
 /****************************************************************************
 *
- *   Copyright (c) 2018 PX4 Development Team. All rights reserved.
+ *   Copyright (c) 2018-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
@@ -44,15 +44,7 @@
 RM3100::RM3100(device::Device *interface, const I2CSPIDriverConfig &config) :
 	I2CSPIDriver(config),
 	_px4_mag(interface->get_device_id(), config.rotation),
-	_interface(interface),
-	_comms_errors(perf_alloc(PC_COUNT, MODULE_NAME": comms_errors")),
-	_conf_errors(perf_alloc(PC_COUNT, MODULE_NAME": conf_errors")),
-	_range_errors(perf_alloc(PC_COUNT, MODULE_NAME": range_errors")),
-	_sample_perf(perf_alloc(PC_ELAPSED, MODULE_NAME": read")),
-	_continuous_mode_set(false),
-	_mode(SINGLE),
-	_measure_interval(0),
-	_check_state_cnt(0)
+	_interface(interface)
 {
 	_px4_mag.set_scale(1.f / (RM3100_SENSITIVITY * UTESLA_TO_GAUSS));
 }
@@ -60,25 +52,25 @@ RM3100::RM3100(device::Device *interface, const I2CSPIDriverConfig &config) :
 RM3100::~RM3100()
 {
 	// free perf counters
-	perf_free(_sample_perf);
-	perf_free(_comms_errors);
-	perf_free(_range_errors);
-	perf_free(_conf_errors);
+	perf_free(_reset_perf);
+	perf_free(_range_error_perf);
+	perf_free(_bad_transfer_perf);

 	delete _interface;
 }

 int RM3100::self_test()
 {
-	/* Chances are that a poll event was triggered, so wait for conversion and read registers in order to clear DRDY bit */
-	usleep(RM3100_CONVERSION_INTERVAL);
-	collect();
+	bool complete = false;

-	/* Fail if calibration is not good */
-	int ret = 0;
-	uint8_t cmd = 0;
+	uint8_t cmd = (CMM_DEFAULT | POLLING_MODE);
+	int ret = _interface->write(ADDR_CMM, &cmd, 1);

-	/* Configure mag into self test mode */
+	if (ret != PX4_OK) {
+		return ret;
+	}
+
+	// Configure sensor to execute BIST upon receipt of a POLL command
 	cmd = BIST_SELFTEST;
 	ret = _interface->write(ADDR_BIST, &cmd, 1);

@@ -86,51 +78,80 @@ int RM3100::self_test()
 		return ret;
 	}

-	/* Now we need to write to POLL to launch self test */
-	cmd = POLL_XYZ;
-	ret = _interface->write(ADDR_POLL, &cmd, 1);
+	/* Perform test procedure until a valid result is obtained or test times out */
+	const hrt_abstime t_start = hrt_absolute_time();

-	if (ret != PX4_OK) {
-		return ret;
+	while ((hrt_absolute_time() - t_start) < BIST_DUR_USEC) {
+
+		// Re-disable DRDY clear
+		cmd = HSHAKE_NO_DRDY_CLEAR;
+		ret = _interface->write(ADDR_HSHAKE, &cmd, 1);
+
+		if (ret != PX4_OK) {
+			return ret;
+		}
+
+		// Poll for a measurement
+		cmd = POLL_XYZ;
+		ret = _interface->write(ADDR_POLL, &cmd, 1);
+
+		if (ret != PX4_OK) {
+			return ret;
+		}
+
+		uint8_t status = 0;
+		ret = _interface->read(ADDR_STATUS, &status, 1);
+
+		if (ret != PX4_OK) {
+			return ret;
+		}
+
+		// If the DRDY bit in the status register is set, BIST should be complete
+		if (status & STATUS_DRDY) {
+			// Check BIST register to evaluate the test result
+			ret = _interface->read(ADDR_BIST, &cmd, 1);
+
+			if (ret != PX4_OK) {
+				return ret;
+			}
+
+			// The test results are not valid if STE is not set. In this case, we try again
+			if (cmd & BIST_STE) {
+				complete = true;
+
+				// If the test passed, disable self-test mode by clearing the STE bit
+				if (cmd & BIST_XYZ_OK) {
+					cmd = 0;
+					ret = _interface->write(ADDR_BIST, &cmd, 1);
+
+					if (ret != PX4_OK) {
+						PX4_ERR("Failed to disable built-in self test");
+					}
+
+					return PX4_OK;
+
+				} else {
+					PX4_ERR("built-in self test failed");
+				}
+			}
+		}
 	}

-	/* Now wait for status register */
-	usleep(RM3100_CONVERSION_INTERVAL);
-
-	if (check_measurement() != PX4_OK) {
-		return -1;;
+	if (!complete) {
+		PX4_ERR("built-in self test incomplete");
 	}

-	/* Now check BIST register to see whether self test is ok or not*/
-	ret = _interface->read(ADDR_BIST, &cmd, 1);
-
-	if (ret != PX4_OK) {
-		return ret;
-	}
-
-	ret = !((cmd & BIST_XYZ_OK) == BIST_XYZ_OK);
-
-	return ret;
+	return PX4_ERROR;
 }

-int RM3100::check_measurement()
+void RM3100::RunImpl()
 {
-	uint8_t status = 0;
-	int ret = _interface->read(ADDR_STATUS, &status, 1);
-
-	if (ret != 0) {
-		return ret;
-	}
-
-	return !((status & STATUS_DRDY) == STATUS_DRDY) ;
-}
-
-int RM3100::collect()
-{
-	/* Check whether a measurement is available or not, otherwise return immediately */
-	if (check_measurement() != 0) {
-		PX4_DEBUG("No measurement available");
-		return 0;
+	// full reset if things are failing consistently
+	if (_failure_count > 10) {
+		_failure_count = 0;
+		set_default_register_values();
+		ScheduleOnInterval(RM3100_INTERVAL);
+		return;
 	}

 	struct {
@@ -139,21 +160,15 @@ int RM3100::collect()
 		uint8_t z[3];
 	} rm_report{};

-	_px4_mag.set_error_count(perf_event_count(_comms_errors));
-
-	perf_begin(_sample_perf);
-
 	const hrt_abstime timestamp_sample = hrt_absolute_time();
 	int ret = _interface->read(ADDR_MX, (uint8_t *)&rm_report, sizeof(rm_report));

 	if (ret != OK) {
-		perf_end(_sample_perf);
-		perf_count(_comms_errors);
-		return ret;
+		perf_count(_bad_transfer_perf);
+		_failure_count++;
+		return;
 	}

-	perf_end(_sample_perf);
-
 	/* Rearrange mag data */
 	int32_t xraw = ((rm_report.x[0] << 16) | (rm_report.x[1] << 8) | rm_report.x[2]);
 	int32_t yraw = ((rm_report.y[0] << 16) | (rm_report.y[1] << 8) | rm_report.y[2]);
@@ -164,12 +179,36 @@ int RM3100::collect()
 	convert_signed(&yraw);
 	convert_signed(&zraw);

-	_px4_mag.update(timestamp_sample, xraw, yraw, zraw);
+	// valid range: -8388608 to 8388607
+	if (xraw < -8388608 || xraw > 8388607 ||
+	    yraw < -8388608 || yraw > 8388607 ||
+	    zraw < -8388608 || zraw > 8388607) {

-	ret = OK;
+		_failure_count++;

+		perf_count(_range_error_perf);
+		return;
+	}

-	return ret;
+	// only publish changes
+	if (_raw_data_prev[0] != xraw || _raw_data_prev[1] != yraw || _raw_data_prev[2] != zraw) {
+
+		_px4_mag.set_error_count(perf_event_count(_bad_transfer_perf)
+					 + perf_event_count(_range_error_perf));
+
+		_px4_mag.update(timestamp_sample, xraw, yraw, zraw);
+
+		_raw_data_prev[0] = xraw;
+		_raw_data_prev[1] = yraw;
+		_raw_data_prev[2] = zraw;
+
+		if (_failure_count > 0) {
+			_failure_count--;
+		}
+
+	} else {
+		_failure_count++;
+	}
 }

 void RM3100::convert_signed(int32_t *n)
@@ -180,106 +219,34 @@ void RM3100::convert_signed(int32_t *n)
 	}
 }

-void RM3100::RunImpl()
-{
-	/* _measure_interval == 0  is used as _task_should_exit */
-	if (_measure_interval == 0) {
-		return;
-	}
-
-	/* Collect last measurement at the start of every cycle */
-	if (collect() != OK) {
-		PX4_DEBUG("collection error");
-		/* restart the measurement state machine */
-		start();
-		return;
-	}
-
-	if (measure() != OK) {
-		PX4_DEBUG("measure error");
-	}
-
-	if (_measure_interval > 0) {
-		/* schedule a fresh cycle call when the measurement is done */
-		ScheduleDelayed(_measure_interval);
-	}
-}
-
 int RM3100::init()
 {
-	/* reset the device configuration */
-	reset();
-
 	int ret = self_test();

 	if (ret != PX4_OK) {
 		PX4_ERR("self test failed");
 	}

-	_measure_interval = RM3100_CONVERSION_INTERVAL;
-	start();
-
-	return ret;
-}
-
-int RM3100::measure()
-{
-	int ret = 0;
-	uint8_t cmd = 0;
-
-	/* Send the command to begin a measurement. */
-	if ((_mode == CONTINUOUS) && !_continuous_mode_set) {
-		cmd = (CMM_DEFAULT | CONTINUOUS_MODE);
-		ret = _interface->write(ADDR_CMM, &cmd, 1);
-		_continuous_mode_set = true;
-
-	} else if (_mode == SINGLE) {
-		if (_continuous_mode_set) {
-			/* This is needed for polling mode */
-			cmd = (CMM_DEFAULT | POLLING_MODE);
-			ret = _interface->write(ADDR_CMM, &cmd, 1);
-
-			if (ret != OK) {
-				perf_count(_comms_errors);
-				return ret;
-			}
-
-			_continuous_mode_set = false;
-		}
-
-		cmd = POLL_XYZ;
-		ret = _interface->write(ADDR_POLL, &cmd, 1);
+	if (set_default_register_values() == PX4_OK) {
+		ScheduleOnInterval(RM3100_INTERVAL);
+		return PX4_OK;
 	}

-
-	if (ret != OK) {
-		perf_count(_comms_errors);
-	}
-
-	return ret;
+	return PX4_ERROR;
 }

 void RM3100::print_status()
 {
 	I2CSPIDriverBase::print_status();
-	perf_print_counter(_sample_perf);
-	perf_print_counter(_comms_errors);
-	PX4_INFO("poll interval:  %u", _measure_interval);
-}
-
-int RM3100::reset()
-{
-	int ret = set_default_register_values();
-
-	if (ret != OK) {
-		return PX4_ERROR;
-	}
-
-	return PX4_OK;
+	perf_print_counter(_reset_perf);
+	perf_print_counter(_range_error_perf);
+	perf_print_counter(_bad_transfer_perf);
 }

 int RM3100::set_default_register_values()
 {
+	perf_count(_reset_perf);
+
 	uint8_t cmd[2] = {0, 0};

 	cmd[0] = CCX_DEFAULT_MSB;
@@ -305,11 +272,3 @@ int RM3100::set_default_register_values()

 	return PX4_OK;
 }
-
-void RM3100::start()
-{
-	set_default_register_values();
-
-	/* schedule a cycle to start things */
-	ScheduleNow();
-}
@@ -1,6 +1,6 @@
 /****************************************************************************
 *
- *   Copyright (c) 2018 PX4 Development Team. All rights reserved.
+ *   Copyright (c) 2018-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
@@ -50,8 +50,7 @@
 * RM3100 internal constants and data structures.
 */

-/* At 146 Hz we encounter errors, 100 Hz is safer */
-#define RM3100_CONVERSION_INTERVAL	10000	// Microseconds, corresponds to 100 Hz (cycle count 200 on 3 axis)
+#define RM3100_INTERVAL			13000	// 13000 Microseconds, corresponds to ~75 Hz (TMRC 0x95)
 #define UTESLA_TO_GAUSS			100.0f
 #define RM3100_SENSITIVITY		75.0f

@@ -75,28 +74,26 @@
 #define CCY_DEFAULT_LSB		CCX_DEFAULT_LSB
 #define CCZ_DEFAULT_MSB		CCX_DEFAULT_MSB
 #define CCZ_DEFAULT_LSB		CCX_DEFAULT_LSB
-#define CMM_DEFAULT		0x70	// No continuous mode
+#define CMM_DEFAULT		0b0111'0001 // continuous mode
 #define CONTINUOUS_MODE		(1 << 0)
 #define POLLING_MODE		(0 << 0)
-#define TMRC_DEFAULT		0x94
-#define BIST_SELFTEST		0x8F
+#define TMRC_DEFAULT		0x95 // ~13 ms, ~75 Hz
+#define BIST_SELFTEST		0b1000'1111
 #define BIST_DEFAULT		0x00
 #define BIST_XYZ_OK		((1 << 4) | (1 << 5) | (1 << 6))
+#define BIST_STE		(1 << 7)
+#define BIST_DUR_USEC		(2*RM3100_INTERVAL)
+#define HSHAKE_DEFAULT		(0x0B)
+#define HSHAKE_NO_DRDY_CLEAR	(0x08)
 #define STATUS_DRDY		(1 << 7)
 #define POLL_XYZ		0x70
-#define RM3100_REVID		0x22

-#define NUM_BUS_OPTIONS		(sizeof(bus_options)/sizeof(bus_options[0]))
+#define RM3100_REVID		0x22

 /* interface factories */
 extern device::Device *RM3100_SPI_interface(int bus, uint32_t devid, int bus_frequency, spi_mode_e spi_mode);
 extern device::Device *RM3100_I2C_interface(int bus, int bus_frequency);

-enum OPERATING_MODE {
-	CONTINUOUS = 0,
-	SINGLE
-};
-
 #define RM3100_ADDRESS		0x20

 class RM3100 : public I2CSPIDriver<RM3100>
@@ -108,8 +105,6 @@ public:
 	static I2CSPIDriverBase *instantiate(const I2CSPIDriverConfig &config, int runtime_instance);
 	static void print_usage();

-	void custom_method(const BusCLIArguments &cli) override;
-
 	int init();

 	void print_status() override;
@@ -122,29 +117,6 @@ public:
 	void RunImpl();

 private:
-	PX4Magnetometer _px4_mag;
-
-	device::Device *_interface;
-
-	perf_counter_t _comms_errors;
-	perf_counter_t _conf_errors;
-	perf_counter_t _range_errors;
-	perf_counter_t _sample_perf;
-
-	/* status reporting */
-	bool _continuous_mode_set;
-
-	enum OPERATING_MODE _mode;
-
-	unsigned int _measure_interval;
-
-	uint8_t _check_state_cnt;
-
-	/**
-	 * Collect the result of the most recent measurement.
-	 */
-	int collect();
-
 	/**
 	 * Run sensor self-test
 	 *
@@ -152,36 +124,21 @@ private:
 	 */
 	int self_test();

-	/**
-	 * Check whether new data is available or not
-	 *
-	 * @return 0 if new data is available, 1 else
-	 */
-	int check_measurement();
-
 	/**
 	* Converts int24_t stored in 32-bit container to int32_t
 	*/
 	void convert_signed(int32_t *n);

-	/**
-	 * Issue a measurement command.
-	 *
-	 * @return              OK if the measurement command was successful.
-	 */
-	int measure();
+	PX4Magnetometer _px4_mag;

-	/**
-	 * @brief Resets the device
-	 */
-	int reset();
+	device::Device *_interface{nullptr};

-	/**
-	 * @brief Initialises the automatic measurement state machine and start it.
-	 *
-	 * @note This function is called at open and error time.  It might make sense
-	 *       to make it more aggressive about resetting the bus in case of errors.
-	 */
-	void start();
+	perf_counter_t _reset_perf{perf_alloc(PC_COUNT, MODULE_NAME": reset")};
+	perf_counter_t _range_error_perf{perf_alloc(PC_COUNT, MODULE_NAME": range error")};
+	perf_counter_t _bad_transfer_perf{perf_alloc(PC_COUNT, MODULE_NAME": bad transfer")};

-}; // class RM3100
+	int32_t _raw_data_prev[3] {};
+
+	int _failure_count{0};
+
+};
@@ -78,12 +78,6 @@ I2CSPIDriverBase *RM3100::instantiate(const I2CSPIDriverConfig &config, int runt
 	return dev;
 }

-void
-RM3100::custom_method(const BusCLIArguments &cli)
-{
-	reset();
-}
-
 void RM3100::print_usage()
 {
 	PRINT_MODULE_USAGE_NAME("rm3100", "driver");
@@ -91,7 +85,6 @@ void RM3100::print_usage()
 	PRINT_MODULE_USAGE_COMMAND("start");
 	PRINT_MODULE_USAGE_PARAMS_I2C_SPI_DRIVER(true, true);
 	PRINT_MODULE_USAGE_PARAM_INT('R', 0, 0, 35, "Rotation", true);
-	PRINT_MODULE_USAGE_COMMAND("reset");
 	PRINT_MODULE_USAGE_DEFAULT_COMMANDS();
 }

@@ -124,20 +117,14 @@ extern "C" int rm3100_main(int argc, char *argv[])

 	if (!strcmp(verb, "start")) {
 		return ThisDriver::module_start(cli, iterator);
-	}

-	if (!strcmp(verb, "stop")) {
+	} else if (!strcmp(verb, "stop")) {
 		return ThisDriver::module_stop(iterator);
-	}

-	if (!strcmp(verb, "status")) {
+	} else if (!strcmp(verb, "status")) {
 		return ThisDriver::module_status(iterator);
 	}

-	if (!strcmp(verb, "reset")) {
-		return ThisDriver::module_custom_method(cli, iterator);
-	}
-
 	ThisDriver::print_usage();
 	return -1;
 }
@@ -124,7 +124,12 @@ int INA228::read(uint8_t address, int32_t &data)
 	const int ret = transfer(&address, 1, (uint8_t *)&received_bytes, sizeof(received_bytes) - 1);

 	if (ret == PX4_OK) {
-		data = swap32(received_bytes) >> ((32 - 24) + 4);
+		data = swap32(received_bytes) >> ((32 - 24) + 4); // Convert to 20bit value
+
+		// Handle negative 20bit twos complement
+		if (data & 0x80000) {
+			data = -((0x000FFFFF & ~data) + 1);
+		}

 	} else {
 		perf_count(_comms_errors);
@@ -114,7 +114,9 @@ int INA238::init()
 		return ret;
 	}

-	write(INA238_REG_CONFIG, (uint16_t)(INA238_RST_RESET | _range));
+	if (write(INA238_REG_CONFIG, (uint16_t)(INA238_RST_RESET | _range)) != PX4_OK) {
+		return ret;
+	}

 	uint16_t shunt_calibration = static_cast<uint16_t>(INA238_CONST * _current_lsb * _rshunt);

@@ -127,7 +129,9 @@ int INA238::init()
 	}

 	// Set the CONFIG for max I
-	write(INA238_REG_CONFIG, (uint16_t) _range);
+	if (write(INA238_REG_CONFIG, (uint16_t) _range) != PX4_OK) {
+		return ret;
+	}

 	// Start ADC continous mode here
 	ret = write(INA238_REG_ADCCONFIG, (uint16_t)INA238_ADCCONFIG);
--- a/Show More
+++ b/Show More