drivers/rc/ghst_rc: create new standalone ghst_rc driver

Add support for Timer15 on H743 boards (#19228 )
* Adds Timer15 to stm32_common, if the timer base is defined * Adds Timer15 logic for DMA and AltFunc config on stm32h7 boards * Adds TIM15 BDTR->MOE support in stm32_common timer init * Adds support for TIM15 pwm channels on Matek H743 Slim
2026-06-03 17:20:06 +08:00 · 2022-02-21 21:49:55 -05:00 · 2022-02-21 16:52:38 -05:00 · 2022-02-21 16:48:20 -05:00 · 2022-02-21 16:47:16 -05:00 · 2022-02-21 16:45:22 -05:00
240 changed files with 4707 additions and 1057 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'
@@ -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

@@ -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
@@ -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/"
@@ -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
@@ -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 =
@@ -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
 */
@@ -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
@@ -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
 */
@@ -21,6 +21,7 @@ CONFIG_DRIVERS_UAVCAN=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 +29,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
@@ -38,6 +38,7 @@ CONFIG_DRIVERS_PWM_INPUT=y
 CONFIG_DRIVERS_PWM_OUT=y
 CONFIG_DRIVERS_PWM_OUT_SIM=y
 CONFIG_DRIVERS_PX4IO=y
+CONFIG_COMMON_RC=y
 CONFIG_DRIVERS_RC_INPUT=y
 CONFIG_DRIVERS_ROBOCLAW=y
 CONFIG_DRIVERS_RPM=y
@@ -48,6 +49,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
@@ -59,6 +61,7 @@ CONFIG_MODULES_FLIGHT_MODE_MANAGER=y
 CONFIG_MODULES_FW_ATT_CONTROL=y
 CONFIG_MODULES_FW_AUTOTUNE_ATTITUDE_CONTROL=y
 CONFIG_MODULES_FW_POS_CONTROL_L1=y
+CONFIG_MODULES_GIMBAL=y
 CONFIG_MODULES_GYRO_CALIBRATION=y
 CONFIG_MODULES_GYRO_FFT=y
 CONFIG_MODULES_LAND_DETECTOR=y
@@ -80,7 +83,6 @@ CONFIG_MODULES_ROVER_POS_CONTROL=y
 CONFIG_MODULES_SENSORS=y
 CONFIG_MODULES_SIH=y
 CONFIG_MODULES_TEMPERATURE_COMPENSATION=y
-CONFIG_MODULES_GIMBAL=y
 CONFIG_MODULES_VTOL_ATT_CONTROL=y
 CONFIG_SYSTEMCMDS_ACTUATOR_TEST=y
 CONFIG_SYSTEMCMDS_BL_UPDATE=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,15 @@ CONFIG_UART4_RXBUFSIZE=600
 CONFIG_UART4_TXBUFSIZE=1500
 CONFIG_UART5_IFLOWCONTROL=y
 CONFIG_UART5_OFLOWCONTROL=y
+CONFIG_UART5_RXDMA=y
+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 +279,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


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

 # 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
@@ -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
@@ -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)
@@ -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);
+		}
+		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;
@@ -380,6 +505,19 @@ 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 static_cast<const uORB::DeviceNode *>(node_handle)->updates_available(last_generation);
+}
+
+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,106 @@ 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 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 +199,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.
@@ -353,9 +458,16 @@ public:

 	static uint8_t orb_get_instance(const void *node_handle);

+#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 static_cast<const DeviceNode *>(node_handle)->updates_available(last_generation); }

 	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
 	/**
@@ -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)
+{
+	orbiocdevdatacopy_t data = {node_handle, dst, generation, 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;
+}
@@ -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) {
@@ -18,6 +18,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
@@ -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;
 	}
@@ -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 */
@@ -108,6 +108,8 @@
 #define DRV_BARO_DEVTYPE_LPS33HW        0x4C
 #define DRV_BARO_DEVTYPE_TCBP001TA      0x4D

+#define DRV_BARO_DEVTYPE_MS5837	0x4E
+
 #define DRV_BARO_DEVTYPE_MPL3115A2	0x51
 #define DRV_ACC_DEVTYPE_FXOS8701C	0x52

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

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

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


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

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

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

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

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

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

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

+/**
+ * Enable sat info (if available)
+ *
+ * Enable publication of satellite info (ORB_ID(satellite_info)) if possible.
+ * Not available on MTK.
+ *
+ * @boolean
+ * @reboot_required true
+ * @group GPS
+ */
+PARAM_DEFINE_INT32(GPS_SAT_INFO, 0);
+
 /**
 * u-blox GPS Mode
 *
@@ -0,0 +1,41 @@
+/****************************************************************************
+ *
+ *   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.
+ *
+ ****************************************************************************/
+
+/**
+ * IR-LOCK Sensor (external I2C)
+ *
+ * @reboot_required true
+ * @group Sensors
+ * @boolean
+  */
+PARAM_DEFINE_INT32(SENS_EN_IRLOCK, 0);
@@ -1,6 +1,6 @@
 /****************************************************************************
 *
- *   Copyright (c) 2018 PX4 Development Team. All rights reserved.
+ *   Copyright (c) 2018-2022 PX4 Development Team. All rights reserved.
 *
 * Redistribution and use in source and binary forms, with or without
 * modification, are permitted provided that the following conditions
@@ -44,15 +44,7 @@
 RM3100::RM3100(device::Device *interface, const I2CSPIDriverConfig &config) :
 	I2CSPIDriver(config),
 	_px4_mag(interface->get_device_id(), config.rotation),
-	_interface(interface),
-	_comms_errors(perf_alloc(PC_COUNT, MODULE_NAME": comms_errors")),
-	_conf_errors(perf_alloc(PC_COUNT, MODULE_NAME": conf_errors")),
-	_range_errors(perf_alloc(PC_COUNT, MODULE_NAME": range_errors")),
-	_sample_perf(perf_alloc(PC_ELAPSED, MODULE_NAME": read")),
-	_continuous_mode_set(false),
-	_mode(SINGLE),
-	_measure_interval(0),
-	_check_state_cnt(0)
+	_interface(interface)
 {
 	_px4_mag.set_scale(1.f / (RM3100_SENSITIVITY * UTESLA_TO_GAUSS));
 }
@@ -60,25 +52,25 @@ RM3100::RM3100(device::Device *interface, const I2CSPIDriverConfig &config) :
 RM3100::~RM3100()
 {
 	// free perf counters
-	perf_free(_sample_perf);
-	perf_free(_comms_errors);
-	perf_free(_range_errors);
-	perf_free(_conf_errors);
+	perf_free(_reset_perf);
+	perf_free(_range_error_perf);
+	perf_free(_bad_transfer_perf);

 	delete _interface;
 }

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

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

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

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

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

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

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

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

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

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

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

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

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

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

-	ret = OK;
+		_failure_count++;

+		perf_count(_range_error_perf);
+		return;
+	}

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

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

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

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

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

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

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

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

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

 	return PX4_OK;
 }
-
-void RM3100::start()
-{
-	set_default_register_values();
-
-	/* schedule a cycle to start things */
-	ScheduleNow();
-}
@@ -1,6 +1,6 @@
 /****************************************************************************
 *
- *   Copyright (c) 2018 PX4 Development Team. All rights reserved.
+ *   Copyright (c) 2018-2022 PX4 Development Team. All rights reserved.
 *
 * Redistribution and use in source and binary forms, with or without
 * modification, are permitted provided that the following conditions
@@ -50,8 +50,7 @@
 * RM3100 internal constants and data structures.
 */

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

-	if (!strcmp(verb, "reset")) {
-		return ThisDriver::module_custom_method(cli, iterator);
-	}
-
 	ThisDriver::print_usage();
 	return -1;
 }
@@ -114,7 +114,9 @@ int INA238::init()
 		return ret;
 	}

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

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

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

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

 	// Start ADC continous mode here
 	ret = write(INA238_REG_ADCCONFIG, (uint16_t)INA238_ADCCONFIG);
