VOXL2 specific drivers, modules, and miscellaneous support files (#22588)

2026-05-10 06:17:26 +08:00 · 2024-01-18 09:14:17 -08:00
parent b60e73c76f
commit 2b69a3d290
96 changed files with 8266 additions and 678 deletions
@@ -80,3 +80,6 @@
 	path = Tools/simulation/gz
 	url = https://github.com/PX4/PX4-gazebo-models.git
 	branch = main
+[submodule "boards/modalai/voxl2/libfc-sensor-api"]
+	path = src/modules/muorb/apps/libfc-sensor-api
+	url = https://gitlab.com/voxl-public/voxl-sdk/core-libs/libfc-sensor-api.git
@@ -5,7 +5,7 @@ CONFIG_BOARD_SERIAL_TEL1="/dev/ttyS6"
 CONFIG_BOARD_SERIAL_TEL2="/dev/ttyS4"
 CONFIG_BOARD_SERIAL_TEL3="/dev/ttyS1"
 CONFIG_BOARD_SERIAL_TEL4="/dev/ttyS3"
-CONFIG_DRIVERS_ACTUATORS_MODAL_IO=y
+CONFIG_DRIVERS_ACTUATORS_VOXL_ESC=y
 CONFIG_DRIVERS_ADC_ADS1115=y
 CONFIG_DRIVERS_ADC_BOARD_ADC=y
 CONFIG_COMMON_BAROMETERS=y
@@ -3,9 +3,9 @@
 # ModalAI FC-v1 specific board sensors init
 #------------------------------------------------------------------------------

-if param greater MODAL_IO_CONFIG 0
+if param greater VOXL_ESC_CONFIG 0
 then
-	modal_io start
+	voxl_esc start
 fi

 board_adc start
@@ -264,7 +264,7 @@
 #define BOARD_NUM_IO_TIMERS 5

 // J4 / TELEM3 / UART2
-#define MODAL_IO_DEFAULT_PORT                      "/dev/ttyS1"
+#define VOXL_ESC_DEFAULT_PORT                      "/dev/ttyS1"

 __BEGIN_DECLS

@@ -3,9 +3,10 @@ CONFIG_BOARD_ARCHITECTURE="cortex-m7"
 CONFIG_BOARD_SERIAL_GPS1="/dev/ttyS0"
 CONFIG_BOARD_SERIAL_TEL1="/dev/ttyS6"
 CONFIG_BOARD_SERIAL_TEL2="/dev/ttyS4"
-CONFIG_DRIVERS_ACTUATORS_MODAL_IO=y
+CONFIG_DRIVERS_ACTUATORS_VOXL_ESC=y
 CONFIG_DRIVERS_ADC_ADS1115=y
 CONFIG_DRIVERS_ADC_BOARD_ADC=y
+# CONFIG_COMMON_BAROMETERS=y
 CONFIG_DRIVERS_BAROMETER_INVENSENSE_ICP201XX=y
 CONFIG_DRIVERS_BATT_SMBUS=y
 CONFIG_DRIVERS_CAMERA_CAPTURE=y
@@ -14,6 +15,10 @@ CONFIG_COMMON_DIFFERENTIAL_PRESSURE=y
 CONFIG_COMMON_DISTANCE_SENSOR=y
 CONFIG_DRIVERS_DSHOT=y
 CONFIG_DRIVERS_GPS=y
+#CONFIG_DRIVERS_IMU_BOSCH_BMI088=y
+#CONFIG_DRIVERS_IMU_INVENSENSE_ICM20602=y
+#CONFIG_DRIVERS_IMU_INVENSENSE_ICM20649=y
+#CONFIG_DRIVERS_IMU_INVENSENSE_ICM20948=y
 CONFIG_DRIVERS_IMU_INVENSENSE_ICM42688P=y
 CONFIG_DRIVERS_IRLOCK=y
 CONFIG_COMMON_LIGHT=y
@@ -25,11 +30,13 @@ CONFIG_DRIVERS_POWER_MONITOR_INA226=y
 CONFIG_DRIVERS_POWER_MONITOR_VOXLPM=y
 CONFIG_DRIVERS_PWM_OUT=y
 CONFIG_DRIVERS_RC_INPUT=y
+CONFIG_DRIVERS_RPM=y
 CONFIG_COMMON_TELEMETRY=y
-CONFIG_DRIVERS_UAVCAN=y
-CONFIG_BOARD_UAVCAN_INTERFACES=1
-CONFIG_BOARD_UAVCAN_TIMER_OVERRIDE=2
+#CONFIG_DRIVERS_UAVCAN=y
+#CONFIG_BOARD_UAVCAN_INTERFACES=1
+#CONFIG_BOARD_UAVCAN_TIMER_OVERRIDE=2
 CONFIG_MODULES_AIRSPEED_SELECTOR=y
+CONFIG_MODULES_ATTITUDE_ESTIMATOR_Q=y
 CONFIG_MODULES_CAMERA_FEEDBACK=y
 CONFIG_MODULES_COMMANDER=y
 CONFIG_MODULES_CONTROL_ALLOCATOR=y
@@ -48,6 +55,7 @@ CONFIG_MODULES_GYRO_FFT=y
 CONFIG_MODULES_LAND_DETECTOR=y
 CONFIG_MODULES_LANDING_TARGET_ESTIMATOR=y
 CONFIG_MODULES_LOAD_MON=y
+CONFIG_MODULES_LOCAL_POSITION_ESTIMATOR=y
 CONFIG_MODULES_LOGGER=y
 CONFIG_MODULES_MAG_BIAS_ESTIMATOR=y
 CONFIG_MODULES_MANUAL_CONTROL=y
@@ -3,9 +3,9 @@
 # ModalAI FC-v2 specific board sensors init
 #------------------------------------------------------------------------------

-if param greater MODAL_IO_CONFIG 0
+if param greater VOXL_ESC_CONFIG 0
 then
-	modal_io start
+	voxl_esc start
 fi

 board_adc start
@@ -351,7 +351,7 @@
 #define BOARD_NUM_IO_TIMERS 5

 // J5 USART5 TELEM2 Port next to PWM connector
-#define MODAL_IO_DEFAULT_PORT                     "/dev/ttyS4"
+#define VOXL_ESC_DEFAULT_PORT                     "/dev/ttyS4"

 __BEGIN_DECLS

@@ -0,0 +1,5 @@
+CONFIG_BOARD_TOOLCHAIN="arm-none-eabi"
+CONFIG_BOARD_ARCHITECTURE="cortex-m3"
+CONFIG_BOARD_ROMFSROOT=""
+CONFIG_BOARD_CONSTRAINED_FLASH=y
+CONFIG_MODULES_PX4IOFIRMWARE=y
@@ -0,0 +1,13 @@
+{
+    "board_id": 41777,
+    "magic": "PX4FWv2",
+    "description": "Firmware for the voxl2-io board",
+    "image": "",
+    "build_time": 0,
+    "summary": "voxl2io",
+    "version": "2.0",
+    "image_size": 0,
+    "image_maxsize": 61440,
+    "git_identity": "",
+    "board_revision": 0
+}
@@ -0,0 +1,141 @@
+/************************************************************************************
+ * nuttx-configs/px4io/include/board.h
+ * include/arch/board/board.h
+ *
+ *   Copyright (C) 2009 Gregory Nutt. All rights reserved.
+ *   Author: Gregory Nutt <gnutt@nuttx.org>
+ *   Copyright (C) 2012 PX4 Development Team. All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright
+ *    notice, this list of conditions and the following disclaimer.
+ * 2. Redistributions in binary form must reproduce the above copyright
+ *    notice, this list of conditions and the following disclaimer in
+ *    the documentation and/or other materials provided with the
+ *    distribution.
+ * 3. Neither the name NuttX nor the names of its contributors may be
+ *    used to endorse or promote products derived from this software
+ *    without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
+ * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
+ * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *
+ ************************************************************************************/
+
+#ifndef __ARCH_BOARD_BOARD_H
+#define __ARCH_BOARD_BOARD_H
+
+/************************************************************************************
+ * Included Files
+ ************************************************************************************/
+
+#include <nuttx/config.h>
+#ifndef __ASSEMBLY__
+# include <stdint.h>
+#endif
+#include <stm32.h>
+
+/************************************************************************************
+ * Definitions
+ ************************************************************************************/
+
+/* Clocking *************************************************************************/
+
+/* On-board crystal frequency is 24MHz (HSE) */
+
+#define STM32_BOARD_XTAL        16000000ul
+
+/* Use the HSE output as the system clock */
+
+#define STM32_SYSCLK_SW         RCC_CFGR_SW_HSE
+#define STM32_SYSCLK_SWS        RCC_CFGR_SWS_HSE
+#define STM32_SYSCLK_FREQUENCY  STM32_BOARD_XTAL
+
+/* AHB clock (HCLK) is SYSCLK (24MHz) */
+
+#define STM32_RCC_CFGR_HPRE     RCC_CFGR_HPRE_SYSCLK
+#define STM32_HCLK_FREQUENCY    STM32_SYSCLK_FREQUENCY
+#define STM32_BOARD_HCLK        STM32_HCLK_FREQUENCY    /* same as above, to satisfy compiler */
+
+/* APB2 clock (PCLK2) is HCLK (24MHz) */
+
+#define STM32_RCC_CFGR_PPRE2    RCC_CFGR_PPRE2_HCLK
+#define STM32_PCLK2_FREQUENCY   STM32_HCLK_FREQUENCY
+#define STM32_APB2_CLKIN        (STM32_PCLK2_FREQUENCY)   /* Timers 2-4 */
+
+/* APB2 timer 1 will receive PCLK2. */
+
+#define STM32_APB2_TIM1_CLKIN   (STM32_PCLK2_FREQUENCY)
+#define STM32_APB2_TIM15_CLKIN  (STM32_PCLK2_FREQUENCY)
+#define STM32_APB2_TIM16_CLKIN  (STM32_PCLK2_FREQUENCY)
+#define STM32_APB2_TIM17_CLKIN  (STM32_PCLK2_FREQUENCY)
+
+/* APB1 clock (PCLK1) is HCLK (24MHz) */
+
+#define STM32_RCC_CFGR_PPRE1    RCC_CFGR_PPRE1_HCLK
+#define STM32_PCLK1_FREQUENCY   (STM32_HCLK_FREQUENCY)
+
+/* All timers run off PCLK */
+
+
+#define STM32_APB1_TIM2_CLKIN   (STM32_PCLK1_FREQUENCY)
+#define STM32_APB1_TIM3_CLKIN   (STM32_PCLK1_FREQUENCY)
+#define STM32_APB1_TIM4_CLKIN   (STM32_PCLK1_FREQUENCY)
+#define STM32_APB1_TIM5_CLKIN   (STM32_PCLK1_FREQUENCY)
+#define STM32_APB1_TIM6_CLKIN   (STM32_PCLK1_FREQUENCY)
+#define STM32_APB1_TIM7_CLKIN   (STM32_PCLK1_FREQUENCY)
+#define STM32_APB1_TIM12_CLKIN  (STM32_PCLK1_FREQUENCY)
+#define STM32_APB1_TIM13_CLKIN  (STM32_PCLK1_FREQUENCY)
+#define STM32_APB1_TIM14_CLKIN  (STM32_PCLK1_FREQUENCY)
+
+/* Timer Frequencies, if APBx is set to 1, frequency is same to APBx
+ * otherwise frequency is 2xAPBx.
+ * Note: TIM1, 15-17 are on APB2, others on APB1
+ */
+
+#define BOARD_TIM1_FREQUENCY    STM32_APB2_TIM1_CLKIN
+#define BOARD_TIM2_FREQUENCY    STM32_APB1_TIM2_CLKIN
+#define BOARD_TIM3_FREQUENCY    STM32_APB1_TIM3_CLKIN
+#define BOARD_TIM4_FREQUENCY    STM32_APB1_TIM4_CLKIN
+#define BOARD_TIM5_FREQUENCY    STM32_APB1_TIM5_CLKIN
+#define BOARD_TIM6_FREQUENCY    STM32_APB1_TIM6_CLKIN
+#define BOARD_TIM7_FREQUENCY    STM32_APB1_TIM7_CLKIN
+#define BOARD_TIM12_FREQUENCY   STM32_APB1_TIM12_CLKIN
+#define BOARD_TIM13_FREQUENCY   STM32_APB1_TIM13_CLKIN
+#define BOARD_TIM14_FREQUENCY   STM32_APB1_TIM14_CLKIN
+#define BOARD_TIM15_FREQUENCY   STM32_APB2_TIM15_CLKIN
+#define BOARD_TIM16_FREQUENCY   STM32_APB2_TIM16_CLKIN
+#define BOARD_TIM17_FREQUENCY   STM32_APB2_TIM17_CLKIN
+
+
+/*
+ * Some of the USART pins are not available; override the GPIO
+ * definitions with an invalid pin configuration.
+ */
+#undef GPIO_USART2_CTS
+#define GPIO_USART2_CTS	0xffffffff
+#undef GPIO_USART2_RTS
+#define GPIO_USART2_RTS	0xffffffff
+#undef GPIO_USART2_CK
+#define GPIO_USART2_CK 	0xffffffff
+#undef GPIO_USART3_CK
+#define GPIO_USART3_CK 	0xffffffff
+#undef GPIO_USART3_CTS
+#define GPIO_USART3_CTS	0xffffffff
+#undef GPIO_USART3_RTS
+#define GPIO_USART3_RTS	0xffffffff
+
+#endif  /* __ARCH_BOARD_BOARD_H */
@@ -0,0 +1,61 @@
+#
+# This file is autogenerated: PLEASE DO NOT EDIT IT.
+#
+# You can use "make menuconfig" to make any modifications to the installed .config file.
+# You can then do "make savedefconfig" to generate a new defconfig file that includes your
+# modifications.
+#
+# CONFIG_DEV_NULL is not set
+CONFIG_ARCH="arm"
+CONFIG_ARCH_BOARD_CUSTOM=y
+CONFIG_ARCH_BOARD_CUSTOM_DIR="../../../../boards/modalai/voxl2-io/nuttx-config"
+CONFIG_ARCH_BOARD_CUSTOM_DIR_RELPATH=y
+CONFIG_ARCH_BOARD_CUSTOM_NAME="px4"
+CONFIG_ARCH_CHIP="stm32"
+CONFIG_ARCH_CHIP_STM32=y
+CONFIG_ARCH_CHIP_STM32F100C8=y
+CONFIG_ARMV7M_USEBASEPRI=y
+CONFIG_ARM_MPU_EARLY_RESET=y
+CONFIG_BOARD_LOOPSPERMSEC=2000
+CONFIG_DEBUG_FULLOPT=y
+CONFIG_DEBUG_SYMBOLS=y
+CONFIG_DEFAULT_SMALL=y
+CONFIG_DISABLE_MOUNTPOINT=y
+CONFIG_FDCLONE_DISABLE=y
+CONFIG_FDCLONE_STDIO=y
+CONFIG_HAVE_CXX=y
+CONFIG_HAVE_CXXINITIALIZE=y
+CONFIG_IDLETHREAD_STACKSIZE=316
+CONFIG_INIT_ENTRYPOINT="user_start"
+CONFIG_INIT_STACKSIZE=1100
+CONFIG_MM_FILL_ALLOCATIONS=y
+CONFIG_MM_SMALL=y
+CONFIG_NAME_MAX=12
+CONFIG_PREALLOC_TIMERS=0
+CONFIG_RAM_SIZE=8192
+CONFIG_RAM_START=0x20000000
+CONFIG_RAW_BINARY=y
+CONFIG_SERIAL_TERMIOS=y
+CONFIG_STACK_COLORATION=y
+CONFIG_START_DAY=30
+CONFIG_START_MONTH=11
+CONFIG_STDIO_DISABLE_BUFFERING=y
+CONFIG_STM32_ADC1=y
+CONFIG_STM32_DISABLE_IDLE_SLEEP_DURING_DEBUG=y
+CONFIG_STM32_DMA1=y
+CONFIG_STM32_JTAG_SW_ENABLE=y
+CONFIG_STM32_USART1=y
+CONFIG_STM32_USART2=y
+CONFIG_STM32_USART3=y
+CONFIG_STM32_USART_SINGLEWIRE=y
+CONFIG_TASK_NAME_SIZE=0
+CONFIG_USART1_RXBUFSIZE=64
+CONFIG_USART1_RXDMA=y
+CONFIG_USART1_SERIAL_CONSOLE=y
+CONFIG_USART1_TXBUFSIZE=32
+CONFIG_USART2_RXBUFSIZE=64
+CONFIG_USART2_TXBUFSIZE=64
+CONFIG_USART3_RXBUFSIZE=64
+CONFIG_USART3_RXDMA=y
+CONFIG_USART3_TXBUFSIZE=64
+CONFIG_USEC_PER_TICK=1000
@@ -0,0 +1,131 @@
+/****************************************************************************
+ * configs/px4io-v2/scripts/ld.script
+ *
+ *   Copyright (C) 2009, 2011 Gregory Nutt. All rights reserved.
+ *   Author: Gregory Nutt <spudmonkey@racsa.co.cr>
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright
+ *    notice, this list of conditions and the following disclaimer.
+ * 2. Redistributions in binary form must reproduce the above copyright
+ *    notice, this list of conditions and the following disclaimer in
+ *    the documentation and/or other materials provided with the
+ *    distribution.
+ * 3. Neither the name NuttX nor the names of its contributors may be
+ *    used to endorse or promote products derived from this software
+ *    without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
+ * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
+ * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *
+ ****************************************************************************/
+
+/* The STM32F100C8 has 64Kb of FLASH beginning at address 0x0800:0000 and
+ * 8Kb of SRAM beginning at address 0x2000:0000.  When booting from FLASH,
+ * FLASH memory is aliased to address 0x0000:0000 where the code expects to
+ * begin execution by jumping to the entry point in the 0x0800:0000 address
+ * range.
+ */
+
+MEMORY
+{
+    flash (rx) : ORIGIN = 0x08001000, LENGTH = 60K
+    sram (rwx) : ORIGIN = 0x20000000, LENGTH = 8K
+}
+
+OUTPUT_ARCH(arm)
+ENTRY(__start)		/* treat __start as the anchor for dead code stripping */
+EXTERN(_vectors)	/* force the vectors to be included in the output */
+
+/*
+ * Ensure that abort() is present in the final object.  The exception handling
+ * code pulled in by libgcc.a requires it (and that code cannot be easily avoided).
+ */
+EXTERN(abort)
+
+SECTIONS
+{
+	.text : {
+		_stext = ABSOLUTE(.);
+		*(.vectors)
+		*(.text .text.*)
+		*(.fixup)
+		*(.gnu.warning)
+		*(.rodata .rodata.*)
+		*(.gnu.linkonce.t.*)
+		*(.glue_7)
+		*(.glue_7t)
+		*(.got)
+		*(.gcc_except_table)
+		*(.gnu.linkonce.r.*)
+		_etext = ABSOLUTE(.);
+	} > flash
+
+	/*
+	 * Init functions (static constructors and the like)
+	 */
+        .init_section : {
+                _sinit = ABSOLUTE(.);
+                KEEP(*(.init_array .init_array.*))
+                _einit = ABSOLUTE(.);
+        } > flash
+
+	.ARM.extab : {
+		*(.ARM.extab*)
+	} > flash
+
+	__exidx_start = ABSOLUTE(.);
+	.ARM.exidx : {
+		*(.ARM.exidx*)
+	} > flash
+	__exidx_end = ABSOLUTE(.);
+
+	_eronly = ABSOLUTE(.);
+
+	/* The STM32F100CB has 8Kb of SRAM beginning at the following address */
+
+	.data : {
+		_sdata = ABSOLUTE(.);
+		*(.data .data.*)
+		*(.gnu.linkonce.d.*)
+		CONSTRUCTORS
+		_edata = ABSOLUTE(.);
+		. = ALIGN(4);
+	} > sram AT > flash
+
+	.bss : {
+		_sbss = ABSOLUTE(.);
+		*(.bss .bss.*)
+		*(.gnu.linkonce.b.*)
+		*(COMMON)
+		. = ALIGN(4);
+		_ebss = ABSOLUTE(.);
+	} > sram
+
+	/* Stabs debugging sections. */
+	.stab 0 : { *(.stab) }
+	.stabstr 0 : { *(.stabstr) }
+	.stab.excl 0 : { *(.stab.excl) }
+	.stab.exclstr 0 : { *(.stab.exclstr) }
+	.stab.index 0 : { *(.stab.index) }
+	.stab.indexstr 0 : { *(.stab.indexstr) }
+	.comment 0 : { *(.comment) }
+	.debug_abbrev 0 : { *(.debug_abbrev) }
+	.debug_info 0 : { *(.debug_info) }
+	.debug_line 0 : { *(.debug_line) }
+	.debug_pubnames 0 : { *(.debug_pubnames) }
+	.debug_aranges 0 : { *(.debug_aranges) }
+}
@@ -0,0 +1,42 @@
+############################################################################
+#
+#   Copyright (c) 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.
+#
+############################################################################
+
+add_library(drivers_board
+	init.c
+	timer_config.cpp
+)
+
+target_link_libraries(drivers_board
+	PRIVATE
+		nuttx_arch
+)
@@ -0,0 +1,157 @@
+/****************************************************************************
+ *
+ *   Copyright (c) 2013, 2014 PX4 Development Team. All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright
+ *    notice, this list of conditions and the following disclaimer.
+ * 2. Redistributions in binary form must reproduce the above copyright
+ *    notice, this list of conditions and the following disclaimer in
+ *    the documentation and/or other materials provided with the
+ *    distribution.
+ * 3. Neither the name PX4 nor the names of its contributors may be
+ *    used to endorse or promote products derived from this software
+ *    without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
+ * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
+ * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *
+ ****************************************************************************/
+
+/**
+ * @file board_config.h
+ *
+ * PX4IOV2 internal definitions
+ */
+
+#pragma once
+
+/******************************************************************************
+ * Included Files
+ ******************************************************************************/
+
+#include <px4_platform_common/px4_config.h>
+#include <nuttx/compiler.h>
+#include <stdint.h>
+
+/******************************************************************************
+ * Definitions
+ ******************************************************************************/
+/* Configuration **************************************************************/
+
+/******************************************************************************
+ * Serial
+ ******************************************************************************/
+#define PX4FMU_SERIAL_BASE	STM32_USART2_BASE
+#define PX4FMU_SERIAL_VECTOR	STM32_IRQ_USART2
+#define PX4FMU_SERIAL_TX_GPIO	GPIO_USART2_TX
+#define PX4FMU_SERIAL_RX_GPIO	GPIO_USART2_RX
+#define PX4FMU_SERIAL_TX_DMA	DMACHAN_USART2_TX
+#define PX4FMU_SERIAL_RX_DMA	DMACHAN_USART2_RX
+#define PX4FMU_SERIAL_CLOCK	STM32_PCLK1_FREQUENCY
+#define PX4FMU_SERIAL_BITRATE	921600
+
+/******************************************************************************
+ * GPIOS
+ ******************************************************************************/
+
+/* LEDS  **********************************************************************/
+
+#define GPIO_LED_BLUE (GPIO_OUTPUT|GPIO_CNF_OUTOD|GPIO_MODE_50MHz|GPIO_OUTPUT_SET|GPIO_PORTB|GPIO_PIN14)
+#define GPIO_LED_AMBER (GPIO_OUTPUT|GPIO_CNF_OUTOD|GPIO_MODE_50MHz|GPIO_OUTPUT_SET|GPIO_PORTB|GPIO_PIN15)
+#define GPIO_LED_SAFETY (GPIO_OUTPUT|GPIO_CNF_OUTOD|GPIO_MODE_50MHz|GPIO_OUTPUT_SET|GPIO_PORTB|GPIO_PIN13)
+
+#define LED_BLUE(on_true)   stm32_gpiowrite(GPIO_LED_BLUE, !(on_true))
+#define LED_AMBER(on_true)  stm32_gpiowrite(GPIO_LED_AMBER, !(on_true))
+#define LED_SAFETY(on_true) stm32_gpiowrite(GPIO_LED_SAFETY, !(on_true))
+
+//#define GPIO_LED4 (GPIO_OUTPUT|GPIO_CNF_OUTOD|GPIO_MODE_50MHz|GPIO_OUTPUT_SET|GPIO_PORTA|GPIO_PIN11)
+
+//#define GPIO_HEATER_OFF 0 // (GPIO_OUTPUT|GPIO_CNF_OUTPP|GPIO_MODE_50MHz|GPIO_OUTPUT_SET|GPIO_PORTB|GPIO_PIN14)
+
+#define GPIO_PC14 (GPIO_INPUT|GPIO_CNF_INFLOAT|GPIO_MODE_INPUT|GPIO_PORTC|GPIO_PIN14)
+#define GPIO_PC15 (GPIO_INPUT|GPIO_CNF_INFLOAT|GPIO_MODE_INPUT|GPIO_PORTC|GPIO_PIN15)
+
+#define GPIO_SENSE_PC14_DN (GPIO_INPUT|GPIO_CNF_INPULLDWN|GPIO_MODE_INPUT|GPIO_PORTC|GPIO_PIN14)
+#define GPIO_SENSE_PC15_UP (GPIO_INPUT|GPIO_CNF_INPULLUP|GPIO_MODE_INPUT|GPIO_PORTC|GPIO_PIN15)
+# define SENSE_PH1 0b10 /* Floating pulled as set */
+# define SENSE_PH2 0b01 /* Driven as tied */
+
+#define GPIO_USART1_RX_SPEKTRUM		(GPIO_OUTPUT|GPIO_CNF_OUTPP|GPIO_MODE_50MHz|GPIO_OUTPUT_SET|GPIO_PORTA|GPIO_PIN10)
+
+/* Safety switch button *******************************************************/
+
+/* CONNECTED TO TP8 - pulled down via SW */
+
+#define GPIO_BTN_SAFETY (GPIO_INPUT|GPIO_CNF_INPULLDWN|GPIO_MODE_INPUT|GPIO_PORTB|GPIO_PIN5)
+
+/* Power switch controls ******************************************************/
+
+#define GPIO_SPEKTRUM_PWR_EN         (GPIO_OUTPUT|GPIO_CNF_OUTPP|GPIO_MODE_50MHz|GPIO_OUTPUT_SET|GPIO_PORTC|GPIO_PIN13)
+#define SPEKTRUM_POWER(_on_true)     px4_arch_gpiowrite(GPIO_SPEKTRUM_PWR_EN, (_on_true))
+
+#define SPEKTRUM_OUT(_one_true)      px4_arch_gpiowrite(GPIO_USART1_RX_SPEKTRUM, (_one_true))
+#define SPEKTRUM_RX_AS_UART()        px4_arch_configgpio(GPIO_USART1_RX)
+#define SPEKTRUM_RX_AS_GPIO_OUTPUT() px4_arch_configgpio(GPIO_USART1_RX_SPEKTRUM)
+
+#define GPIO_SERVO_FAULT_DETECT 0 // (GPIO_INPUT|GPIO_CNF_INPULLUP|GPIO_MODE_INPUT|GPIO_PORTA|GPIO_PIN15)
+
+/* Analog inputs **************************************************************/
+
+/* PWM pins  **************************************************************/
+
+#define GPIO_PWM1 (GPIO_OUTPUT|GPIO_CNF_OUTPP|GPIO_MODE_50MHz|GPIO_OUTPUT_CLEAR|GPIO_PORTA|GPIO_PIN0)
+#define GPIO_PWM2 (GPIO_OUTPUT|GPIO_CNF_OUTPP|GPIO_MODE_50MHz|GPIO_OUTPUT_CLEAR|GPIO_PORTA|GPIO_PIN1)
+#define GPIO_PWM3 (GPIO_OUTPUT|GPIO_CNF_OUTPP|GPIO_MODE_50MHz|GPIO_OUTPUT_CLEAR|GPIO_PORTB|GPIO_PIN8)
+#define GPIO_PWM4 (GPIO_OUTPUT|GPIO_CNF_OUTPP|GPIO_MODE_50MHz|GPIO_OUTPUT_CLEAR|GPIO_PORTB|GPIO_PIN9)
+#define GPIO_PWM5 (GPIO_OUTPUT|GPIO_CNF_OUTPP|GPIO_MODE_50MHz|GPIO_OUTPUT_CLEAR|GPIO_PORTA|GPIO_PIN6)
+#define GPIO_PWM6 (GPIO_OUTPUT|GPIO_CNF_OUTPP|GPIO_MODE_50MHz|GPIO_OUTPUT_CLEAR|GPIO_PORTA|GPIO_PIN7)
+#define GPIO_PWM7 (GPIO_OUTPUT|GPIO_CNF_OUTPP|GPIO_MODE_50MHz|GPIO_OUTPUT_CLEAR|GPIO_PORTB|GPIO_PIN0)
+#define GPIO_PWM8 (GPIO_OUTPUT|GPIO_CNF_OUTPP|GPIO_MODE_50MHz|GPIO_OUTPUT_CLEAR|GPIO_PORTB|GPIO_PIN1)
+
+#define DIRECT_PWM_OUTPUT_CHANNELS 8
+#define BOARD_HAS_NO_CAPTURE
+
+/* SBUS pins  *************************************************************/
+
+/* XXX these should be UART pins */
+#define GPIO_SBUS_INPUT   (GPIO_INPUT|GPIO_CNF_INFLOAT|GPIO_MODE_INPUT|GPIO_PORTB|GPIO_PIN11)
+#define GPIO_SBUS_OUTPUT  (GPIO_OUTPUT|GPIO_CNF_OUTPP|GPIO_MODE_50MHz|GPIO_OUTPUT_CLEAR|GPIO_PORTB|GPIO_PIN10)
+#define GPIO_SBUS_OENABLE 0 // (GPIO_OUTPUT|GPIO_CNF_OUTPP|GPIO_MODE_50MHz|GPIO_OUTPUT_SET|GPIO_PORTB|GPIO_PIN4)
+
+/*
+ * High-resolution timer
+ */
+#define HRT_TIMER		1	/* use timer1 for the HRT */
+#define HRT_TIMER_CHANNEL	2	/* use capture/compare channel 2 */
+#define HRT_PPM_CHANNEL		1	/* use capture/compare channel 1 PA8 */
+#define GPIO_PPM_IN		(GPIO_ALT|GPIO_CNF_INPULLUP|GPIO_PORTA|GPIO_PIN8)
+
+/* LED definitions ******************************************************************/
+/* PX4 has two LEDs that we will encode as: */
+
+#define LED_STARTED       0  /* LED? */
+#define LED_HEAPALLOCATE  1  /* LED? */
+#define LED_IRQSENABLED   2  /* LED? + LED? */
+#define LED_STACKCREATED  3  /* LED? */
+#define LED_INIRQ         4  /* LED? + LED? */
+#define LED_SIGNAL        5  /* LED? + LED? */
+#define LED_ASSERTION     6  /* LED? + LED? + LED? */
+#define LED_PANIC         7  /* N/C  + N/C  + N/C + LED? */
+
+#define BOARD_NUM_IO_TIMERS 3
+
+#include <px4_platform_common/board_common.h>
@@ -0,0 +1,133 @@
+/****************************************************************************
+ *
+ *   Copyright (C) 2012 PX4 Development Team. All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright
+ *    notice, this list of conditions and the following disclaimer.
+ * 2. Redistributions in binary form must reproduce the above copyright
+ *    notice, this list of conditions and the following disclaimer in
+ *    the documentation and/or other materials provided with the
+ *    distribution.
+ * 3. Neither the name PX4 nor the names of its contributors may be
+ *    used to endorse or promote products derived from this software
+ *    without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
+ * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
+ * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *
+ ****************************************************************************/
+
+/**
+ * @file init.c
+ *
+ * PX4FMU-specific early startup code.  This file implements the
+ * stm32_boardinitialize() function that is called during cpu startup.
+ *
+ * Code here is run before the rcS script is invoked; it should start required
+ * subsystems and perform board-specific initialization.
+ */
+
+/****************************************************************************
+ * Included Files
+ ****************************************************************************/
+
+#include <px4_platform_common/px4_config.h>
+
+#include <stdbool.h>
+#include <stdio.h>
+#include <debug.h>
+#include <errno.h>
+#include <syslog.h>
+
+#include <nuttx/board.h>
+
+#include <stm32.h>
+#include "board_config.h"
+
+#include <arch/board/board.h>
+
+/****************************************************************************
+ * Pre-Processor Definitions
+ ****************************************************************************/
+
+/****************************************************************************
+ * Protected Functions
+ ****************************************************************************/
+
+/****************************************************************************
+ * Public Functions
+ ****************************************************************************/
+
+/************************************************************************************
+ * Name: stm32_boardinitialize
+ *
+ * Description:
+ *   All STM32 architectures must provide the following entry point.  This entry point
+ *   is called early in the intitialization -- after all memory has been configured
+ *   and mapped but before any devices have been initialized.
+ *
+ ************************************************************************************/
+
+__EXPORT void stm32_boardinitialize(void)
+{
+	/* configure GPIOs */
+
+	/* Set up for sensing HW */
+	stm32_configgpio(GPIO_SENSE_PC14_DN);
+	stm32_configgpio(GPIO_SENSE_PC15_UP);
+
+	/* LEDS - default to off */
+	stm32_configgpio(GPIO_LED_BLUE);
+	stm32_configgpio(GPIO_LED_AMBER);
+	stm32_configgpio(GPIO_LED_SAFETY);
+
+	stm32_configgpio(GPIO_PC14);
+	stm32_configgpio(GPIO_PC15);
+
+
+	stm32_configgpio(GPIO_BTN_SAFETY);
+
+	/* spektrum power enable is active high - enable it by default */
+	stm32_configgpio(GPIO_SPEKTRUM_PWR_EN);
+
+	stm32_configgpio(GPIO_SBUS_INPUT); /* xxx alternate function */
+	stm32_configgpio(GPIO_SBUS_OUTPUT);
+
+	stm32_gpiowrite(GPIO_PWM1, true);
+	stm32_configgpio(GPIO_PWM1);
+
+	stm32_gpiowrite(GPIO_PWM2, true);
+	stm32_configgpio(GPIO_PWM2);
+
+	stm32_gpiowrite(GPIO_PWM3, true);
+	stm32_configgpio(GPIO_PWM3);
+
+	stm32_gpiowrite(GPIO_PWM4, true);
+	stm32_configgpio(GPIO_PWM4);
+
+	stm32_gpiowrite(GPIO_PWM5, true);
+	stm32_configgpio(GPIO_PWM5);
+
+	stm32_gpiowrite(GPIO_PWM6, true);
+	stm32_configgpio(GPIO_PWM6);
+
+	stm32_gpiowrite(GPIO_PWM7, true);
+	stm32_configgpio(GPIO_PWM7);
+
+	stm32_gpiowrite(GPIO_PWM8, true);
+	stm32_configgpio(GPIO_PWM8);
+}
@@ -0,0 +1,54 @@
+/****************************************************************************
+ *
+ *  Copyright (C) 2012 PX4 Development Team. All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright
+ *    notice, this list of conditions and the following disclaimer.
+ * 2. Redistributions in binary form must reproduce the above copyright
+ *    notice, this list of conditions and the following disclaimer in
+ *    the documentation and/or other materials provided with the
+ *    distribution.
+ * 3. Neither the name PX4 nor the names of its contributors may be
+ *    used to endorse or promote products derived from this software
+ *    without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
+ * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
+ * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *
+ ****************************************************************************/
+
+#include <px4_arch/io_timer_hw_description.h>
+
+constexpr io_timers_t io_timers[MAX_IO_TIMERS] = {
+	initIOTimer(Timer::Timer2),
+	initIOTimer(Timer::Timer3),
+	initIOTimer(Timer::Timer4),
+};
+
+constexpr timer_io_channels_t timer_io_channels[MAX_TIMER_IO_CHANNELS] = {
+	initIOTimerChannel(io_timers, {Timer::Timer2, Timer::Channel1}, {GPIO::PortA, GPIO::Pin0}),
+	initIOTimerChannel(io_timers, {Timer::Timer2, Timer::Channel2}, {GPIO::PortA, GPIO::Pin1}),
+	initIOTimerChannel(io_timers, {Timer::Timer4, Timer::Channel3}, {GPIO::PortB, GPIO::Pin8}),
+	initIOTimerChannel(io_timers, {Timer::Timer4, Timer::Channel4}, {GPIO::PortB, GPIO::Pin9}),
+	initIOTimerChannel(io_timers, {Timer::Timer3, Timer::Channel1}, {GPIO::PortA, GPIO::Pin6}),
+	initIOTimerChannel(io_timers, {Timer::Timer3, Timer::Channel2}, {GPIO::PortA, GPIO::Pin7}),
+	initIOTimerChannel(io_timers, {Timer::Timer3, Timer::Channel3}, {GPIO::PortB, GPIO::Pin0}),
+	initIOTimerChannel(io_timers, {Timer::Timer3, Timer::Channel4}, {GPIO::PortB, GPIO::Pin1}),
+};
+
+constexpr io_timers_channel_mapping_t io_timers_channel_mapping =
+	initIOTimerChannelMapping(io_timers, timer_io_channels);
@@ -1,26 +1,37 @@
 CONFIG_PLATFORM_QURT=y
 CONFIG_BOARD_TOOLCHAIN="qurt"
-CONFIG_DRIVERS_ACTUATORS_MODAL_IO=y
-CONFIG_DRIVERS_BAROMETER_INVENSENSE_ICP101XX=y
-CONFIG_DRIVERS_GPS=y
-CONFIG_DRIVERS_LIGHTS_RGBLED_NCP5623C=y
-CONFIG_DRIVERS_MAGNETOMETER_ISENTEK_IST8310=y
-CONFIG_DRIVERS_MAGNETOMETER_QMC5883L=y
-CONFIG_DRIVERS_POWER_MONITOR_VOXLPM=y
-CONFIG_DRIVERS_QSHELL_QURT=y
-CONFIG_DRIVERS_RC_CRSF_RC=y
-CONFIG_MODULES_COMMANDER=y
-CONFIG_MODULES_CONTROL_ALLOCATOR=y
-CONFIG_MODULES_EKF2=y
-CONFIG_MODULES_LAND_DETECTOR=y
-CONFIG_MODULES_MANUAL_CONTROL=y
-CONFIG_MODULES_MC_ATT_CONTROL=y
-CONFIG_MODULES_MC_AUTOTUNE_ATTITUDE_CONTROL=y
-CONFIG_MODULES_MC_HOVER_THRUST_ESTIMATOR=y
-CONFIG_MODULES_MC_POS_CONTROL=y
-CONFIG_MODULES_MC_RATE_CONTROL=y
+CONFIG_BOARD_ROOTFSDIR="/"
 CONFIG_MODULES_MUORB_SLPI=y
-CONFIG_MODULES_RC_UPDATE=y
-CONFIG_MODULES_SENSORS=y
 CONFIG_SYSTEMCMDS_UORB=y
 CONFIG_ORB_COMMUNICATOR=y
+CONFIG_DRIVERS_POWER_MONITOR_VOXLPM=y
+CONFIG_DRIVERS_QSHELL_QURT=y
+CONFIG_DRIVERS_ACTUATORS_VOXL_ESC=y
+CONFIG_DRIVERS_VOXL2_IO=y
+CONFIG_DRIVERS_BAROMETER_INVENSENSE_ICP101XX=y
+CONFIG_DRIVERS_DISTANCE_SENSOR_VL53L0X=y
+CONFIG_DRIVERS_DISTANCE_SENSOR_VL53L1X=y
+# CONFIG_DRIVERS_DISTANCE_SENSOR_LIGHTWARE_LASER_SERIAL=y
+CONFIG_DRIVERS_LIGHTS_RGBLED_NCP5623C=y
+CONFIG_DRIVERS_MAGNETOMETER_ISENTEK_IST8310=y
+CONFIG_DRIVERS_MAGNETOMETER_ISENTEK_IST8308=y
+CONFIG_DRIVERS_MAGNETOMETER_QMC5883L=y
+# CONFIG_DRIVERS_GPS=y
+# CONFIG_DRIVERS_RC_CRSF_RC=y
+CONFIG_MODULES_MANUAL_CONTROL=y
+CONFIG_MODULES_CONTROL_ALLOCATOR=y
+CONFIG_MODULES_SENSORS=y
+CONFIG_MODULES_EKF2=y
+CONFIG_MODULES_MC_POS_CONTROL=y
+CONFIG_MODULES_MC_ATT_CONTROL=y
+CONFIG_MODULES_MC_RATE_CONTROL=y
+CONFIG_MODULES_MC_HOVER_THRUST_ESTIMATOR=y
+CONFIG_MODULES_MC_AUTOTUNE_ATTITUDE_CONTROL=y
+CONFIG_MODULES_LAND_DETECTOR=y
+CONFIG_MODULES_RC_UPDATE=y
+CONFIG_MODULES_COMMANDER=y
+CONFIG_MODULES_LOAD_MON=y
+CONFIG_MODULES_PARAM_SET_SELECTOR=y
+CONFIG_MODULES_FLIGHT_MODE_MANAGER=y
+CONFIG_MODULES_SIMULATION_PWM_OUT_SIM=y
+CONFIG_PARAM_CLIENT=y
@@ -44,7 +44,11 @@ add_library(drivers_board
    )

 # Add custom drivers for SLPI
-add_subdirectory(${PX4_BOARD_DIR}/src/drivers/icm42688p)
+# add_subdirectory(${PX4_BOARD_DIR}/src/drivers/icm42688p)
 add_subdirectory(${PX4_BOARD_DIR}/src/drivers/rc_controller)
 add_subdirectory(${PX4_BOARD_DIR}/src/drivers/mavlink_rc_in)
-add_subdirectory(${PX4_BOARD_DIR}/src/drivers/spektrum_rc)
+# add_subdirectory(${PX4_BOARD_DIR}/src/drivers/spektrum_rc)
+# add_subdirectory(${PX4_BOARD_DIR}/src/drivers/ghst_rc)
+# add_subdirectory(${PX4_BOARD_DIR}/src/drivers/dsp_hitl)
+# add_subdirectory(${PX4_BOARD_DIR}/src/drivers/dsp_sbus)
+add_subdirectory(${PX4_BOARD_DIR}/src/drivers/elrs_led)
@@ -62,4 +62,21 @@
 /*
 *  Default port for the ESC
 */
-#define MODAL_IO_DEFAULT_PORT 	"2"
+#define VOXL_ESC_DEFAULT_PORT 	"2"
+
+/*
+ *  Default port for the GHST RC
+ */
+#define GHST_RC_DEFAULT_PORT 	"7"
+
+/*
+ * Default port for M0065
+*/
+#define VOXL2_IO_DEFAULT_PORT 	"2"
+
+
+/*
+ * M0065 PWM
+ */
+#define DIRECT_PWM_OUTPUT_CHANNELS 4
+#define MAX_IO_TIMERS 3
@@ -0,0 +1,49 @@
+############################################################################
+#
+#   Copyright (c) 2023 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.
+#
+############################################################################
+
+message(STATUS "Mavlink include directory: ${PX4_SOURCE_DIR}/../build/modalai_voxl2_default/mavlink/common")
+
+px4_add_module(
+	MODULE drivers__modalai__dsp_hitl
+	MAIN dsp_hitl
+	INCLUDES
+		${PX4_SOURCE_DIR}/src/drivers/dsp_hitl
+		${PX4_SOURCE_DIR}/build/modalai_voxl2_default/mavlink/common
+	SRCS
+		dsp_hitl.cpp
+	DEPENDS
+		px4_work_queue
+		drivers_accelerometer
+		drivers_gyroscope
+		drivers_magnetometer
+	)
@@ -0,0 +1,41 @@
+############################################################################
+#
+#   Copyright (c) 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.
+#
+############################################################################
+
+px4_add_module(
+	MODULE drivers__dsp_sbus
+	MAIN dsp_sbus
+	COMPILE_FLAGS
+		-Wno-cast-align # TODO: fix and enable
+	SRCS
+		dsp_sbus.cpp
+	)
@@ -0,0 +1,383 @@
+/****************************************************************************
+ *
+ *   Copyright (c) 2012-2020 PX4 Development Team. All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright
+ *    notice, this list of conditions and the following disclaimer.
+ * 2. Redistributions in binary form must reproduce the above copyright
+ *    notice, this list of conditions and the following disclaimer in
+ *    the documentation and/or other materials provided with the
+ *    distribution.
+ * 3. Neither the name PX4 nor the names of its contributors may be
+ *    used to endorse or promote products derived from this software
+ *    without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
+ * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
+ * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *
+ ****************************************************************************/
+
+
+#include <string>
+#include <px4_log.h>
+#include <px4_platform_common/tasks.h>
+#include <px4_platform_common/getopt.h>
+#include <uORB/PublicationMulti.hpp>
+#include <drivers/device/qurt/uart.h>
+#include <drivers/drv_hrt.h>
+#include <uORB/topics/input_rc.h>
+#include <lib/parameters/param.h>
+
+#include "protocol.h"
+
+#define ASYNC_UART_READ_WAIT_US 2000
+
+
+extern "C" { __EXPORT int dsp_sbus_main(int argc, char *argv[]); }
+
+namespace dsp_sbus
+{
+
+std::string _port = "7";
+int _uart_fd = -1;
+IOPacket _packet;
+bool _initialized = false;
+bool _is_running = false;
+uint64_t _rc_last_valid;		///< last valid timestamp
+uint16_t _rc_valid_update_count = 0;
+
+static px4_task_t _task_handle = -1;
+
+uORB::PublicationMulti<input_rc_s> _rc_pub{ORB_ID(input_rc)};
+
+int bus_exchange(IOPacket *packet)
+{
+	int ret = 0;
+	int read_retries = 3;
+	int read_succeeded = 0;
+	int packet_size = sizeof(IOPacket);
+
+	(void) qurt_uart_write(_uart_fd, (const char *) packet, packet_size);
+
+	usleep(100);
+
+	// The UART read on SLPI is via an asynchronous service so specify a timeout
+	// for the return. The driver will poll periodically until the read comes in
+	// so this may block for a while. However, it will timeout if no read comes in.
+	while (read_retries) {
+		ret = qurt_uart_read(_uart_fd, (char *) packet, packet_size, ASYNC_UART_READ_WAIT_US);
+
+		if (ret) {
+			// PX4_INFO("Read %d bytes", ret);
+
+			/* Check CRC */
+			uint8_t crc = packet->crc;
+			packet->crc = 0;
+
+			if (crc != crc_packet(packet)) {
+				PX4_ERR("PX4IO packet CRC error");
+				return -EIO;
+
+			} else if (PKT_CODE(*packet) == PKT_CODE_CORRUPT) {
+				PX4_ERR("PX4IO packet corruption");
+				return -EIO;
+
+			} else {
+				read_succeeded = 1;
+				break;
+			}
+		}
+
+		PX4_ERR("Read attempt %d failed", read_retries);
+		read_retries--;
+	}
+
+
+	if (! read_succeeded) {
+		return -EIO;
+	}
+
+	return 0;
+}
+
+int io_reg_get(uint8_t page, uint8_t offset, uint16_t *values, unsigned num_values)
+{
+	/* range check the transfer */
+	// if (num_values > ((_max_transfer) / sizeof(*values))) {
+	// 	PX4_ERR("io_reg_get: too many registers (%u, max %u)", num_values, _max_transfer / 2);
+	// 	return -1;
+	// }
+
+	// int ret = _interface->read((page << 8) | offset, reinterpret_cast<void *>(values), num_values);
+	int ret = 0;
+
+	_packet.count_code = num_values | PKT_CODE_READ;
+	_packet.page = page;
+	_packet.offset = offset;
+
+	_packet.crc = 0;
+	_packet.crc = crc_packet(&_packet);
+
+	ret = bus_exchange(&_packet);
+
+	if (ret != 0) {
+		// PX4_ERR("px4io io_reg_get(%hhu,%hhu,%u): data error %d", page, offset, num_values, ret);
+		return -1;
+	}
+
+	memcpy(values, &_packet.regs[0], num_values * 2);
+
+	return OK;
+}
+
+uint32_t io_reg_get(uint8_t page, uint8_t offset)
+{
+	uint16_t value;
+
+	if (io_reg_get(page, offset, &value, 1) != OK) {
+		// Registers are only 16 bit so any value over 0xFFFF can signal a fault
+		return 0xFFFFFFFF;
+	}
+
+	return value;
+}
+
+int initialize()
+{
+	if (_initialized) {
+		// Already successfully initialized
+		return 0;
+	}
+
+	if (_uart_fd < 0) {
+		_uart_fd = qurt_uart_open(_port.c_str(), 921600);
+	}
+
+	if (_uart_fd < 0) {
+		PX4_ERR("Open failed in %s", __FUNCTION__);
+		return -1;
+	}
+
+	// Verify connectivity and version number
+	unsigned protocol = io_reg_get(PX4IO_PAGE_CONFIG, PX4IO_P_CONFIG_PROTOCOL_VERSION);
+
+	if (protocol != PX4IO_PROTOCOL_VERSION) {
+		PX4_ERR("dsp_sbus version error: %u", protocol);
+		_uart_fd = -1;
+		return -1;
+	}
+
+	_initialized = true;
+
+	return 0;
+}
+
+void dsp_sbus_task()
+{
+
+	uint16_t status_regs[2] {};
+	input_rc_s	rc_val;
+	const unsigned prolog = (PX4IO_P_RAW_RC_BASE - PX4IO_P_RAW_RC_COUNT);
+	uint16_t rc_regs[input_rc_s::RC_INPUT_MAX_CHANNELS + prolog];
+	uint32_t channel_count = 0;
+
+	_is_running = true;
+
+	while (true) {
+
+		usleep(20000); // Update every 20ms
+
+		memset(&rc_val, 0, sizeof(input_rc_s));
+
+		if (io_reg_get(PX4IO_PAGE_STATUS, PX4IO_P_STATUS_FLAGS, &status_regs[0],
+			       sizeof(status_regs) / sizeof(status_regs[0])) == OK) {
+			// PX4_INFO("dsp_sbus status 0x%.4x", status_regs[0]);
+			// PX4_INFO("dsp_sbus alarms 0x%.4x", status_regs[1]);
+		} else {
+			// PX4_ERR("Failed to read status / alarm registers");
+			continue;
+		}
+
+		/* fetch values from IO */
+
+		// When starting the RC flag will not be okay if the receiver isn't
+		// getting a signal from the transmitter. Once it does, then this flag
+		// will say okay even if later the signal is lost.
+		if (!(status_regs[0] & PX4IO_P_STATUS_FLAGS_RC_OK)) {
+			// PX4_INFO("RC lost status flag set");
+			rc_val.rc_lost = true;
+
+		} else {
+			// PX4_INFO("RC lost status flag is not set");
+			rc_val.rc_lost = false;
+		}
+
+		if (status_regs[0] & PX4IO_P_STATUS_FLAGS_RC_SBUS) {
+			rc_val.input_source = input_rc_s::RC_INPUT_SOURCE_PX4IO_SBUS;
+			// PX4_INFO("Got valid SBUS");
+
+		} else {
+			rc_val.input_source = input_rc_s::RC_INPUT_SOURCE_UNKNOWN;
+			// PX4_INFO("SBUS not valid");
+		}
+
+		rc_val.timestamp = hrt_absolute_time();
+
+		// No point in reading the registers if we haven't acquired a transmitter signal yet
+		if (! rc_val.rc_lost) {
+			if (io_reg_get(PX4IO_PAGE_RAW_RC_INPUT, PX4IO_P_RAW_RC_COUNT, &rc_regs[0],
+				       sizeof(rc_regs) / sizeof(rc_regs[0])) != OK) {
+				// PX4_ERR("Failed to read RC registers");
+				continue;
+				// } else {
+				// 	PX4_INFO("Successfully read RC registers");
+				// 	PX4_INFO("Prolog: %u 0x%.4x 0x%.4x 0x%.4x 0x%.4x 0x%.4x",
+				// 			 rc_regs[0], rc_regs[1], rc_regs[2], rc_regs[3], rc_regs[4], rc_regs[5]);
+			}
+
+			channel_count = rc_regs[PX4IO_P_RAW_RC_COUNT];
+
+			// const uint16_t rc_valid_update_count = rc_regs[PX4IO_P_RAW_FRAME_COUNT];
+			// const bool rc_updated = (rc_valid_update_count != _rc_valid_update_count);
+			//
+			// if (!rc_updated) {
+			// 	PX4_INFO("Didn't get an RC update indication. %u %u", rc_valid_update_count, _rc_valid_update_count);
+			// 	continue;
+			// }
+			//
+			// _rc_valid_update_count = rc_valid_update_count;
+			//
+			// PX4_INFO("Got an RC update indication");
+
+			/* limit the channel count */
+			if (channel_count > input_rc_s::RC_INPUT_MAX_CHANNELS) {
+				// PX4_INFO("Got %u for channel count. Limiting to 18", channel_count);
+				channel_count = input_rc_s::RC_INPUT_MAX_CHANNELS;
+			}
+
+			rc_val.channel_count = channel_count;
+			// PX4_INFO("RC channel count: %u", rc_val.channel_count);
+
+			// rc_val.rc_ppm_frame_length = rc_regs[PX4IO_P_RAW_RC_DATA];
+			rc_val.rc_ppm_frame_length = 0;
+
+			rc_val.rc_failsafe = (rc_regs[PX4IO_P_RAW_RC_FLAGS] & PX4IO_P_RAW_RC_FLAGS_FAILSAFE);
+			// rc_val.rc_lost = !(rc_regs[PX4IO_P_RAW_RC_FLAGS] & PX4IO_P_RAW_RC_FLAGS_RC_OK);
+			rc_val.rc_lost = rc_val.rc_failsafe;
+			rc_val.rc_lost_frame_count = rc_regs[PX4IO_P_RAW_LOST_FRAME_COUNT];
+			rc_val.rc_total_frame_count = rc_regs[PX4IO_P_RAW_FRAME_COUNT];
+
+			if (!rc_val.rc_lost && !rc_val.rc_failsafe) {
+				_rc_last_valid = rc_val.timestamp;
+				rc_val.rssi = rc_regs[PX4IO_P_RAW_RC_NRSSI];
+				rc_val.link_quality = rc_regs[PX4IO_P_RAW_RC_NRSSI];
+
+				/* last thing set are the actual channel values as 16 bit values */
+				for (unsigned i = 0; i < channel_count; i++) {
+					rc_val.values[i] = rc_regs[prolog + i];
+					// PX4_INFO("RC channel %u: %.4u", i, rc_val.values[i]);
+				}
+
+				/* zero the remaining fields */
+				for (unsigned i = channel_count; i < (sizeof(rc_val.values) / sizeof(rc_val.values[0])); i++) {
+					rc_val.values[i] = 0;
+				}
+			}
+
+			rc_val.timestamp_last_signal = _rc_last_valid;
+		}
+
+		_rc_pub.publish(rc_val);
+
+	}
+}
+
+int start(int argc, char *argv[])
+{
+
+	int ch;
+	int myoptind = 1;
+	const char *myoptarg = nullptr;
+
+	while ((ch = px4_getopt(argc, argv, "p:", &myoptind, &myoptarg)) != EOF) {
+		switch (ch) {
+		case 'p':
+			_port = myoptarg;
+			PX4_INFO("Setting port to %s", _port.c_str());
+			break;
+
+		default:
+			break;
+		}
+	}
+
+	if (! _initialized) {
+		if (initialize()) {
+			return -1;
+		}
+	}
+
+	if (_is_running) {
+		PX4_WARN("Already started");
+		return 0;
+	}
+
+	_task_handle = px4_task_spawn_cmd("dsp_sbus_main",
+					  SCHED_DEFAULT,
+					  SCHED_PRIORITY_DEFAULT,
+					  2000,
+					  (px4_main_t) &dsp_sbus_task,
+					  (char *const *)argv);
+
+	if (_task_handle < 0) {
+		PX4_ERR("task start failed");
+		return -1;
+	}
+
+	return 0;
+}
+
+void
+usage()
+{
+	PX4_INFO("Usage: dsp_sbus start [options]");
+	PX4_INFO("Options: -p <number>    uart port number");
+}
+
+} // End namespance dsp_sbus
+
+int dsp_sbus_main(int argc, char *argv[])
+{
+	int myoptind = 1;
+
+	if (argc <= 1) {
+		dsp_sbus::usage();
+		return -1;
+	}
+
+	const char *verb = argv[myoptind];
+
+	if (!strcmp(verb, "start")) {
+		return dsp_sbus::start(argc - 1, argv + 1);
+
+	} else {
+		dsp_sbus::usage();
+		return -1;
+	}
+
+	return 0;
+}
@@ -0,0 +1,405 @@
+/****************************************************************************
+ *
+ *   Copyright (c) 2012-2017 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 <inttypes.h>
+
+/**
+ * @file protocol.h
+ *
+ * PX4IO interface protocol.
+ *
+ * @author Lorenz Meier <lorenz@px4.io>
+ *
+ * Communication is performed via writes to and reads from 16-bit virtual
+ * registers organised into pages of 255 registers each.
+ *
+ * The first two bytes of each write select a page and offset address
+ * respectively. Subsequent reads and writes increment the offset within
+ * the page.
+ *
+ * Some pages are read- or write-only.
+ *
+ * Note that some pages may permit offset values greater than 255, which
+ * can only be achieved by long writes. The offset does not wrap.
+ *
+ * Writes to unimplemented registers are ignored. Reads from unimplemented
+ * registers return undefined values.
+ *
+ * As convention, values that would be floating point in other parts of
+ * the PX4 system are expressed as signed integer values scaled by 10000,
+ * e.g. control values range from -10000..10000.  Use the REG_TO_SIGNED and
+ * SIGNED_TO_REG macros to convert between register representation and
+ * the signed version, and REG_TO_FLOAT/FLOAT_TO_REG to convert to float.
+ *
+ * Note that the implementation of readable pages prefers registers within
+ * readable pages to be densely packed. Page numbers do not need to be
+ * packed.
+ *
+ * Definitions marked [1] are only valid on PX4IOv1 boards. Likewise,
+ * [2] denotes definitions specific to the PX4IOv2 board.
+ */
+
+/* Per C, this is safe for all 2's complement systems */
+#define REG_TO_SIGNED(_reg)	((int16_t)(_reg))
+#define SIGNED_TO_REG(_signed)	((uint16_t)(_signed))
+
+#define REG_TO_FLOAT(_reg)	((float)REG_TO_SIGNED(_reg) / 10000.0f)
+#define FLOAT_TO_REG(_float)	SIGNED_TO_REG((int16_t)floorf((_float + 0.00005f) * 10000.0f))
+
+#define REG_TO_BOOL(_reg) 	((bool)(_reg))
+
+#define PX4IO_PROTOCOL_VERSION		4
+
+/* maximum allowable sizes on this protocol version */
+#define PX4IO_PROTOCOL_MAX_CONTROL_COUNT	8	/**< The protocol does not support more than set here, individual units might support less - see PX4IO_P_CONFIG_CONTROL_COUNT */
+
+/* static configuration page */
+#define PX4IO_PAGE_CONFIG					0
+#define PX4IO_P_CONFIG_PROTOCOL_VERSION		0	/* PX4IO_PROTOCOL_VERSION */
+#define PX4IO_P_CONFIG_HARDWARE_VERSION		1	/* magic numbers TBD */
+#define PX4IO_P_CONFIG_BOOTLOADER_VERSION	2	/* get this how? */
+#define PX4IO_P_CONFIG_MAX_TRANSFER			3	/* maximum I2C transfer size */
+#define PX4IO_P_CONFIG_CONTROL_COUNT		4	/* hardcoded max control count supported */
+#define PX4IO_P_CONFIG_ACTUATOR_COUNT		5	/* hardcoded max actuator output count */
+#define PX4IO_P_CONFIG_RC_INPUT_COUNT		6	/* hardcoded max R/C input count supported */
+#define PX4IO_P_CONFIG_ADC_INPUT_COUNT		7	/* hardcoded max ADC inputs */
+#define PX4IO_P_CONFIG_RELAY_COUNT			8	/* hardcoded # of relay outputs */
+#define PX4IO_MAX_TRANSFER_LEN				64
+
+/* dynamic status page */
+#define PX4IO_PAGE_STATUS		1
+#define PX4IO_P_STATUS_FREEMEM			0
+#define PX4IO_P_STATUS_CPULOAD			1
+
+#define PX4IO_P_STATUS_FLAGS			2	 /* monitoring flags */
+#define PX4IO_P_STATUS_FLAGS_OUTPUTS_ARMED	(1 << 0) /* arm-ok and locally armed */
+#define PX4IO_P_STATUS_FLAGS_OVERRIDE		(1 << 1) /* in manual override */
+#define PX4IO_P_STATUS_FLAGS_RC_OK		(1 << 2) /* RC input is valid */
+#define PX4IO_P_STATUS_FLAGS_RC_PPM		(1 << 3) /* PPM input is valid */
+#define PX4IO_P_STATUS_FLAGS_RC_DSM		(1 << 4) /* DSM input is valid */
+#define PX4IO_P_STATUS_FLAGS_RC_SBUS		(1 << 5) /* SBUS input is valid */
+#define PX4IO_P_STATUS_FLAGS_FMU_OK		(1 << 6) /* controls from FMU are valid */
+#define PX4IO_P_STATUS_FLAGS_RAW_PWM		(1 << 7) /* raw PWM from FMU is bypassing the mixer */
+#define PX4IO_P_STATUS_FLAGS_MIXER_OK		(1 << 8) /* mixer is OK */
+#define PX4IO_P_STATUS_FLAGS_ARM_SYNC		(1 << 9) /* the arming state between IO and FMU is in sync */
+#define PX4IO_P_STATUS_FLAGS_INIT_OK		(1 << 10) /* initialisation of the IO completed without error */
+#define PX4IO_P_STATUS_FLAGS_FAILSAFE		(1 << 11) /* failsafe is active */
+#define PX4IO_P_STATUS_FLAGS_SAFETY_OFF		(1 << 12) /* safety is off */
+#define PX4IO_P_STATUS_FLAGS_FMU_INITIALIZED	(1 << 13) /* FMU was initialized and OK once */
+#define PX4IO_P_STATUS_FLAGS_RC_ST24		(1 << 14) /* ST24 input is valid */
+#define PX4IO_P_STATUS_FLAGS_RC_SUMD		(1 << 15) /* SUMD input is valid */
+
+#define PX4IO_P_STATUS_ALARMS			3	 /* alarm flags - alarms latch, write 1 to a bit to clear it */
+#define PX4IO_P_STATUS_ALARMS_VBATT_LOW		(1 << 0) /* [1] VBatt is very close to regulator dropout */
+#define PX4IO_P_STATUS_ALARMS_TEMPERATURE	(1 << 1) /* board temperature is high */
+#define PX4IO_P_STATUS_ALARMS_SERVO_CURRENT	(1 << 2) /* [1] servo current limit was exceeded */
+#define PX4IO_P_STATUS_ALARMS_ACC_CURRENT	(1 << 3) /* [1] accessory current limit was exceeded */
+#define PX4IO_P_STATUS_ALARMS_FMU_LOST		(1 << 4) /* timed out waiting for controls from FMU */
+#define PX4IO_P_STATUS_ALARMS_RC_LOST		(1 << 5) /* timed out waiting for RC input */
+#define PX4IO_P_STATUS_ALARMS_PWM_ERROR		(1 << 6) /* PWM configuration or output was bad */
+#define PX4IO_P_STATUS_ALARMS_VSERVO_FAULT	(1 << 7) /* [2] VServo was out of the valid range (2.5 - 5.5 V) */
+
+#define PX4IO_P_STATUS_VSERVO			6	/* [2] servo rail voltage in mV */
+#define PX4IO_P_STATUS_VRSSI			7	/* [2] RSSI voltage */
+#define PX4IO_P_STATUS_PRSSI			8	/* [2] RSSI PWM value */
+
+#define PX4IO_P_STATUS_MIXER			9	 /* mixer actuator limit flags */
+
+/* array of post-mix actuator outputs, -10000..10000 */
+#define PX4IO_PAGE_ACTUATORS		2		/* 0..CONFIG_ACTUATOR_COUNT-1 */
+
+/* array of PWM servo output values, microseconds */
+#define PX4IO_PAGE_SERVOS		3		/* 0..CONFIG_ACTUATOR_COUNT-1 */
+
+/* array of raw RC input values, microseconds */
+#define PX4IO_PAGE_RAW_RC_INPUT		4
+#define PX4IO_P_RAW_RC_COUNT			0	/* number of valid channels */
+#define PX4IO_P_RAW_RC_FLAGS			1	/* RC detail status flags */
+#define PX4IO_P_RAW_RC_FLAGS_FRAME_DROP		(1 << 0) /* single frame drop */
+#define PX4IO_P_RAW_RC_FLAGS_FAILSAFE		(1 << 1) /* receiver is in failsafe mode */
+#define PX4IO_P_RAW_RC_FLAGS_RC_DSM11		(1 << 2) /* DSM decoding is 11 bit mode */
+#define PX4IO_P_RAW_RC_FLAGS_MAPPING_OK		(1 << 3) /* Channel mapping is ok */
+#define PX4IO_P_RAW_RC_FLAGS_RC_OK		(1 << 4) /* RC reception ok */
+
+#define PX4IO_P_RAW_RC_NRSSI			2	/* [2] Normalized RSSI value, 0: no reception, 255: perfect reception */
+#define PX4IO_P_RAW_RC_DATA			3	/* [1] + [2] Details about the RC source (PPM frame length, Spektrum protocol type) */
+#define PX4IO_P_RAW_FRAME_COUNT			4	/* Number of total received frames (wrapping counter) */
+#define PX4IO_P_RAW_LOST_FRAME_COUNT		5	/* Number of total dropped frames (wrapping counter) */
+#define PX4IO_P_RAW_RC_BASE			6	/* CONFIG_RC_INPUT_COUNT channels from here */
+
+/* array of scaled RC input values, -10000..10000 */
+#define PX4IO_PAGE_RC_INPUT		5
+#define PX4IO_P_RC_VALID			0	/* bitmask of valid controls */
+#define PX4IO_P_RC_BASE				1	/* CONFIG_RC_INPUT_COUNT controls from here */
+
+/* array of raw ADC values */
+#define PX4IO_PAGE_RAW_ADC_INPUT	6		/* 0..CONFIG_ADC_INPUT_COUNT-1 */
+
+/* PWM servo information */
+#define PX4IO_PAGE_PWM_INFO		7
+#define PX4IO_RATE_MAP_BASE			0	/* 0..CONFIG_ACTUATOR_COUNT bitmaps of PWM rate groups */
+
+/* setup page */
+#define PX4IO_PAGE_SETUP		50
+#define PX4IO_P_SETUP_FEATURES			0
+#define PX4IO_P_SETUP_FEATURES_SBUS1_OUT	(1 << 0) /**< enable S.Bus v1 output */
+#define PX4IO_P_SETUP_FEATURES_SBUS2_OUT	(1 << 1) /**< enable S.Bus v2 output */
+#define PX4IO_P_SETUP_FEATURES_PWM_RSSI		(1 << 2) /**< enable PWM RSSI parsing */
+#define PX4IO_P_SETUP_FEATURES_ADC_RSSI		(1 << 3) /**< enable ADC RSSI parsing */
+
+#define PX4IO_P_SETUP_ARMING			1	 /* arming controls */
+#define PX4IO_P_SETUP_ARMING_IO_ARM_OK		(1 << 0) /* OK to arm the IO side */
+#define PX4IO_P_SETUP_ARMING_FMU_ARMED		(1 << 1) /* FMU is already armed */
+#define PX4IO_P_SETUP_ARMING_FMU_PREARMED	(1 << 2) /* FMU is already prearmed */
+#define PX4IO_P_SETUP_ARMING_MANUAL_OVERRIDE_OK	(1 << 3) /* OK to switch to manual override via override RC channel */
+#define PX4IO_P_SETUP_ARMING_FAILSAFE_CUSTOM	(1 << 4) /* use custom failsafe values, not 0 values of mixer */
+#define PX4IO_P_SETUP_ARMING_INAIR_RESTART_OK	(1 << 5) /* OK to try in-air restart */
+#define PX4IO_P_SETUP_ARMING_ALWAYS_PWM_ENABLE	(1 << 6) /* Output of PWM right after startup enabled to help ESCs initialize and prevent them from beeping */
+#define PX4IO_P_SETUP_ARMING_RC_HANDLING_DISABLED	(1 << 7) /* Disable the IO-internal evaluation of the RC */
+#define PX4IO_P_SETUP_ARMING_LOCKDOWN		(1 << 8) /* If set, the system operates normally, but won't actuate any servos */
+#define PX4IO_P_SETUP_ARMING_FORCE_FAILSAFE	(1 << 9) /* If set, the system will always output the failsafe values */
+#define PX4IO_P_SETUP_ARMING_TERMINATION_FAILSAFE	(1 << 10) /* If set, the system will never return from a failsafe, but remain in failsafe once triggered. */
+#define PX4IO_P_SETUP_ARMING_OVERRIDE_IMMEDIATE	(1 << 11) /* If set then on FMU failure override is immediate. Othewise it waits for the mode switch to go past the override thrshold */
+
+#define PX4IO_P_SETUP_PWM_RATES			2	/* bitmask, 0 = low rate, 1 = high rate */
+#define PX4IO_P_SETUP_PWM_DEFAULTRATE		3	/* 'low' PWM frame output rate in Hz */
+#define PX4IO_P_SETUP_PWM_ALTRATE		4	/* 'high' PWM frame output rate in Hz */
+#define PX4IO_P_SETUP_RELAYS_PAD		5
+
+#define PX4IO_P_SETUP_VSERVO_SCALE		6	/* hardware rev [2] servo voltage correction factor (float) */
+#define PX4IO_P_SETUP_DSM			7	/* DSM bind state */
+enum {							/* DSM bind states */
+	dsm_bind_power_down = 0,
+	dsm_bind_power_up,
+	dsm_bind_set_rx_out,
+	dsm_bind_send_pulses,
+	dsm_bind_reinit_uart
+};
+/* 8 */
+#define PX4IO_P_SETUP_SET_DEBUG			9	/* debug level for IO board */
+
+#define PX4IO_P_SETUP_REBOOT_BL			10	/* reboot IO into bootloader */
+#define PX4IO_REBOOT_BL_MAGIC			14662	/* required argument for reboot (random) */
+
+#define PX4IO_P_SETUP_CRC			11	/* get CRC of IO firmware */
+/* storage space of 12 occupied by CRC */
+#define PX4IO_P_SETUP_FORCE_SAFETY_OFF		12	/* force safety switch into
+                                                           'armed' (PWM enabled) state - this is a non-data write and
+                                                           hence index 12 can safely be used. */
+#define PX4IO_P_SETUP_RC_THR_FAILSAFE_US	13	/**< the throttle failsafe pulse length in microseconds */
+
+#define PX4IO_P_SETUP_FORCE_SAFETY_ON		14	/* force safety switch into 'disarmed' (PWM disabled state) */
+#define PX4IO_FORCE_SAFETY_MAGIC		22027	/* required argument for force safety (random) */
+
+#define PX4IO_P_SETUP_PWM_REVERSE		15	/**< Bitmask to reverse PWM channels 1-8 */
+#define PX4IO_P_SETUP_TRIM_ROLL			16	/**< Roll trim, in actuator units */
+#define PX4IO_P_SETUP_TRIM_PITCH		17	/**< Pitch trim, in actuator units */
+#define PX4IO_P_SETUP_TRIM_YAW			18	/**< Yaw trim, in actuator units */
+#define PX4IO_P_SETUP_SCALE_ROLL		19	/**< Roll scale, in actuator units */
+#define PX4IO_P_SETUP_SCALE_PITCH		20	/**< Pitch scale, in actuator units */
+#define PX4IO_P_SETUP_SCALE_YAW			21	/**< Yaw scale, in actuator units */
+
+#define PX4IO_P_SETUP_SBUS_RATE			22	/**< frame rate of SBUS1 output in Hz */
+
+#define PX4IO_P_SETUP_MOTOR_SLEW_MAX		24 	/**< max motor slew rate */
+
+#define PX4IO_P_SETUP_THR_MDL_FAC 		25	/**< factor for modelling motor control signal output to static thrust relationship */
+
+#define PX4IO_P_SETUP_THERMAL			26	/**< thermal management */
+
+#define PX4IO_P_SETUP_AIRMODE 			27 	/**< air-mode */
+
+#define PX4IO_P_SETUP_ENABLE_FLIGHTTERMINATION	28	/**< flight termination; false if the circuit breaker (CBRK_FLIGHTTERM) is set */
+
+#define PX4IO_THERMAL_IGNORE			UINT16_MAX
+#define PX4IO_THERMAL_OFF			0
+#define PX4IO_THERMAL_FULL			10000
+
+/* autopilot control values, -10000..10000 */
+#define PX4IO_PAGE_CONTROLS			51	/**< actuator control groups, one after the other, 8 wide */
+#define PX4IO_P_CONTROLS_GROUP_0		(PX4IO_PROTOCOL_MAX_CONTROL_COUNT * 0)	/**< 0..PX4IO_PROTOCOL_MAX_CONTROL_COUNT - 1 */
+#define PX4IO_P_CONTROLS_GROUP_1		(PX4IO_PROTOCOL_MAX_CONTROL_COUNT * 1)	/**< 0..PX4IO_PROTOCOL_MAX_CONTROL_COUNT - 1 */
+#define PX4IO_P_CONTROLS_GROUP_2		(PX4IO_PROTOCOL_MAX_CONTROL_COUNT * 2)	/**< 0..PX4IO_PROTOCOL_MAX_CONTROL_COUNT - 1 */
+#define PX4IO_P_CONTROLS_GROUP_3		(PX4IO_PROTOCOL_MAX_CONTROL_COUNT * 3)	/**< 0..PX4IO_PROTOCOL_MAX_CONTROL_COUNT - 1 */
+
+#define PX4IO_P_CONTROLS_GROUP_VALID		64
+#define PX4IO_P_CONTROLS_GROUP_VALID_GROUP0	(1 << 0) /**< group 0 is valid / received */
+#define PX4IO_P_CONTROLS_GROUP_VALID_GROUP1	(1 << 1) /**< group 1 is valid / received */
+#define PX4IO_P_CONTROLS_GROUP_VALID_GROUP2	(1 << 2) /**< group 2 is valid / received */
+#define PX4IO_P_CONTROLS_GROUP_VALID_GROUP3	(1 << 3) /**< group 3 is valid / received */
+
+/* raw text load to the mixer parser - ignores offset */
+#define PX4IO_PAGE_MIXERLOAD			52
+
+/* R/C channel config */
+#define PX4IO_PAGE_RC_CONFIG			53		/**< R/C input configuration */
+#define PX4IO_P_RC_CONFIG_MIN			0		/**< lowest input value */
+#define PX4IO_P_RC_CONFIG_CENTER		1		/**< center input value */
+#define PX4IO_P_RC_CONFIG_MAX			2		/**< highest input value */
+#define PX4IO_P_RC_CONFIG_DEADZONE		3		/**< band around center that is ignored */
+#define PX4IO_P_RC_CONFIG_ASSIGNMENT		4		/**< mapped input value */
+#define PX4IO_P_RC_CONFIG_ASSIGNMENT_MODESWITCH	100		/**< magic value for mode switch */
+#define PX4IO_P_RC_CONFIG_OPTIONS		5		/**< channel options bitmask */
+#define PX4IO_P_RC_CONFIG_OPTIONS_ENABLED	(1 << 0)
+#define PX4IO_P_RC_CONFIG_OPTIONS_REVERSE	(1 << 1)
+#define PX4IO_P_RC_CONFIG_STRIDE		6		/**< spacing between channel config data */
+
+/* PWM output - overrides mixer */
+#define PX4IO_PAGE_DIRECT_PWM			54		/**< 0..CONFIG_ACTUATOR_COUNT-1 */
+
+/* PWM failsafe values - zero disables the output */
+#define PX4IO_PAGE_FAILSAFE_PWM			55		/**< 0..CONFIG_ACTUATOR_COUNT-1 */
+
+/* PWM failsafe values - zero disables the output */
+#define PX4IO_PAGE_SENSORS			56		/**< Sensors connected to PX4IO */
+#define PX4IO_P_SENSORS_ALTITUDE		0		/**< Altitude of an external sensor (HoTT or S.BUS2) */
+
+/* Debug and test page - not used in normal operation */
+#define PX4IO_PAGE_TEST				127
+#define PX4IO_P_TEST_LED			0		/**< set the amber LED on/off */
+
+/* PWM minimum values for certain ESCs */
+#define PX4IO_PAGE_CONTROL_MIN_PWM		106		/**< 0..CONFIG_ACTUATOR_COUNT-1 */
+
+/* PWM maximum values for certain ESCs */
+#define PX4IO_PAGE_CONTROL_MAX_PWM		107		/**< 0..CONFIG_ACTUATOR_COUNT-1 */
+
+/* PWM mtrim values for central position */
+#define PX4IO_PAGE_CONTROL_TRIM_PWM		108		/**< 0..CONFIG_ACTUATOR_COUNT-1 */
+
+/* PWM disarmed values that are active, even when SAFETY_SAFE */
+#define PX4IO_PAGE_DISARMED_PWM		109			/* 0..CONFIG_ACTUATOR_COUNT-1 */
+
+/**
+ * As-needed mixer data upload.
+ *
+ * This message adds text to the mixer text buffer; the text
+ * buffer is drained as the definitions are consumed.
+ */
+#pragma pack(push, 1)
+struct px4io_mixdata {
+	uint16_t	f2i_mixer_magic;
+#define F2I_MIXER_MAGIC		0x6d74
+
+	uint8_t		action;
+#define F2I_MIXER_ACTION_RESET			0
+#define F2I_MIXER_ACTION_APPEND			1
+
+	char		text[0];	/* actual text size may vary */
+};
+#pragma pack(pop)
+
+/**
+ * Serial protocol encapsulation.
+ */
+
+#define PKT_MAX_REGS	32 // by agreement w/FMU
+
+#pragma pack(push, 1)
+struct IOPacket {
+	uint8_t 	count_code;
+	uint8_t 	crc;
+	uint8_t 	page;
+	uint8_t 	offset;
+	uint16_t	regs[PKT_MAX_REGS];
+};
+#pragma pack(pop)
+
+#if (PX4IO_MAX_TRANSFER_LEN > PKT_MAX_REGS * 2)
+#error The max transfer length of the IO protocol must not be larger than the IO packet size
+#endif
+
+#define PKT_CODE_READ		0x00	/* FMU->IO read transaction */
+#define PKT_CODE_WRITE		0x40	/* FMU->IO write transaction */
+#define PKT_CODE_SUCCESS	0x00	/* IO->FMU success reply */
+#define PKT_CODE_CORRUPT	0x40	/* IO->FMU bad packet reply */
+#define PKT_CODE_ERROR		0x80	/* IO->FMU register op error reply */
+
+#define PKT_CODE_MASK		0xc0
+#define PKT_COUNT_MASK		0x3f
+
+#define PKT_COUNT(_p)	((_p).count_code & PKT_COUNT_MASK)
+#define PKT_CODE(_p)	((_p).count_code & PKT_CODE_MASK)
+#define PKT_SIZE(_p)	((size_t)((uint8_t *)&((_p).regs[PKT_COUNT(_p)]) - ((uint8_t *)&(_p))))
+
+static const uint8_t crc8_tab[256] __attribute__((unused)) = {
+	0x00, 0x07, 0x0E, 0x09, 0x1C, 0x1B, 0x12, 0x15,
+	0x38, 0x3F, 0x36, 0x31, 0x24, 0x23, 0x2A, 0x2D,
+	0x70, 0x77, 0x7E, 0x79, 0x6C, 0x6B, 0x62, 0x65,
+	0x48, 0x4F, 0x46, 0x41, 0x54, 0x53, 0x5A, 0x5D,
+	0xE0, 0xE7, 0xEE, 0xE9, 0xFC, 0xFB, 0xF2, 0xF5,
+	0xD8, 0xDF, 0xD6, 0xD1, 0xC4, 0xC3, 0xCA, 0xCD,
+	0x90, 0x97, 0x9E, 0x99, 0x8C, 0x8B, 0x82, 0x85,
+	0xA8, 0xAF, 0xA6, 0xA1, 0xB4, 0xB3, 0xBA, 0xBD,
+	0xC7, 0xC0, 0xC9, 0xCE, 0xDB, 0xDC, 0xD5, 0xD2,
+	0xFF, 0xF8, 0xF1, 0xF6, 0xE3, 0xE4, 0xED, 0xEA,
+	0xB7, 0xB0, 0xB9, 0xBE, 0xAB, 0xAC, 0xA5, 0xA2,
+	0x8F, 0x88, 0x81, 0x86, 0x93, 0x94, 0x9D, 0x9A,
+	0x27, 0x20, 0x29, 0x2E, 0x3B, 0x3C, 0x35, 0x32,
+	0x1F, 0x18, 0x11, 0x16, 0x03, 0x04, 0x0D, 0x0A,
+	0x57, 0x50, 0x59, 0x5E, 0x4B, 0x4C, 0x45, 0x42,
+	0x6F, 0x68, 0x61, 0x66, 0x73, 0x74, 0x7D, 0x7A,
+	0x89, 0x8E, 0x87, 0x80, 0x95, 0x92, 0x9B, 0x9C,
+	0xB1, 0xB6, 0xBF, 0xB8, 0xAD, 0xAA, 0xA3, 0xA4,
+	0xF9, 0xFE, 0xF7, 0xF0, 0xE5, 0xE2, 0xEB, 0xEC,
+	0xC1, 0xC6, 0xCF, 0xC8, 0xDD, 0xDA, 0xD3, 0xD4,
+	0x69, 0x6E, 0x67, 0x60, 0x75, 0x72, 0x7B, 0x7C,
+	0x51, 0x56, 0x5F, 0x58, 0x4D, 0x4A, 0x43, 0x44,
+	0x19, 0x1E, 0x17, 0x10, 0x05, 0x02, 0x0B, 0x0C,
+	0x21, 0x26, 0x2F, 0x28, 0x3D, 0x3A, 0x33, 0x34,
+	0x4E, 0x49, 0x40, 0x47, 0x52, 0x55, 0x5C, 0x5B,
+	0x76, 0x71, 0x78, 0x7F, 0x6A, 0x6D, 0x64, 0x63,
+	0x3E, 0x39, 0x30, 0x37, 0x22, 0x25, 0x2C, 0x2B,
+	0x06, 0x01, 0x08, 0x0F, 0x1A, 0x1D, 0x14, 0x13,
+	0xAE, 0xA9, 0xA0, 0xA7, 0xB2, 0xB5, 0xBC, 0xBB,
+	0x96, 0x91, 0x98, 0x9F, 0x8A, 0x8D, 0x84, 0x83,
+	0xDE, 0xD9, 0xD0, 0xD7, 0xC2, 0xC5, 0xCC, 0xCB,
+	0xE6, 0xE1, 0xE8, 0xEF, 0xFA, 0xFD, 0xF4, 0xF3
+};
+
+static uint8_t crc_packet(struct IOPacket *pkt) __attribute__((unused));
+static uint8_t
+crc_packet(struct IOPacket *pkt)
+{
+	uint8_t *end = (uint8_t *)(&pkt->regs[PKT_COUNT(*pkt)]);
+	uint8_t *p = (uint8_t *)pkt;
+	uint8_t c = 0;
+
+	while (p < end) {
+		c = crc8_tab[c ^ * (p++)];
+	}
+
+	return c;
+}
@@ -0,0 +1,41 @@
+############################################################################
+#
+#   Copyright (c) 2023 ModalAI, Inc. 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__elrs_led
+	MAIN elrs_led
+	COMPILE_FLAGS
+		-Wno-cast-align # TODO: fix and enable
+	SRCS
+		elrs_led.cpp
+	)
@@ -0,0 +1,317 @@
+/****************************************************************************
+ *
+ *   Copyright (c) 2023 ModalAI, Inc. 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 <string>
+#include <px4_log.h>
+#include <px4_platform_common/tasks.h>
+#include <px4_platform_common/getopt.h>
+#include <uORB/Subscription.hpp>
+#include <drivers/device/qurt/uart.h>
+#include <uORB/topics/manual_control_setpoint.h>
+#include <lib/parameters/param.h>
+#include "elrs_led.h"
+
+
+extern "C" { __EXPORT int elrs_led_main(int argc, char *argv[]); }
+
+namespace elrs_led
+{
+
+std::string _port = "7";
+int _uart_fd = -1;
+bool _initialized = false;
+bool _is_running = false;
+static bool _debug = false;
+
+static GENERIC_CRC8 crsf_crc{};
+static LEDState _state = LEDState::DEFAULT;
+static ControllerInput _off = ControllerInput::DEFAULT;
+static ControllerInput _on = ControllerInput::DEFAULT;
+static ControllerInput _ir = ControllerInput::DEFAULT;
+static ControllerInput _cmd = ControllerInput::DEFAULT;
+static ControllerInput _prev_cmd = ControllerInput::DEFAULT;
+static std::map<ControllerInput, std::string> ControllerInputMap{
+	{ControllerInput::DLEFT, "DLEFT"},
+	{ControllerInput::DRIGHT, "DRIGHT"},
+	{ControllerInput::DDOWN, "DDOWN"},
+	{ControllerInput::DUP, "DUP"},
+	{ControllerInput::BACK, "BACK"},
+	{ControllerInput::START, "START"},
+	{ControllerInput::Y, "Y"},
+	{ControllerInput::B, "B"},
+	{ControllerInput::A, "A"},
+	{ControllerInput::X, "X"},
+	{ControllerInput::STICK_RIGHT, "STICK_RIGHT"},
+	{ControllerInput::STICK_LEFT, "STICK_LEFT"},
+	{ControllerInput::BUMPER_RIGHT, "BUMPER_RIGHT"},
+	{ControllerInput::BUMPER_LEFT, "BUMPER_LEFT"},
+	{ControllerInput::DEFAULT, "Unkown"}
+};
+static px4_task_t _task_handle = -1;
+void debug_info(LEDState, uint8_t *);
+void make_packet(LEDState, uint8_t *);
+
+int initialize()
+{
+	if (_initialized) {
+		// Already successfully initialized
+		return 0;
+	}
+
+	if (_uart_fd < 0) {
+		_uart_fd = qurt_uart_open(_port.c_str(), 420000);
+	}
+
+	if (_uart_fd < 0) {
+		PX4_ERR("Open failed in %s", __FUNCTION__);
+		return -1;
+	}
+
+	_initialized = true;
+
+	return 0;
+}
+
+void elrs_led_task()
+{
+
+	PX4_INFO("Starting task for elrs_led");
+
+	int ret = 0;
+	int manual_control_input_fd  = orb_subscribe(ORB_ID(manual_control_input));
+	uint8_t pwmPacket[11] = {0xEC, 0x09, 0x32, 0x70, 0x77, 0x6D, 0x07, 0x75, 0x00, 0x00, 0x00};
+
+	px4_pollfd_struct_t fds[1] = { { .fd = manual_control_input_fd,  .events = POLLIN }	};
+
+	struct manual_control_setpoint_s setpoint_req;
+
+	_is_running = true;
+
+	while (true) {
+		px4_poll(fds, 1, 10000);
+
+		if (fds[0].revents & POLLIN) {
+
+			orb_copy(ORB_ID(manual_control_input), manual_control_input_fd, &setpoint_req);
+
+			_cmd = (ControllerInput)setpoint_req.aux1;
+
+			// skip duplicate cmds
+			if (_cmd == _prev_cmd) {
+				continue;
+			}
+
+			if (_cmd == _off) {
+				_prev_cmd = _cmd;
+				_state = LEDState::OFF;
+				make_packet(_state, pwmPacket);
+				ret = qurt_uart_write(_uart_fd, (char *) &pwmPacket[0], sizeof(pwmPacket));
+
+				if (_debug) {
+					debug_info(_state, pwmPacket);
+				}
+
+			} else if (_cmd == _on) {
+				_prev_cmd = _cmd;
+				_state = LEDState::ON;
+				make_packet(_state, pwmPacket);
+				ret = qurt_uart_write(_uart_fd, (char *) &pwmPacket[0], sizeof(pwmPacket));
+
+				if (_debug) {
+					debug_info(_state, pwmPacket);
+				}
+
+			} else if (_cmd == _ir) {
+				_prev_cmd = _cmd;
+				_state = LEDState::IR;
+				make_packet(_state, pwmPacket);
+				ret = qurt_uart_write(_uart_fd, (char *) &pwmPacket[0], sizeof(pwmPacket));
+
+				if (_debug) {
+					debug_info(_state, pwmPacket);
+				}
+
+			}
+
+		}
+	}
+}
+
+int start(int argc, char *argv[])
+{
+
+	int ch;
+	int myoptind = 1;
+	const char *myoptarg = nullptr;
+
+	while ((ch = px4_getopt(argc, argv, "p:o:l:i:d", &myoptind, &myoptarg)) != EOF) {
+		switch (ch) {
+		case 'p':
+			_port = myoptarg;
+			break;
+
+		case 'o':
+			_off = getKey(ControllerInputMap, myoptarg);
+			break;
+
+		case 'l':
+			_on = getKey(ControllerInputMap, myoptarg);
+			break;
+
+		case 'i':
+			_ir = getKey(ControllerInputMap, myoptarg);
+			break;
+
+		case 'd':
+			_debug = true;
+			break;
+
+		default:
+			break;
+		}
+	}
+
+	if (_debug) {
+		PX4_INFO("ELRS LED Debug Mode Enabled");
+		PX4_INFO("Port: %s", _port.c_str());
+		PX4_INFO("Button Configuration:");
+		PX4_INFO("\tOn: %s", ControllerInputMap.at(_on).c_str());
+		PX4_INFO("\tIR: %s", ControllerInputMap.at(_ir).c_str());
+		PX4_INFO("\tOff: %s", ControllerInputMap.at(_off).c_str());
+	}
+
+	if (! _initialized) {
+		if (initialize()) {
+			return -1;
+		}
+	}
+
+	if (_is_running) {
+		PX4_WARN("Already started");
+		return 0;
+	}
+
+	_task_handle = px4_task_spawn_cmd("elrs_led_main",
+					  SCHED_DEFAULT,
+					  SCHED_PRIORITY_DEFAULT,
+					  2000,
+					  (px4_main_t) &elrs_led_task,
+					  (char *const *)argv);
+
+	if (_task_handle < 0) {
+		PX4_ERR("task start failed");
+		return -1;
+	}
+
+	return 0;
+}
+
+void
+usage()
+{
+	PX4_INFO("Usage: elrs_led start [options]");
+	PX4_INFO("Options: -p <number>    uart port number");
+	PX4_INFO("Options: -o <number>    LEDs off button");
+	PX4_INFO("Options: -l <number>    Overt LEDs on button");
+	PX4_INFO("Options: -i <number>    IR LEDs on button");
+	PX4_INFO("Options: -d <number>    enable debug messages");
+}
+
+void debug_info(LEDState led_state, uint8_t *pwmPacket)
+{
+	PX4_INFO("");
+
+	if (led_state == LEDState::ON) {
+		PX4_INFO("Turning LEDs on");
+
+	} else if (led_state == LEDState::OFF) {
+		PX4_INFO("Turning LEDs off");
+
+	} else if (led_state == LEDState::IR) {
+		PX4_INFO("Turning IR LEDs on");
+
+	} else {
+		PX4_WARN("ELRS LED: LED state unknown: 0x%x", led_state);
+	}
+
+	PX4_INFO("Wrote packet: [0x%x, 0x%x, 0x%x, 0x%x, 0x%x, 0x%x, 0x%x, 0x%x, 0x%x, 0x%x, 0x%x]",
+		 pwmPacket[0], pwmPacket[1], pwmPacket[2], pwmPacket[3], pwmPacket[4], pwmPacket[5],
+		 pwmPacket[6], pwmPacket[7], pwmPacket[8], pwmPacket[9], pwmPacket[10]);
+}
+
+void make_packet(LEDState led_state, uint8_t *pwmPacket)
+{
+	if (led_state == LEDState::OFF) {
+		pwmPacket[8] = 0x03;
+		pwmPacket[9] = 0x84;
+		pwmPacket[10] = crsf_crc.calc(&pwmPacket[CRSF_FRAME_NOT_COUNTED_BYTES], PWM_FRAME_SIZE - 1, 0);
+
+	} else if (led_state == LEDState::ON) {
+		pwmPacket[8] = 0x05;
+		pwmPacket[9] = 0xAA;
+		pwmPacket[10] = crsf_crc.calc(&pwmPacket[CRSF_FRAME_NOT_COUNTED_BYTES], PWM_FRAME_SIZE - 1, 0);
+
+	} else if (led_state == LEDState::IR) {
+		pwmPacket[8] = 0x07;
+		pwmPacket[9] = 0xFF;
+		pwmPacket[10] = crsf_crc.calc(&pwmPacket[CRSF_FRAME_NOT_COUNTED_BYTES], PWM_FRAME_SIZE - 1, 0);
+
+	} else {
+		PX4_WARN("ELRS LED: Unknown LED state.");
+	}
+}
+
+} // End namespance elrs_led
+
+int elrs_led_main(int argc, char *argv[])
+{
+	int myoptind = 1;
+
+	if (argc <= 1) {
+		elrs_led::usage();
+		return -1;
+	}
+
+	const char *verb = argv[myoptind];
+
+	if (!strcmp(verb, "start")) {
+		return elrs_led::start(argc - 1, argv + 1);
+
+	} else {
+		elrs_led::usage();
+		return -1;
+	}
+
+	return 0;
+}
@@ -0,0 +1,113 @@
+/****************************************************************************
+ *
+ *   Copyright (c) 2023 ModalAI, Inc. 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 <string>
+#include <map>
+
+#define crclen 256
+#define CRSF_CRC_POLY 0xd5
+#define CRSF_FRAME_NOT_COUNTED_BYTES 2
+#define PWM_FRAME_SIZE 9
+
+
+enum class ControllerInput : uint32_t {
+	DLEFT = 0x2000,
+	DRIGHT = 0x4000,
+	DDOWN = 0x1000,
+	DUP = 0x800,
+	BACK = 0x10,
+	START = 0x40,
+	Y = 0x08,
+	B = 0x02,
+	A = 0x01,
+	X = 0x04,
+	STICK_RIGHT = 0x100,
+	STICK_LEFT = 0x80,
+	BUMPER_RIGHT = 0x400,
+	BUMPER_LEFT = 0x200,
+	DEFAULT = 0xFFFFFFFF
+};
+
+enum class LEDState : uint8_t {
+	OFF = 0x00,
+	ON = 0x01,
+	IR = 0x02,
+	DEFAULT = 0xFF
+};
+
+
+class GENERIC_CRC8
+{
+public:
+	GENERIC_CRC8() {};
+	uint8_t calc(const uint8_t *data, uint16_t len, uint8_t crc)
+	{
+		while (len--) {
+			crc = crc8tab[crc ^ *data++];
+		}
+
+		return crc;
+	}
+
+private:
+	uint8_t crc8tab[crclen] = {0, 213, 127, 170, 254, 43, 129, 84, 41, 252, 86, 131, 215, 2, 168, 125, 82, 135,
+				   45, 248, 172, 121, 211, 6, 123, 174, 4, 209, 133, 80, 250, 47, 164, 113, 219, 14,
+				   90, 143, 37, 240, 141, 88, 242, 39, 115, 166, 12, 217, 246, 35, 137, 92, 8, 221,
+				   119, 162, 223, 10, 160, 117, 33, 244, 94, 139, 157, 72, 226, 55, 99, 182, 28, 201,
+				   180, 97, 203, 30, 74, 159, 53, 224, 207, 26, 176, 101, 49, 228, 78, 155, 230, 51,
+				   153, 76, 24, 205, 103, 178, 57, 236, 70, 147, 199, 18, 184, 109, 16, 197, 111, 186,
+				   238, 59, 145, 68, 107, 190, 20, 193, 149, 64, 234, 63, 66, 151, 61, 232, 188, 105,
+				   195, 22, 239, 58, 144, 69, 17, 196, 110, 187, 198, 19, 185, 108, 56, 237, 71, 146,
+				   189, 104, 194, 23, 67, 150, 60, 233, 148, 65, 235, 62, 106, 191, 21, 192, 75, 158,
+				   52, 225, 181, 96, 202, 31, 98, 183, 29, 200, 156, 73, 227, 54, 25, 204, 102, 179, 231,
+				   50, 152, 77, 48, 229, 79, 154, 206, 27, 177, 100, 114, 167, 13, 216, 140, 89, 243,
+				   38, 91, 142, 36, 241, 165, 112, 218, 15, 32, 245, 95, 138, 222, 11, 161, 116, 9, 220,
+				   118, 163, 247, 34, 136, 93, 214, 3, 169, 124, 40, 253, 87, 130, 255, 42, 128, 85, 1,
+				   212, 126, 171, 132, 81, 251, 46, 122, 175, 5, 208, 173, 120, 210, 7, 83, 134, 44, 249
+				  };
+};
+
+
+ControllerInput getKey(const std::map<ControllerInput, std::string> &map, const std::string &value)
+{
+	for (const auto &pair : map) {
+		if (pair.second == value) {
+			return pair.first;
+		}
+	}
+
+	return ControllerInput::DEFAULT;
+}
+
+
+
@@ -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__ghst_rc
+	MAIN ghst_rc
+	COMPILE_FLAGS
+	INCLUDES
+	${PX4_SOURCE_DIR}/src/drivers/rc_input
+	${PX4_SOURCE_DIR}/src/lib/rc/spektrum_rssi.h
+	SRCS
+		ghst_rc.cpp
+		ghst_rc.hpp
+	MODULE_CONFIG
+		module.yaml
+	DEPENDS
+		rc
+	)
@@ -0,0 +1,5 @@
+menuconfig DRIVERS_GHST_RC
+	bool "ghst_rc"
+	default n
+	---help---
+		Enable support for ghst rc
@@ -0,0 +1,312 @@
+/****************************************************************************
+ *
+ *   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 <lib/rc/common_rc.h>
+#include "ghst_rc.hpp"
+#include <lib/rc/ghst.hpp>
+
+#include <px4_log.h>
+
+#include <drivers/device/qurt/uart.h>
+
+#include <termios.h>
+#include <fcntl.h>
+
+#include <uORB/topics/battery_status.h>
+#include <uORB/topics/vehicle_attitude.h>
+#include <uORB/topics/sensor_gps.h>
+#include <uORB/topics/vehicle_status.h>
+
+using namespace time_literals;
+
+uint32_t GhstRc::baudrate = GHST_BAUDRATE;
+
+GhstRc::GhstRc(const char *device) :
+	ModuleParams(nullptr),
+	ScheduledWorkItem(MODULE_NAME, px4::serial_port_to_wq(GHST_RC_DEFAULT_PORT))
+{
+	if (device) {
+		strncpy(_device, device, sizeof(_device) - 1);
+		_device[sizeof(_device) - 1] = '\0';
+	}
+}
+
+GhstRc::~GhstRc()
+{
+	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_name = nullptr;
+
+	while ((ch = px4_getopt(argc, argv, "d:b:", &myoptind, &myoptarg)) != EOF) {
+		switch (ch) {
+		case 'd':
+			device_name = myoptarg;
+			break;
+
+		case 'b':
+			baudrate = atoi(myoptarg);
+			PX4_INFO("Setting GHST baudrate to %u", baudrate);
+			break;
+
+		case '?':
+			error_flag = true;
+			break;
+
+		default:
+			PX4_WARN("unrecognized flag");
+			error_flag = true;
+			break;
+		}
+	}
+
+	if (error_flag) {
+		return PX4_ERROR;
+	}
+
+	if (!device_name) {
+		PX4_ERR("Valid device required");
+		return PX4_ERROR;
+	}
+
+	GhstRc *instance = new GhstRc(device_name);
+
+	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::fill_rc_in(uint16_t raw_rc_count_local,
+			uint16_t raw_rc_values_local[GHST_MAX_NUM_CHANNELS],
+			hrt_abstime now, bool frame_drop, bool failsafe,
+			unsigned frame_drops, int rssi = -1)
+{
+	// fill rc_in struct for publishing
+	_rc_in.channel_count = raw_rc_count_local;
+
+	if (_rc_in.channel_count > GHST_MAX_NUM_CHANNELS) {
+		_rc_in.channel_count = GHST_MAX_NUM_CHANNELS;
+	}
+
+	unsigned valid_chans = 0;
+
+	for (unsigned i = 0; i < _rc_in.channel_count; i++) {
+		_rc_in.values[i] = raw_rc_values_local[i];
+
+		if (raw_rc_values_local[i] != UINT16_MAX) {
+			valid_chans++;
+		}
+
+		// once filled, reset values back to default
+		_raw_rc_values[i] = UINT16_MAX;
+	}
+
+	_rc_in.timestamp = now;
+	_rc_in.timestamp_last_signal = _rc_in.timestamp;
+	_rc_in.rc_ppm_frame_length = 0;
+
+	/* fake rssi if no value was provided */
+	if (rssi == -1) {
+		_rc_in.rssi = 255;
+
+	} else {
+		_rc_in.rssi = rssi;
+	}
+
+	if (valid_chans == 0) {
+		_rc_in.rssi = 0;
+	}
+
+	_rc_in.rc_failsafe = failsafe;
+	_rc_in.rc_lost = (valid_chans == 0);
+	_rc_in.rc_lost_frame_count = frame_drops;
+	_rc_in.rc_total_frame_count = 0;
+}
+
+void GhstRc::Run()
+{
+	if (should_exit()) {
+		ScheduleClear();
+		_rc_fd = -1;
+		exit_and_cleanup();
+		return;
+	}
+
+	if (_rc_fd < 0) {
+		_rc_fd = qurt_uart_open(_device, baudrate);
+
+		if (_rc_fd < 0) {
+			PX4_ERR("Error opening port: %s", _device);
+			return;
+		}
+
+		if (_rc_fd >= 0) {
+			_is_singlewire = true;
+
+			// Configure serial port for GHST
+			ghst_config(_rc_fd);
+		}
+
+		_rc_in.rssi_dbm = NAN;
+		_rc_in.link_quality = -1;
+
+	}
+
+	const hrt_abstime time_now_us = hrt_absolute_time();
+	const hrt_abstime cycle_timestamp = time_now_us;
+	perf_count_interval(_cycle_interval_perf, time_now_us);
+
+	// Read all available data from the serial RC input UART
+	int new_bytes = qurt_uart_read(_rc_fd, (char *) &_rcs_buf[0], RC_MAX_BUFFER_SIZE, 500);
+
+	if (new_bytes > 0) {
+		_bytes_rx += new_bytes;
+		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, GHST_MAX_NUM_CHANNELS);
+
+		if (rc_updated) {
+			_last_packet_seen = time_now_us;
+			// 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 (!_ghst_telemetry) {
+				_ghst_telemetry = new GHSTTelemetry(_rcs_fd);
+			}
+
+			if (_ghst_telemetry) {
+				_ghst_telemetry->update(cycle_timestamp);
+			}
+
+#endif /* BOARD_SUPPORTS_RC_SERIAL_PORT_OUTPUT */
+		}
+	}
+
+	// If no communication
+	if (time_now_us - _last_packet_seen > 100_ms) {
+		// Invalidate link statistics
+		_rc_in.rssi_dbm = NAN;
+		_rc_in.link_quality = -1;
+	}
+
+	// If we have not gotten RC updates specifically
+	if (time_now_us - _rc_in.timestamp_last_signal > 50_ms) {
+		_rc_in.rc_lost = 1;
+		_rc_in.rc_failsafe = 1;
+		_rc_in.rssi_dbm = NAN;
+		_rc_in.link_quality = -1;
+
+	} else {
+		_rc_in.rc_lost = 0;
+		_rc_in.rc_failsafe = 0;
+	}
+
+	_rc_in.input_source = input_rc_s::RC_INPUT_SOURCE_PX4FMU_GHST;
+	_rc_in.timestamp = hrt_absolute_time();
+	_input_rc_pub.publish(_rc_in);
+
+	perf_count(_publish_interval_perf);
+
+	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);
+	}
+
+	if (_is_singlewire) {
+		PX4_INFO("Telemetry disabled: Singlewire RC port");
+	}
+
+	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 parses the GHST RC uplink protocol and can generate GHST downlink telemetry data
+
+)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,126 @@
+/****************************************************************************
+ *
+ *   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 <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 <drivers/drv_hrt.h>
+#include <lib/perf/perf_counter.h>
+#include <uORB/PublicationMulti.hpp>
+#include <uORB/topics/input_rc.h>
+
+// telemetry
+#include <uORB/Subscription.hpp>
+#include <uORB/topics/battery_status.h>
+#include <uORB/topics/vehicle_attitude.h>
+#include <uORB/topics/sensor_gps.h>
+#include <uORB/topics/vehicle_status.h>
+
+#define GHST_MAX_NUM_CHANNELS			(16)
+
+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;
+
+	void fill_rc_in(uint16_t raw_rc_count_local,
+			uint16_t raw_rc_values_local[GHST_MAX_NUM_CHANNELS],
+			hrt_abstime now, bool frame_drop, bool failsafe,
+			unsigned frame_drops, int rssi);
+
+private:
+	void Run() override;
+
+	bool SendTelemetryBattery(const uint16_t voltage, const uint16_t current, const int fuel, const uint8_t remaining);
+
+	bool SendTelemetryGps(const int32_t latitude, const int32_t longitude, const uint16_t groundspeed,
+			      const uint16_t gps_heading, const uint16_t altitude, const uint8_t num_satellites);
+
+	bool SendTelemetryAttitude(const int16_t pitch, const int16_t roll, const int16_t yaw);
+
+	bool SendTelemetryFlightMode(const char *flight_mode);
+
+
+	uORB::PublicationMulti<input_rc_s> _input_rc_pub{ORB_ID(input_rc)};
+	input_rc_s	_rc_in{};
+	int _rc_fd{-1};
+	char _device[20] {}; // device / serial port path
+	bool _is_singlewire{true};
+
+	static constexpr size_t RC_MAX_BUFFER_SIZE{64};
+	uint8_t _rcs_buf[RC_MAX_BUFFER_SIZE] {};
+	uint32_t _bytes_rx{0};
+
+	uint16_t _raw_rc_values[GHST_MAX_NUM_CHANNELS] {};
+	uint16_t _raw_rc_count{};
+
+	hrt_abstime _last_packet_seen{0};
+
+	// telemetry
+	hrt_abstime _telemetry_update_last{0};
+	static constexpr int num_data_types{4}; ///< number of different telemetry data types
+	int _next_type{0};
+	static uint32_t baudrate;
+	uORB::Subscription _battery_status_sub{ORB_ID(battery_status)};
+	uORB::Subscription _vehicle_attitude_sub{ORB_ID(vehicle_attitude)};
+	uORB::Subscription _vehicle_gps_position_sub{ORB_ID(vehicle_gps_position)};
+	uORB::Subscription _vehicle_status_sub{ORB_ID(vehicle_status)};
+
+
+	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")};
+
+	// DEFINE_PARAMETERS(
+	// (ParamBool<px4::params::RC_GHST_TEL_EN>) _param_rc_ghst_tel_en
+	// )
+};
+
@@ -0,0 +1,11 @@
+module_name: GHST RC Input Driver
+serial_config:
+    - command: "ghst_rc start -d ${SERIAL_DEV}"
+      port_config_param:
+        name: RC_GHST_PRT_CFG
+        group: Serial
+        #default: RC
+        #depends_on_port: RC
+        description_extended: |
+            Ghost RC (GHST) driver.
+
@@ -650,8 +650,7 @@ bool ICM42688P::FIFORead(const hrt_abstime &timestamp_sample, uint16_t samples)
 		if (ProcessTemperature(buffer.f, valid_samples)) {
 			ProcessGyro(timestamp_sample, buffer.f, valid_samples);
 			ProcessAccel(timestamp_sample, buffer.f, valid_samples);
-			// Pass only most recent valid sample to IMU server
-			// ProcessIMU(timestamp_sample, buffer.f[valid_samples - 1]);
+			ProcessIMU(timestamp_sample, buffer.f, valid_samples);
 			return true;
 		}
 	}
@@ -687,79 +686,86 @@ static constexpr int32_t reassemble_20bit(const uint32_t a, const uint32_t b, co
 	return static_cast<int32_t>(x);
 }

-void ICM42688P::ProcessIMU(const hrt_abstime &timestamp_sample, const FIFO::DATA &fifo)
+void ICM42688P::ProcessIMU(const hrt_abstime &timestamp_sample, const FIFO::DATA fifo[], const uint8_t samples)
 {
-	// float accel_x = 0.0, accel_y = 0.0, accel_z = 0.0;
-	// float gyro_x = 0.0,  gyro_y = 0.0,  gyro_z = 0.0;
-	//
-	// // 20 bit hires mode
-	//
-	// // Sign extension + Accel [19:12] + Accel [11:4] + Accel [3:2] (20 bit extension byte)
-	// // Accel data is 18 bit
-	// int32_t temp_accel_x = reassemble_20bit(fifo.ACCEL_DATA_X1, fifo.ACCEL_DATA_X0,
-	// 				   fifo.Ext_Accel_X_Gyro_X & 0xF0 >> 4);
-	// int32_t temp_accel_y = reassemble_20bit(fifo.ACCEL_DATA_Y1, fifo.ACCEL_DATA_Y0,
-	// 				   fifo.Ext_Accel_Y_Gyro_Y & 0xF0 >> 4);
-	// int32_t temp_accel_z = reassemble_20bit(fifo.ACCEL_DATA_Z1, fifo.ACCEL_DATA_Z0,
-	// 				   fifo.Ext_Accel_Z_Gyro_Z & 0xF0 >> 4);
-	//
-	// // Gyro [19:12] + Gyro [11:4] + Gyro [3:0] (bottom 4 bits of 20 bit extension byte)
-	// int32_t temp_gyro_x = reassemble_20bit(fifo.GYRO_DATA_X1, fifo.GYRO_DATA_X0,
-	//                    fifo.Ext_Accel_X_Gyro_X & 0x0F);
-	// int32_t temp_gyro_y = reassemble_20bit(fifo.GYRO_DATA_Y1, fifo.GYRO_DATA_Y0,
-	//                    fifo.Ext_Accel_Y_Gyro_Y & 0x0F);
-	// int32_t temp_gyro_z = reassemble_20bit(fifo.GYRO_DATA_Z1, fifo.GYRO_DATA_Z0,
-	//                    fifo.Ext_Accel_Z_Gyro_Z & 0x0F);
+	float accel_x = 0.0, accel_y = 0.0, accel_z = 0.0;
+	float gyro_x = 0.0,  gyro_y = 0.0,  gyro_z = 0.0;

-	// // accel samples invalid if -524288
-	// if (temp_accel_x != -524288 && temp_accel_y != -524288 && temp_accel_z != -524288) {
-	// 	// shift accel by 2 (2 least significant bits are always 0)
-	// 	accel_x = (float) temp_accel_x / 4.f;
-	// 	accel_y = (float) temp_accel_y / 4.f;
-	// 	accel_z = (float) temp_accel_z / 4.f;
-	//
-	// 	// shift gyro by 1 (least significant bit is always 0)
-	// 	gyro_x = (float) temp_gyro_x / 2.f;
-	// 	gyro_y = (float) temp_gyro_y / 2.f;
-	// 	gyro_z = (float) temp_gyro_z / 2.f;
-	//
-	// 	// correct frame for publication
-	// 	// sensor's frame is +x forward, +y left, +z up
-	// 	// flip y & z to publish right handed with z down (x forward, y right, z down)
-	// 	accel_y = -accel_y;
-	// 	accel_z = -accel_z;
-	// 	gyro_y  = -gyro_y;
-	// 	gyro_z  = -gyro_z;
-	//
-	// 	// Scale everything appropriately
-	// 	float accel_scale_factor = (CONSTANTS_ONE_G / 8192.f);
-	// 	accel_x *= accel_scale_factor;
-	// 	accel_y *= accel_scale_factor;
-	// 	accel_z *= accel_scale_factor;
-	//
-	// 	float gyro_scale_factor = math::radians(1.f / 131.f);
-	// 	gyro_x *= gyro_scale_factor;
-	// 	gyro_y *= gyro_scale_factor;
-	// 	gyro_z *= gyro_scale_factor;
-	//
-	// 	// Store the data in our array
-	// 	_imu_server_data.accel_x[_imu_server_index] = accel_x;
-	// 	_imu_server_data.accel_y[_imu_server_index] = accel_y;
-	// 	_imu_server_data.accel_z[_imu_server_index] = accel_z;
-	// 	_imu_server_data.gyro_x[_imu_server_index]  = gyro_x;
-	// 	_imu_server_data.gyro_y[_imu_server_index]  = gyro_y;
-	// 	_imu_server_data.gyro_z[_imu_server_index]  = gyro_z;
-	// 	_imu_server_data.ts[_imu_server_index]      = timestamp_sample;
-	// 	_imu_server_index++;
-	//
-	// 	// If array is full, publish the data
-	// 	if (_imu_server_index == 10) {
-	// 		_imu_server_index = 0;
-	// 		_imu_server_data.timestamp = hrt_absolute_time();
-	// 		_imu_server_data.temperature = 0; // Not used right now
-	// 		_imu_server_pub.publish(_imu_server_data);
-	// 	}
-	// }
+	for (int i = 0; i < samples; i++) {
+		_imu_server_decimator++;
+
+		if (_imu_server_decimator == 8) {
+			_imu_server_decimator = 0;
+			// 20 bit hires mode
+
+			// Sign extension + Accel [19:12] + Accel [11:4] + Accel [3:2] (20 bit extension byte)
+			// Accel data is 18 bit
+			int32_t temp_accel_x = reassemble_20bit(fifo[i].ACCEL_DATA_X1, fifo[i].ACCEL_DATA_X0,
+								fifo[i].Ext_Accel_X_Gyro_X & 0xF0 >> 4);
+			int32_t temp_accel_y = reassemble_20bit(fifo[i].ACCEL_DATA_Y1, fifo[i].ACCEL_DATA_Y0,
+								fifo[i].Ext_Accel_Y_Gyro_Y & 0xF0 >> 4);
+			int32_t temp_accel_z = reassemble_20bit(fifo[i].ACCEL_DATA_Z1, fifo[i].ACCEL_DATA_Z0,
+								fifo[i].Ext_Accel_Z_Gyro_Z & 0xF0 >> 4);
+
+			// Gyro [19:12] + Gyro [11:4] + Gyro [3:0] (bottom 4 bits of 20 bit extension byte)
+			int32_t temp_gyro_x = reassemble_20bit(fifo[i].GYRO_DATA_X1, fifo[i].GYRO_DATA_X0,
+							       fifo[i].Ext_Accel_X_Gyro_X & 0x0F);
+			int32_t temp_gyro_y = reassemble_20bit(fifo[i].GYRO_DATA_Y1, fifo[i].GYRO_DATA_Y0,
+							       fifo[i].Ext_Accel_Y_Gyro_Y & 0x0F);
+			int32_t temp_gyro_z = reassemble_20bit(fifo[i].GYRO_DATA_Z1, fifo[i].GYRO_DATA_Z0,
+							       fifo[i].Ext_Accel_Z_Gyro_Z & 0x0F);
+
+			// accel samples invalid if -524288
+			if (temp_accel_x != -524288 && temp_accel_y != -524288 && temp_accel_z != -524288) {
+				// shift accel by 2 (2 least significant bits are always 0)
+				accel_x = (float) temp_accel_x / 4.f;
+				accel_y = (float) temp_accel_y / 4.f;
+				accel_z = (float) temp_accel_z / 4.f;
+
+				// shift gyro by 1 (least significant bit is always 0)
+				gyro_x = (float) temp_gyro_x / 2.f;
+				gyro_y = (float) temp_gyro_y / 2.f;
+				gyro_z = (float) temp_gyro_z / 2.f;
+
+				// correct frame for publication
+				// sensor's frame is +x forward, +y left, +z up
+				// flip y & z to publish right handed with z down (x forward, y right, z down)
+				accel_y = -accel_y;
+				accel_z = -accel_z;
+				gyro_y  = -gyro_y;
+				gyro_z  = -gyro_z;
+
+				// Scale everything appropriately
+				float accel_scale_factor = (CONSTANTS_ONE_G / 8192.f);
+				accel_x *= accel_scale_factor;
+				accel_y *= accel_scale_factor;
+				accel_z *= accel_scale_factor;
+
+				float gyro_scale_factor = math::radians(1.f / 131.f);
+				gyro_x *= gyro_scale_factor;
+				gyro_y *= gyro_scale_factor;
+				gyro_z *= gyro_scale_factor;
+
+				// Store the data in our array
+				_imu_server_data.accel_x[_imu_server_index] = accel_x;
+				_imu_server_data.accel_y[_imu_server_index] = accel_y;
+				_imu_server_data.accel_z[_imu_server_index] = accel_z;
+				_imu_server_data.gyro_x[_imu_server_index]  = gyro_x;
+				_imu_server_data.gyro_y[_imu_server_index]  = gyro_y;
+				_imu_server_data.gyro_z[_imu_server_index]  = gyro_z;
+				_imu_server_data.ts[_imu_server_index]      = timestamp_sample - (125 * (samples - 1 - i));
+				_imu_server_index++;
+
+				// If array is full, publish the data
+				if (_imu_server_index == 10) {
+					_imu_server_index = 0;
+					_imu_server_data.timestamp = hrt_absolute_time();
+					_imu_server_data.temperature = 0; // Not used right now
+					_imu_server_pub.publish(_imu_server_data);
+				}
+			}
+		}
+	}
 }

 void ICM42688P::ProcessAccel(const hrt_abstime &timestamp_sample, const FIFO::DATA fifo[], const uint8_t samples)
@@ -50,6 +50,7 @@
 #include <lib/perf/perf_counter.h>
 #include <px4_platform_common/atomic.h>
 #include <px4_platform_common/i2c_spi_buses.h>
+#include <uORB/topics/imu_server.h>
 #include <uORB/topics/sensor_accel_fifo.h>
 #include <uORB/topics/sensor_gyro_fifo.h>
 #include <memory>
@@ -147,7 +148,7 @@ private:
 	bool FIFORead(const hrt_abstime &timestamp_sample, uint16_t samples);
 	void FIFOReset();

-	void ProcessIMU(const hrt_abstime &timestamp_sample, const FIFO::DATA &fifo);
+	void ProcessIMU(const hrt_abstime &timestamp_sample, const FIFO::DATA fifo[], const uint8_t samples);
 	void ProcessAccel(const hrt_abstime &timestamp_sample, const FIFO::DATA fifo[], const uint8_t samples);
 	void ProcessGyro(const hrt_abstime &timestamp_sample, const FIFO::DATA fifo[], const uint8_t samples);
 	bool ProcessTemperature(const FIFO::DATA fifo[], const uint8_t samples);
@@ -226,8 +227,9 @@ private:
 	uint32_t _temperature_samples{0};

 	// Support for the IMU server
-	// uint32_t _imu_server_index{0};
-	// imu_server_s _imu_server_data;
-	// uORB::Publication<imu_server_s> _imu_server_pub{ORB_ID(imu_server)};
+	uint32_t _imu_server_index{0};
+	uint32_t _imu_server_decimator{0};
+	imu_server_s _imu_server_data;
+	uORB::Publication<imu_server_s> _imu_server_pub{ORB_ID(imu_server)};

 };
@@ -133,15 +133,6 @@ enum INT_CONFIG_BIT : uint8_t {
 	INT1_POLARITY       = Bit0,
 };

-// INTF_CONFIG0
-enum INTF_CONFIG0_BIT : uint8_t {
-	FIFO_HOLD_LAST_DATA_EN  = Bit7,
-	FIFO_COUNT_REC          = Bit6,
-	FIFO_COUNT_ENDIAN       = Bit5,
-	SENSOR_DATA_ENDIAN      = Bit4,
-	UI_SIFS_CFG_DISABLE_I2C = Bit1 | Bit0,
-};
-
 // FIFO_CONFIG
 enum FIFO_CONFIG_BIT : uint8_t {
 	// 7:6 FIFO_MODE
@@ -150,11 +150,15 @@ void task_main(int argc, char *argv[])
 		newbytes = read(uart_fd, &rx_buf[0], sizeof(rx_buf));
 #endif

+		uint8_t protocol_version = rx_buf[1] & 0x0F;
+
 		if (newbytes <= 0) {
 			if (print_msg) { PX4_INFO("Spektrum RC: Read no bytes from UART"); }

-		} else if (((newbytes != DSM_FRAME_SIZE) || ((rx_buf[1] & 0x0F) != 0x02)) && (! first_correct_frame_received)) {
-			PX4_ERR("Spektrum RC: Read something other than correct DSM frame on read. Got %d bytes. Protocol byte is 0x%.2x",
+		} else if (((newbytes != DSM_FRAME_SIZE) ||
+			    ((protocol_version != 0x02) && (protocol_version != 0x01))) &&
+			   (! first_correct_frame_received)) {
+			PX4_ERR("Spektrum RC: Invalid DSM frame. %d bytes. Protocol byte 0x%.2x",
 				newbytes, rx_buf[1]);

 		} else {
@@ -1,7 +1,7 @@

-# libfc_sensor.so is provided in the Docker build environment
+# Link against the public stub version of the proprietary fc sensor library
 target_link_libraries(px4 PRIVATE
-		/home/libfc_sensor.so
+		${PX4_SOURCE_DIR}/src//modules/muorb/apps/libfc-sensor-api/build/libfc_sensor.so
 		px4_layer
 		${module_libraries}
 )
@@ -2,23 +2,29 @@ CONFIG_PLATFORM_POSIX=y
 CONFIG_BOARD_LINUX_TARGET=y
 CONFIG_BOARD_TOOLCHAIN="aarch64-linux-gnu"
 CONFIG_BOARD_ROOTFSDIR="/data/px4"
-CONFIG_DRIVERS_ACTUATORS_MODAL_IO=y
-CONFIG_DRIVERS_OSD_MSP_OSD=y
-CONFIG_DRIVERS_QSHELL_POSIX=y
-CONFIG_MODULES_COMMANDER=y
-CONFIG_MODULES_CONTROL_ALLOCATOR=y
-CONFIG_MODULES_DATAMAN=y
-CONFIG_MODULES_FLIGHT_MODE_MANAGER=y
-CONFIG_MODULES_LOGGER=y
-CONFIG_MODULES_MAVLINK=y
-CONFIG_MODULES_MUORB_APPS=y
-CONFIG_MODULES_NAVIGATOR=y
-CONFIG_MODULES_RC_UPDATE=y
-CONFIG_SYSTEMCMDS_ACTUATOR_TEST=y
-CONFIG_SYSTEMCMDS_BSONDUMP=y
-CONFIG_SYSTEMCMDS_PARAM=y
-CONFIG_SYSTEMCMDS_PERF=y
-CONFIG_SYSTEMCMDS_TOPIC_LISTENER=y
-CONFIG_SYSTEMCMDS_UORB=y
-CONFIG_SYSTEMCMDS_VER=y
 CONFIG_ORB_COMMUNICATOR=y
+CONFIG_PARAM_SERVER=y
+CONFIG_DRIVERS_GPS=y
+CONFIG_DRIVERS_RC_INPUT=y
+CONFIG_DRIVERS_QSHELL_POSIX=y
+CONFIG_DRIVERS_OSD_MSP_OSD=y
+CONFIG_DRIVERS_VOXL2_IO=y
+CONFIG_DRIVERS_ACTUATORS_VOXL_ESC=y
+CONFIG_DRIVERS_IMU_SERVER=y
+CONFIG_SYSTEMCMDS_PERF=y
+CONFIG_SYSTEMCMDS_UORB=y
+CONFIG_SYSTEMCMDS_PARAM=y
+CONFIG_SYSTEMCMDS_TOPIC_LISTENER=y
+CONFIG_SYSTEMCMDS_BSONDUMP=y
+CONFIG_SYSTEMCMDS_ACTUATOR_TEST=y
+CONFIG_SYSTEMCMDS_VER=y
+CONFIG_MODULES_LOGGER=y
+CONFIG_MODULES_DATAMAN=y
+CONFIG_MODULES_NAVIGATOR=y
+CONFIG_MODULES_COMMANDER=y
+CONFIG_MODULES_MAVLINK=y
+CONFIG_MODULES_LOAD_MON=y
+CONFIG_MODULES_CONTROL_ALLOCATOR=y
+CONFIG_MODULES_SIMULATION_PWM_OUT_SIM=y
+CONFIG_MODULES_MUORB_APPS=y
+CONFIG_MODULES_UXRCE_DDS_CLIENT=y
@@ -0,0 +1,10 @@
+#!/bin/bash
+
+cd src/modules/muorb/apps/libfc-sensor-api
+rm -fR build
+mkdir build
+cd build
+CC=/home/4.1.0.4/tools/linaro64/bin/aarch64-linux-gnu-gcc cmake ..
+make
+cd ../../../../../..
+
@@ -6,6 +6,4 @@ source /home/build-env.sh

 make modalai_voxl2-slpi

-cat build/modalai_voxl2-slpi_default/src/lib/version/build_git_version.h
-
 echo "*** End of qurt slpi build ***"
@@ -1,5 +1,8 @@
 #!/bin/bash

 # Clean out the build artifacts
-source /home/build-env.sh
-make clean
+# source /home/build-env.sh
+# make clean
+
+# This gets rid of everything and starts fresh
+sudo rm -fR build
@@ -0,0 +1,9 @@
+#!/bin/bash
+
+# Push slpi image to voxl2
+adb push build/modalai_voxl2-slpi_default/platforms/qurt/libpx4.so /usr/lib/rfsa/adsp
+
+# Push apps processor image to voxl2
+adb push build/modalai_voxl2_default/bin/px4 /usr/bin
+
+adb shell sync
@@ -10,14 +10,19 @@ adb push build/modalai_voxl2_default/bin/px4 /usr/bin
 adb push build/modalai_voxl2_default/bin/px4-alias.sh /usr/bin
 adb push boards/modalai/voxl2/target/voxl-px4 /usr/bin
 adb push boards/modalai/voxl2/target/voxl-px4-start /usr/bin
+adb push boards/modalai/voxl2/target/voxl-px4-hitl /usr/bin
+adb push boards/modalai/voxl2/target/voxl-px4-hitl-start /usr/bin
 adb shell chmod a+x /usr/bin/px4-alias.sh
 adb shell chmod a+x /usr/bin/voxl-px4
 adb shell chmod a+x /usr/bin/voxl-px4-start
+adb shell chmod a+x /usr/bin/voxl-px4-hitl
+adb shell chmod a+x /usr/bin/voxl-px4-hitl-start

 # Push configuration file
 adb shell mkdir -p /etc/modalai
 adb push boards/modalai/voxl2/target/voxl-px4-set-default-parameters.config /etc/modalai
 adb push boards/modalai/voxl2/target/voxl-px4-fake-imu-calibration.config /etc/modalai
+adb push boards/modalai/voxl2/target/voxl-px4-hitl-set-default-parameters.config /etc/modalai

 # Make sure to setup all of the needed px4 aliases.
 adb shell "cd /usr/bin; /bin/ln -f -s px4 px4-accelsim"
@@ -34,6 +39,7 @@ adb shell "cd /usr/bin; /bin/ln -f -s px4 px4-commander_tests"
 adb shell "cd /usr/bin; /bin/ln -f -s px4 px4-control_allocator"
 adb shell "cd /usr/bin; /bin/ln -f -s px4 px4-controllib_test"
 adb shell "cd /usr/bin; /bin/ln -f -s px4 px4-dataman"
+adb shell "cd /usr/bin; /bin/ln -f -s px4 px4-dsp_hitl"
 adb shell "cd /usr/bin; /bin/ln -f -s px4 px4-ekf2"
 adb shell "cd /usr/bin; /bin/ln -f -s px4 px4-esc_calib"
 adb shell "cd /usr/bin; /bin/ln -f -s px4 px4-ets_airspeed"
@@ -65,7 +71,9 @@ adb shell "cd /usr/bin; /bin/ln -f -s px4 px4-mb12xx"
 adb shell "cd /usr/bin; /bin/ln -f -s px4 px4-mc_att_control"
 adb shell "cd /usr/bin; /bin/ln -f -s px4 px4-mc_pos_control"
 adb shell "cd /usr/bin; /bin/ln -f -s px4 px4-measairspeedsim"
+adb shell "cd /usr/bin; /bin/ln -f -s px4 px4-microdds_client"
 adb shell "cd /usr/bin; /bin/ln -f -s px4 px4-mixer"
+adb shell "cd /usr/bin; /bin/ln -f -s px4 px4-voxl2_io_bridge"
 adb shell "cd /usr/bin; /bin/ln -f -s px4 px4-motor_ramp"
 adb shell "cd /usr/bin; /bin/ln -f -s px4 px4-modalai_gps_timer"
 adb shell "cd /usr/bin; /bin/ln -f -s px4 px4-ms4525_airspeed"
@@ -119,6 +127,7 @@ adb shell "cd /usr/bin; /bin/ln -f -s px4 px4-rc_controller"
 adb shell "cd /usr/bin; /bin/ln -f -s px4 px4-uart_esc_driver"
 adb shell "cd /usr/bin; /bin/ln -f -s px4 px4-flight_mode_manager"
 adb shell "cd /usr/bin; /bin/ln -f -s px4 px4-imu_server"
+adb shell "cd /usr/bin; /bin/ln -f -s px4 px4-apps_sbus"

 # Make sure any required directories exist
 adb shell "/bin/mkdir -p /data/px4/param"
@@ -0,0 +1,13 @@
+#!/bin/bash
+
+cd Tools/simulation/gazebo-classic/sitl_gazebo-classic/src
+patch < ../../../../../boards/modalai/voxl2/gazebo-docker/patch/mavlink_interface.patch
+cd -
+
+cd Tools/simulation/gazebo-classic/sitl_gazebo-classic/models/iris_hitl
+patch < ../../../../../../boards/modalai/voxl2/gazebo-docker/patch/iris_hitl.patch
+cd -
+
+cd Tools/simulation/gazebo-classic/sitl_gazebo-classic/models/iris_vision
+patch < ../../../../../../boards/modalai/voxl2/gazebo-docker/patch/iris_vision.patch
+cd -
@@ -41,3 +41,6 @@ add_library(drivers_board
 	board_config.h
 	init.c
    )
+
+# Add custom drivers
+add_subdirectory(${PX4_BOARD_DIR}/src/drivers/apps_sbus)
@@ -51,4 +51,5 @@
 #define BOARD_OVERRIDE_UUID "MODALAIVOXL20000" // must be of length 16
 #define PX4_SOC_ARCH_ID PX4_SOC_ARCH_ID_VOXL2

-#define MODAL_IO_DEFAULT_PORT 	"2"
+#define VOXL_ESC_DEFAULT_PORT 	"2"
+#define VOXL2_IO_DEFAULT_PORT 	"2"
@@ -0,0 +1,41 @@
+############################################################################
+#
+#   Copyright (c) 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.
+#
+############################################################################
+
+px4_add_module(
+	MODULE drivers__apps_sbus
+	MAIN apps_sbus
+	COMPILE_FLAGS
+		-Wno-cast-align # TODO: fix and enable
+	SRCS
+		apps_sbus.cpp
+	)
@@ -0,0 +1,453 @@
+/****************************************************************************
+ *
+ *   Copyright (c) 2012-2020 PX4 Development Team. All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright
+ *    notice, this list of conditions and the following disclaimer.
+ * 2. Redistributions in binary form must reproduce the above copyright
+ *    notice, this list of conditions and the following disclaimer in
+ *    the documentation and/or other materials provided with the
+ *    distribution.
+ * 3. Neither the name PX4 nor the names of its contributors may be
+ *    used to endorse or promote products derived from this software
+ *    without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
+ * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
+ * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *
+ ****************************************************************************/
+
+
+#include <string>
+#include <px4_log.h>
+#include <px4_platform_common/tasks.h>
+#include <px4_platform_common/getopt.h>
+#include <uORB/PublicationMulti.hpp>
+#include <drivers/drv_hrt.h>
+#include <uORB/topics/input_rc.h>
+#include <lib/parameters/param.h>
+#include <poll.h>
+#include <termios.h>
+
+#include "protocol.h"
+
+extern "C" { __EXPORT int apps_sbus_main(int argc, char *argv[]); }
+
+namespace apps_sbus
+{
+
+std::string _port = "";
+int _uart_fd = -1;
+IOPacket _packet;
+bool _initialized = false;
+bool _is_running = false;
+uint64_t _rc_last_valid;		///< last valid timestamp
+uint16_t _rc_valid_update_count = 0;
+
+static px4_task_t _task_handle = -1;
+
+uORB::PublicationMulti<input_rc_s> _rc_pub{ORB_ID(input_rc)};
+
+int bus_exchange(IOPacket *packet)
+{
+	int ret = 0;
+	int read_retries = 3;
+	int read_succeeded = 0;
+	int packet_size = sizeof(IOPacket);
+
+	tcflush(_uart_fd, TCIOFLUSH);
+
+	ret = ::write(_uart_fd, packet, packet_size);
+
+	usleep(100);
+
+	struct pollfd fd_monitor;
+	fd_monitor.fd     = _uart_fd;
+	fd_monitor.events = POLLIN;
+
+	while (read_retries) {
+		(void) ::poll(&fd_monitor, 1, 1000);
+		ret = ::read(_uart_fd, packet, sizeof(IOPacket));
+
+		if (ret) {
+			// PX4_INFO("Read %d bytes", ret);
+
+			/* Check CRC */
+			uint8_t crc = packet->crc;
+			packet->crc = 0;
+
+			if (crc != crc_packet(packet)) {
+				PX4_ERR("PX4IO packet CRC error");
+				return -EIO;
+
+			} else if (PKT_CODE(*packet) == PKT_CODE_CORRUPT) {
+				PX4_ERR("PX4IO packet corruption");
+				return -EIO;
+
+			} else {
+				read_succeeded = 1;
+				break;
+			}
+		}
+
+		PX4_ERR("Read attempt %d failed", read_retries);
+		read_retries--;
+	}
+
+
+	if (! read_succeeded) {
+		return -EIO;
+	}
+
+	return 0;
+}
+
+int io_reg_get(uint8_t page, uint8_t offset, uint16_t *values, unsigned num_values)
+{
+	/* range check the transfer */
+	// if (num_values > ((_max_transfer) / sizeof(*values))) {
+	// 	PX4_ERR("io_reg_get: too many registers (%u, max %u)", num_values, _max_transfer / 2);
+	// 	return -1;
+	// }
+
+	// int ret = _interface->read((page << 8) | offset, reinterpret_cast<void *>(values), num_values);
+	int ret = 0;
+
+	_packet.count_code = num_values | PKT_CODE_READ;
+	_packet.page = page;
+	_packet.offset = offset;
+
+	_packet.crc = 0;
+	_packet.crc = crc_packet(&_packet);
+
+	ret = bus_exchange(&_packet);
+
+	if (ret != 0) {
+		// PX4_ERR("px4io io_reg_get(%hhu,%hhu,%u): data error %d", page, offset, num_values, ret);
+		return -1;
+	}
+
+	memcpy(values, &_packet.regs[0], num_values * 2);
+
+	return OK;
+}
+
+uint32_t io_reg_get(uint8_t page, uint8_t offset)
+{
+	uint16_t value;
+
+	if (io_reg_get(page, offset, &value, 1) != OK) {
+		// Registers are only 16 bit so any value over 0xFFFF can signal a fault
+		return 0xFFFFFFFF;
+	}
+
+	return value;
+}
+
+int initialize()
+{
+	if (_initialized) {
+		// Already successfully initialized
+		return 0;
+	}
+
+	if (_uart_fd < 0) {
+		_uart_fd = open("/dev/ttyHS1", O_RDWR | O_NONBLOCK);
+
+		if (_uart_fd < 0) {
+			PX4_ERR("Open failed in %s", __FUNCTION__);
+			return -1;
+
+		} else {
+			PX4_INFO("serial port fd %d", _uart_fd);
+		}
+
+		// Configuration copied from dsm_config
+		struct termios uart_config;
+
+		int termios_state;
+
+		/* fill the struct for the new configuration */
+		tcgetattr(_uart_fd, &uart_config);
+
+		/* properly configure the terminal (see also https://en.wikibooks.org/wiki/Serial_Programming/termios ) */
+
+		//
+		// Input flags - Turn off input processing
+		//
+		// convert break to null byte, no CR to NL translation,
+		// no NL to CR translation, don't mark parity errors or breaks
+		// no input parity check, don't strip high bit off,
+		// no XON/XOFF software flow control
+		//
+		uart_config.c_iflag &= ~(IGNBRK | BRKINT | ICRNL |
+					 INLCR | PARMRK | INPCK | ISTRIP | IXON);
+		//
+		// Output flags - Turn off output processing
+		//
+		// no CR to NL translation, no NL to CR-NL translation,
+		// no NL to CR translation, no column 0 CR suppression,
+		// no Ctrl-D suppression, no fill characters, no case mapping,
+		// no local output processing
+		//
+		// config.c_oflag &= ~(OCRNL | ONLCR | ONLRET |
+		//                     ONOCR | ONOEOT| OFILL | OLCUC | OPOST);
+		uart_config.c_oflag = 0;
+
+		//
+		// No line processing
+		//
+		// echo off, echo newline off, canonical mode off,
+		// extended input processing off, signal chars off
+		//
+		uart_config.c_lflag &= ~(ECHO | ECHONL | ICANON | IEXTEN | ISIG);
+
+		/* no parity, one stop bit, disable flow control */
+		uart_config.c_cflag &= ~(CSTOPB | PARENB | CRTSCTS);
+
+		/* set baud rate */
+		if ((termios_state = cfsetispeed(&uart_config, B921600)) < 0) {
+			PX4_ERR("ERR: %d (cfsetispeed)", termios_state);
+			return -1;
+		}
+
+		if ((termios_state = cfsetospeed(&uart_config, B921600)) < 0) {
+			PX4_ERR("ERR: %d (cfsetospeed)", termios_state);
+			return -1;
+		}
+
+		if ((termios_state = tcsetattr(_uart_fd, TCSANOW, &uart_config)) < 0) {
+			PX4_ERR("ERR: %d (tcsetattr)", termios_state);
+			return -1;
+		}
+
+	}
+
+	if (_uart_fd < 0) {
+		PX4_ERR("Open failed in %s", __FUNCTION__);
+		return -1;
+	}
+
+	// Verify connectivity and version number
+	unsigned protocol = io_reg_get(PX4IO_PAGE_CONFIG, PX4IO_P_CONFIG_PROTOCOL_VERSION);
+
+	if (protocol != PX4IO_PROTOCOL_VERSION) {
+		PX4_ERR("apps_sbus version error: %u", protocol);
+		_uart_fd = -1;
+		return -1;
+	}
+
+	_initialized = true;
+
+	return 0;
+}
+
+void apps_sbus_task()
+{
+
+	uint16_t status_regs[2] {};
+	input_rc_s	rc_val;
+	const unsigned prolog = (PX4IO_P_RAW_RC_BASE - PX4IO_P_RAW_RC_COUNT);
+	uint16_t rc_regs[input_rc_s::RC_INPUT_MAX_CHANNELS + prolog];
+	uint32_t channel_count = 0;
+
+	_is_running = true;
+
+	while (true) {
+
+		usleep(20000); // Update every 20ms
+
+		memset(&rc_val, 0, sizeof(input_rc_s));
+
+		if (io_reg_get(PX4IO_PAGE_STATUS, PX4IO_P_STATUS_FLAGS, &status_regs[0],
+			       sizeof(status_regs) / sizeof(status_regs[0])) == OK) {
+			// PX4_INFO("apps_sbus status 0x%.4x", status_regs[0]);
+			// PX4_INFO("apps_sbus alarms 0x%.4x", status_regs[1]);
+		} else {
+			// PX4_ERR("Failed to read status / alarm registers");
+			continue;
+		}
+
+		/* fetch values from IO */
+
+		// When starting the RC flag will not be okay if the receiver isn't
+		// getting a signal from the transmitter. Once it does, then this flag
+		// will say okay even if later the signal is lost.
+		if (!(status_regs[0] & PX4IO_P_STATUS_FLAGS_RC_OK)) {
+			// PX4_INFO("RC lost status flag set");
+			rc_val.rc_lost = true;
+
+		} else {
+			// PX4_INFO("RC lost status flag is not set");
+			rc_val.rc_lost = false;
+		}
+
+		if (status_regs[0] & PX4IO_P_STATUS_FLAGS_RC_SBUS) {
+			rc_val.input_source = input_rc_s::RC_INPUT_SOURCE_PX4IO_SBUS;
+			// PX4_INFO("Got valid SBUS");
+
+		} else {
+			rc_val.input_source = input_rc_s::RC_INPUT_SOURCE_UNKNOWN;
+			// PX4_INFO("SBUS not valid");
+		}
+
+		rc_val.timestamp = hrt_absolute_time();
+
+		// No point in reading the registers if we haven't acquired a transmitter signal yet
+		if (! rc_val.rc_lost) {
+			if (io_reg_get(PX4IO_PAGE_RAW_RC_INPUT, PX4IO_P_RAW_RC_COUNT, &rc_regs[0],
+				       sizeof(rc_regs) / sizeof(rc_regs[0])) != OK) {
+				// PX4_ERR("Failed to read RC registers");
+				continue;
+				// } else {
+				// 	PX4_INFO("Successfully read RC registers");
+				// 	PX4_INFO("Prolog: %u 0x%.4x 0x%.4x 0x%.4x 0x%.4x 0x%.4x",
+				// 			 rc_regs[0], rc_regs[1], rc_regs[2], rc_regs[3], rc_regs[4], rc_regs[5]);
+			}
+
+			channel_count = rc_regs[PX4IO_P_RAW_RC_COUNT];
+
+			// const uint16_t rc_valid_update_count = rc_regs[PX4IO_P_RAW_FRAME_COUNT];
+			// const bool rc_updated = (rc_valid_update_count != _rc_valid_update_count);
+			//
+			// if (!rc_updated) {
+			// 	PX4_INFO("Didn't get an RC update indication. %u %u", rc_valid_update_count, _rc_valid_update_count);
+			// 	continue;
+			// }
+			//
+			// _rc_valid_update_count = rc_valid_update_count;
+			//
+			// PX4_INFO("Got an RC update indication");
+
+			/* limit the channel count */
+			if (channel_count > input_rc_s::RC_INPUT_MAX_CHANNELS) {
+				// PX4_INFO("Got %u for channel count. Limiting to 18", channel_count);
+				channel_count = input_rc_s::RC_INPUT_MAX_CHANNELS;
+			}
+
+			rc_val.channel_count = channel_count;
+			// PX4_INFO("RC channel count: %u", rc_val.channel_count);
+
+			// rc_val.rc_ppm_frame_length = rc_regs[PX4IO_P_RAW_RC_DATA];
+			rc_val.rc_ppm_frame_length = 0;
+
+			rc_val.rc_failsafe = (rc_regs[PX4IO_P_RAW_RC_FLAGS] & PX4IO_P_RAW_RC_FLAGS_FAILSAFE);
+			// rc_val.rc_lost = !(rc_regs[PX4IO_P_RAW_RC_FLAGS] & PX4IO_P_RAW_RC_FLAGS_RC_OK);
+			rc_val.rc_lost = rc_val.rc_failsafe;
+			rc_val.rc_lost_frame_count = rc_regs[PX4IO_P_RAW_LOST_FRAME_COUNT];
+			rc_val.rc_total_frame_count = rc_regs[PX4IO_P_RAW_FRAME_COUNT];
+
+			if (!rc_val.rc_lost && !rc_val.rc_failsafe) {
+				_rc_last_valid = rc_val.timestamp;
+				rc_val.rssi = rc_regs[PX4IO_P_RAW_RC_NRSSI];
+				rc_val.link_quality = rc_regs[PX4IO_P_RAW_RC_NRSSI];
+
+				/* last thing set are the actual channel values as 16 bit values */
+				for (unsigned i = 0; i < channel_count; i++) {
+					rc_val.values[i] = rc_regs[prolog + i];
+					// PX4_INFO("RC channel %u: %.4u", i, rc_val.values[i]);
+				}
+
+				/* zero the remaining fields */
+				for (unsigned i = channel_count; i < (sizeof(rc_val.values) / sizeof(rc_val.values[0])); i++) {
+					rc_val.values[i] = 0;
+				}
+			}
+
+			rc_val.timestamp_last_signal = _rc_last_valid;
+		}
+
+		_rc_pub.publish(rc_val);
+
+	}
+}
+
+int start(int argc, char *argv[])
+{
+
+	int ch;
+	int myoptind = 1;
+	const char *myoptarg = nullptr;
+
+	while ((ch = px4_getopt(argc, argv, "p:", &myoptind, &myoptarg)) != EOF) {
+		switch (ch) {
+		case 'p':
+			_port = myoptarg;
+			PX4_INFO("Setting port to %s", _port.c_str());
+			break;
+
+		default:
+			break;
+		}
+	}
+
+	if (! _initialized) {
+		if (initialize()) {
+			return -1;
+		}
+	}
+
+	if (_is_running) {
+		PX4_WARN("Already started");
+		return 0;
+	}
+
+	_task_handle = px4_task_spawn_cmd("apps_sbus_main",
+					  SCHED_DEFAULT,
+					  SCHED_PRIORITY_DEFAULT,
+					  2000,
+					  (px4_main_t) &apps_sbus_task,
+					  (char *const *)argv);
+
+	if (_task_handle < 0) {
+		PX4_ERR("task start failed");
+		return -1;
+	}
+
+	return 0;
+}
+
+void
+usage()
+{
+	PX4_INFO("Usage: apps_sbus start [options]");
+	PX4_INFO("Options: -p <number>    uart port number");
+}
+
+} // End namespance apps_sbus
+
+int apps_sbus_main(int argc, char *argv[])
+{
+	int myoptind = 1;
+
+	if (argc <= 1) {
+		apps_sbus::usage();
+		return -1;
+	}
+
+	const char *verb = argv[myoptind];
+
+	if (!strcmp(verb, "start")) {
+		return apps_sbus::start(argc - 1, argv + 1);
+
+	} else {
+		apps_sbus::usage();
+		return -1;
+	}
+
+	return 0;
+}
@@ -0,0 +1,405 @@
+/****************************************************************************
+ *
+ *   Copyright (c) 2012-2017 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 <inttypes.h>
+
+/**
+ * @file protocol.h
+ *
+ * PX4IO interface protocol.
+ *
+ * @author Lorenz Meier <lorenz@px4.io>
+ *
+ * Communication is performed via writes to and reads from 16-bit virtual
+ * registers organised into pages of 255 registers each.
+ *
+ * The first two bytes of each write select a page and offset address
+ * respectively. Subsequent reads and writes increment the offset within
+ * the page.
+ *
+ * Some pages are read- or write-only.
+ *
+ * Note that some pages may permit offset values greater than 255, which
+ * can only be achieved by long writes. The offset does not wrap.
+ *
+ * Writes to unimplemented registers are ignored. Reads from unimplemented
+ * registers return undefined values.
+ *
+ * As convention, values that would be floating point in other parts of
+ * the PX4 system are expressed as signed integer values scaled by 10000,
+ * e.g. control values range from -10000..10000.  Use the REG_TO_SIGNED and
+ * SIGNED_TO_REG macros to convert between register representation and
+ * the signed version, and REG_TO_FLOAT/FLOAT_TO_REG to convert to float.
+ *
+ * Note that the implementation of readable pages prefers registers within
+ * readable pages to be densely packed. Page numbers do not need to be
+ * packed.
+ *
+ * Definitions marked [1] are only valid on PX4IOv1 boards. Likewise,
+ * [2] denotes definitions specific to the PX4IOv2 board.
+ */
+
+/* Per C, this is safe for all 2's complement systems */
+#define REG_TO_SIGNED(_reg)	((int16_t)(_reg))
+#define SIGNED_TO_REG(_signed)	((uint16_t)(_signed))
+
+#define REG_TO_FLOAT(_reg)	((float)REG_TO_SIGNED(_reg) / 10000.0f)
+#define FLOAT_TO_REG(_float)	SIGNED_TO_REG((int16_t)floorf((_float + 0.00005f) * 10000.0f))
+
+#define REG_TO_BOOL(_reg) 	((bool)(_reg))
+
+#define PX4IO_PROTOCOL_VERSION		4
+
+/* maximum allowable sizes on this protocol version */
+#define PX4IO_PROTOCOL_MAX_CONTROL_COUNT	8	/**< The protocol does not support more than set here, individual units might support less - see PX4IO_P_CONFIG_CONTROL_COUNT */
+
+/* static configuration page */
+#define PX4IO_PAGE_CONFIG					0
+#define PX4IO_P_CONFIG_PROTOCOL_VERSION		0	/* PX4IO_PROTOCOL_VERSION */
+#define PX4IO_P_CONFIG_HARDWARE_VERSION		1	/* magic numbers TBD */
+#define PX4IO_P_CONFIG_BOOTLOADER_VERSION	2	/* get this how? */
+#define PX4IO_P_CONFIG_MAX_TRANSFER			3	/* maximum I2C transfer size */
+#define PX4IO_P_CONFIG_CONTROL_COUNT		4	/* hardcoded max control count supported */
+#define PX4IO_P_CONFIG_ACTUATOR_COUNT		5	/* hardcoded max actuator output count */
+#define PX4IO_P_CONFIG_RC_INPUT_COUNT		6	/* hardcoded max R/C input count supported */
+#define PX4IO_P_CONFIG_ADC_INPUT_COUNT		7	/* hardcoded max ADC inputs */
+#define PX4IO_P_CONFIG_RELAY_COUNT			8	/* hardcoded # of relay outputs */
+#define PX4IO_MAX_TRANSFER_LEN				64
+
+/* dynamic status page */
+#define PX4IO_PAGE_STATUS		1
+#define PX4IO_P_STATUS_FREEMEM			0
+#define PX4IO_P_STATUS_CPULOAD			1
+
+#define PX4IO_P_STATUS_FLAGS			2	 /* monitoring flags */
+#define PX4IO_P_STATUS_FLAGS_OUTPUTS_ARMED	(1 << 0) /* arm-ok and locally armed */
+#define PX4IO_P_STATUS_FLAGS_OVERRIDE		(1 << 1) /* in manual override */
+#define PX4IO_P_STATUS_FLAGS_RC_OK		(1 << 2) /* RC input is valid */
+#define PX4IO_P_STATUS_FLAGS_RC_PPM		(1 << 3) /* PPM input is valid */
+#define PX4IO_P_STATUS_FLAGS_RC_DSM		(1 << 4) /* DSM input is valid */
+#define PX4IO_P_STATUS_FLAGS_RC_SBUS		(1 << 5) /* SBUS input is valid */
+#define PX4IO_P_STATUS_FLAGS_FMU_OK		(1 << 6) /* controls from FMU are valid */
+#define PX4IO_P_STATUS_FLAGS_RAW_PWM		(1 << 7) /* raw PWM from FMU is bypassing the mixer */
+#define PX4IO_P_STATUS_FLAGS_MIXER_OK		(1 << 8) /* mixer is OK */
+#define PX4IO_P_STATUS_FLAGS_ARM_SYNC		(1 << 9) /* the arming state between IO and FMU is in sync */
+#define PX4IO_P_STATUS_FLAGS_INIT_OK		(1 << 10) /* initialisation of the IO completed without error */
+#define PX4IO_P_STATUS_FLAGS_FAILSAFE		(1 << 11) /* failsafe is active */
+#define PX4IO_P_STATUS_FLAGS_SAFETY_OFF		(1 << 12) /* safety is off */
+#define PX4IO_P_STATUS_FLAGS_FMU_INITIALIZED	(1 << 13) /* FMU was initialized and OK once */
+#define PX4IO_P_STATUS_FLAGS_RC_ST24		(1 << 14) /* ST24 input is valid */
+#define PX4IO_P_STATUS_FLAGS_RC_SUMD		(1 << 15) /* SUMD input is valid */
+
+#define PX4IO_P_STATUS_ALARMS			3	 /* alarm flags - alarms latch, write 1 to a bit to clear it */
+#define PX4IO_P_STATUS_ALARMS_VBATT_LOW		(1 << 0) /* [1] VBatt is very close to regulator dropout */
+#define PX4IO_P_STATUS_ALARMS_TEMPERATURE	(1 << 1) /* board temperature is high */
+#define PX4IO_P_STATUS_ALARMS_SERVO_CURRENT	(1 << 2) /* [1] servo current limit was exceeded */
+#define PX4IO_P_STATUS_ALARMS_ACC_CURRENT	(1 << 3) /* [1] accessory current limit was exceeded */
+#define PX4IO_P_STATUS_ALARMS_FMU_LOST		(1 << 4) /* timed out waiting for controls from FMU */
+#define PX4IO_P_STATUS_ALARMS_RC_LOST		(1 << 5) /* timed out waiting for RC input */
+#define PX4IO_P_STATUS_ALARMS_PWM_ERROR		(1 << 6) /* PWM configuration or output was bad */
+#define PX4IO_P_STATUS_ALARMS_VSERVO_FAULT	(1 << 7) /* [2] VServo was out of the valid range (2.5 - 5.5 V) */
+
+#define PX4IO_P_STATUS_VSERVO			6	/* [2] servo rail voltage in mV */
+#define PX4IO_P_STATUS_VRSSI			7	/* [2] RSSI voltage */
+#define PX4IO_P_STATUS_PRSSI			8	/* [2] RSSI PWM value */
+
+#define PX4IO_P_STATUS_MIXER			9	 /* mixer actuator limit flags */
+
+/* array of post-mix actuator outputs, -10000..10000 */
+#define PX4IO_PAGE_ACTUATORS		2		/* 0..CONFIG_ACTUATOR_COUNT-1 */
+
+/* array of PWM servo output values, microseconds */
+#define PX4IO_PAGE_SERVOS		3		/* 0..CONFIG_ACTUATOR_COUNT-1 */
+
+/* array of raw RC input values, microseconds */
+#define PX4IO_PAGE_RAW_RC_INPUT		4
+#define PX4IO_P_RAW_RC_COUNT			0	/* number of valid channels */
+#define PX4IO_P_RAW_RC_FLAGS			1	/* RC detail status flags */
+#define PX4IO_P_RAW_RC_FLAGS_FRAME_DROP		(1 << 0) /* single frame drop */
+#define PX4IO_P_RAW_RC_FLAGS_FAILSAFE		(1 << 1) /* receiver is in failsafe mode */
+#define PX4IO_P_RAW_RC_FLAGS_RC_DSM11		(1 << 2) /* DSM decoding is 11 bit mode */
+#define PX4IO_P_RAW_RC_FLAGS_MAPPING_OK		(1 << 3) /* Channel mapping is ok */
+#define PX4IO_P_RAW_RC_FLAGS_RC_OK		(1 << 4) /* RC reception ok */
+
+#define PX4IO_P_RAW_RC_NRSSI			2	/* [2] Normalized RSSI value, 0: no reception, 255: perfect reception */
+#define PX4IO_P_RAW_RC_DATA			3	/* [1] + [2] Details about the RC source (PPM frame length, Spektrum protocol type) */
+#define PX4IO_P_RAW_FRAME_COUNT			4	/* Number of total received frames (wrapping counter) */
+#define PX4IO_P_RAW_LOST_FRAME_COUNT		5	/* Number of total dropped frames (wrapping counter) */
+#define PX4IO_P_RAW_RC_BASE			6	/* CONFIG_RC_INPUT_COUNT channels from here */
+
+/* array of scaled RC input values, -10000..10000 */
+#define PX4IO_PAGE_RC_INPUT		5
+#define PX4IO_P_RC_VALID			0	/* bitmask of valid controls */
+#define PX4IO_P_RC_BASE				1	/* CONFIG_RC_INPUT_COUNT controls from here */
+
+/* array of raw ADC values */
+#define PX4IO_PAGE_RAW_ADC_INPUT	6		/* 0..CONFIG_ADC_INPUT_COUNT-1 */
+
+/* PWM servo information */
+#define PX4IO_PAGE_PWM_INFO		7
+#define PX4IO_RATE_MAP_BASE			0	/* 0..CONFIG_ACTUATOR_COUNT bitmaps of PWM rate groups */
+
+/* setup page */
+#define PX4IO_PAGE_SETUP		50
+#define PX4IO_P_SETUP_FEATURES			0
+#define PX4IO_P_SETUP_FEATURES_SBUS1_OUT	(1 << 0) /**< enable S.Bus v1 output */
+#define PX4IO_P_SETUP_FEATURES_SBUS2_OUT	(1 << 1) /**< enable S.Bus v2 output */
+#define PX4IO_P_SETUP_FEATURES_PWM_RSSI		(1 << 2) /**< enable PWM RSSI parsing */
+#define PX4IO_P_SETUP_FEATURES_ADC_RSSI		(1 << 3) /**< enable ADC RSSI parsing */
+
+#define PX4IO_P_SETUP_ARMING			1	 /* arming controls */
+#define PX4IO_P_SETUP_ARMING_IO_ARM_OK		(1 << 0) /* OK to arm the IO side */
+#define PX4IO_P_SETUP_ARMING_FMU_ARMED		(1 << 1) /* FMU is already armed */
+#define PX4IO_P_SETUP_ARMING_FMU_PREARMED	(1 << 2) /* FMU is already prearmed */
+#define PX4IO_P_SETUP_ARMING_MANUAL_OVERRIDE_OK	(1 << 3) /* OK to switch to manual override via override RC channel */
+#define PX4IO_P_SETUP_ARMING_FAILSAFE_CUSTOM	(1 << 4) /* use custom failsafe values, not 0 values of mixer */
+#define PX4IO_P_SETUP_ARMING_INAIR_RESTART_OK	(1 << 5) /* OK to try in-air restart */
+#define PX4IO_P_SETUP_ARMING_ALWAYS_PWM_ENABLE	(1 << 6) /* Output of PWM right after startup enabled to help ESCs initialize and prevent them from beeping */
+#define PX4IO_P_SETUP_ARMING_RC_HANDLING_DISABLED	(1 << 7) /* Disable the IO-internal evaluation of the RC */
+#define PX4IO_P_SETUP_ARMING_LOCKDOWN		(1 << 8) /* If set, the system operates normally, but won't actuate any servos */
+#define PX4IO_P_SETUP_ARMING_FORCE_FAILSAFE	(1 << 9) /* If set, the system will always output the failsafe values */
+#define PX4IO_P_SETUP_ARMING_TERMINATION_FAILSAFE	(1 << 10) /* If set, the system will never return from a failsafe, but remain in failsafe once triggered. */
+#define PX4IO_P_SETUP_ARMING_OVERRIDE_IMMEDIATE	(1 << 11) /* If set then on FMU failure override is immediate. Othewise it waits for the mode switch to go past the override thrshold */
+
+#define PX4IO_P_SETUP_PWM_RATES			2	/* bitmask, 0 = low rate, 1 = high rate */
+#define PX4IO_P_SETUP_PWM_DEFAULTRATE		3	/* 'low' PWM frame output rate in Hz */
+#define PX4IO_P_SETUP_PWM_ALTRATE		4	/* 'high' PWM frame output rate in Hz */
+#define PX4IO_P_SETUP_RELAYS_PAD		5
+
+#define PX4IO_P_SETUP_VSERVO_SCALE		6	/* hardware rev [2] servo voltage correction factor (float) */
+#define PX4IO_P_SETUP_DSM			7	/* DSM bind state */
+enum {							/* DSM bind states */
+	dsm_bind_power_down = 0,
+	dsm_bind_power_up,
+	dsm_bind_set_rx_out,
+	dsm_bind_send_pulses,
+	dsm_bind_reinit_uart
+};
+/* 8 */
+#define PX4IO_P_SETUP_SET_DEBUG			9	/* debug level for IO board */
+
+#define PX4IO_P_SETUP_REBOOT_BL			10	/* reboot IO into bootloader */
+#define PX4IO_REBOOT_BL_MAGIC			14662	/* required argument for reboot (random) */
+
+#define PX4IO_P_SETUP_CRC			11	/* get CRC of IO firmware */
+/* storage space of 12 occupied by CRC */
+#define PX4IO_P_SETUP_FORCE_SAFETY_OFF		12	/* force safety switch into
+                                                           'armed' (PWM enabled) state - this is a non-data write and
+                                                           hence index 12 can safely be used. */
+#define PX4IO_P_SETUP_RC_THR_FAILSAFE_US	13	/**< the throttle failsafe pulse length in microseconds */
+
+#define PX4IO_P_SETUP_FORCE_SAFETY_ON		14	/* force safety switch into 'disarmed' (PWM disabled state) */
+#define PX4IO_FORCE_SAFETY_MAGIC		22027	/* required argument for force safety (random) */
+
+#define PX4IO_P_SETUP_PWM_REVERSE		15	/**< Bitmask to reverse PWM channels 1-8 */
+#define PX4IO_P_SETUP_TRIM_ROLL			16	/**< Roll trim, in actuator units */
+#define PX4IO_P_SETUP_TRIM_PITCH		17	/**< Pitch trim, in actuator units */
+#define PX4IO_P_SETUP_TRIM_YAW			18	/**< Yaw trim, in actuator units */
+#define PX4IO_P_SETUP_SCALE_ROLL		19	/**< Roll scale, in actuator units */
+#define PX4IO_P_SETUP_SCALE_PITCH		20	/**< Pitch scale, in actuator units */
+#define PX4IO_P_SETUP_SCALE_YAW			21	/**< Yaw scale, in actuator units */
+
+#define PX4IO_P_SETUP_SBUS_RATE			22	/**< frame rate of SBUS1 output in Hz */
+
+#define PX4IO_P_SETUP_MOTOR_SLEW_MAX		24 	/**< max motor slew rate */
+
+#define PX4IO_P_SETUP_THR_MDL_FAC 		25	/**< factor for modelling motor control signal output to static thrust relationship */
+
+#define PX4IO_P_SETUP_THERMAL			26	/**< thermal management */
+
+#define PX4IO_P_SETUP_AIRMODE 			27 	/**< air-mode */
+
+#define PX4IO_P_SETUP_ENABLE_FLIGHTTERMINATION	28	/**< flight termination; false if the circuit breaker (CBRK_FLIGHTTERM) is set */
+
+#define PX4IO_THERMAL_IGNORE			UINT16_MAX
+#define PX4IO_THERMAL_OFF			0
+#define PX4IO_THERMAL_FULL			10000
+
+/* autopilot control values, -10000..10000 */
+#define PX4IO_PAGE_CONTROLS			51	/**< actuator control groups, one after the other, 8 wide */
+#define PX4IO_P_CONTROLS_GROUP_0		(PX4IO_PROTOCOL_MAX_CONTROL_COUNT * 0)	/**< 0..PX4IO_PROTOCOL_MAX_CONTROL_COUNT - 1 */
+#define PX4IO_P_CONTROLS_GROUP_1		(PX4IO_PROTOCOL_MAX_CONTROL_COUNT * 1)	/**< 0..PX4IO_PROTOCOL_MAX_CONTROL_COUNT - 1 */
+#define PX4IO_P_CONTROLS_GROUP_2		(PX4IO_PROTOCOL_MAX_CONTROL_COUNT * 2)	/**< 0..PX4IO_PROTOCOL_MAX_CONTROL_COUNT - 1 */
+#define PX4IO_P_CONTROLS_GROUP_3		(PX4IO_PROTOCOL_MAX_CONTROL_COUNT * 3)	/**< 0..PX4IO_PROTOCOL_MAX_CONTROL_COUNT - 1 */
+
+#define PX4IO_P_CONTROLS_GROUP_VALID		64
+#define PX4IO_P_CONTROLS_GROUP_VALID_GROUP0	(1 << 0) /**< group 0 is valid / received */
+#define PX4IO_P_CONTROLS_GROUP_VALID_GROUP1	(1 << 1) /**< group 1 is valid / received */
+#define PX4IO_P_CONTROLS_GROUP_VALID_GROUP2	(1 << 2) /**< group 2 is valid / received */
+#define PX4IO_P_CONTROLS_GROUP_VALID_GROUP3	(1 << 3) /**< group 3 is valid / received */
+
+/* raw text load to the mixer parser - ignores offset */
+#define PX4IO_PAGE_MIXERLOAD			52
+
+/* R/C channel config */
+#define PX4IO_PAGE_RC_CONFIG			53		/**< R/C input configuration */
+#define PX4IO_P_RC_CONFIG_MIN			0		/**< lowest input value */
+#define PX4IO_P_RC_CONFIG_CENTER		1		/**< center input value */
+#define PX4IO_P_RC_CONFIG_MAX			2		/**< highest input value */
+#define PX4IO_P_RC_CONFIG_DEADZONE		3		/**< band around center that is ignored */
+#define PX4IO_P_RC_CONFIG_ASSIGNMENT		4		/**< mapped input value */
+#define PX4IO_P_RC_CONFIG_ASSIGNMENT_MODESWITCH	100		/**< magic value for mode switch */
+#define PX4IO_P_RC_CONFIG_OPTIONS		5		/**< channel options bitmask */
+#define PX4IO_P_RC_CONFIG_OPTIONS_ENABLED	(1 << 0)
+#define PX4IO_P_RC_CONFIG_OPTIONS_REVERSE	(1 << 1)
+#define PX4IO_P_RC_CONFIG_STRIDE		6		/**< spacing between channel config data */
+
+/* PWM output - overrides mixer */
+#define PX4IO_PAGE_DIRECT_PWM			54		/**< 0..CONFIG_ACTUATOR_COUNT-1 */
+
+/* PWM failsafe values - zero disables the output */
+#define PX4IO_PAGE_FAILSAFE_PWM			55		/**< 0..CONFIG_ACTUATOR_COUNT-1 */
+
+/* PWM failsafe values - zero disables the output */
+#define PX4IO_PAGE_SENSORS			56		/**< Sensors connected to PX4IO */
+#define PX4IO_P_SENSORS_ALTITUDE		0		/**< Altitude of an external sensor (HoTT or S.BUS2) */
+
+/* Debug and test page - not used in normal operation */
+#define PX4IO_PAGE_TEST				127
+#define PX4IO_P_TEST_LED			0		/**< set the amber LED on/off */
+
+/* PWM minimum values for certain ESCs */
+#define PX4IO_PAGE_CONTROL_MIN_PWM		106		/**< 0..CONFIG_ACTUATOR_COUNT-1 */
+
+/* PWM maximum values for certain ESCs */
+#define PX4IO_PAGE_CONTROL_MAX_PWM		107		/**< 0..CONFIG_ACTUATOR_COUNT-1 */
+
+/* PWM mtrim values for central position */
+#define PX4IO_PAGE_CONTROL_TRIM_PWM		108		/**< 0..CONFIG_ACTUATOR_COUNT-1 */
+
+/* PWM disarmed values that are active, even when SAFETY_SAFE */
+#define PX4IO_PAGE_DISARMED_PWM		109			/* 0..CONFIG_ACTUATOR_COUNT-1 */
+
+/**
+ * As-needed mixer data upload.
+ *
+ * This message adds text to the mixer text buffer; the text
+ * buffer is drained as the definitions are consumed.
+ */
+#pragma pack(push, 1)
+struct px4io_mixdata {
+	uint16_t	f2i_mixer_magic;
+#define F2I_MIXER_MAGIC		0x6d74
+
+	uint8_t		action;
+#define F2I_MIXER_ACTION_RESET			0
+#define F2I_MIXER_ACTION_APPEND			1
+
+	char		text[0];	/* actual text size may vary */
+};
+#pragma pack(pop)
+
+/**
+ * Serial protocol encapsulation.
+ */
+
+#define PKT_MAX_REGS	32 // by agreement w/FMU
+
+#pragma pack(push, 1)
+struct IOPacket {
+	uint8_t 	count_code;
+	uint8_t 	crc;
+	uint8_t 	page;
+	uint8_t 	offset;
+	uint16_t	regs[PKT_MAX_REGS];
+};
+#pragma pack(pop)
+
+#if (PX4IO_MAX_TRANSFER_LEN > PKT_MAX_REGS * 2)
+#error The max transfer length of the IO protocol must not be larger than the IO packet size
+#endif
+
+#define PKT_CODE_READ		0x00	/* FMU->IO read transaction */
+#define PKT_CODE_WRITE		0x40	/* FMU->IO write transaction */
+#define PKT_CODE_SUCCESS	0x00	/* IO->FMU success reply */
+#define PKT_CODE_CORRUPT	0x40	/* IO->FMU bad packet reply */
+#define PKT_CODE_ERROR		0x80	/* IO->FMU register op error reply */
+
+#define PKT_CODE_MASK		0xc0
+#define PKT_COUNT_MASK		0x3f
+
+#define PKT_COUNT(_p)	((_p).count_code & PKT_COUNT_MASK)
+#define PKT_CODE(_p)	((_p).count_code & PKT_CODE_MASK)
+#define PKT_SIZE(_p)	((size_t)((uint8_t *)&((_p).regs[PKT_COUNT(_p)]) - ((uint8_t *)&(_p))))
+
+static const uint8_t crc8_tab[256] __attribute__((unused)) = {
+	0x00, 0x07, 0x0E, 0x09, 0x1C, 0x1B, 0x12, 0x15,
+	0x38, 0x3F, 0x36, 0x31, 0x24, 0x23, 0x2A, 0x2D,
+	0x70, 0x77, 0x7E, 0x79, 0x6C, 0x6B, 0x62, 0x65,
+	0x48, 0x4F, 0x46, 0x41, 0x54, 0x53, 0x5A, 0x5D,
+	0xE0, 0xE7, 0xEE, 0xE9, 0xFC, 0xFB, 0xF2, 0xF5,
+	0xD8, 0xDF, 0xD6, 0xD1, 0xC4, 0xC3, 0xCA, 0xCD,
+	0x90, 0x97, 0x9E, 0x99, 0x8C, 0x8B, 0x82, 0x85,
+	0xA8, 0xAF, 0xA6, 0xA1, 0xB4, 0xB3, 0xBA, 0xBD,
+	0xC7, 0xC0, 0xC9, 0xCE, 0xDB, 0xDC, 0xD5, 0xD2,
+	0xFF, 0xF8, 0xF1, 0xF6, 0xE3, 0xE4, 0xED, 0xEA,
+	0xB7, 0xB0, 0xB9, 0xBE, 0xAB, 0xAC, 0xA5, 0xA2,
+	0x8F, 0x88, 0x81, 0x86, 0x93, 0x94, 0x9D, 0x9A,
+	0x27, 0x20, 0x29, 0x2E, 0x3B, 0x3C, 0x35, 0x32,
+	0x1F, 0x18, 0x11, 0x16, 0x03, 0x04, 0x0D, 0x0A,
+	0x57, 0x50, 0x59, 0x5E, 0x4B, 0x4C, 0x45, 0x42,
+	0x6F, 0x68, 0x61, 0x66, 0x73, 0x74, 0x7D, 0x7A,
+	0x89, 0x8E, 0x87, 0x80, 0x95, 0x92, 0x9B, 0x9C,
+	0xB1, 0xB6, 0xBF, 0xB8, 0xAD, 0xAA, 0xA3, 0xA4,
+	0xF9, 0xFE, 0xF7, 0xF0, 0xE5, 0xE2, 0xEB, 0xEC,
+	0xC1, 0xC6, 0xCF, 0xC8, 0xDD, 0xDA, 0xD3, 0xD4,
+	0x69, 0x6E, 0x67, 0x60, 0x75, 0x72, 0x7B, 0x7C,
+	0x51, 0x56, 0x5F, 0x58, 0x4D, 0x4A, 0x43, 0x44,
+	0x19, 0x1E, 0x17, 0x10, 0x05, 0x02, 0x0B, 0x0C,
+	0x21, 0x26, 0x2F, 0x28, 0x3D, 0x3A, 0x33, 0x34,
+	0x4E, 0x49, 0x40, 0x47, 0x52, 0x55, 0x5C, 0x5B,
+	0x76, 0x71, 0x78, 0x7F, 0x6A, 0x6D, 0x64, 0x63,
+	0x3E, 0x39, 0x30, 0x37, 0x22, 0x25, 0x2C, 0x2B,
+	0x06, 0x01, 0x08, 0x0F, 0x1A, 0x1D, 0x14, 0x13,
+	0xAE, 0xA9, 0xA0, 0xA7, 0xB2, 0xB5, 0xBC, 0xBB,
+	0x96, 0x91, 0x98, 0x9F, 0x8A, 0x8D, 0x84, 0x83,
+	0xDE, 0xD9, 0xD0, 0xD7, 0xC2, 0xC5, 0xCC, 0xCB,
+	0xE6, 0xE1, 0xE8, 0xEF, 0xFA, 0xFD, 0xF4, 0xF3
+};
+
+static uint8_t crc_packet(struct IOPacket *pkt) __attribute__((unused));
+static uint8_t
+crc_packet(struct IOPacket *pkt)
+{
+	uint8_t *end = (uint8_t *)(&pkt->regs[PKT_COUNT(*pkt)]);
+	uint8_t *p = (uint8_t *)pkt;
+	uint8_t c = 0;
+
+	while (p < end) {
+		c = crc8_tab[c ^ * (p++)];
+	}
+
+	return c;
+}
@@ -4,7 +4,9 @@ CONFIG_FILE="/etc/modalai/voxl-px4.conf"

 GPS=NONE
 RC=SPEKTRUM
