fixup! Update submodule nuttx

uavcan_v1: Add Publishers dir to includes
uavcan_v1: SocketCAN set 'can_fd' based on CONFIG
2026-05-26 00:57: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
370 changed files with 13376 additions and 8497 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
@@ -46,7 +46,7 @@ param set-default COM_RC_LOSS_T 3


 # ekf2
-param set-default EKF2_AID_MASK 35
+param set-default EKF2_AID_MASK 33
 param set-default EKF2_BARO_DELAY 0
 param set-default EKF2_BARO_NOISE 2.0

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


@@ -149,6 +149,19 @@ then
 	ms5525_airspeed start -X
 fi

+# IR-LOCK sensor external I2C
+if param compare -s SENS_EN_IRLOCK 1
+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

@@ -268,10 +268,14 @@ for serial_command in serial_commands:
                default_port_str = port_config['default'][i]
            else:
                default_port_str = port_config['default']
+
            if default_port_str != "":
                if default_port_str not in serial_ports:
                    raise Exception("Default Port {:} not found for {:}".format(default_port_str, serial_command['label']))
-                default_port = serial_ports[default_port_str]['index']
+
+                if default_port_str in dict(board_ports).keys():
+                    default_port = serial_ports[default_port_str]['index']
+

        commands.append({
            'command': serial_command['command'],
@@ -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
@@ -48,6 +48,7 @@ CONFIG_DRIVERS_TONE_ALARM=y
 CONFIG_DRIVERS_UAVCAN=y
 CONFIG_MODULES_AIRSPEED_SELECTOR=y
 CONFIG_MODULES_ATTITUDE_ESTIMATOR_Q=y
+CONFIG_MODULES_BATTERY_STATUS=y
 CONFIG_MODULES_CAMERA_FEEDBACK=y
 CONFIG_MODULES_COMMANDER=y
 CONFIG_MODULES_CONTROL_ALLOCATOR=y
@@ -2,7 +2,13 @@
 #
 # PX4 FMUv5X specific board sensors init
 #------------------------------------------------------------------------------
-board_adc start
+
+if param compare -s ADC_ADS1115_EN 1
+then
+	ads1115 start -X
+else
+	board_adc start
+fi


 if param compare SENS_EN_INA226 1
@@ -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)
@@ -45,6 +45,7 @@ uint8 CS_EV_VEL = 24		# 24 - true when local frame velocity data fusion from ext
 uint8 CS_SYNTHETIC_MAG_Z = 25	# 25 - true when we are using a synthesized measurement for the magnetometer Z component
 uint8 CS_VEHICLE_AT_REST = 26	# 26 - true when the vehicle is at rest
 uint8 CS_GPS_YAW_FAULT = 27	# 27 - true when the GNSS heading has been declared faulty and is no longer being used
+uint8 CS_RNG_FAULT = 28		# 28 - true when the range finder has been declared faulty and is no longer being used

 uint32 filter_fault_flags	# Bitmask to indicate EKF internal faults
 # 0 - true if the fusion of the magnetometer X-axis has encountered a numerical error
@@ -32,6 +32,9 @@ bool cs_ev_vel                # 24 - true when local frame velocity data fusion
 bool cs_synthetic_mag_z       # 25 - true when we are using a synthesized measurement for the magnetometer Z component
 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
@@ -145,7 +145,10 @@
 #  define BOARD_BATT_I_LIST       {ADC_BATTERY_CURRENT_CHANNEL}
 #  define BOARD_BRICK_VALID_LIST  {BOARD_ADC_BRICK_VALID}
 #elif BOARD_NUMBER_BRICKS == 2
-#  if  !defined(BOARD_NUMBER_DIGITAL_BRICKS)
+#  if  defined(BOARD_NUMBER_DIGITAL_BRICKS)
+#    define BOARD_BATT_V_LIST       {-1, -1}
+#    define BOARD_BATT_I_LIST       {-1, -1}
+#  else
 #    define BOARD_BATT_V_LIST       {ADC_BATTERY1_VOLTAGE_CHANNEL, ADC_BATTERY2_VOLTAGE_CHANNEL}
 #    define BOARD_BATT_I_LIST       {ADC_BATTERY1_CURRENT_CHANNEL, ADC_BATTERY2_CURRENT_CHANNEL}
 #  endif
@@ -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) {
@@ -0,0 +1,44 @@
+/****************************************************************************
+ *
+ *   Copyright (c) 2022 PX4 Development Team. All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright
+ *    notice, this list of conditions and the following disclaimer.
+ * 2. Redistributions in binary form must reproduce the above copyright
+ *    notice, this list of conditions and the following disclaimer in
+ *    the documentation and/or other materials provided with the
+ *    distribution.
+ * 3. Neither the name PX4 nor the names of its contributors may be
+ *    used to endorse or promote products derived from this software
+ *    without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
+ * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
+ * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *
+ ****************************************************************************/
+
+/**
+ * Enable external ADS1115 ADC
+ *
+ * If enabled, the internal ADC is not used.
+ *
+ * @boolean
+ * @reboot_required true
+ * @group Sensors
+ */
+PARAM_DEFINE_INT32(ADC_ADS1115_EN, 0);
+
@@ -0,0 +1,46 @@
+############################################################################
+#
+#   Copyright (c) 2022 PX4 Development Team. All rights reserved.
+#
+# Redistribution and use in source and binary forms, with or without
+# modification, are permitted provided that the following conditions
+# are met:
+#
+# 1. Redistributions of source code must retain the above copyright
+#    notice, this list of conditions and the following disclaimer.
+# 2. Redistributions in binary form must reproduce the above copyright
+#    notice, this list of conditions and the following disclaimer in
+#    the documentation and/or other materials provided with the
+#    distribution.
+# 3. Neither the name PX4 nor the names of its contributors may be
+#    used to endorse or promote products derived from this software
+#    without specific prior written permission.
+#
+# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+# "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+# LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+# FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+# COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+# INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+# BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
+# OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
+# AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+# LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+# ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+# POSSIBILITY OF SUCH DAMAGE.
+#
+############################################################################
+
+px4_add_module(
+	MODULE drivers__barometer__ms5837
+	MAIN ms5837
+	COMPILE_FLAGS
+	SRCS
+		ms5837_main.cpp
+		ms5837_registers.h
+		MS5837.cpp
+		MS5837.hpp
+	DEPENDS
+		drivers_barometer
+		px4_work_queue
+	)
@@ -0,0 +1,5 @@
+menuconfig DRIVERS_BAROMETER_MS5837
+	bool "ms5837"
+	default n
+	---help---
+		Enable support for ms5837
@@ -0,0 +1,441 @@
+/****************************************************************************
+ *
+ *   Copyright (c) 2022 PX4 Development Team. All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright
+ *    notice, this list of conditions and the following disclaimer.
+ * 2. Redistributions in binary form must reproduce the above copyright
+ *    notice, this list of conditions and the following disclaimer in
+ *    the documentation and/or other materials provided with the
+ *    distribution.
+ * 3. Neither the name PX4 nor the names of its contributors may be
+ *    used to endorse or promote products derived from this software
+ *    without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
+ * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
+ * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *
+ ****************************************************************************/
+
+/**
+ * @file ms5837.cpp
+ * Driver for the MS5837 barometric pressure sensor connected via I2C.
+ */
+
+#include "MS5837.hpp"
+
+MS5837::MS5837(const I2CSPIDriverConfig &config) :
+	I2C(config),
+	I2CSPIDriver(config),
+	_px4_barometer(get_device_id()),
+	_sample_perf(perf_alloc(PC_ELAPSED, MODULE_NAME": read")),
+	_measure_perf(perf_alloc(PC_ELAPSED, MODULE_NAME": measure")),
+	_comms_errors(perf_alloc(PC_COUNT, MODULE_NAME": com_err"))
+{
+}
+
+MS5837::~MS5837()
+{
+	// free perf counters
+	perf_free(_sample_perf);
+	perf_free(_measure_perf);
+	perf_free(_comms_errors);
+}
+
+int MS5837::init()
+{
+
+	int ret = I2C::init();
+
+	if (ret != PX4_OK) {
+		DEVICE_DEBUG("I2C::init failed (%i)", ret);
+		return ret;
+	}
+
+	/* do a first measurement cycle to populate reports with valid data */
+	_measure_phase = 0;
+
+	while (true) {
+		/* do temperature first */
+		if (OK != _measure()) {
+			ret = -EIO;
+			break;
+		}
+
+		px4_usleep(MS5837_CONVERSION_INTERVAL);
+
+		if (OK != _collect()) {
+			ret = -EIO;
+			break;
+		}
+
+		/* now do a pressure measurement */
+		if (OK != _measure()) {
+			ret = -EIO;
+			break;
+		}
+
+		px4_usleep(MS5837_CONVERSION_INTERVAL);
+
+		if (OK != _collect()) {
+			ret = -EIO;
+			break;
+		}
+
+		_px4_barometer.set_device_type(DRV_BARO_DEVTYPE_MS5837);
+
+		ret = OK;
+
+		break;
+
+	}
+
+	if (ret == 0) {
+		_start();
+	}
+
+	return ret;
+}
+
+int MS5837::_reset()
+{
+	unsigned old_retrycount = _retries;
+	uint8_t	cmd = MS5837_RESET;
+
+	/* bump the retry count */
+	_retries = 3;
+	int result = transfer(&cmd, 1, nullptr, 0);
+	_retries = old_retrycount;
+
+	return result;
+}
+
+int MS5837::probe()
+{
+	if ((PX4_OK == _probe_address(MS5837_ADDRESS))) {
+
+		return PX4_OK;
+	}
+
+	_retries = 1;
+
+	return -EIO;
+}
+
+int MS5837::_probe_address(uint8_t address)
+{
+	/* select the address we are going to try */
+	set_device_address(address);
+
+	/* send reset command */
+	if (PX4_OK != _reset()) {
+		return -EIO;
+	}
+
+	/* read PROM */
+	if (PX4_OK != _read_prom()) {
+		return -EIO;
+	}
+
+	return PX4_OK;
+}
+
+int MS5837::read(unsigned offset, void *data, unsigned count)
+{
+	union _cvt {
+		uint8_t	b[4];
+		uint32_t w;
+	} *cvt = (_cvt *)data;
+	uint8_t buf[3];
+
+	/* read the most recent measurement */
+	uint8_t cmd = 0;
+	int ret = transfer(&cmd, 1, &buf[0], 3);
+
+	if (ret == PX4_OK) {
+		/* fetch the raw value */
+		cvt->b[0] = buf[2];
+		cvt->b[1] = buf[1];
+		cvt->b[2] = buf[0];
+		cvt->b[3] = 0;
+	}
+
+	return ret;
+}
+
+void MS5837::RunImpl()
+{
+	int ret;
+
+	/* collection phase? */
+	if (_collect_phase) {
+
+		/* perform collection */
+		ret = _collect();
+
+		if (ret != OK) {
+			if (ret == -6) {
+				/*
+				 * The ms5837 seems to regularly fail to respond to
+				 * its address; this happens often enough that we'd rather not
+				 * spam the console with a message for this.
+				 */
+			} else {
+				//DEVICE_LOG("collection error %d", ret);
+			}
+
+			/* issue a reset command to the sensor */
+			_reset();
+			/* reset the collection state machine and try again - we need
+			 * to wait 2.8 ms after issuing the sensor reset command
+			 * according to the MS5837 datasheet
+			 */
+			ScheduleDelayed(2800);
+			return;
+		}
+
+		/* next phase is measurement */
+		_collect_phase = false;
+	}
+
+	/* measurement phase */
+	ret = _measure();
+
+	if (ret != OK) {
+		/* issue a reset command to the sensor */
+		_reset();
+		/* reset the collection state machine and try again */
+		_start();
+		return;
+	}
+
+	/* next phase is collection */
+	_collect_phase = true;
+
+	/* schedule a fresh cycle call when the measurement is done */
+	ScheduleDelayed(MS5837_CONVERSION_INTERVAL);
+}
+
+void MS5837::_start()
+{
+	/* reset the report ring and state machine */
+	_collect_phase = false;
+	_measure_phase = 0;
+
+	/* schedule a cycle to start things */
+	ScheduleDelayed(MS5837_CONVERSION_INTERVAL);
+}
+
+int MS5837::_measure()
+{
+	perf_begin(_measure_perf);
+
+	/*
+	 * In phase zero, request temperature; in other phases, request pressure.
+	 */
+	unsigned addr = (_measure_phase == 0) ? ADDR_CMD_CONVERT_D2 : ADDR_CMD_CONVERT_D1;
+
+	/*
+	 * Send the command to begin measuring.
+	 */
+	uint8_t cmd = addr;
+	int ret = transfer(&cmd, 1, nullptr, 0);
+
+	if (OK != ret) {
+		perf_count(_comms_errors);
+	}
+
+	_px4_barometer.set_error_count(perf_event_count(_comms_errors));
+
+	perf_end(_measure_perf);
+
+	return ret;
+}
+
+int MS5837::_collect()
+{
+	uint32_t raw;
+
+	perf_begin(_sample_perf);
+
+	/* read the most recent measurement - read offset/size are hardcoded in the interface */
+	const hrt_abstime timestamp_sample = hrt_absolute_time();
+	int ret = read(0, (void *)&raw, 0);
+
+	if (ret < 0) {
+		perf_count(_comms_errors);
+		perf_end(_sample_perf);
+		return ret;
+	}
+
+	/* handle a measurement */
+	if (_measure_phase == 0) {
+
+		/* temperature offset (in ADC units) */
+		int32_t dT = (int32_t)raw - ((int32_t)_prom.s.c5_reference_temp << 8);
+
+		/* absolute temperature in centidegrees - note intermediate value is outside 32-bit range */
+		int32_t TEMP = 2000 + (int32_t)(((int64_t)dT * _prom.s.c6_temp_coeff_temp) >> 23);
+
+		/* base sensor scale/offset values */
+
+		/* Perform MS5837 Caculation */
+		_OFF  = ((int64_t)_prom.s.c2_pressure_offset << 16) + (((int64_t)_prom.s.c4_temp_coeff_pres_offset * dT) >> 7);
+		_SENS = ((int64_t)_prom.s.c1_pressure_sens << 15) + (((int64_t)_prom.s.c3_temp_coeff_pres_sens * dT) >> 8);
+
+		/* MS5837 temperature compensation */
+		int64_t T2 = 0;
+
+		int64_t f = 0;
+		int64_t OFF2 = 0;
+		int64_t SENS2 = 0;
+
+		if (TEMP < 2000) {
+
+			T2 = 3 * ((int64_t)POW2(dT) >> 33);
+
+			f = POW2((int64_t)TEMP - 2000);
+			OFF2 = 3 * f >> 1;
+			SENS2 = 5 * f >> 3;
+
+			if (TEMP < -1500) {
+
+				int64_t f2 = POW2(TEMP + 1500);
+				OFF2 += 7 * f2;
+				SENS2 += f2 << 2;
+			}
+
+		} else if (TEMP >= 2000) {
+			T2 = 2 * ((int64_t)POW2(dT) >> 37);
+
+			f = POW2((int64_t)TEMP - 2000);
+			OFF2 = 1 * f >> 4;
+			SENS2 = 0;
+		}
+
+		TEMP -= (int32_t)T2;
+		_OFF  -= OFF2;
+		_SENS -= SENS2;
+
+
+		float temperature = TEMP / 100.0f;
+		_px4_barometer.set_temperature(temperature);
+
+	} else {
+		/* pressure calculation, result in Pa */
+		int32_t P = (((raw * _SENS) >> 21) - _OFF) >> 13;
+
+		float pressure = P / 10.0f;		/* convert to millibar */
+
+		_px4_barometer.update(timestamp_sample, pressure);
+	}
+
+	/* update the measurement state machine */
+	INCREMENT(_measure_phase, MS5837_MEASUREMENT_RATIO + 1);
+
+	perf_end(_sample_perf);
+
+	return OK;
+}
+
+void MS5837::print_status()
+{
+	I2CSPIDriverBase::print_status();
+	perf_print_counter(_sample_perf);
+	perf_print_counter(_comms_errors);
+
+	printf("pressure:         %f\n", (double)_px4_barometer.get().pressure);
+	printf("temperature:      %f\n", (double)_px4_barometer.get().temperature);
+}
+
+int MS5837::_read_prom()
+{
+	uint8_t		prom_buf[2];
+	union {
+		uint8_t		b[2];
+		uint16_t	w;
+	} cvt;
+
+	/*
+	 * Wait for PROM contents to be in the device (2.8 ms) in the case we are
+	 * called immediately after reset.
+	 */
+	px4_usleep(3000);
+
+	uint8_t last_val = 0;
+	bool bits_stuck = true;
+
+	/* read and convert PROM words */
+	for (int i = 0; i < 7; i++) {
+		uint8_t cmd = MS5837_PROM_READ + (i * 2);
+
+		if (PX4_OK != transfer(&cmd, 1, &prom_buf[0], 2)) {
+			break;
+		}
+
+		/* check if all bytes are zero */
+		if (i == 0) {
+			/* initialize to first byte read */
+			last_val = prom_buf[0];
+		}
+
+		if ((prom_buf[0] != last_val) || (prom_buf[1] != last_val)) {
+			bits_stuck = false;
+		}
+
+		/* assemble 16 bit value and convert from big endian (sensor) to little endian (MCU) */
+		cvt.b[0] = prom_buf[1];
+		cvt.b[1] = prom_buf[0];
+		_prom.c[i] = cvt.w;
+	}
+
+	/* calculate CRC and return success/failure accordingly */
+	return (_crc4(&_prom.c[0]) && !bits_stuck) ? PX4_OK : -EIO;
+}
+
+/**
+ * MS5837 crc4 cribbed from the datasheet
+ */
+bool MS5837::_crc4(uint16_t *n_prom)
+{
+	uint16_t n_rem = 0;
+	uint16_t crcRead = n_prom[0] >> 12;
+	n_prom[0] = ((n_prom[0]) & 0x0FFF);
+	n_prom[7] = 0;
+
+	for (uint8_t i = 0 ; i < 16; i++) {
+		if (i % 2 == 1) {
+			n_rem ^= (uint16_t)((n_prom[i >> 1]) & 0x00FF);
+
+		} else {
+			n_rem ^= (uint16_t)(n_prom[i >> 1] >> 8);
+		}
+
+		for (uint8_t n_bit = 8 ; n_bit > 0 ; n_bit--) {
+			if (n_rem & 0x8000) {
+				n_rem = (n_rem << 1) ^ 0x3000;
+
+			} else {
+				n_rem = (n_rem << 1);
+			}
+		}
+	}
+
+	n_rem = ((n_rem >> 12) & 0x000F);
+
+	return (n_rem ^ 0x00) == crcRead;
+}
@@ -0,0 +1,134 @@
+/****************************************************************************
+ *
+ *   Copyright (c) 2022 PX4 Development Team. All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright
+ *    notice, this list of conditions and the following disclaimer.
+ * 2. Redistributions in binary form must reproduce the above copyright
+ *    notice, this list of conditions and the following disclaimer in
+ *    the documentation and/or other materials provided with the
+ *    distribution.
+ * 3. Neither the name PX4 nor the names of its contributors may be
+ *    used to endorse or promote products derived from this software
+ *    without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
+ * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
+ * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *
+ ****************************************************************************/
+
+#pragma once
+
+#include <drivers/device/device.h>
+#include <drivers/device/i2c.h>
+#include <lib/drivers/barometer/PX4Barometer.hpp>
+#include <lib/perf/perf_counter.h>
+#include <px4_platform_common/i2c_spi_buses.h>
+#include <px4_platform_common/px4_work_queue/ScheduledWorkItem.hpp>
+#include <systemlib/err.h>
+
+#include "ms5837_registers.h"
+
+/* helper macro for handling report buffer indices */
+#define INCREMENT(_x, _lim)	do { __typeof__(_x) _tmp = _x+1; if (_tmp >= _lim) _tmp = 0; _x = _tmp; } while(0)
+
+/* helper macro for arithmetic - returns the square of the argument */
+#define POW2(_x)		((_x) * (_x))
+
+class MS5837 : public device::I2C, public I2CSPIDriver<MS5837>
+{
+public:
+	MS5837(const I2CSPIDriverConfig &config);
+	~MS5837() override;
+
+	static void 		print_usage();
+
+	int			init();
+
+	/**
+	 * Perform a poll cycle; collect from the previous measurement
+	 * and start a new one.
+	 *
+	 * This is the heart of the measurement state machine.  This function
+	 * alternately starts a measurement, or collects the data from the
+	 * previous measurement.
+	 *
+	 * When the interval between measurements is greater than the minimum
+	 * measurement interval, a gap is inserted between collection
+	 * and measurement to provide the most recent measurement possible
+	 * at the next interval.
+	 */
+	void			RunImpl();
+	void 			print_status() override;
+	int			read(unsigned offset, void *data, unsigned count) override;
+
+private:
+	int			probe() override;
+
+	PX4Barometer		_px4_barometer;
+
+	ms5837::prom_u	   	_prom{};
+
+	bool			_collect_phase{false};
+	unsigned		_measure_phase{false};
+
+	/* intermediate temperature values per MS5611/MS5607 datasheet */
+	int64_t			_OFF{0};
+	int64_t			_SENS{0};
+
+	perf_counter_t		_sample_perf;
+	perf_counter_t		_measure_perf;
+	perf_counter_t		_comms_errors;
+
+	/**
+	 * Initialize 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();
+
+	/**
+	 * Issue a measurement command for the current state.
+	 *
+	 * @return		OK if the measurement command was successful.
+	 */
+	int			_measure();
+
+	/**
+	 * Collect the result of the most recent measurement.
+	 */
+	int			_collect();
+
+	int			_probe_address(uint8_t address);
+
+	/**
+	 * Send a reset command to the MS5837.
+	 *
+	 * This is required after any bus reset.
+	 */
+	int			_reset();
+
+	/**
+	 * Read the MS5837 PROM
+	 *
+	 * @return		PX4_OK if the PROM reads successfully.
+	 */
+	int			_read_prom();
+
+	bool		_crc4(uint16_t *n_prom);
+};
@@ -0,0 +1,82 @@
+/****************************************************************************
+ *
+ *   Copyright (c) 2022 PX4 Development Team. All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright
+ *    notice, this list of conditions and the following disclaimer.
+ * 2. Redistributions in binary form must reproduce the above copyright
+ *    notice, this list of conditions and the following disclaimer in
+ *    the documentation and/or other materials provided with the
+ *    distribution.
+ * 3. Neither the name PX4 nor the names of its contributors may be
+ *    used to endorse or promote products derived from this software
+ *    without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
+ * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
+ * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *
+ ****************************************************************************/
+
+#include <px4_platform_common/px4_config.h>
+#include <px4_platform_common/getopt.h>
+#include <px4_platform_common/i2c_spi_buses.h>
+#include <px4_platform_common/module.h>
+
+#include "MS5837.hpp"
+
+void MS5837::print_usage()
+{
+	PRINT_MODULE_USAGE_NAME("ms5837", "driver");
+	PRINT_MODULE_USAGE_SUBCATEGORY("baro");
+	PRINT_MODULE_USAGE_COMMAND("start");
+	PRINT_MODULE_USAGE_PARAMS_I2C_SPI_DRIVER(true, false);
+	PRINT_MODULE_USAGE_DEFAULT_COMMANDS();
+}
+
+extern "C" int ms5837_main(int argc, char *argv[])
+{
+	using ThisDriver = MS5837;
+	BusCLIArguments cli{true, false};
+	cli.default_i2c_frequency = 400000;
+	uint16_t dev_type_driver = DRV_BARO_DEVTYPE_MS5837;
+
+	const char *verb = cli.parseDefaultArguments(argc, argv);
+
+	if (!verb) {
+		ThisDriver::print_usage();
+		return -1;
+	}
+
+	cli.i2c_address = MS5837_ADDRESS;
+
+	BusInstanceIterator iterator(MODULE_NAME, cli, dev_type_driver);
+
+	if (!strcmp(verb, "start")) {
+		return ThisDriver::module_start(cli, iterator);
+	}
+
+	if (!strcmp(verb, "stop")) {
+		return ThisDriver::module_stop(iterator);
+	}
+
+	if (!strcmp(verb, "status")) {
+		return ThisDriver::module_status(iterator);
+	}
+
+	ThisDriver::print_usage();
+	return -1;
+}
@@ -0,0 +1,115 @@
+/****************************************************************************
+ *
+ *   Copyright (C) 2022 PX4 Development Team. All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright
+ *    notice, this list of conditions and the following disclaimer.
+ * 2. Redistributions in binary form must reproduce the above copyright
+ *    notice, this list of conditions and the following disclaimer in
+ *    the documentation and/or other materials provided with the
+ *    distribution.
+ * 3. Neither the name PX4 nor the names of its contributors may be
+ *    used to endorse or promote products derived from this software
+ *    without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
+ * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
+ * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *
+ ****************************************************************************/
+
+/**
+ * @file ms5837_registers.h
+ *
+ * Shared defines for the ms5837 driver.
+ */
+
+#pragma once
+
+#include <string.h>
+#include "board_config.h"
+
+/* interface ioctls */
+#define IOCTL_RESET		2
+#define IOCTL_MEASURE		3
+
+#define MS5837_ADDRESS		0x76
+
+#define MS5837_RESET  		0x1E
+#define MS5837_ADC_READ  	0x00
+#define MS5837_PROM_READ  	0xA0
+
+#define MS5837_30BA26  		0x1A // Sensor version: From MS5837_30BA datasheet Version PROM Word 0
+
+/*
+ * MS5837 internal constants and data structures.
+ */
+#define ADDR_CMD_CONVERT_D1_OSR256		0x40	/* write to this address to start pressure conversion */
+#define ADDR_CMD_CONVERT_D1_OSR512		0x42	/* write to this address to start pressure conversion */
+#define ADDR_CMD_CONVERT_D1_OSR1024		0x44	/* write to this address to start pressure conversion */
+#define ADDR_CMD_CONVERT_D1_OSR2048		0x46	/* write to this address to start pressure conversion */
+#define ADDR_CMD_CONVERT_D1_OSR4096		0x48	/* write to this address to start pressure conversion */
+#define ADDR_CMD_CONVERT_D2_OSR256		0x50	/* write to this address to start temperature conversion */
+#define ADDR_CMD_CONVERT_D2_OSR512		0x52	/* write to this address to start temperature conversion */
+#define ADDR_CMD_CONVERT_D2_OSR1024		0x54	/* write to this address to start temperature conversion */
+#define ADDR_CMD_CONVERT_D2_OSR2048		0x56	/* write to this address to start temperature conversion */
+#define ADDR_CMD_CONVERT_D2_OSR4096		0x58	/* write to this address to start temperature conversion */
+
+/*
+  use an OSR of 1024 to reduce the self-heating effect of the
+  sensor. Information from MS tells us that some individual sensors
+  are quite sensitive to this effect and that reducing the OSR can
+  make a big difference
+ */
+#define ADDR_CMD_CONVERT_D1			ADDR_CMD_CONVERT_D1_OSR1024
+#define ADDR_CMD_CONVERT_D2			ADDR_CMD_CONVERT_D2_OSR1024
+
+/*
+ * Maximum internal conversion time for OSR 1024 is 2.28 ms. We set an update
+ * rate of 100Hz which is be very safe not to read the ADC before the
+ * conversion finished
+ */
+#define MS5837_CONVERSION_INTERVAL	10000	/* microseconds */
+#define MS5837_MEASUREMENT_RATIO	3	/* pressure measurements per temperature measurement */
+
+namespace ms5837
+{
+
+/**
+ * Calibration PROM as reported by the device.
+ */
+#pragma pack(push,1)
+struct prom_s {
+	uint16_t serial_and_crc;
+	uint16_t c1_pressure_sens;
+	uint16_t c2_pressure_offset;
+	uint16_t c3_temp_coeff_pres_sens;
+	uint16_t c4_temp_coeff_pres_offset;
+	uint16_t c5_reference_temp;
+	uint16_t c6_temp_coeff_temp;
+	uint16_t factory_setup;
+};
+
+/**
+ * Grody hack for crc4()
+ */
+union prom_u {
+	uint16_t c[8];
+	prom_s s;
+};
+#pragma pack(pop)
+
+} /* namespace */
@@ -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
--- a/Show More
+++ b/Show More