@@ -0,0 +1,9 @@
+menu "RC"
+    menuconfig COMMON_RC
+        bool "Common RC"
+        default n
+        select DRIVERS_RC_GHST_RC
+        ---help---
+            Enable default set of magnetometer drivers
+    rsource "*/Kconfig"
+endmenu
@@ -0,0 +1,47 @@
+############################################################################
+#
+#   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__rc__ghst_rc
+	MAIN ghst_rc
+	COMPILE_FLAGS
+	SRCS
+		ghst_telemetry.cpp
+		ghst_telemetry.hpp
+		GhstRc.cpp
+		GhstRc.hpp
+
+	MODULE_CONFIG
+		module.yaml
+	DEPENDS
+		rc
+	)
@@ -0,0 +1,241 @@
+/****************************************************************************
+ *
+ *   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 "GhstRc.hpp"
+
+#include <poll.h>
+#include <termios.h>
+
+using namespace time_literals;
+
+GhstRc::GhstRc(const char *device) :
+	ModuleParams(nullptr),
+	ScheduledWorkItem(MODULE_NAME, px4::serial_port_to_wq(device))
+{
+	if (device) {
+		strncpy(_device, device, sizeof(_device) - 1);
+		_device[sizeof(_device) - 1] = '\0';
+	}
+}
+
+GhstRc::~GhstRc()
+{
+	delete _ghst_telemetry;
+
+	perf_free(_cycle_interval_perf);
+	perf_free(_publish_interval_perf);
+}
+
+int GhstRc::task_spawn(int argc, char *argv[])
+{
+	bool error_flag = false;
+
+	int myoptind = 1;
+	int ch;
+	const char *myoptarg = nullptr;
+	const char *device = nullptr;
+
+	while ((ch = px4_getopt(argc, argv, "d:", &myoptind, &myoptarg)) != EOF) {
+		switch (ch) {
+		case 'd':
+			device = myoptarg;
+			break;
+
+		case '?':
+			error_flag = true;
+			break;
+
+		default:
+			PX4_WARN("unrecognized flag");
+			error_flag = true;
+			break;
+		}
+	}
+
+	if (error_flag) {
+		return -1;
+	}
+
+	if (device == nullptr) {
+		PX4_ERR("valid device required");
+		return PX4_ERROR;
+	}
+
+	GhstRc *instance = new GhstRc(device);
+
+	if (instance == nullptr) {
+		PX4_ERR("alloc failed");
+		return PX4_ERROR;
+	}
+
+	_object.store(instance);
+	_task_id = task_id_is_work_queue;
+
+	instance->ScheduleNow();
+
+	return PX4_OK;
+}
+
+void GhstRc::Run()
+{
+	if (should_exit()) {
+		ScheduleClear();
+		::close(_rc_fd);
+		exit_and_cleanup();
+		return;
+	}
+
+	if (_rc_fd < 0) {
+		_rc_fd = ::open(_device, O_RDWR | O_NONBLOCK);
+	}
+
+	// poll with 3 second timeout
+	pollfd fds[1];
+	fds[0].fd = _rc_fd;
+	fds[0].events = POLLIN;
+	::poll(fds, 1, 3000);
+
+	perf_count(_cycle_interval_perf);
+
+	const hrt_abstime cycle_timestamp = hrt_absolute_time();
+
+	// read all available data from the serial RC input UART
+	int new_bytes = ::read(_rc_fd, &_rcs_buf[0], RC_MAX_BUFFER_SIZE);
+
+	if (new_bytes > 0) {
+		_bytes_rx += new_bytes;
+	}
+
+	if (_rc_scan_begin == 0) {
+		_rc_scan_begin = cycle_timestamp;
+		// Configure serial port for GHST
+		ghst_config(_rc_fd);
+	}
+
+	if (_rc_locked || (cycle_timestamp - _rc_scan_begin < 300_ms)) {
+
+	}
+
+	// parse new data
+	if (new_bytes > 0) {
+		uint16_t raw_rc_values[input_rc_s::RC_INPUT_MAX_CHANNELS] {};
+		uint16_t raw_rc_count = 0;
+		int8_t ghst_rssi = -1;
+		bool rc_updated = ghst_parse(cycle_timestamp, &_rcs_buf[0], new_bytes, &raw_rc_values[0], &ghst_rssi, &raw_rc_count,
+					     input_rc_s::RC_INPUT_MAX_CHANNELS);
+
+		if (rc_updated) {
+			if (!_rc_locked) {
+				_rc_locked = true;
+				PX4_INFO("RC input locked");
+			}
+
+			// we have a new GHST frame. Publish it.
+			input_rc_s input_rc{};
+			input_rc.timestamp_last_signal = cycle_timestamp;
+			input_rc.channel_count = math::max(raw_rc_count, (uint16_t)input_rc_s::RC_INPUT_MAX_CHANNELS);
+			input_rc.rssi = ghst_rssi;
+			input_rc.rc_lost = (raw_rc_count == 0);
+			input_rc.input_source = input_rc_s::RC_INPUT_SOURCE_PX4FMU_GHST;
+
+			for (int i = 0; i < input_rc.channel_count; i++) {
+				input_rc.values[i] = raw_rc_values[i];
+			}
+
+			input_rc.timestamp = hrt_absolute_time();
+			_input_rc_pub.publish(input_rc);
+			perf_count(_publish_interval_perf);
+
+			if (!_ghst_telemetry) {
+				_ghst_telemetry = new GHSTTelemetry(_rc_fd);
+			}
+
+			if (_ghst_telemetry) {
+				_ghst_telemetry->update(cycle_timestamp);
+			}
+		}
+	}
+
+	ScheduleDelayed(4_ms);
+}
+
+int GhstRc::print_status()
+{
+	if (_device[0] != '\0') {
+		PX4_INFO("UART device: %s", _device);
+		PX4_INFO("UART RX bytes: %"  PRIu32, _bytes_rx);
+	}
+
+	PX4_INFO("RC state: %s", _rc_locked ? "found" : "searching for signal");
+
+	if (_rc_locked) {
+		PX4_INFO("Telemetry: %s", _ghst_telemetry ? "yes" : "no");
+	}
+
+	perf_print_counter(_cycle_interval_perf);
+	perf_print_counter(_publish_interval_perf);
+
+	return 0;
+}
+
+int GhstRc::custom_command(int argc, char *argv[])
+{
+	return print_usage("unknown command");
+}
+
+int GhstRc::print_usage(const char *reason)
+{
+	if (reason) {
+		PX4_WARN("%s\n", reason);
+	}
+
+	PRINT_MODULE_DESCRIPTION(
+		R"DESCR_STR(
+### Description
+This module does the GHST RC input parsing.
+
+)DESCR_STR");
+
+	PRINT_MODULE_USAGE_NAME("ghst_rc", "driver");
+	PRINT_MODULE_USAGE_COMMAND("start");
+	PRINT_MODULE_USAGE_PARAM_STRING('d', "/dev/ttyS3", "<file:dev>", "RC device", true);
+
+	PRINT_MODULE_USAGE_DEFAULT_COMMANDS();
+
+	return 0;
+}
+
+extern "C" __EXPORT int ghst_rc_main(int argc, char *argv[])
+{
+	return GhstRc::main(argc, argv);
+}
@@ -0,0 +1,92 @@
+/****************************************************************************
+ *
+ *   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 <float.h>
+
+#include <drivers/drv_hrt.h>
+#include <lib/perf/perf_counter.h>
+#include <lib/rc/ghst.hpp>
+#include <px4_platform_common/px4_config.h>
+#include <px4_platform_common/getopt.h>
+#include <px4_platform_common/log.h>
+#include <px4_platform_common/module.h>
+#include <px4_platform_common/module_params.h>
+#include <px4_platform_common/px4_work_queue/ScheduledWorkItem.hpp>
+#include <uORB/PublicationMulti.hpp>
+#include <uORB/Subscription.hpp>
+#include <uORB/SubscriptionInterval.hpp>
+#include <uORB/topics/input_rc.h>
+
+#include "ghst_telemetry.hpp"
+
+class GhstRc : public ModuleBase<GhstRc>, public ModuleParams, public px4::ScheduledWorkItem
+{
+public:
+	GhstRc(const char *device);
+	~GhstRc() override;
+
+	/** @see ModuleBase */
+	static int task_spawn(int argc, char *argv[]);
+
+	/** @see ModuleBase */
+	static int custom_command(int argc, char *argv[]);
+
+	/** @see ModuleBase */
+	static int print_usage(const char *reason = nullptr);
+
+	/** @see ModuleBase::print_status() */
+	int print_status() override;
+
+private:
+	void Run() override;
+
+	hrt_abstime _rc_scan_begin{0};
+
+	bool _rc_locked{false};
+
+	uORB::PublicationMulti<input_rc_s> _input_rc_pub{ORB_ID(input_rc)};
+
+	int _rc_fd{-1};
+	char _device[20] {}; ///< device / serial port path
+
+	static constexpr size_t RC_MAX_BUFFER_SIZE{64};
+	uint8_t _rcs_buf[RC_MAX_BUFFER_SIZE] {};
+	uint32_t _bytes_rx{0};
+
+	GHSTTelemetry *_ghst_telemetry{nullptr};
+
+	perf_counter_t	_cycle_interval_perf{perf_alloc(PC_INTERVAL, MODULE_NAME": cycle interval")};
+	perf_counter_t	_publish_interval_perf{perf_alloc(PC_INTERVAL, MODULE_NAME": publish interval")};
+};
@@ -0,0 +1,5 @@
+menuconfig DRIVERS_RC_GHST_RC
+	bool "ghst_rc"
+	default n
+	---help---
+		Enable support for ghst rc
@@ -41,6 +41,7 @@
 */

 #include "ghst_telemetry.hpp"