-IMU_ROTATION=NONE
+ESC=VOXL_ESC
+POWER_MANAGER=VOXLPM
+DISTANCE_SENSOR=NONE
 OSD=DISABLE
 DAEMON_MODE=DISABLE
 SENSOR_CAL=ACTUAL
@@ -39,9 +41,8 @@ fi
 print_usage() {
    echo -e "\nUsage: voxl-px4 [-b (Specify Holybro GPS unit)]"
    echo "                [-c delete configuration file and exit]"
-    echo "                [-d start px4 in daemon mode]"
+    echo "                [-d start px4 without daemon mode]"
    echo "                [-f (Use fake rc input instead of from a real transmitter)]"
-    echo "                [-i (Set IMU_ROTATION to ROTATION_YAW_180)]"
    echo "                [-m (Specify Matek GPS unit)]"
    echo "                [-o (Start OSD module on the apps processor)]"
    echo "                [-r (Specify TBS Crossfire RC receiver, MAVLINK)]"
@@ -56,7 +57,9 @@ print_config_settings(){
    echo -e "\n*************************"
    echo "GPS=$GPS"
    echo "RC=$RC"
-    echo "IMU_ROTATION=$IMU_ROTATION"
+    echo "ESC=$ESC"
+    echo "POWER MANAGER=$POWER_MANAGER"
+    echo "DISTANCE SENSOR=$DISTANCE_SENSOR"
    echo "OSD=$OSD"
    echo "DAEMON_MODE=$DAEMON_MODE"
    echo "SENSOR_CAL=$SENSOR_CAL"
@@ -68,7 +71,7 @@ print_config_settings(){
    echo -e "*************************\n"
 }

-while getopts "bcdhifmorwz" flag
+while getopts "bcdhfmorwz" flag
 do
    case "${flag}" in
        b) 
@@ -83,16 +86,12 @@ do
            exit 0
            ;;
        d) 
-            echo "[INFO] Enabling daemon mode"
-            DAEMON_MODE=ENABLE
+            echo "[INFO] Disabling daemon mode"
+            DAEMON_MODE=DISABLE
            ;;
        h) 
            print_usage
            ;;
-        i) 
-            echo "[INFO] Setting IMU_ROTATION to 4: ROTATION_YAW_180"
-            IMU_ROTATION=4
-            ;;
        f) 
            echo "[INFO] Setting RC to FAKE_RC_INPUT"
            RC=FAKE_RC_INPUT
@@ -144,4 +143,5 @@ fi

 print_config_settings

-GPS=$GPS RC=$RC OSD=$OSD IMU_ROTATION=$IMU_ROTATION EXTRA_STEPS=$EXTRA_STEPS px4 $DAEMON -s /usr/bin/voxl-px4-start
+GPS=$GPS RC=$RC ESC=$ESC POWER_MANAGER=$POWER_MANAGER DISTANCE_SENSOR=$DISTANCE_SENSOR \
+OSD=$OSD EXTRA_STEPS=$EXTRA_STEPS px4 $DAEMON -s /usr/bin/voxl-px4-start
--- a/Show More
+++ b/Show More