+
 #include <lib/rc/ghst.hpp>

 using time_literals::operator ""_s;
@@ -0,0 +1,11 @@
+module_name: GHST RC Input Driver
+serial_config:
+    - command: "ghst_rc start -d ${SERIAL_DEV}"
+      port_config_param:
+        name: GHST_RC_PRT_CFG
+        group: Serial
+        #default: RC
+        #depends_on_port: RC
+        description_extended: |
+            Ghost RC input driver.
+
@@ -37,7 +37,6 @@ px4_add_module(
 	SRCS
 		RCInput.cpp
 		crsf_telemetry.cpp
-		ghst_telemetry.cpp
 	MODULE_CONFIG
 		module.yaml
 	DEPENDS
@@ -74,7 +74,6 @@ RCInput::~RCInput()
 	dsm_deinit();

 	delete _crsf_telemetry;
-	delete _ghst_telemetry;

 	perf_free(_cycle_perf);
 	perf_free(_publish_interval_perf);
@@ -123,15 +122,15 @@ RCInput::task_spawn(int argc, char *argv[])
 	int myoptind = 1;
 	int ch;
 	const char *myoptarg = nullptr;
-	const char *device = nullptr;
+	const char *device_name = nullptr;
 #if defined(RC_SERIAL_PORT)
-	device = RC_SERIAL_PORT;
+	device_name = RC_SERIAL_PORT;
 #endif // RC_SERIAL_PORT

 	while ((ch = px4_getopt(argc, argv, "d:", &myoptind, &myoptarg)) != EOF) {
 		switch (ch) {
 		case 'd':
-			device = myoptarg;
+			device_name = myoptarg;
 			break;

 		case '?':
@@ -149,24 +148,31 @@ RCInput::task_spawn(int argc, char *argv[])
 		return -1;
 	}

-	if (device == nullptr) {
-		PX4_ERR("valid device required");
-		return PX4_ERROR;
+	if (device_name && (access(device_name, R_OK | W_OK) == 0)) {
+		RCInput *instance = new RCInput(device_name);
+
+		if (instance == nullptr) {
+			PX4_ERR("alloc failed");
+			return PX4_ERROR;
+		}
+
+		_object.store(instance);
+		_task_id = task_id_is_work_queue;
+
+		instance->ScheduleOnInterval(_current_update_interval);
+
+		return PX4_OK;
+
+	} else {
+		if (device_name) {
+			PX4_ERR("invalid device (-d) %s", device_name);
+
+		} else {
+			PX4_INFO("valid device required");
+		}
 	}

-	RCInput *instance = new RCInput(device);
-
-	if (instance == nullptr) {
-		PX4_ERR("alloc failed");
-		return PX4_ERROR;
-	}
-
-	_object.store(instance);
-	_task_id = task_id_is_work_queue;
-
-	instance->ScheduleOnInterval(_current_update_interval);
-
-	return PX4_OK;
+	return PX4_ERROR;
 }

 void
@@ -658,56 +664,6 @@ void RCInput::Run()
 					}
 				}

-			} else {
-				// Scan the next protocol
-				set_rc_scan_state(RC_SCAN_GHST);
-			}
-
-			break;
-
-		case RC_SCAN_GHST:
-			if (_rc_scan_begin == 0) {
-				_rc_scan_begin = cycle_timestamp;
-				// Configure serial port for GHST
-				ghst_config(_rcs_fd);
-
-				// flush serial buffer and any existing buffered data
-				tcflush(_rcs_fd, TCIOFLUSH);
-				memset(_rcs_buf, 0, sizeof(_rcs_buf));
-
-			} else if (_rc_scan_locked
-				   || cycle_timestamp - _rc_scan_begin < rc_scan_max) {
-
-				// parse new data
-				if (newBytes > 0) {
-					int8_t ghst_rssi = -1;
-					rc_updated = ghst_parse(cycle_timestamp, &_rcs_buf[0], newBytes, &_raw_rc_values[0], &ghst_rssi,
-								&_raw_rc_count, input_rc_s::RC_INPUT_MAX_CHANNELS);
-
-					if (rc_updated) {
-						// we have a new GHST frame. Publish it.
-						_rc_in.input_source = input_rc_s::RC_INPUT_SOURCE_PX4FMU_GHST;
-						fill_rc_in(_raw_rc_count, _raw_rc_values, cycle_timestamp, false, false, 0, ghst_rssi);
-
-						// ghst telemetry works on fmu-v5
-						// on other Pixhawk (-related) boards we cannot write to the RC UART
-						// another option is to use a different UART port
-#ifdef BOARD_SUPPORTS_RC_SERIAL_PORT_OUTPUT
-
-						if (!_rc_scan_locked && !_ghst_telemetry) {
-							_ghst_telemetry = new GHSTTelemetry(_rcs_fd);
-						}
-
-#endif /* BOARD_SUPPORTS_RC_SERIAL_PORT_OUTPUT */
-
-						_rc_scan_locked = true;
-
-						if (_ghst_telemetry) {
-							_ghst_telemetry->update(cycle_timestamp);
-						}
-					}
-				}
-
 			} else {
 				// Scan the next protocol
 				set_rc_scan_state(RC_SCAN_SBUS);
@@ -820,10 +776,6 @@ int RCInput::print_status()
 			PX4_INFO("CRSF Telemetry: %s", _crsf_telemetry ? "yes" : "no");
 			break;

-		case RC_SCAN_GHST:
-			PX4_INFO("GHST Telemetry: %s", _ghst_telemetry ? "yes" : "no");
-			break;
-
 		case RC_SCAN_SBUS:
 			PX4_INFO("SBUS frame drops: %u", sbus_dropped_frames());
 			break;
@@ -40,7 +40,6 @@
 #include <drivers/drv_hrt.h>
 #include <lib/perf/perf_counter.h>
 #include <lib/rc/crsf.h>
-#include <lib/rc/ghst.hpp>
 #include <lib/rc/dsm.h>
 #include <lib/rc/sbus.h>
 #include <lib/rc/st24.h>
@@ -61,7 +60,6 @@
 #include <uORB/topics/vehicle_status.h>

 #include "crsf_telemetry.h"
-#include "ghst_telemetry.hpp"

 #ifdef HRT_PPM_CHANNEL
 # include <systemlib/ppm_decode.h>
@@ -97,7 +95,6 @@ private:
 		RC_SCAN_SUMD,
 		RC_SCAN_ST24,
 		RC_SCAN_CRSF,
-		RC_SCAN_GHST
 	} _rc_scan_state{RC_SCAN_SBUS};

 	static constexpr char const *RC_SCAN_STRING[7] {
@@ -107,7 +104,6 @@ private:
 		"SUMD",
 		"ST24",
 		"CRSF",
-		"GHST"
 	};

 	void Run() override;
@@ -159,7 +155,6 @@ private:
 	uint16_t _raw_rc_count{};

 	CRSFTelemetry *_crsf_telemetry{nullptr};
-	GHSTTelemetry *_ghst_telemetry{nullptr};

 	perf_counter_t	_cycle_perf;
 	perf_counter_t	_publish_interval_perf;
@@ -5,7 +5,6 @@ serial_config:
        name: RC_PORT_CONFIG
        group: Serial
        default: RC
-        depends_on_port: RC
        description_extended: |
            Setting this to 'Disabled' will use a board-specific default port
            for RC input.
@@ -51,41 +51,83 @@ int PCF8583::init()
 		  _param_pcf8583_reset.get(),
 		  _param_pcf8583_magnet.get());

-	// set counter mode
-	setRegister(0x00, 0b00100000);
-
-	// start measurement
-	resetCounter();
-
-	_rpm_pub.advertise();
-
+	initCounter();
 	ScheduleOnInterval(_param_pcf8583_pool.get());
+	_rpm_pub.advertise();

 	return PX4_OK;
 }

 int PCF8583::probe()
 {
+	setRegister(0x00, 0b00100000);
+
+	uint8_t s = readRegister(0x00);
+	PX4_DEBUG("status register: %" PRId8 " fail_count: %" PRId8, s, _tranfer_fail_count);
+
+	// PCF8583 contains free RAM registers
+	// This checks if I2C devices contains this RAM memory registers
+	// Some values are stored into this registers
+	// then it is vertified that the entered values fit.
+	setRegister(0x04, 10);
+	setRegister(0x05, 10);
+	setRegister(0x06, 10);
+	setRegister(0x0c, 5);
+	setRegister(0x0d, 5);
+	setRegister(0x0e, 5);
+	uint32_t tmp{0};
+
+	// check values stored in free RAM parts
+	tmp += readRegister(0x04);
+	tmp += readRegister(0x05);
+	tmp += readRegister(0x06);
+	tmp += readRegister(0x0c);
+	tmp += readRegister(0x0d);
+	tmp += readRegister(0x0e);
+
+	if (tmp != 45) {
+		return PX4_ERROR;
+	}
+
 	return PX4_OK;
 }

-int PCF8583::getCounter()
+void PCF8583::initCounter()
 {
+	// set counter mode
+	_tranfer_fail_count = 0;
+	setRegister(0x00, 0b00100000);
+	resetCounter();
+
+}
+
+uint32_t PCF8583::getCounter()
+{
+	// Counter value is stored in 9 words
+	// in 3 register as BCD value
 	uint8_t a = readRegister(0x01);
 	uint8_t b = readRegister(0x02);
 	uint8_t c = readRegister(0x03);

-	return int((a & 0x0f) * 1 + ((a & 0xf0) >> 4) * 10 + (b & 0x0f) * 100 + ((b & 0xf0) >> 4) * 1000 +
-		   (c & 0x0f) * 10000 + ((c & 0xf0) >> 4) * 1000000);
+	return uint32_t(
+		       hiWord(a) * 1u + loWord(a) * 10u
+		       + hiWord(b) * 100u + loWord(b) * 1000u
+		       + hiWord(c) * 10000u + loWord(c) * 1000000u);
 }

 void PCF8583::resetCounter()
 {
+	_last_measurement_time = hrt_absolute_time();
 	setRegister(0x01, 0x00);
 	setRegister(0x02, 0x00);
 	setRegister(0x03, 0x00);
+	_count = 0;
+	_last_reset_time = _last_measurement_time;
+	_reset_count ++;
 }

+
+// Configure PCF8583 driver into counting mode
 void PCF8583::setRegister(uint8_t reg, uint8_t value)
 {
 	uint8_t buff[2];
@@ -93,8 +135,13 @@ void PCF8583::setRegister(uint8_t reg, uint8_t value)
 	buff[1] = value;
 	int ret = transfer(buff, 2, nullptr, 0);

+	if (reg == 0x00) {
+		_last_config_register_content = value;
+	}
+
 	if (PX4_OK != ret) {
 		PX4_DEBUG("setRegister : i2c::transfer returned %d", ret);
+		_tranfer_fail_count++;
 	}
 }

@@ -105,6 +152,7 @@ uint8_t PCF8583::readRegister(uint8_t reg)

 	if (PX4_OK != ret) {
 		PX4_DEBUG("readRegister : i2c::transfer returned %d", ret);
+		_tranfer_fail_count++;
 	}

 	return rcv;
@@ -113,34 +161,61 @@ uint8_t PCF8583::readRegister(uint8_t reg)
 void PCF8583::RunImpl()
 {
 	// read sensor and compute frequency
-	int oldcount = _count;
-	uint64_t oldtime = _last_measurement_time;
+	int32_t oldcount = _count;
+
+	int32_t diffTime = hrt_elapsed_time(&_last_measurement_time);
+
+	// check if delay is enought
+	if (diffTime < _param_pcf8583_pool.get() / 2) {
+		PX4_ERR("pcf8583 loop called too early");
+		return;
+	}

 	_count = getCounter();
 	_last_measurement_time = hrt_absolute_time();

-	int diffCount = _count - oldcount;
-	uint64_t diffTime = _last_measurement_time - oldtime;
+	int32_t diffCount = _count - oldcount;

-	if (_param_pcf8583_reset.get() < _count + diffCount) {
+	// check if there is enought space in counter
+	// Otherwise, reset counter
+	if (diffCount > (999999 - oldcount)) {
+		PX4_ERR("pcf8583 RPM register overflow");
 		resetCounter();
-		_last_measurement_time = hrt_absolute_time();
-		_count = 0;
+		return;
 	}

-	float indicated_rpm = (float)diffCount / _param_pcf8583_magnet.get() / ((float)diffTime / 1000000) * 60.f;
+	//check if device failed or reset
+	uint8_t s = readRegister(0x00);
+
+	if (_tranfer_fail_count > 0 || s != 0b00100000 || diffCount < 0) {
+		PX4_ERR("pcf8583 RPM sensor restart: fail count %d, status: %d, diffCount: %ld",
+			_tranfer_fail_count, s, diffCount);
+		initCounter();
+		return;
+	}
+
+	// Calculate RPM and accuracy estimation
+	float indicated_rpm = (((float)diffCount / _param_pcf8583_magnet.get()) / ((float)diffTime / 1000000.f)) * 60.f;
 	float estimated_accurancy = 1 / (float)_param_pcf8583_magnet.get() / ((float)diffTime / 1000000) * 60.f;

-	// publish
+	// publish data to uorb
 	rpm_s msg{};
 	msg.indicated_frequency_rpm = indicated_rpm;
 	msg.estimated_accurancy_rpm = estimated_accurancy;
 	msg.timestamp = hrt_absolute_time();
 	_rpm_pub.publish(msg);
+
+	//check counter range
+	if (_param_pcf8583_reset.get() < diffCount + (int)_count) {
+		resetCounter();
+	}
 }

 void PCF8583::print_status()
 {
 	I2CSPIDriverBase::print_status();
 	PX4_INFO("poll interval:  %" PRId32 " us", _param_pcf8583_pool.get());
+	PX4_INFO("Last reset %.3fs ago, Count of resets: %d", (double)(hrt_absolute_time() - _last_reset_time) / 1000000.0,
+		 _reset_count);
+	PX4_INFO("Last count %ld", _count);
 }
@@ -48,6 +48,7 @@
 #include <px4_platform_common/i2c_spi_buses.h>
 #include <drivers/device/i2c.h>
 #include <uORB/Publication.hpp>
+#include <uORB/PublicationMulti.hpp>
 #include <uORB/topics/rpm.h>
 #include <drivers/drv_hrt.h>

@@ -71,16 +72,24 @@ private:

 	int  probe() override;

-	int            getCounter();
+	void           initCounter();
+	uint32_t       getCounter();
 	void           resetCounter();

 	uint8_t        readRegister(uint8_t reg);
 	void           setRegister(uint8_t reg, uint8_t value);

-	int            _count{0};
-	hrt_abstime    _last_measurement_time{0};
+	uint8_t        hiWord(uint8_t in) { return (in & 0x0fu); }
+	uint8_t        loWord(uint8_t in) { return ((in & 0xf0u) >> 4); }

-	uORB::Publication<rpm_s> _rpm_pub{ORB_ID(rpm)};
+	uint32_t       _count{0};
+	uint16_t       _reset_count{0};
+	hrt_abstime    _last_measurement_time{0};
+	hrt_abstime    _last_reset_time{0};
+	int            _tranfer_fail_count{0};
+	uint8_t        _last_config_register_content{0x00};
+
+	uORB::PublicationMulti<rpm_s> _rpm_pub{ORB_ID(rpm)};

 	DEFINE_PARAMETERS(
 		(ParamInt<px4::params::PCF8583_POOL>) _param_pcf8583_pool,
@@ -31,6 +31,21 @@
 *
 ****************************************************************************/

+/**
+* PCF8583 eneable driver
+*
+* Run PCF8583 driver automatically
+*
+* @reboot_required true
+* @min 0
+* @max 1
+* @group Sensors
+* @value 0 Disabled
+* @value 1 Eneabled
+*/
+PARAM_DEFINE_INT32(SENS_EN_PCF8583, 0);
+
+
 /**
 * PCF8583 rotorfreq (i2c) pool interval
 *
@@ -42,16 +57,6 @@
 */
 PARAM_DEFINE_INT32(PCF8583_POOL, 1000000);

-/**
- * PCF8583 rotorfreq (i2c) i2c address
- *
- * @reboot_required true
- * @group Sensors
- * @value 80 Address 0x50 (80)
- * @value 81 Address 0x51 (81)
- */
-PARAM_DEFINE_INT32(PCF8583_ADDR, 80);
-
 /**
 * PCF8583 rotorfreq (i2c) pulse reset value
 *
@@ -40,9 +40,11 @@ void
 PCF8583::print_usage()
 {
 	PRINT_MODULE_USAGE_NAME("pcf8583", "driver");
+	PRINT_MODULE_USAGE_SUBCATEGORY("rpm_sensor");
 	PRINT_MODULE_USAGE_COMMAND("start");
 	PRINT_MODULE_USAGE_PARAMS_I2C_SPI_DRIVER(true, false);
-	PRINT_MODULE_USAGE_PARAMS_I2C_ADDRESS(80);
+	PRINT_MODULE_USAGE_PARAMS_I2C_ADDRESS(0x50);
+	PRINT_MODULE_USAGE_PARAMS_I2C_KEEP_RUNNING_FLAG();
 	PRINT_MODULE_USAGE_DEFAULT_COMMANDS();
 }

@@ -50,11 +52,11 @@ extern "C" __EXPORT int pcf8583_main(int argc, char *argv[])
 {
 	using ThisDriver = PCF8583;
 	BusCLIArguments cli{true, false};
-	cli.default_i2c_frequency = 400000;
+	cli.default_i2c_frequency = 100000;

-	int32_t addr{80};
-	param_get(param_find("PCF8583_ADDR"), &addr);
-	cli.i2c_address = addr;
+	//int32_t addr{0x50};
+	cli.i2c_address = 0x50;
+	cli.support_keep_running = true;

 	const char *verb = cli.parseDefaultArguments(argc, argv);

@@ -239,6 +239,10 @@
                            "name": "orbit",
                            "description": "Orbit"
                        },
+                        "15": {
+                            "name": "auto_vtol_takeoff",
+                            "description": "Vtol Takeoff"
+                        },
                        "255": {
                            "name": "unknown",
                            "description": "[Unknown]"
@@ -58,7 +58,7 @@ void NPFG::guideToPath(const Vector2f &ground_vel, const Vector2f &wind_vel, con
 	const float wind_speed = wind_vel.norm();

 	// on-track wind triangle projections
-	const float wind_cross_upt = cross2D(wind_vel, unit_path_tangent);
+	const float wind_cross_upt = wind_vel.cross(unit_path_tangent);
 	const float wind_dot_upt = wind_vel.dot(unit_path_tangent);

 	// calculate the bearing feasibility on the track at the current closest point
@@ -85,7 +85,7 @@ void NPFG::guideToPath(const Vector2f &ground_vel, const Vector2f &wind_vel, con
 	bearing_vec_ = bearingVec(unit_path_tangent, look_ahead_ang, signed_track_error);

 	// wind triangle projections
-	const float wind_cross_bearing = cross2D(wind_vel, bearing_vec_);
+	const float wind_cross_bearing = wind_vel.cross(bearing_vec_);
 	const float wind_dot_bearing = wind_vel.dot(bearing_vec_);

 	// continuous representation of the bearing feasibility
@@ -130,7 +130,7 @@ void NPFG::guideToPoint(const Vector2f &ground_vel, const Vector2f &wind_vel, co
 	const float wind_speed = wind_vel.norm();

 	// wind triangle projections
-	const float wind_cross_bearing = cross2D(wind_vel, bearing_vec);
+	const float wind_cross_bearing = wind_vel.cross(bearing_vec);
 	const float wind_dot_bearing = wind_vel.dot(bearing_vec);

 	// continuous representation of the bearing feasibility
@@ -501,7 +501,7 @@ float NPFG::lateralAccel(const Vector2f &air_vel, const Vector2f &air_vel_ref, c
 	// lateral acceleration demand only from the heading error

 	const float dot_air_vel_err = air_vel.dot(air_vel_ref);
-	const float cross_air_vel_err = cross2D(air_vel, air_vel_ref);
+	const float cross_air_vel_err = air_vel.cross(air_vel_ref);

 	if (dot_air_vel_err < 0.0f) {
 		// hold max lateral acceleration command above 90 deg heading error
@@ -544,7 +544,7 @@ void NPFG::navigateWaypoints(const Vector2d &waypoint_A, const Vector2d &waypoin

 		// guidance to the line through A and B
 		unit_path_tangent_ = vector_A_to_B.normalized();
-		signed_track_error_ = cross2D(unit_path_tangent_, vector_A_to_vehicle);
+		signed_track_error_ = unit_path_tangent_.cross(vector_A_to_vehicle);
 		guideToPath(ground_vel, wind_vel, unit_path_tangent_, signed_track_error_, 0.0f);

 		const Vector2f bearing_vec_to_point = -vector_A_to_vehicle.normalized();
@@ -566,7 +566,7 @@ void NPFG::navigateWaypoints(const Vector2d &waypoint_A, const Vector2d &waypoin
 	} else {
 		// track the line segment between A and B
 		unit_path_tangent_ = vector_A_to_B.normalized();
-		signed_track_error_ = cross2D(unit_path_tangent_, vector_A_to_vehicle);
+		signed_track_error_ = unit_path_tangent_.cross(vector_A_to_vehicle);
 		guideToPath(ground_vel, wind_vel, unit_path_tangent_, signed_track_error_, 0.0f);
 	}

@@ -629,7 +629,7 @@ void NPFG::navigatePathTangent(const matrix::Vector2d &vehicle_pos, const matrix

 	// closest point to vehicle
 	matrix::Vector2f error_vector = getLocalPlanarVector(position_setpoint, vehicle_pos);
-	signed_track_error_ = cross2D(unit_path_tangent_, error_vector);
+	signed_track_error_ = unit_path_tangent_.cross(error_vector);

 	guideToPath(ground_vel, wind_vel, unit_path_tangent_, signed_track_error_, curvature);

@@ -717,16 +717,6 @@ private:
 	float lateralAccel(const matrix::Vector2f &air_vel, const matrix::Vector2f &air_vel_ref,
 			   const float airspeed) const;

-	/*
-	 * Calculates two-dimensional "cross product" of two vectors.
-	 * TODO: move to matrix lib (Vector2 operation)
-	 *
-	 * @param[in] vec_1 Vector 1
-	 * @param[in] vec_2 Vector 2
-	 * @return 2D cross product
-	 */
-	float cross2D(const matrix::Vector2f &vec_1, const matrix::Vector2f &vec_2) const { return vec_1(0) * vec_2(1) - vec_1(1) * vec_2(0); }
-
 	/*******************************************************************************
 	 * PX4 POSITION SETPOINT INTERFACE FUNCTIONS
 	 */
@@ -2,4 +2,11 @@ menuconfig MODULES_AIRSHIP_ATT_CONTROL
 	bool "airship_att_control"
 	default n
 	---help---
-		Enable support for airship_att_control
+		Enable support for airship_att_control
+
+menuconfig USER_AIRSHIP_ATT_CONTROL
+	bool "airship_att_control running as userspace module"
+	default y
+	depends on BOARD_PROTECTED && MODULES_AIRSHIP_ATT_CONTROL
+	---help---
+		Put airship_att_control in userspace memory
@@ -2,4 +2,11 @@ menuconfig MODULES_AIRSPEED_SELECTOR
 	bool "airspeed_selector"
 	default n
 	---help---
-		Enable support for airspeed_selector
+		Enable support for airspeed_selector
+
+menuconfig USER_AIRSPEED_SELECTOR
+	bool "airspeed_selector running as userspace module"
+	default y
+	depends on BOARD_PROTECTED && MODULES_AIRSPEED_SELECTOR
+	---help---
+		Put airspeed_selector in userspace memory
@@ -593,9 +593,13 @@ void AirspeedModule::select_airspeed_and_publish()

 	// print warning or info, depending of whether airspeed got declared invalid or healthy
 	if (_valid_airspeed_index != _prev_airspeed_index &&
-	    (_number_of_airspeed_sensors > 0 || !_vehicle_land_detected.landed) &&
-	    _valid_airspeed_index != _prev_airspeed_index) {
-		if (_prev_airspeed_index > 0) {
+	    _number_of_airspeed_sensors > 0) {
+		if (_vehicle_status.arming_state != vehicle_status_s::ARMING_STATE_ARMED && _prev_airspeed_index > 0) {
+			mavlink_log_critical(&_mavlink_log_pub, "Airspeed sensor failure detected. Check connection and reboot.\t");
+			events::send(events::ID("airspeed_selector_sensor_failure_disarmed"), events::Log::Critical,
+				     "Airspeed sensor failure detected. Check connection and reboot");
+
+		} else if (_prev_airspeed_index > 0) {
 			mavlink_log_critical(&_mavlink_log_pub, "Airspeed sensor failure detected. Return to launch (RTL) is advised.\t");
 			events::send(events::ID("airspeed_selector_sensor_failure"), events::Log::Critical,
 				     "Airspeed sensor failure detected. Return to launch (RTL) is advised");
@@ -2,4 +2,11 @@ menuconfig MODULES_ANGULAR_VELOCITY_CONTROLLER
 	bool "angular_velocity_controller"
 	default n
 	---help---
-		Enable support for angular_velocity_controller
+		Enable support for angular_velocity_controller
+
+menuconfig USER_ANGULAR_VELOCITY_CONTROLLER
+	bool "angular_velocity_controller running as userspace module"
+	default y
+	depends on BOARD_PROTECTED && MODULES_ANGULAR_VELOCITY_CONTROLLER
+	---help---
+		Put angular_velocity_controller in userspace memory
@@ -2,4 +2,11 @@ menuconfig MODULES_ATTITUDE_ESTIMATOR_Q
 	bool "attitude_estimator_q"
 	default n
 	---help---
-		Enable support for attitude_estimator_q
+		Enable support for attitude_estimator_q
+
+menuconfig USER_ATTITUDE_ESTIMATOR_Q
+	bool "attitude_estimator_q running as userspace module"
+	default n
+	depends on BOARD_PROTECTED && MODULES_ATTITUDE_ESTIMATOR_Q
+	---help---
+		Put attitude_estimator_q in userspace memory
@@ -2,4 +2,11 @@ menuconfig MODULES_BATTERY_STATUS
 	bool "battery_status"
 	default n
 	---help---
-		Enable support for battery_status
+		Enable support for battery_status
+
+menuconfig USER_BATTERY_STATUS
+	bool "battery_status running as userspace module"
+	default n
+	depends on BOARD_PROTECTED && MODULES_BATTERY_STATUS
+	---help---
+		Put battery_status in userspace memory
@@ -41,12 +41,11 @@
 static constexpr int   DEFAULT_V_CHANNEL[BOARD_NUMBER_BRICKS] = BOARD_BATT_V_LIST;
 static constexpr int   DEFAULT_I_CHANNEL[BOARD_NUMBER_BRICKS] = BOARD_BATT_I_LIST;
 #else
-static constexpr int   DEFAULT_V_CHANNEL[BOARD_NUMBER_BRICKS] = {0};
-static constexpr int   DEFAULT_I_CHANNEL[BOARD_NUMBER_BRICKS] = {0};
+#error  "BOARD_BATT_V_LIST and BOARD_BATT_I_LIST need to be defined"
 #endif
 #else
-static constexpr int DEFAULT_V_CHANNEL[1] = {0};
-static constexpr int DEFAULT_I_CHANNEL[1] = {0};
+static constexpr int DEFAULT_V_CHANNEL[1] = {-1};
+static constexpr int DEFAULT_I_CHANNEL[1] = {-1};
 #endif

 AnalogBattery::AnalogBattery(int index, ModuleParams *parent, const int sample_interval_us, const uint8_t source,
@@ -177,6 +177,7 @@ BatteryStatus::adc_poll()
 	/* Per Brick readings with default unread channels at 0 */
 	float bat_current_adc_readings[BOARD_NUMBER_BRICKS] {};
 	float bat_voltage_adc_readings[BOARD_NUMBER_BRICKS] {};
+	bool has_bat_voltage_adc_channel[BOARD_NUMBER_BRICKS] {};

 	int selected_source = -1;

@@ -201,16 +202,19 @@ BatteryStatus::adc_poll()

 				/* look for specific channels and process the raw voltage to measurement data */

-				if (adc_report.channel_id[i] == _analogBatteries[b]->get_voltage_channel()) {
-					/* Voltage in volts */
-					bat_voltage_adc_readings[b] = adc_report.raw_data[i] *
-								      adc_report.v_ref /
-								      adc_report.resolution;
+				if (adc_report.channel_id[i] >= 0) {
+					if (adc_report.channel_id[i] == _analogBatteries[b]->get_voltage_channel()) {
+						/* Voltage in volts */
+						bat_voltage_adc_readings[b] = adc_report.raw_data[i] *
+									      adc_report.v_ref /
+									      adc_report.resolution;
+						has_bat_voltage_adc_channel[b] = true;

-				} else if (adc_report.channel_id[i] == _analogBatteries[b]->get_current_channel()) {
-					bat_current_adc_readings[b] = adc_report.raw_data[i] *
-								      adc_report.v_ref /
-								      adc_report.resolution;
+					} else if (adc_report.channel_id[i] == _analogBatteries[b]->get_current_channel()) {
+						bat_current_adc_readings[b] = adc_report.raw_data[i] *
+									      adc_report.v_ref /
+									      adc_report.resolution;
+					}
 				}

 			}
@@ -218,11 +222,13 @@ BatteryStatus::adc_poll()

 		for (int b = 0; b < BOARD_NUMBER_BRICKS; b++) {

-			_analogBatteries[b]->updateBatteryStatusADC(
-				hrt_absolute_time(),
-				bat_voltage_adc_readings[b],
-				bat_current_adc_readings[b]
-			);
+			if (has_bat_voltage_adc_channel[b]) { // Do not publish if no voltage channel configured
+				_analogBatteries[b]->updateBatteryStatusADC(
+					hrt_absolute_time(),
+					bat_voltage_adc_readings[b],
+					bat_current_adc_readings[b]
+				);
+			}
 		}
 	}
 }
@@ -2,4 +2,11 @@ menuconfig MODULES_CAMERA_FEEDBACK
 	bool "camera_feedback"
 	default n
 	---help---
-		Enable support for camera_feedback
+		Enable support for camera_feedback
+
+menuconfig USER_CAMERA_FEEDBACK
+	bool "camera_feedback running as userspace module"
+	default y
+	depends on BOARD_PROTECTED && MODULES_CAMERA_FEEDBACK
+	---help---
+		Put camera_feedback in userspace memory
@@ -629,9 +629,12 @@ static inline navigation_mode_t navigation_mode(uint8_t main_state)
 	case commander_state_s::MAIN_STATE_AUTO_PRECLAND: return navigation_mode_t::auto_precland;

 	case commander_state_s::MAIN_STATE_ORBIT: return navigation_mode_t::orbit;
+
+	case commander_state_s::MAIN_STATE_AUTO_VTOL_TAKEOFF: return navigation_mode_t::auto_vtol_takeoff;
 	}

-	static_assert(commander_state_s::MAIN_STATE_MAX - 1 == (int)navigation_mode_t::orbit, "enum definition mismatch");
+	static_assert(commander_state_s::MAIN_STATE_MAX - 1 == (int)navigation_mode_t::auto_vtol_takeoff,
+		      "enum definition mismatch");

 	return navigation_mode_t::unknown;
 }
@@ -1149,6 +1152,24 @@ Commander::handle_command(const vehicle_command_s &cmd)
 		}
 		break;

+	case vehicle_command_s::VEHICLE_CMD_NAV_VTOL_TAKEOFF:
+
+		/* ok, home set, use it to take off */
+		if (TRANSITION_CHANGED == main_state_transition(_status, commander_state_s::MAIN_STATE_AUTO_VTOL_TAKEOFF,
+				_status_flags,
+				_internal_state)) {
+			cmd_result = vehicle_command_s::VEHICLE_CMD_RESULT_ACCEPTED;
+
+		} else if (_internal_state.main_state == commander_state_s::MAIN_STATE_AUTO_VTOL_TAKEOFF) {
+			cmd_result = vehicle_command_s::VEHICLE_CMD_RESULT_ACCEPTED;
+
+		} else {
+			mavlink_log_critical(&_mavlink_log_pub, "VTOL Takeoff denied! Please disarm and retry");
+			cmd_result = vehicle_command_s::VEHICLE_CMD_RESULT_TEMPORARILY_REJECTED;
+		}
+
+		break;
+
 	case vehicle_command_s::VEHICLE_CMD_NAV_LAND: {
 			if (TRANSITION_DENIED != main_state_transition(_status, commander_state_s::MAIN_STATE_AUTO_LAND, _status_flags,
 					_internal_state)) {
@@ -2393,7 +2414,8 @@ Commander::run()

 			// Transition main state to loiter or auto-mission after takeoff is completed.
 			if (_armed.armed && !_land_detector.landed
-			    && (_status.nav_state == vehicle_status_s::NAVIGATION_STATE_AUTO_TAKEOFF)
+			    && (_status.nav_state == vehicle_status_s::NAVIGATION_STATE_AUTO_TAKEOFF ||
+				_status.nav_state == vehicle_status_s::NAVIGATION_STATE_AUTO_VTOL_TAKEOFF)
 			    && (mission_result.timestamp >= _status.nav_state_timestamp)
 			    && mission_result.finished) {

@@ -3332,6 +3354,7 @@ Commander::update_control_mode()
 	case vehicle_status_s::NAVIGATION_STATE_AUTO_MISSION:
 	case vehicle_status_s::NAVIGATION_STATE_AUTO_LOITER:
 	case vehicle_status_s::NAVIGATION_STATE_AUTO_TAKEOFF:
+	case vehicle_status_s::NAVIGATION_STATE_AUTO_VTOL_TAKEOFF:
 		_vehicle_control_mode.flag_control_auto_enabled = true;
 		_vehicle_control_mode.flag_control_rates_enabled = true;
 		_vehicle_control_mode.flag_control_attitude_enabled = true;
@@ -2,4 +2,11 @@ menuconfig MODULES_COMMANDER
 	bool "commander"
 	default n
 	---help---
-		Enable support for commander
+		Enable support for commander
+
+menuconfig USER_COMMANDER
+	bool "commander running as userspace module"
+	default y
+	depends on BOARD_PROTECTED && MODULES_COMMANDER
+	---help---
+		Put commander in userspace memory
@@ -63,7 +63,8 @@ enum PX4_CUSTOM_SUB_MODE_AUTO {
 	PX4_CUSTOM_SUB_MODE_AUTO_LAND,
 	PX4_CUSTOM_SUB_MODE_AUTO_RESERVED_DO_NOT_USE, // was PX4_CUSTOM_SUB_MODE_AUTO_RTGS, deleted 2020-03-05
 	PX4_CUSTOM_SUB_MODE_AUTO_FOLLOW_TARGET,
-	PX4_CUSTOM_SUB_MODE_AUTO_PRECLAND
+	PX4_CUSTOM_SUB_MODE_AUTO_PRECLAND,
+	PX4_CUSTOM_SUB_MODE_AUTO_VTOL_TAKEOFF
 };

 enum PX4_CUSTOM_SUB_MODE_POSCTL {
--- a/Show More
+++ b/Show More
Author	SHA1	Message	Date
Daniel Agar	8a4d93cca3	drivers/rc/ghst_rc: create new standalone ghst_rc driver	2022-02-21 21:49:55 -05:00
Charles Cross	f31f3370ef	Add support for Timer15 on H743 boards (#19228 ) * Adds Timer15 to stm32_common, if the timer base is defined * Adds Timer15 logic for DMA and AltFunc config on stm32h7 boards * Adds TIM15 BDTR->MOE support in stm32_common timer init * Adds support for TIM15 pwm channels on Matek H743 Slim	2022-02-21 16:52:38 -05:00
Daniel Agar	710185d2ad	drivers/magnetometer/rm3100: cleanup and simplify (#19238 ) * switch to continuous mode at 75 Hz * add simple failure count and reset mechanism * add range checks and perf counts	2022-02-21 16:48:20 -05:00
Roman Dvořák	976c994156	Extend the PCF8583 driver to support multiple instances (#19232 ) * Add some restart events into pcf8583 driver Co-authored-by: Vít Hanousek <vithanousek@seznam.cz>	2022-02-21 16:47:16 -05:00
Daniel Agar	3c58932aff	boards: px4_fmu-v2_default disable ms4525 to save flash	2022-02-21 16:45:22 -05:00
Charles Cross	699f34ba83	drivers/magnetometer/rm3100: self-test procedure revision (#19207 ) * Adds retry behavior and additional checks to RM3100 BIST Signed-off-by: Charles Cross <charles@missionrobotics.us>	2022-02-21 11:27:42 -05:00
Beat Küng	960003a86a	control_allocator: fix weak authority matrix check & update - use max(fabsf(val)) instead of max(val) - use correct actuator count	2022-02-21 11:17:00 -05:00
Daniel Agar	591b7b6934	ekf2: add new estimator_gps_status.msg - includes the estimator status check fail bits broken out as descriptive booleans - absorbs ekf_gps_drift.msg	2022-02-21 08:58:59 -05:00
Daniel Agar	f8d7574d3c	ekf2: simplify global position publication validity check	2022-02-21 08:57:33 -05:00
fury1895	be92165c54	AirspeedSelector: fix messaging	2022-02-21 15:21:13 +03:00
David Sidrane	c0facec889	cdc_acm_check:Prevent USB disconect on param save 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.	2022-02-21 10:56:41 +01:00
Julian Oes	9b35b680f6	gimbal: fix input selection Once we have valid input we should use that and not overwrite our update_result with the result from other inputs, otherwise the automatic selection does not actually work. This behavior means that only the first update will be used if there are several sources updating in the same cycle.	2022-02-21 08:38:16 +01:00
Igor Mišić	3c09448daf	dataman: add silence flag for _file_initialize	2022-02-21 08:36:36 +01:00
murata	cc6c6c3b8c	power_monitor: Determine I2C communication errors	2022-02-20 16:05:44 -05:00
Daniel Agar	09e36e6cb4	gimbal: new pitch limits [MNT_LND_P_MIN, MNT_LND_P_MAX] when landed	2022-02-20 16:04:09 -05:00
Justin	59f28517c5	Force ignition to use version 5 (edifice) which is what it is built for so it doesn't default to a newer version	2022-02-19 19:04:26 +01:00
Daniel Agar	d147ad3a9a	gimbal: add parameter units	2022-02-18 14:15:58 -05:00
Daniel Agar	2a8aa17a81	mavlink: streams MOUNT_ORIENTATION populate time_boot_ms	2022-02-18 14:14:56 -05:00
Daniel Agar	1f77a3750e	mavlink: don't reset _src_addr_initialized on parameter updates	2022-02-18 14:14:36 -05:00
Daniel Agar	e0a8d217fc	mavlink increase HEARTBEAT timeout 1.5->2.5s - tolerate missing one HEARTBEAT + jitter	2022-02-18 10:16:20 -05:00
Daniel Agar	1addbe469e	mavlink: STATUSTEXT increase stale message timeout	2022-02-18 10:16:20 -05:00
Daniel Agar	d5d50d5855	gyro_calibration: add additional validity check before finalizing calibrations - this can slip through if sensors are running normally, but then stop publishing valid data	2022-02-18 09:12:30 -05:00
Jukka Laitinen	dab7b007de	Auto-generate a list of kernel-side built-in modules(drivers) Signed-off-by: Jukka Laitinen <jukkax@ssrc.tii.ae>	2022-02-18 07:58:58 +01:00
Jukka Laitinen	9449ed6e66	Add support for protected build in drivers, systemcmds and modules Kconfig Signed-off-by: Jukka Laitinen <jukkax@ssrc.tii.ae>	2022-02-18 07:58:58 +01:00
Jukka Laitinen	6071b87afc	platforms/common/uORB: Separate IOCTLs going through boardctl interface from the original ones It was a mistake to mix these two together, it is simpler to implement the boardctl interface for the protected build, if the boardctl ioctls are different Signed-off-by: Jukka Laitinen <jukkax@ssrc.tii.ae>	2022-02-18 07:53:13 +01:00
Jukka Laitinen	f0d9f44f45	Add an ioctl handler to launch built-in modules in kernel side Signed-off-by: Jukka Laitinen <jukkax@ssrc.tii.ae>	2022-02-18 07:53:13 +01:00
Jukka Laitinen	db3baf6c26	Add an ioctl interface for userspace to kernel calls This adds an ioctl interface for NuttX protected build, allowing system calls from user space to kernel for uORB, HRT and crypto Signed-off-by: Jukka Laitinen <jukkax@ssrc.tii.ae>	2022-02-18 07:53:13 +01:00
Daniel Agar	8489cec08f	boards: spracing h7extreme keep icm42688p in RAM - to make space move rc_update itcm -> sram	2022-02-17 10:30:14 -05:00
Daniel Agar	9aca693945	boards: spracing h7extreme add alternate IMU icm42688p	2022-02-17 10:30:14 -05:00
Silvan Fuhrer	c51dc3b4b7	ControlAllocator: Set all the elements of a row to 0 if that row has weak authority Weak authority on a axis is currently defined as: none of the actuators have an effectivness on this particular axis larger than 0.05. Signed-off-by: Silvan Fuhrer <silvan@auterion.com>	2022-02-17 13:21:50 +01:00
Silvan Fuhrer	97a280d41d	CA: Tiltrotor: set tilts to exactly -1 or 1 if close to these end points Signed-off-by: Silvan Fuhrer <silvan@auterion.com>	2022-02-17 13:21:50 +01:00
Silvan Fuhrer	25f5152583	Tiltrotor: make sure tilt doesn't exceed the FW tilt angle Signed-off-by: Silvan Fuhrer <silvan@auterion.com>	2022-02-17 13:21:50 +01:00
Silvan Fuhrer	c13726af66	Enable DO_SET_SPEED commands outside of missions in other AUTO modes (#18834 ) * Navigator: enable DO_CHANGE_SPEED for outside of mission - update _mission_cruising_speed_mc/_fw also if DO_CHANGE_SPEED command is received outside of mission (e.g. while Loitering doing an Orbit) - if vehicle is in AUTO_LOITER when receiving the change speed, then immediately apply it by doing a reposition without updating any other field than cruising_speed and cruising_throttle -when RTL is activated reset the cruising speed and throttle * Navigator: reset cruise speed and throttle to default when VTOL-transitioning Signed-off-by: Silvan Fuhrer <silvan@auterion.com>	2022-02-17 11:19:02 +01:00
Jaeyoung-Lim	b0f9611eb9	NPFG: use cross product for matrix library This removes the use of cross products from NPFG and takes advantage of the matrix library	2022-02-17 09:52:47 +01:00
Daniel Agar	ca97b9ba5f	drivers/gps: add new GPS_SAT_INFO parameter to enable satelitte_info - this replaces the command line argument -s	2022-02-17 08:30:25 +01:00
Daniel Agar	abfa3d23a5	boards: matek h743-slim fix STM32_SDMMC_SDXFR_CLKDIV typo	2022-02-16 10:28:01 -05:00
David Sidrane	1c66fb44aa	holybro_kakutef7:fit in flash	2022-02-16 10:09:02 -05:00
David Sidrane	2e67b92b4d	px4_fmu-v5_stackcheck:Fit in flash	2022-02-16 10:09:02 -05:00
David Sidrane	3593cf795d	NuttX With DMA/FLASH Backports	2022-02-16 10:09:02 -05:00
David Sidrane	d05d7f4154	bl:Clean up formatting	2022-02-16 10:09:02 -05:00
David Sidrane	92590155fc	px4_fmu-v6x:Bootloader move to TELEM1 with DMA	2022-02-16 10:09:02 -05:00
David Sidrane	c7bd7323ec	serial_test:Fix infinte TX loop with H7/DMA	2022-02-16 10:09:02 -05:00
David Sidrane	b5916ac712	px4_fmu-v6x:Document the DMA usage	2022-02-16 10:09:02 -05:00
David Sidrane	7eefdd1e3d	px4_fmu-v6x:Enable DMA on TELEM{1\|2}	2022-02-16 10:09:02 -05:00
Charles Cross	f9feb04f8b	Changes SDIO clock speed to 20MHz for the Matek H743 slim Signed-off-by: Charles Cross <charles@missionrobotics.us>	2022-02-15 09:38:09 -08:00
Yannick Fuhrer	26ea70e729	Airspeed selector: fix condition for messaging (#19173 ) Change warning message after airspeed failure on the ground, recommend RTL only if flying.	2022-02-15 17:23:36 +01:00
Roman Bapst	6de5d24e00	Added VTOL Takeoff navigation mode (#19027 ) * Commander: added support for MAIN_STATE_AUTO_VTOL_TAKEOFF * navigator: added support for vtol_takeoff navigation mode	2022-02-15 14:56:57 +01:00
Silvan Fuhrer	374bcb105a	RTL: fill loiter radius in state Climb (#19165 ) Signed-off-by: Silvan Fuhrer <silvan@auterion.com>	2022-02-15 14:50:14 +01:00
Daniel Agar	5370733d62	ekf2: publish flow vel only if compensated flow is available - fix a few publication timestamp_samples	2022-02-14 10:55:10 -05:00
Daniel Agar	b157afde6a	ekf2: prefer airspeed_validated over raw airspeed if available (#19159 ) * ekf2: prefer airspeed_validated over raw airspeed if available Co-authored-by: Silvan Fuhrer <silvan@auterion.com>	2022-02-14 15:37:29 +01:00
Silvan Fuhrer	df0e402c44	CA: refactor logic for matrix updating -pass flag EffectivenessUpdateReason into effectiveness, indicating if there was an external update or not. Reasons for external updates are: -config changes (parameter) -motor failure detected or certain redundant motors are switched off to save energy Signed-off-by: Silvan Fuhrer <silvan@auterion.com>	2022-02-14 09:23:10 +01:00
Jukka Laitinen	36d440f895	Add IOCTL interface to uORBManager for nuttx protected/kernel build split When building uORB for NuttX flat build, or for some other target, everything works as before. When building uORB for NuttX protected or kernel build, this does the following: - The kernel side uORB library reigsters a boardctl handler for calls from userspace and services the boardctl_ioctls by calling the actual uORB functions - For user mode binaries, the uORBManager acts as a proxy, making boardctl_ioctl calls to the kernel side Signed-off-by: Jukka Laitinen <jukkax@ssrc.tii.ae>	2022-02-14 09:10:49 +01:00
Daniel Agar	8b9a856cf7	drivers/barometer: new ms5837 driver (#18213 ) Co-authored-by: xn365 <xn_365@163.com>	2022-02-12 14:41:31 -05:00
PX4 BuildBot	41e48435c9	Update submodule mavlink to latest Sat Feb 12 12:38:48 UTC 2022 - mavlink in PX4/Firmware (9457e7b25c4fa51f5ccc0bd887760e910926ba8a): https://github.com/mavlink/mavlink/commit/311eee010bb82f5fb2e4e0f64f7961a83212b003 - mavlink current upstream: https://github.com/mavlink/mavlink/commit/4b0558d0d10efbdd550cb5321d56f6a611d0ab14 - Changes: https://github.com/mavlink/mavlink/compare/311eee010bb82f5fb2e4e0f64f7961a83212b003...4b0558d0d10efbdd550cb5321d56f6a611d0ab14 4b0558d0 2022-02-11 Andrew Tridgell - added CANFD_FRAME and CAN_FILTER_MODIFY messages 7f032afe 2022-02-10 Daniel Agar - Apply suggestions from code review 9379a601 2022-02-06 Andrew Tridgell - common: added MAV_CMD_CAN_FORWARD and CAN_FRAME	2022-02-12 13:16:42 -05:00
Daniel Agar	fca886e05a	drivers/irlock: add SENS_EN_IRLOCK parameter to start driver	2022-02-11 22:57:56 -05:00
Silvan Fuhrer	2eba1847fd	HTE: add new parameter HTE_THR_RANGE to define range of estimated thrust Signed-off-by: Silvan Fuhrer <silvan@auterion.com>	2022-02-11 17:04:14 +01:00
Silvan Fuhrer	34805e43fd	HTE: remove unused method setMeasurementNoiseStdDev() Signed-off-by: Silvan Fuhrer <silvan@auterion.com>	2022-02-11 17:04:14 +01:00
bresch	493e35b72e	ekf_terr: reset rng fault when on ground	2022-02-11 10:57:45 -05:00
bresch	502ec7ef46	ekf_terr: fix unit test the flags were valid ony because they were based on timeouts and when starting the unit test, it takes a couple of seconds to actually timeout	2022-02-11 10:57:45 -05:00
bresch	2fb7b35a8b	ekf2_terr: refactor terrain estimator - flow aiding	2022-02-11 10:57:45 -05:00
bresch	33fd1849e0	ekf2_terr: refactor terrain estimator - rng aiding	2022-02-11 10:57:45 -05:00
bresch	5818974f0f	ekf: add range finder "faulty" status When delclared faulty, the range finder cannot be used anymore	2022-02-11 10:57:45 -05:00
bresch	a3b2550f07	mc_auto: only check for offtrack, infront and behind in XY-plane This fixes the issue when changing the altitude during a goto for example, where the vehicle was going backwards and upwards to reach the closest point to the line. Now the vehicle simply goes towards the target waypoint.	2022-02-11 16:14:48 +01:00
alessandro	1febba315a	mantis: disable optical flow fusion in EKF2 Above grass fields I can frequently observe position instabilities with the mantis due to the optical flow fusion. Let's disable flow fusion for now.	2022-02-11 15:33:34 +01:00
Daniel Agar	0b9f60a037	drivers/rc_input: always provide RC_PORT_CONFIG parameter - RC_PORT_CONFIG is disabled by default if the board doesn't have CONFIG_BOARD_SERIAL_RC set - allows user facing custom RC configuration that overrides board defaults	2022-02-10 09:41:32 -05:00
Daniel Agar	97a75fc388	sensors: skip selection and failover checks during parameter update cycles	2022-02-10 09:31:23 -05:00
Beat Küng	b6607a7b78	battery_status: do not publish if no voltage channel is defined This is the case for boards with digital readout, like v5x, but still enable the battery_status module for external analog driver options. An alternative would be to not run battery_status depending on config.	2022-02-09 16:54:45 -05:00
Nico van Duijn	10ad553f1d	v5x: add battery_status module to enable analog bat sensing	2022-02-09 16:54:45 -05:00
Beat Küng	28c27f1b9a	px4/fmu-v5x: add ADC_ADS1115_EN param to use external ADC	2022-02-09 16:54:45 -05:00