rc_input: correctly init SBUS inversion for other RC protocols

boards: mro_ctrl-zero-classic updates to sync with ctrl-zero-h7-oem
ekf2: remove unnecessary const references
2026-06-05 05:20:05 +08:00 · 2022-04-30 13:36:20 -04:00 · 2022-04-30 09:40:52 -04:00 · 2022-04-29 21:39:02 -04:00 · 2022-04-29 13:49:32 -04:00 · 2022-04-29 07:21:58 +02:00
56 changed files with 340 additions and 377 deletions
@@ -330,7 +330,7 @@ px4io_update: px4_io-v2_default cubepilot_io-v2_default
 	cp build/cubepilot_io-v2_default/cubepilot_io-v2_default.bin boards/cubepilot/cubeyellow/extras/cubepilot_io-v2_default.bin
 	git status

-bootloaders_update: cuav_nora_bootloader cuav_x7pro_bootloader cubepilot_cubeorange_bootloader holybro_durandal-v1_bootloader matek_h743-slim_bootloader modalai_fc-v2_bootloader mro_ctrl-zero-h7_bootloader mro_ctrl-zero-h7-oem_bootloader mro_pixracerpro_bootloader px4_fmu-v6u_bootloader px4_fmu-v6x_bootloader
+bootloaders_update: cuav_nora_bootloader cuav_x7pro_bootloader cubepilot_cubeorange_bootloader holybro_durandal-v1_bootloader matek_h743-slim_bootloader modalai_fc-v2_bootloader mro_ctrl-zero-classic_bootloader mro_ctrl-zero-h7_bootloader mro_ctrl-zero-h7-oem_bootloader mro_pixracerpro_bootloader px4_fmu-v6u_bootloader px4_fmu-v6x_bootloader
 	git status

 .PHONY: coverity_scan
@@ -113,7 +113,7 @@
 #define RC_SERIAL_PORT		"/dev/ttyS0"

 // #define GPIO_RSSI_IN                (GPIO_INPUT|GPIO_PULLUP|GPIO_PORTC|GPIO_PIN1)
-#define GPIO_SBUS_INV                  (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_2MHz|GPIO_OUTPUT_SET|GPIO_PORTA|GPIO_PIN10)
+#define GPIO_SBUS_INV                  (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_2MHz|GPIO_OUTPUT_CLEAR|GPIO_PORTA|GPIO_PIN10)
 #define RC_INVERT_INPUT(_invert_true)  px4_arch_gpiowrite(GPIO_SBUS_INV, _invert_true);

 #define GPIO_FRSKY_INV                (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_2MHz|GPIO_OUTPUT_SET|GPIO_PORTB|GPIO_PIN12)
@@ -149,7 +149,6 @@

 /* RC Serial port */
 #define RC_SERIAL_PORT                     "/dev/ttyS5"
-#define RC_SERIAL_SINGLEWIRE

 /* power on/off */
 #define MS_PWR_BUTTON_DOWN 750
@@ -160,7 +160,6 @@
 #define HRT_PPM_CHANNEL         /* T3C1 */  1  /* use capture/compare channel 1 */
 #define GPIO_PPM_IN             /* PB4 T3C1 */ GPIO_TIM3_CH1IN_2
 #define RC_SERIAL_PORT          "/dev/ttyS5"
-#define RC_SERIAL_SINGLEWIRE

 /**
 * GPIO PPM_IN on PB4 T3C1
@@ -169,7 +168,6 @@
 */
 #define GPIO_PPM_IN_AS_OUT           (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_2MHz|GPIO_OUTPUT_SET|GPIO_PORTB|GPIO_PIN4)
 #define SPEKTRUM_RX_AS_GPIO_OUTPUT() px4_arch_configgpio(GPIO_PPM_IN_AS_OUT)
-#define SPEKTRUM_RX_AS_UART()       /* Can be left as uart */
 #define SPEKTRUM_OUT(_one_true)      px4_arch_gpiowrite(GPIO_PPM_IN_AS_OUT, (_one_true))

 /* RSSI_IN */
@@ -160,7 +160,6 @@
 #define HRT_PPM_CHANNEL         /* T3C1 */  1  /* use capture/compare channel 1 */
 #define GPIO_PPM_IN             /* PB4 T3C1 */ GPIO_TIM3_CH1IN_2
 #define RC_SERIAL_PORT          "/dev/ttyS5"
-#define RC_SERIAL_SINGLEWIRE

 /**
 * GPIO PPM_IN on PB4 T3C1
@@ -169,7 +168,6 @@
 */
 #define GPIO_PPM_IN_AS_OUT           (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_2MHz|GPIO_OUTPUT_SET|GPIO_PORTB|GPIO_PIN4)
 #define SPEKTRUM_RX_AS_GPIO_OUTPUT() px4_arch_configgpio(GPIO_PPM_IN_AS_OUT)
-#define SPEKTRUM_RX_AS_UART()       /* Can be left as uart */
 #define SPEKTRUM_OUT(_one_true)      px4_arch_gpiowrite(GPIO_PPM_IN_AS_OUT, (_one_true))

 /* RSSI_IN */
@@ -141,12 +141,6 @@
 * SPEKTRUM_RX (it's TX or RX in Bind) on PA10 UART1
 * The FMU can drive GPIO PPM_IN as an output
 */
-// TODO?
-//#define GPIO_PPM_IN_AS_OUT            (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_2MHz|GPIO_OUTPUT_SET|GPIO_PORTC|GPIO_PIN6)
-//#define SPEKTRUM_RX_AS_GPIO_OUTPUT()  px4_arch_configgpio(GPIO_PPM_IN_AS_OUT)
-//#define SPEKTRUM_RX_AS_UART()         px4_arch_configgpio(GPIO_USART1_RX)
-//#define SPEKTRUM_OUT(_one_true)       px4_arch_gpiowrite(GPIO_PPM_IN_AS_OUT, (_one_true))
-
 #define BOARD_HAS_PWM    DIRECT_PWM_OUTPUT_CHANNELS

 /* This board provides a DMA pool and APIs */
@@ -30,7 +30,7 @@ CONFIG_BOARD_RESET_ON_ASSERT=2
 CONFIG_C99_BOOL8=y
 CONFIG_CDCACM=y
 CONFIG_CDCACM_IFLOWCONTROL=y
-CONFIG_CDCACM_PRODUCTID=0x004b
+CONFIG_CDCACM_PRODUCTID=0x0050
 CONFIG_CDCACM_PRODUCTSTR="PX4 BL Holybro KakuteH7"
 CONFIG_CDCACM_RXBUFSIZE=600
 CONFIG_CDCACM_TXBUFSIZE=12000
@@ -281,9 +281,7 @@
 #define GPIO_PPM_IN             /* PI5 T8C1 */ GPIO_TIM8_CH1IN_2

 /* RC Serial port */
-
 #define RC_SERIAL_PORT                     "/dev/ttyS4"
-#define RC_SERIAL_SINGLEWIRE

 /* PWM input driver. Use FMU AUX5 pins attached to timer4 channel 2 */
 #define PWMIN_TIMER                       4
@@ -327,7 +325,6 @@

 #define GPIO_PPM_IN_AS_OUT             (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_2MHz|GPIO_OUTPUT_SET|GPIO_PORTI|GPIO_PIN5)
 #define SPEKTRUM_RX_AS_GPIO_OUTPUT()   px4_arch_configgpio(GPIO_PPM_IN_AS_OUT)
-#define SPEKTRUM_RX_AS_UART()          /* Can be left as uart */
 #define SPEKTRUM_OUT(_one_true)        px4_arch_gpiowrite(GPIO_PPM_IN_AS_OUT, (_one_true))

 #define SDIO_SLOTNO                    0  /* Only one slot */
@@ -205,7 +205,6 @@

 #define SPEKTRUM_RX_AS_OUT             (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_2MHz|GPIO_OUTPUT_SET|GPIO_PORTC|GPIO_PIN7)
 #define SPEKTRUM_RX_AS_GPIO_OUTPUT()   px4_arch_configgpio(SPEKTRUM_RX_AS_OUT)
-#define SPEKTRUM_RX_AS_UART()          /* Can be left as uart */
 #define SPEKTRUM_OUT(_one_true)        px4_arch_gpiowrite(SPEKTRUM_RX_AS_OUT, (_one_true))

 #define SDIO_SLOTNO                    0  /* Only one slot */
@@ -264,7 +264,6 @@

 #define SPEKTRUM_RX_AS_OUT             (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_2MHz|GPIO_OUTPUT_SET|GPIO_PORTC|GPIO_PIN7)
 #define SPEKTRUM_RX_AS_GPIO_OUTPUT()   px4_arch_configgpio(SPEKTRUM_RX_AS_OUT)
-#define SPEKTRUM_RX_AS_UART()          /* Can be left as uart */
 #define SPEKTRUM_OUT(_one_true)        px4_arch_gpiowrite(SPEKTRUM_RX_AS_OUT, (_one_true))

 #define SDIO_SLOTNO                    0  /* Only one slot */
@@ -39,6 +39,7 @@ CONFIG_MODULES_ATTITUDE_ESTIMATOR_Q=y
 CONFIG_MODULES_BATTERY_STATUS=y
 CONFIG_MODULES_CAMERA_FEEDBACK=y
 CONFIG_MODULES_COMMANDER=y
+CONFIG_MODULES_CONTROL_ALLOCATOR=y
 CONFIG_MODULES_DATAMAN=y
 CONFIG_MODULES_EKF2=y
 CONFIG_MODULES_ESC_BATTERY=y
@@ -55,14 +56,13 @@ CONFIG_MODULES_LOAD_MON=y
 CONFIG_MODULES_LOCAL_POSITION_ESTIMATOR=y
 CONFIG_MODULES_LOGGER=y
 CONFIG_MODULES_MAG_BIAS_ESTIMATOR=y
-CONFIG_MODULES_MAVLINK=y
 CONFIG_MODULES_MANUAL_CONTROL=y
+CONFIG_MODULES_MAVLINK=y
 CONFIG_MODULES_MC_ATT_CONTROL=y
 CONFIG_MODULES_MC_AUTOTUNE_ATTITUDE_CONTROL=y
 CONFIG_MODULES_MC_HOVER_THRUST_ESTIMATOR=y
 CONFIG_MODULES_MC_POS_CONTROL=y
 CONFIG_MODULES_MC_RATE_CONTROL=y
-CONFIG_MODULES_CONTROL_ALLOCATOR=y
 CONFIG_MODULES_NAVIGATOR=y
 CONFIG_MODULES_RC_UPDATE=y
 CONFIG_MODULES_ROVER_POS_CONTROL=y
@@ -71,20 +71,18 @@ CONFIG_MODULES_SIH=y
 CONFIG_MODULES_TEMPERATURE_COMPENSATION=y
 CONFIG_MODULES_UUV_ATT_CONTROL=y
 CONFIG_MODULES_UUV_POS_CONTROL=y
-CONFIG_MODULES_VMOUNT=y
+CONFIG_MODULES_GIMBAL=y
 CONFIG_MODULES_VTOL_ATT_CONTROL=y
-CONFIG_SYSTEMCMDS_BL_UPDATE=y
 CONFIG_SYSTEMCMDS_ACTUATOR_TEST=y
+CONFIG_SYSTEMCMDS_BL_UPDATE=y
 CONFIG_SYSTEMCMDS_DMESG=y
 CONFIG_SYSTEMCMDS_DUMPFILE=y
-CONFIG_SYSTEMCMDS_ESC_CALIB=y
 CONFIG_SYSTEMCMDS_GPIO=y
 CONFIG_SYSTEMCMDS_HARDFAULT_LOG=y
 CONFIG_SYSTEMCMDS_I2CDETECT=y
 CONFIG_SYSTEMCMDS_LED_CONTROL=y
 CONFIG_SYSTEMCMDS_MFT=y
 CONFIG_SYSTEMCMDS_MIXER=y
-CONFIG_SYSTEMCMDS_MOTOR_RAMP=y
 CONFIG_SYSTEMCMDS_MOTOR_TEST=y
 CONFIG_SYSTEMCMDS_MTD=y
 CONFIG_SYSTEMCMDS_NSHTERM=y
@@ -4,7 +4,6 @@
 #------------------------------------------------------------------------------

 param set-default BAT1_V_DIV 10.1
-param set-default BAT1_A_PER_V 24
-
+param set-default BAT1_A_PER_V 17

 safety_button start
@@ -11,11 +11,11 @@
 # CONFIG_STM32H7_SYSCFG is not set
 CONFIG_ARCH="arm"
 CONFIG_ARCH_BOARD_CUSTOM=y
-CONFIG_ARCH_BOARD_CUSTOM_DIR="../nuttx-config"
+CONFIG_ARCH_BOARD_CUSTOM_DIR="../../../../boards/mro/ctrl-zero-classic/nuttx-config"
 CONFIG_ARCH_BOARD_CUSTOM_DIR_RELPATH=y
 CONFIG_ARCH_BOARD_CUSTOM_NAME="px4"
 CONFIG_ARCH_CHIP="stm32h7"
-CONFIG_ARCH_CHIP_STM32H743ZIT6=y
+CONFIG_ARCH_CHIP_STM32H743II=y
 CONFIG_ARCH_CHIP_STM32H7=y
 CONFIG_ARCH_INTERRUPTSTACK=512
 CONFIG_ARMV7M_BASEPRI_WAR=y
@@ -29,6 +29,7 @@ CONFIG_BOARD_LOOPSPERMSEC=95150
 CONFIG_BOARD_RESET_ON_ASSERT=2
 CONFIG_C99_BOOL8=y
 CONFIG_CDCACM=y
+CONFIG_CDCACM_IFLOWCONTROL=y
 CONFIG_CDCACM_PRODUCTID=0x1022
 CONFIG_CDCACM_PRODUCTSTR="PX4 BL mRo ControlZero Classic"
 CONFIG_CDCACM_RXBUFSIZE=600
@@ -222,34 +222,35 @@


 /* UART/USART */
-#define GPIO_USART2_RX   GPIO_USART2_RX_2       /* PD6 */
-#define GPIO_USART2_TX   GPIO_USART2_TX_2       /* PD5 */
-#define GPIO_USART2_RTS  GPIO_USART2_RTS_2      /* PD4 */
-#define GPIO_USART2_CTS  GPIO_USART2_CTS_NSS_2  /* PD3 */
+#define GPIO_USART2_TX   GPIO_USART2_TX_2      /* PD5  */
+#define GPIO_USART2_RX   GPIO_USART2_RX_2      /* PD6  */
+#define GPIO_USART2_CTS  GPIO_USART2_CTS_NSS_2 /* PD3  */
+#define GPIO_USART2_RTS  GPIO_USART2_RTS_2     /* PD4  */

-#define GPIO_USART3_RX   GPIO_USART3_RX_3       /* PD9 */
-#define GPIO_USART3_TX   GPIO_USART3_TX_3       /* PD8 */
-#define GPIO_USART3_RTS  GPIO_USART3_RTS_2      /* PD12 */
-#define GPIO_USART3_CTS  GPIO_USART3_CTS_NSS_2  /* PD11 */
+#define GPIO_USART3_TX   GPIO_USART3_TX_3      /* PD8  */
+#define GPIO_USART3_RX   GPIO_USART3_RX_3      /* PD9  */
+#define GPIO_USART3_CTS  GPIO_USART3_CTS_NSS_2 /* PD11 */
+#define GPIO_USART3_RTS  GPIO_USART3_RTS_2     /* PD12 */

-#define GPIO_UART4_RX    GPIO_UART4_RX_1        /* PA1 */
-#define GPIO_UART4_TX    GPIO_UART4_TX_1        /* PA0 */
+#define GPIO_UART4_TX    GPIO_UART4_TX_2       /* PA0  */
+#define GPIO_UART4_RX    GPIO_UART4_RX_2       /* PA1  */

-#define GPIO_USART6_RX   GPIO_USART6_RX_1       /* PC7 */
-#define GPIO_USART6_TX   GPIO_USART6_TX_2       /* PG14 */
+#define GPIO_USART6_TX   0                     /* USART6 is RX-only */
+#define GPIO_USART6_RX   GPIO_USART6_RX_1      /* PC7  */

-#define GPIO_UART7_RX    GPIO_UART7_RX_1        /* PE7 */
-#define GPIO_UART7_TX    GPIO_UART7_TX_1        /* PE8 */
+#define GPIO_UART7_TX    GPIO_UART7_TX_3       /* PE8  */
+#define GPIO_UART7_RX    GPIO_UART7_RX_3       /* PE7  */

-#define GPIO_UART8_RX    GPIO_UART8_RX_1        /* PE0 */
-#define GPIO_UART8_TX    GPIO_UART8_TX_1        /* PE1 */
+#define GPIO_UART8_TX    GPIO_UART8_TX_1       /* PE1  */
+#define GPIO_UART8_RX    GPIO_UART8_RX_1       /* PE0  */


 /* CAN */
-#define GPIO_CAN1_RX     GPIO_CAN1_RX_3     /* PD0  */
-#define GPIO_CAN1_TX     GPIO_CAN1_TX_3     /* PD1 */
-#define GPIO_CAN2_RX     GPIO_CAN2_RX_1     /* PB12 */
-#define GPIO_CAN2_TX     GPIO_CAN2_TX_1     /* PB13 */
+#define GPIO_CAN1_RX     GPIO_CAN1_RX_3        /* PD0  */
+#define GPIO_CAN1_TX     GPIO_CAN1_TX_3        /* PD1  */
+
+#define GPIO_CAN2_RX     GPIO_CAN2_RX_1        /* PB12 */
+#define GPIO_CAN2_TX     GPIO_CAN2_TX_1        /* PB13 */


 /* SPI */
@@ -28,7 +28,7 @@ CONFIG_ARCH_BOARD_CUSTOM_DIR="../../../../boards/mro/ctrl-zero-classic/nuttx-con
 CONFIG_ARCH_BOARD_CUSTOM_DIR_RELPATH=y
 CONFIG_ARCH_BOARD_CUSTOM_NAME="px4"
 CONFIG_ARCH_CHIP="stm32h7"
-CONFIG_ARCH_CHIP_STM32H743ZIT6=y
+CONFIG_ARCH_CHIP_STM32H743II=y
 CONFIG_ARCH_CHIP_STM32H7=y
 CONFIG_ARCH_INTERRUPTSTACK=512
 CONFIG_ARCH_STACKDUMP=y
@@ -38,6 +38,7 @@ CONFIG_ARMV7M_DTCM=y
 CONFIG_ARMV7M_ICACHE=y
 CONFIG_ARMV7M_MEMCPY=y
 CONFIG_ARMV7M_USEBASEPRI=y
+CONFIG_ARM_MPU_EARLY_RESET=y
 CONFIG_BOARDCTL_RESET=y
 CONFIG_BOARD_CRASHDUMP=y
 CONFIG_BOARD_LOOPSPERMSEC=95150
@@ -45,6 +46,7 @@ CONFIG_BOARD_RESET_ON_ASSERT=2
 CONFIG_BUILTIN=y
 CONFIG_C99_BOOL8=y
 CONFIG_CDCACM=y
+CONFIG_CDCACM_IFLOWCONTROL=y
 CONFIG_CDCACM_PRODUCTID=0x1022
 CONFIG_CDCACM_PRODUCTSTR="mRoControlZero Classic"
 CONFIG_CDCACM_RXBUFSIZE=600
@@ -73,6 +75,7 @@ CONFIG_FS_FAT=y
 CONFIG_FS_FATTIME=y
 CONFIG_FS_PROCFS=y
 CONFIG_FS_PROCFS_INCLUDE_PROGMEM=y
+CONFIG_FS_PROCFS_MAX_TASKS=64
 CONFIG_FS_PROCFS_REGISTER=y
 CONFIG_FS_ROMFS=y
 CONFIG_GRAN=y
@@ -85,10 +88,8 @@ CONFIG_IDLETHREAD_STACKSIZE=750
 CONFIG_LIBC_FLOATINGPOINT=y
 CONFIG_LIBC_LONG_LONG=y
 CONFIG_LIBC_STRERROR=y
-CONFIG_FS_PROCFS_MAX_TASKS=64
 CONFIG_MEMSET_64BIT=y
 CONFIG_MEMSET_OPTSPEED=y
-CONFIG_MKFATFS_BUFFER_ALIGMENT=32
 CONFIG_MMCSD=y
 CONFIG_MMCSD_SDIO=y
 CONFIG_MMCSD_SDIOWAIT_WRCOMPLETE=y
@@ -199,7 +199,6 @@ SECTIONS
 	} > AXI_SRAM

 	/* Emit the the D3 power domain section for locating BDMA data */
-
 	.sram4_reserve (NOLOAD) :
 	{
 		*(.sram4)
@@ -135,7 +135,6 @@
 */
 #define GPIO_PPM_IN_AS_OUT             (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_2MHz|GPIO_OUTPUT_SET|GPIO_PORTB|GPIO_PIN0)
 #define SPEKTRUM_RX_AS_GPIO_OUTPUT()   px4_arch_configgpio(GPIO_PPM_IN_AS_OUT)
-#define SPEKTRUM_RX_AS_UART()          /* Can be left as uart */
 #define SPEKTRUM_OUT(_one_true)        px4_arch_gpiowrite(GPIO_PPM_IN_AS_OUT, (_one_true))

 /* By Providing BOARD_ADC_USB_CONNECTED (using the px4_arch abstraction)
@@ -156,9 +155,8 @@
 #define BOARD_HAS_STATIC_MANIFEST 1


-#define BOARD_NUM_IO_TIMERS 3
+#define BOARD_NUM_IO_TIMERS 4

-#define BOARD_DSHOT_MOTOR_ASSIGNMENT {3, 2, 1, 0, 4, 5, 6, 7};

 #define BOARD_ENABLE_CONSOLE_BUFFER

@@ -123,16 +123,6 @@ __EXPORT void board_on_reset(int status)
 ************************************************************************************/
 __EXPORT void stm32_boardinitialize(void)
 {
-	// clear all existing MPU configuration from bootloader
-	for (int region = 0; region < CONFIG_ARM_MPU_NREGIONS; region++) {
-		putreg32(region, MPU_RNR);
-		putreg32(0, MPU_RBAR);
-		putreg32(0, MPU_RASR);
-
-		// save
-		putreg32(0, MPU_CTRL);
-	}
-
 	/* Reset PWM first thing */
 	board_on_reset(-1);

@@ -196,12 +186,10 @@ __EXPORT int board_app_initialize(uintptr_t arg)

 	if (!sdio_dev) {
 		syslog(LOG_ERR, "[boot] Failed to initialize SDIO slot %d\n", 0);
-		return ERROR;
 	}

 	if (mmcsd_slotinitialize(0, sdio_dev) != OK) {
 		syslog(LOG_ERR, "[boot] Failed to bind SDIO to the MMC/SD driver\n");
-		return ERROR;
 	}

 	/* Assume that the SD card is inserted.  What choice do we have? */
@@ -158,7 +158,6 @@
 */
 #define GPIO_PPM_IN_AS_OUT             (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_2MHz|GPIO_OUTPUT_SET|GPIO_PORTB|GPIO_PIN0)
 #define SPEKTRUM_RX_AS_GPIO_OUTPUT()   px4_arch_configgpio(GPIO_PPM_IN_AS_OUT)
-#define SPEKTRUM_RX_AS_UART()          /* Can be left as uart */
 #define SPEKTRUM_OUT(_one_true)        px4_arch_gpiowrite(GPIO_PPM_IN_AS_OUT, (_one_true))

 #define SDIO_SLOTNO                    0  /* Only one slot */
@@ -157,7 +157,6 @@
 */
 #define GPIO_PPM_IN_AS_OUT             (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_2MHz|GPIO_OUTPUT_SET|GPIO_PORTB|GPIO_PIN0)
 #define SPEKTRUM_RX_AS_GPIO_OUTPUT()   px4_arch_configgpio(GPIO_PPM_IN_AS_OUT)
-#define SPEKTRUM_RX_AS_UART()          /* Can be left as uart */
 #define SPEKTRUM_OUT(_one_true)        px4_arch_gpiowrite(GPIO_PPM_IN_AS_OUT, (_one_true))

 #define SDIO_SLOTNO                    0  /* Only one slot */
@@ -134,7 +134,6 @@
 */
 #define GPIO_PPM_IN_AS_OUT             (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_2MHz|GPIO_OUTPUT_SET|GPIO_PORTB|GPIO_PIN0)
 #define SPEKTRUM_RX_AS_GPIO_OUTPUT()   px4_arch_configgpio(GPIO_PPM_IN_AS_OUT)
-#define SPEKTRUM_RX_AS_UART()          /* Can be left as uart */
 #define SPEKTRUM_OUT(_one_true)        px4_arch_gpiowrite(GPIO_PPM_IN_AS_OUT, (_one_true))

 /* By Providing BOARD_ADC_USB_CONNECTED (using the px4_arch abstraction)
@@ -134,7 +134,6 @@
 */
 #define GPIO_PPM_IN_AS_OUT             (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_2MHz|GPIO_OUTPUT_SET|GPIO_PORTB|GPIO_PIN0)
 #define SPEKTRUM_RX_AS_GPIO_OUTPUT()   px4_arch_configgpio(GPIO_PPM_IN_AS_OUT)
-#define SPEKTRUM_RX_AS_UART()          /* Can be left as uart */
 #define SPEKTRUM_OUT(_one_true)        px4_arch_gpiowrite(GPIO_PPM_IN_AS_OUT, (_one_true))

 /* By Providing BOARD_ADC_USB_CONNECTED (using the px4_arch abstraction)
@@ -134,7 +134,6 @@
 */
 #define GPIO_PPM_IN_AS_OUT             (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_2MHz|GPIO_OUTPUT_SET|GPIO_PORTB|GPIO_PIN0)
 #define SPEKTRUM_RX_AS_GPIO_OUTPUT()   px4_arch_configgpio(GPIO_PPM_IN_AS_OUT)
-#define SPEKTRUM_RX_AS_UART()          /* Can be left as uart */
 #define SPEKTRUM_OUT(_one_true)        px4_arch_gpiowrite(GPIO_PPM_IN_AS_OUT, (_one_true))

 /* By Providing BOARD_ADC_USB_CONNECTED (using the px4_arch abstraction)
@@ -312,7 +312,6 @@
 #define GPIO_PPM_IN             /* GPIO_B1_06 GPT1_CAPTURE2 */ (GPIO_GPT1_CAPTURE2_2 | GENERAL_INPUT_IOMUX)

 #define RC_SERIAL_PORT          "/dev/ttyS5"
-#define RC_SERIAL_SINGLEWIRE

 /* PWM input driver. Use FMU AUX5 pins attached to GPIO_EMC_33 GPIO3_IO19 FLEXPWM3_PWMA2 */

@@ -128,7 +128,7 @@
 #define GPIO_PERIPH_3V3_EN           (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_2MHz|GPIO_OUTPUT_SET|GPIO_PORTC|GPIO_PIN5)

 /* For R12, this signal is active high. */
-#define GPIO_SBUS_INV                (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_2MHz|GPIO_OUTPUT_SET|GPIO_PORTC|GPIO_PIN13)
+#define GPIO_SBUS_INV                (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_2MHz|GPIO_OUTPUT_CLEAR|GPIO_PORTC|GPIO_PIN13)
 #define RC_INVERT_INPUT(_invert_true) px4_arch_gpiowrite(GPIO_SBUS_INV, _invert_true)

 #define GPIO_SPEKTRUM_PWR_EN         (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_2MHz|GPIO_OUTPUT_CLEAR|GPIO_PORTE|GPIO_PIN4)
@@ -158,7 +158,6 @@

 #define GPIO_PPM_IN_AS_OUT           (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_2MHz|GPIO_OUTPUT_SET|GPIO_PORTB|GPIO_PIN0)
 #define SPEKTRUM_RX_AS_GPIO_OUTPUT() px4_arch_configgpio(GPIO_PPM_IN_AS_OUT)
-#define SPEKTRUM_RX_AS_UART()       /* Can be left as uart */
 #define SPEKTRUM_OUT(_one_true)      px4_arch_gpiowrite(GPIO_PPM_IN_AS_OUT, (_one_true))

 /**
@@ -274,7 +274,6 @@ static inline bool board_get_external_lockout_state(void)
 #define GPIO_nVDD_5V_HIPOWER_EN         /* PF12 */ (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_2MHz|GPIO_OUTPUT_SET|GPIO_PORTF|GPIO_PIN12)
 #define GPIO_nVDD_5V_HIPOWER_OC         /* PF13 */ (GPIO_INPUT |GPIO_FLOAT|GPIO_PORTF|GPIO_PIN13)
 #define GPIO_VDD_3V3_SPEKTRUM_POWER_EN  /* PE4  */ (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_2MHz|GPIO_OUTPUT_CLEAR|GPIO_PORTE|GPIO_PIN4)
-#define GPIO_VDD_3V3_SPEKTRUM_PASSIVE   /* PE4  */ (GPIO_INPUT|GPIO_PULLUP|GPIO_PORTE|GPIO_PIN4)
 #define GPIO_VDD_5V_RC_EN               /* PG5  */ (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_2MHz|GPIO_OUTPUT_CLEAR|GPIO_PORTG|GPIO_PIN5)
 #define GPIO_VDD_5V_WIFI_EN             /* PG6  */ (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_2MHz|GPIO_OUTPUT_CLEAR|GPIO_PORTG|GPIO_PIN5)
 #define GPIO_VDD_3V3_SD_CARD_EN         /* PG7  */ (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_2MHz|GPIO_OUTPUT_CLEAR|GPIO_PORTG|GPIO_PIN7)
@@ -314,9 +313,7 @@ static inline bool board_get_external_lockout_state(void)
 #define GPIO_PPM_IN             /* PI5 T8C1 */ GPIO_TIM8_CH1IN_2

 /* RC Serial port */
-
 #define RC_SERIAL_PORT                     "/dev/ttyS4"
-#define RC_SERIAL_SINGLEWIRE
 #define BOARD_SUPPORTS_RC_SERIAL_PORT_OUTPUT

 /* PWM input driver. Use FMU AUX5 pins attached to timer4 channel 2 */
@@ -349,7 +346,6 @@ static inline bool board_get_external_lockout_state(void)
 /* Power switch controls ******************************************************/

 #define SPEKTRUM_POWER(_on_true)           VDD_3V3_SPEKTRUM_POWER_EN(_on_true)
-#define SPEKTRUM_POWER_PASSIVE()           px4_arch_configgpio(GPIO_VDD_3V3_SPEKTRUM_PASSIVE)
 #define SPEKTRUM_POWER_CONFIG()           px4_arch_configgpio(GPIO_VDD_3V3_SPEKTRUM_POWER_EN)

 /*
@@ -363,7 +359,6 @@ static inline bool board_get_external_lockout_state(void)

 #define GPIO_PPM_IN_AS_OUT             (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_2MHz|GPIO_OUTPUT_SET|GPIO_PORTI|GPIO_PIN5)
 #define SPEKTRUM_RX_AS_GPIO_OUTPUT()   px4_arch_configgpio(GPIO_PPM_IN_AS_OUT)
-#define SPEKTRUM_RX_AS_UART()          /* Can be left as uart */
 #define SPEKTRUM_OUT(_one_true)        px4_arch_gpiowrite(GPIO_PPM_IN_AS_OUT, (_one_true))

 #define SDIO_SLOTNO                    0  /* Only one slot */
@@ -212,10 +212,6 @@ __EXPORT int board_app_initialize(uintptr_t arg)
 	VDD_5V_PERIPH_EN(true);
 	VDD_5V_HIPOWER_EN(true);
 	board_control_spi_sensors_power(true, 0xffff);
-#ifdef SPEKTRUM_POWER_PASSIVE
-	// Turn power controls to passive
-	SPEKTRUM_POWER_PASSIVE();
-#endif
 	VDD_5V_RC_EN(true);
 	VDD_5V_WIFI_EN(true);

@@ -292,9 +292,7 @@
 #define GPIO_PPM_IN             /* PI5 T8C1 */ GPIO_TIM8_CH1IN_2

 /* RC Serial port */
-
 #define RC_SERIAL_PORT                     "/dev/ttyS5"
-#define RC_SERIAL_SINGLEWIRE

 /* Input Capture Channels. */
 #define INPUT_CAP1_TIMER                  5
@@ -334,7 +332,6 @@

 #define GPIO_PPM_IN_AS_OUT             (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_2MHz|GPIO_OUTPUT_SET|GPIO_PORTI|GPIO_PIN5)
 #define SPEKTRUM_RX_AS_GPIO_OUTPUT()   px4_arch_configgpio(GPIO_PPM_IN_AS_OUT)
-#define SPEKTRUM_RX_AS_UART()          /* Can be left as uart */
 #define SPEKTRUM_OUT(_one_true)        px4_arch_gpiowrite(GPIO_PPM_IN_AS_OUT, (_one_true))

 #define SDIO_SLOTNO                    0  /* Only one slot */
@@ -258,10 +258,7 @@
 #define GPIO_PPM_IN             /* PI5 T8C1 */ GPIO_TIM8_CH1IN_2

 /* RC Serial port */
-
 #define RC_SERIAL_PORT                     "/dev/ttyS5"
-#define RC_SERIAL_SINGLEWIRE
-#define RC_SERIAL_SWAP_RXTX

 /* Input Capture Channels. */
 #define INPUT_CAP1_TIMER                  1
@@ -302,7 +299,6 @@

 #define GPIO_PPM_IN_AS_OUT             (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_2MHz|GPIO_OUTPUT_SET|GPIO_PORTI|GPIO_PIN5)
 #define SPEKTRUM_RX_AS_GPIO_OUTPUT()   px4_arch_configgpio(GPIO_PPM_IN_AS_OUT)
-#define SPEKTRUM_RX_AS_UART()          /* Can be left as uart */
 #define SPEKTRUM_OUT(_one_true)        px4_arch_gpiowrite(GPIO_PPM_IN_AS_OUT, (_one_true))

 #define SDIO_SLOTNO                    0  /* Only one slot */
@@ -316,9 +316,7 @@
 #define GPIO_PPM_IN             /* PI5 T8C1 */ GPIO_TIM8_CH1IN_2

 /* RC Serial port */
-
 #define RC_SERIAL_PORT                     "/dev/ttyS5"
-#define RC_SERIAL_SINGLEWIRE

 /* Input Capture Channels. */
 #define INPUT_CAP1_TIMER                  1
@@ -359,7 +357,6 @@

 #define GPIO_PPM_IN_AS_OUT             (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_2MHz|GPIO_OUTPUT_SET|GPIO_PORTI|GPIO_PIN5)
 #define SPEKTRUM_RX_AS_GPIO_OUTPUT()   px4_arch_configgpio(GPIO_PPM_IN_AS_OUT)
-#define SPEKTRUM_RX_AS_UART()          /* Can be left as uart */
 #define SPEKTRUM_OUT(_one_true)        px4_arch_gpiowrite(GPIO_PPM_IN_AS_OUT, (_one_true))

 #define SDIO_SLOTNO                    0  /* Only one slot */
@@ -253,9 +253,7 @@
 #define GPIO_PPM_IN             /* PI5 T8C1 */ GPIO_TIM8_CH1IN_2

 /* RC Serial port */
-
 #define RC_SERIAL_PORT                     "/dev/ttyS5"
-#define RC_SERIAL_SINGLEWIRE

 /* Input Capture Channels. */
 #define INPUT_CAP1_TIMER                  5
@@ -293,7 +291,6 @@

 #define GPIO_PPM_IN_AS_OUT             (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_2MHz|GPIO_OUTPUT_SET|GPIO_PORTI|GPIO_PIN5)
 #define SPEKTRUM_RX_AS_GPIO_OUTPUT()   px4_arch_configgpio(GPIO_PPM_IN_AS_OUT)
-#define SPEKTRUM_RX_AS_UART()          /* Can be left as uart */
 #define SPEKTRUM_OUT(_one_true)        px4_arch_gpiowrite(GPIO_PPM_IN_AS_OUT, (_one_true))

 #define SDIO_SLOTNO                    0  /* Only one slot */
@@ -126,7 +126,7 @@
 #define GPIO_PERIPH_3V3_EN           (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_2MHz|GPIO_OUTPUT_SET|GPIO_PORTC|GPIO_PIN5)

 /* For,this signal is active high. */
-#define GPIO_SBUS_INV                (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_2MHz|GPIO_OUTPUT_SET|GPIO_PORTC|GPIO_PIN13)
+#define GPIO_SBUS_INV                (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_2MHz|GPIO_OUTPUT_CLEAR|GPIO_PORTC|GPIO_PIN13)
 #define RC_INVERT_INPUT(_invert_true) px4_arch_gpiowrite(GPIO_SBUS_INV, _invert_true)

 #define GPIO_SPEKTRUM_PWR_EN         (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_2MHz|GPIO_OUTPUT_CLEAR|GPIO_PORTE|GPIO_PIN4)
@@ -157,7 +157,6 @@

 #define GPIO_PPM_IN_AS_OUT           (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_2MHz|GPIO_OUTPUT_SET|GPIO_PORTB|GPIO_PIN0)
 #define SPEKTRUM_RX_AS_GPIO_OUTPUT() px4_arch_configgpio(GPIO_PPM_IN_AS_OUT)
-#define SPEKTRUM_RX_AS_UART()       /* Can be left as uart */
 #define SPEKTRUM_OUT(_one_true)      px4_arch_gpiowrite(GPIO_PPM_IN_AS_OUT, (_one_true))

 /**
@@ -2,4 +2,4 @@
 uint64 timestamp        # time since system start (microseconds)
 uint64 timestamp_sample # timestamp of the data sample on which this message is based (microseconds)

-float32[3] xyz          # thrust setpoint along X, Y, Z body axis (in N)
+float32[3] xyz          # thrust setpoint along X, Y, Z body axis [-1, 1]
@@ -2,4 +2,4 @@
 uint64 timestamp        # time since system start (microseconds)
 uint64 timestamp_sample # timestamp of the data sample on which this message is based (microseconds)

-float32[3] xyz          # torque setpoint about X, Y, Z body axis (in N.m)
+float32[3] xyz          # torque setpoint about X, Y, Z body axis (normalized)
@@ -411,79 +411,6 @@ typedef uint8_t px4_guid_t[PX4_GUID_BYTE_LENGTH];
 ************************************************************************************/
 __BEGIN_DECLS

-
-/************************************************************************************
- * Name: board_rc_singlewire
- *
- * Description:
- *   A board may define RC_SERIAL_SINGLEWIRE, so that RC_SERIAL_PORT is configured
- *   as singlewire UART.
- *
- * Input Parameters:
- *   device: serial device, e.g. "/dev/ttyS0"
- *
- * Returned Value:
- *   true singlewire should be enabled.
- *   false if not.
- *
- ************************************************************************************/
-
-#if defined(RC_SERIAL_SINGLEWIRE)
-static inline bool board_rc_singlewire(const char *device) { return strcmp(device, RC_SERIAL_PORT) == 0; }
-#else
-static inline bool board_rc_singlewire(const char *device) { return false; }
-#endif
-
-/************************************************************************************
- * Name: board_rc_swap_rxtx
- *
- * Description:
- *   A board may define RC_SERIAL_SWAP_RXTX, so that RC_SERIAL_PORT is configured
- *   as UART with RX/TX swapped.
- *
- * Input Parameters:
- *   device: serial device, e.g. "/dev/ttyS0"
- *
- * Returned Value:
- *   true RX/RX should be swapped.
- *   false if not.
- *
- ************************************************************************************/
-
-#if defined(RC_SERIAL_SWAP_RXTX)
-static inline bool board_rc_swap_rxtx(const char *device) { return strcmp(device, RC_SERIAL_PORT) == 0; }
-#else
-static inline bool board_rc_swap_rxtx(const char *device) { return false; }
-#endif
-
-/************************************************************************************
- * Name: board_rc_invert_input
- *
- * Description:
- *   All boards may optionally define RC_INVERT_INPUT(bool invert) that is
- *   used to invert the RC_SERIAL_PORT RC port (e.g. to toggle an external XOR via
- *   GPIO).
- *
- * Input Parameters:
- *   invert_on - A positive logic value, that when true (on) will set the HW in
- *               inverted NRZ mode where a MARK will be 0 and SPACE will be a 1.
- *
- * Returned Value:
- *   true the UART inversion got set.
- *
- ************************************************************************************/
-
-#ifdef RC_INVERT_INPUT
-static inline bool board_rc_invert_input(const char *device, bool invert)
-{
-	if (strcmp(device, RC_SERIAL_PORT) == 0) { RC_INVERT_INPUT(invert); return true; }
-
-	return false;
-}
-#else
-static inline bool board_rc_invert_input(const char *device, bool invert) { return false; }
-#endif
-
 /* Provide an interface for reading the connected state of VBUS */

 /************************************************************************************
@@ -47,7 +47,6 @@ DShot::DShot() :
 	_mixing_output.setAllDisarmedValues(DSHOT_DISARM_VALUE);
 	_mixing_output.setAllMinValues(DSHOT_MIN_THROTTLE);
 	_mixing_output.setAllMaxValues(DSHOT_MAX_THROTTLE);
-	_mixing_output.setAllFailsafeValues(UINT16_MAX);
 }

 DShot::~DShot()
@@ -1,6 +1,6 @@
 /****************************************************************************
 *
- *   Copyright (c) 2012-2021 PX4 Development Team. All rights reserved.
+ *   Copyright (c) 2012-2022 PX4 Development Team. All rights reserved.
 *
 * Redistribution and use in source and binary forms, with or without
 * modification, are permitted provided that the following conditions
@@ -63,14 +63,14 @@ RCInput::RCInput(const char *device) :
 		strncpy(_device, device, sizeof(_device) - 1);
 		_device[sizeof(_device) - 1] = '\0';
 	}
+
+	if ((_param_rc_input_proto.get() >= 0) && (_param_rc_input_proto.get() <= RC_SCAN::RC_SCAN_GHST)) {
+		_rc_scan_state = static_cast<RC_SCAN>(_param_rc_input_proto.get());
+	}
 }

 RCInput::~RCInput()
 {
-#if defined(SPEKTRUM_POWER_PASSIVE)
-	// Disable power controls for Spektrum receiver
-	SPEKTRUM_POWER_PASSIVE();
-#endif
 	dsm_deinit();

 	delete _crsf_telemetry;
@@ -80,41 +80,6 @@ RCInput::~RCInput()
 	perf_free(_publish_interval_perf);
 }

-int
-RCInput::init()
-{
-#ifdef RF_RADIO_POWER_CONTROL
-	// power radio on
-	RF_RADIO_POWER_CONTROL(true);
-#endif // RF_RADIO_POWER_CONTROL
-
-	// dsm_init sets some file static variables and returns a file descriptor
-	// it also powers on the radio if needed
-	_rcs_fd = dsm_init(_device);
-
-	if (_rcs_fd < 0) {
-		return -errno;
-	}
-
-	if (board_rc_swap_rxtx(_device)) {
-#if defined(TIOCSSWAP)
-		ioctl(_rcs_fd, TIOCSSWAP, SER_SWAP_ENABLED);
-#endif // TIOCSSWAP
-	}
-
-	// assume SBUS input and immediately switch it to
-	// so that if Single wire mode on TX there will be only
-	// a short contention
-	sbus_config(_rcs_fd, board_rc_singlewire(_device));
-
-#ifdef GPIO_PPM_IN
-	// disable CPPM input by mapping it away from the timer capture input
-	px4_arch_unconfiggpio(GPIO_PPM_IN);
-#endif // GPIO_PPM_IN
-
-	return 0;
-}
-
 int
 RCInput::task_spawn(int argc, char *argv[])
 {
@@ -251,9 +216,21 @@ RCInput::fill_rc_in(uint16_t raw_rc_count_local,

 void RCInput::set_rc_scan_state(RC_SCAN newState)
 {
-	PX4_DEBUG("RCscan: %s failed, trying %s", RCInput::RC_SCAN_STRING[_rc_scan_state], RCInput::RC_SCAN_STRING[newState]);
+	if ((_param_rc_input_proto.get() > RC_SCAN::RC_SCAN_NONE)
+	    && (_param_rc_input_proto.get() <= RC_SCAN::RC_SCAN_GHST)) {
+
+		_rc_scan_state = static_cast<RC_SCAN>(_param_rc_input_proto.get());
+
+	} else if (_param_rc_input_proto.get() < 0) {
+		// only auto change if RC_INPUT_PROTO set to auto (-1)
+		PX4_DEBUG("RCscan: %s failed, trying %s", RCInput::RC_SCAN_STRING[_rc_scan_state], RCInput::RC_SCAN_STRING[newState]);
+		_rc_scan_state = newState;
+
+	} else {
+		_rc_scan_state = RC_SCAN::RC_SCAN_NONE;
+	}
+
 	_rc_scan_begin = 0;
-	_rc_scan_state = newState;
 	_rc_scan_locked = false;

 	_report_lock = true;
@@ -261,20 +238,28 @@ void RCInput::set_rc_scan_state(RC_SCAN newState)

 void RCInput::rc_io_invert(bool invert)
 {
-	// First check if the board provides a board-specific inversion method (e.g. via GPIO),
-	// and if not use an IOCTL
-	if (!board_rc_invert_input(_device, invert)) {
+#if defined(RC_SERIAL_PORT) && defined(RC_INVERT_INPUT)
+
+	if (strcmp(_device, RC_SERIAL_PORT) == 0) {
+		RC_INVERT_INPUT(invert);
+	}
+
+#endif // RC_SERIAL_PORT && RC_INVERT_INPUT
+
+
 #if defined(TIOCSINVERT)

+	if (_rcs_fd >= 0) {
 		if (invert) {
+			//int ret_invert = ioctl(_rcs_fd, TIOCSINVERT, SER_INVERT_ENABLED_RX | SER_INVERT_ENABLED_TX);
 			ioctl(_rcs_fd, TIOCSINVERT, SER_INVERT_ENABLED_RX | SER_INVERT_ENABLED_TX);

 		} else {
 			ioctl(_rcs_fd, TIOCSINVERT, 0);
 		}
+	}

 #endif // TIOCSINVERT
-	}
 }

 void RCInput::Run()
@@ -284,16 +269,7 @@ void RCInput::Run()
 		return;
 	}

-	if (!_initialized) {
-		if (init() == PX4_OK) {
-			_initialized = true;
-
-		} else {
-			PX4_ERR("init failed");
-			exit_and_cleanup();
-		}
-
-	} else {
+	{

 		perf_begin(_cycle_perf);

@@ -329,6 +305,17 @@ void RCInput::Run()

 				if (!_rc_scan_locked && !_armed) {
 					if ((int)vcmd.param1 == 0) {
+
+						rc_io_invert(false);
+
+						if (_rcs_fd >= 0) {
+							close(_rcs_fd);
+							_rcs_fd = -1;
+						}
+
+						// Configure serial port for DSM
+						_rcs_fd = dsm_init(_device);
+
 						// DSM binding command
 						int dsm_bind_mode = (int)vcmd.param2;

@@ -347,6 +334,8 @@ void RCInput::Run()
 						bind_spektrum(dsm_bind_pulses);

 						cmd_ret = vehicle_command_s::VEHICLE_CMD_RESULT_ACCEPTED;
+
+						dsm_deinit();
 					}

 				} else {
@@ -402,7 +391,7 @@ void RCInput::Run()

 		// This block scans for a supported serial RC input and locks onto the first one found
 		// Scan for 300 msec, then switch protocol
-		constexpr hrt_abstime rc_scan_max = 300_ms;
+		constexpr hrt_abstime rc_scan_max = 700_ms;

 		unsigned frame_drops = 0;

@@ -412,23 +401,38 @@ void RCInput::Run()
 		// read all available data from the serial RC input UART

 		// read all available data from the serial RC input UART
-		int newBytes = ::read(_rcs_fd, &_rcs_buf[0], RC_MAX_BUFFER_SIZE);
+		uint8_t rcs_buf[RC_MAX_BUFFER_SIZE];
+		int newBytes = ::read(_rcs_fd, &rcs_buf[0], RC_MAX_BUFFER_SIZE);

 		if (newBytes > 0) {
 			_bytes_rx += newBytes;
 		}

 		switch (_rc_scan_state) {
+		case RC_SCAN_NONE:
+			// do nothing
+			break;
+
 		case RC_SCAN_SBUS:
 			if (_rc_scan_begin == 0) {
-				_rc_scan_begin = cycle_timestamp;
-				// Configure serial port for SBUS
-				sbus_config(_rcs_fd, board_rc_singlewire(_device));
+
+				if (_rcs_fd < 0) {
+					_rcs_fd = open(_device, O_RDWR | O_NONBLOCK);
+				}
+
+				// assume SBUS input and immediately switch it to
+				// so that if Single wire mode on TX there will be only
+				// a short contention
+				sbus_config(_rcs_fd, true);
+
+
+#if defined(TIOCSSINGLEWIRE)
+				ioctl(_rcs_fd, TIOCSSINGLEWIRE, SER_SINGLEWIRE_ENABLED);
+#endif // TIOCSSINGLEWIRE
+
 				rc_io_invert(true);

-				// flush serial buffer and any existing buffered data
-				tcflush(_rcs_fd, TCIOFLUSH);
-				memset(_rcs_buf, 0, sizeof(_rcs_buf));
+				_rc_scan_begin = hrt_absolute_time();

 			} else if (_rc_scan_locked
 				   || cycle_timestamp - _rc_scan_begin < rc_scan_max) {
@@ -438,7 +442,7 @@ void RCInput::Run()
 					bool sbus_failsafe = false;
 					bool sbus_frame_drop = false;

-					rc_updated = sbus_parse(cycle_timestamp, &_rcs_buf[0], newBytes, &_raw_rc_values[0], &_raw_rc_count, &sbus_failsafe,
+					rc_updated = sbus_parse(cycle_timestamp, &rcs_buf[0], newBytes, &_raw_rc_values[0], &_raw_rc_count, &sbus_failsafe,
 								&sbus_frame_drop, &frame_drops, input_rc_s::RC_INPUT_MAX_CHANNELS);

 					if (rc_updated) {
@@ -451,8 +455,16 @@ void RCInput::Run()
 				}

 			} else {
-				// Scan the next protocol
 				rc_io_invert(false);
+
+#if defined(TIOCSSINGLEWIRE)
+				ioctl(_rcs_fd, TIOCSSINGLEWIRE, 0);
+#endif // TIOCSSINGLEWIRE
+
+				::close(_rcs_fd);
+				_rcs_fd = -1;
+
+				// Scan the next protocol
 				set_rc_scan_state(RC_SCAN_DSM);
 			}

@@ -460,13 +472,20 @@ void RCInput::Run()

 		case RC_SCAN_DSM:
 			if (_rc_scan_begin == 0) {
-				_rc_scan_begin = cycle_timestamp;
-				// Configure serial port for DSM
-				dsm_config(_rcs_fd);
+				// dsm_init sets some file static variables and returns a file descriptor
+				// it also powers on the radio if needed
+				if (_rcs_fd < 0) {
+					//_rcs_fd = open(_device, O_RDWR | O_NONBLOCK);
+				}

-				// flush serial buffer and any existing buffered data
-				tcflush(_rcs_fd, TCIOFLUSH);
-				memset(_rcs_buf, 0, sizeof(_rcs_buf));
+				// Configure serial port for DSM
+				_rcs_fd = dsm_init(_device);
+
+				if (_rcs_fd < 0) {
+					PX4_ERR("dsm init failed");
+				}
+
+				_rc_scan_begin = hrt_absolute_time();

 			} else if (_rc_scan_locked
 				   || cycle_timestamp - _rc_scan_begin < rc_scan_max) {
@@ -476,7 +495,7 @@ void RCInput::Run()
 					bool dsm_11_bit = false;

 					// parse new data
-					rc_updated = dsm_parse(cycle_timestamp, &_rcs_buf[0], newBytes, &_raw_rc_values[0], &_raw_rc_count,
+					rc_updated = dsm_parse(cycle_timestamp, &rcs_buf[0], newBytes, &_raw_rc_values[0], &_raw_rc_count,
 							       &dsm_11_bit, &frame_drops, &dsm_rssi, input_rc_s::RC_INPUT_MAX_CHANNELS);

 					if (rc_updated) {
@@ -489,6 +508,9 @@ void RCInput::Run()
 				}

 			} else {
+				dsm_deinit();
+				_rcs_fd = -1;
+
 				// Scan the next protocol
 				set_rc_scan_state(RC_SCAN_ST24);
 			}
@@ -499,11 +521,7 @@ void RCInput::Run()
 			if (_rc_scan_begin == 0) {
 				_rc_scan_begin = cycle_timestamp;
 				// Configure serial port for DSM
-				dsm_config(_rcs_fd);
-
-				// flush serial buffer and any existing buffered data
-				tcflush(_rcs_fd, TCIOFLUSH);
-				memset(_rcs_buf, 0, sizeof(_rcs_buf));
+				_rcs_fd = dsm_init(_device);

 			} else if (_rc_scan_locked
 				   || cycle_timestamp - _rc_scan_begin < rc_scan_max) {
@@ -517,7 +535,7 @@ void RCInput::Run()
 					for (unsigned i = 0; i < (unsigned)newBytes; i++) {
 						/* set updated flag if one complete packet was parsed */
 						st24_rssi = input_rc_s::RSSI_MAX;
-						rc_updated = (OK == st24_decode(_rcs_buf[i], &st24_rssi, &lost_count,
+						rc_updated = (OK == st24_decode(rcs_buf[i], &st24_rssi, &lost_count,
 										&_raw_rc_count, _raw_rc_values, input_rc_s::RC_INPUT_MAX_CHANNELS));
 					}

@@ -540,6 +558,8 @@ void RCInput::Run()
 				}

 			} else {
+				dsm_deinit();
+				_rcs_fd = -1;
 				// Scan the next protocol
 				set_rc_scan_state(RC_SCAN_SUMD);
 			}
@@ -550,12 +570,9 @@ void RCInput::Run()
 			if (_rc_scan_begin == 0) {
 				_rc_scan_begin = cycle_timestamp;
 				// Configure serial port for DSM
+				_rcs_fd = dsm_init(_device);
 				dsm_config(_rcs_fd);

-				// flush serial buffer and any existing buffered data
-				tcflush(_rcs_fd, TCIOFLUSH);
-				memset(_rcs_buf, 0, sizeof(_rcs_buf));
-
 			} else if (_rc_scan_locked
 				   || cycle_timestamp - _rc_scan_begin < rc_scan_max) {

@@ -569,7 +586,7 @@ void RCInput::Run()
 					for (unsigned i = 0; i < (unsigned)newBytes; i++) {
 						/* set updated flag if one complete packet was parsed */
 						sumd_rssi = input_rc_s::RSSI_MAX;
-						rc_updated = (OK == sumd_decode(_rcs_buf[i], &sumd_rssi, &rx_count,
+						rc_updated = (OK == sumd_decode(rcs_buf[i], &sumd_rssi, &rx_count,
 										&_raw_rc_count, _raw_rc_values, input_rc_s::RC_INPUT_MAX_CHANNELS, &sumd_failsafe));
 					}

@@ -583,6 +600,8 @@ void RCInput::Run()
 				}

 			} else {
+				::close(_rcs_fd);
+				_rcs_fd = -1;
 				// Scan the next protocol
 				set_rc_scan_state(RC_SCAN_PPM);
 			}
@@ -627,19 +646,20 @@ void RCInput::Run()
 		case RC_SCAN_CRSF:
 			if (_rc_scan_begin == 0) {
 				_rc_scan_begin = cycle_timestamp;
+
+				if (_rcs_fd < 0) {
+					_rcs_fd = open(_device, O_RDWR | O_NONBLOCK);
+				}
+
 				// Configure serial port for CRSF
 				crsf_config(_rcs_fd);

-				// flush serial buffer and any existing buffered data
-				tcflush(_rcs_fd, TCIOFLUSH);
-				memset(_rcs_buf, 0, sizeof(_rcs_buf));
-
 			} else if (_rc_scan_locked
 				   || cycle_timestamp - _rc_scan_begin < rc_scan_max) {

 				// parse new data
 				if (newBytes > 0) {
-					rc_updated = crsf_parse(cycle_timestamp, &_rcs_buf[0], newBytes, &_raw_rc_values[0], &_raw_rc_count,
+					rc_updated = crsf_parse(cycle_timestamp, &rcs_buf[0], newBytes, &_raw_rc_values[0], &_raw_rc_count,
 								input_rc_s::RC_INPUT_MAX_CHANNELS);

 					if (rc_updated) {
@@ -666,6 +686,8 @@ void RCInput::Run()
 				}

 			} else {
+				::close(_rcs_fd);
+				_rcs_fd = -1;
 				// Scan the next protocol
 				set_rc_scan_state(RC_SCAN_GHST);
 			}
@@ -675,12 +697,13 @@ void RCInput::Run()
 		case RC_SCAN_GHST:
 			if (_rc_scan_begin == 0) {
 				_rc_scan_begin = cycle_timestamp;
-				// Configure serial port for GHST
-				ghst_config(_rcs_fd);

-				// flush serial buffer and any existing buffered data
-				tcflush(_rcs_fd, TCIOFLUSH);
-				memset(_rcs_buf, 0, sizeof(_rcs_buf));
+				// Configure serial port for GHST
+				if (_rcs_fd < 0) {
+					_rcs_fd = open(_device, O_RDWR | O_NONBLOCK);
+				}
+
+				ghst_config(_rcs_fd);

 			} else if (_rc_scan_locked
 				   || cycle_timestamp - _rc_scan_begin < rc_scan_max) {
@@ -688,7 +711,7 @@ void RCInput::Run()
 				// parse new data
 				if (newBytes > 0) {
 					int8_t ghst_rssi = -1;
-					rc_updated = ghst_parse(cycle_timestamp, &_rcs_buf[0], newBytes, &_raw_rc_values[0], &ghst_rssi,
+					rc_updated = ghst_parse(cycle_timestamp, &rcs_buf[0], newBytes, &_raw_rc_values[0], &ghst_rssi,
 								&_raw_rc_count, input_rc_s::RC_INPUT_MAX_CHANNELS);

 					if (rc_updated) {
@@ -716,6 +739,8 @@ void RCInput::Run()
 				}

 			} else {
+				::close(_rcs_fd);
+				_rcs_fd = -1;
 				// Scan the next protocol
 				set_rc_scan_state(RC_SCAN_SBUS);
 			}
@@ -737,6 +762,12 @@ void RCInput::Run()
 		if (_report_lock && _rc_scan_locked) {
 			_report_lock = false;
 			PX4_INFO("RC scan: %s RC input locked", RC_SCAN_STRING[_rc_scan_state]);
+
+			if (!_armed && (_param_rc_input_proto.get() < 0)) {
+				// RC_INPUT_PROTO auto => locked selection
+				_param_rc_input_proto.set(_rc_scan_state);
+				_param_rc_input_proto.commit();
+			}
 		}
 	}
 }
@@ -823,6 +854,9 @@ int RCInput::print_status()

 	if (_rc_scan_locked) {
 		switch (_rc_scan_state) {
+		case RC_SCAN_NONE:
+			break;
+
 		case RC_SCAN_CRSF:
 			PX4_INFO("CRSF Telemetry: %s", _crsf_telemetry ? "yes" : "no");
 			break;
@@ -1,6 +1,6 @@
 /****************************************************************************
 *
- *   Copyright (c) 2012-2019, 2021 PX4 Development Team. All rights reserved.
+ *   Copyright (c) 2012-2022 PX4 Development Team. All rights reserved.
 *
 * Redistribution and use in source and binary forms, with or without
 * modification, are permitted provided that the following conditions
@@ -91,21 +91,23 @@ public:
 private:

 	enum RC_SCAN {
-		RC_SCAN_PPM = 0,
-		RC_SCAN_SBUS,
-		RC_SCAN_DSM,
-		RC_SCAN_SUMD,
-		RC_SCAN_ST24,
-		RC_SCAN_CRSF,
-		RC_SCAN_GHST
+		RC_SCAN_NONE = 0,
+		RC_SCAN_PPM  = 1,
+		RC_SCAN_SBUS = 2,
+		RC_SCAN_DSM  = 3,
+		RC_SCAN_ST24 = 5,
+		RC_SCAN_SUMD = 4,
+		RC_SCAN_CRSF = 6,
+		RC_SCAN_GHST = 7,
 	} _rc_scan_state{RC_SCAN_SBUS};

-	static constexpr char const *RC_SCAN_STRING[7] {
+	static constexpr char const *RC_SCAN_STRING[] {
+		"None",
 		"PPM",
 		"SBUS",
 		"DSM",
-		"SUMD",
 		"ST24",
+		"SUMD",
 		"CRSF",
 		"GHST"
 	};
@@ -127,7 +129,6 @@ private:

 	hrt_abstime _rc_scan_begin{0};

-	bool _initialized{false};
 	bool _rc_scan_locked{false};
 	bool _report_lock{true};

@@ -153,7 +154,6 @@ private:
 	char		_device[20] {};					///< device / serial port path

 	static constexpr size_t RC_MAX_BUFFER_SIZE{SBUS_BUFFER_SIZE};
-	uint8_t _rcs_buf[RC_MAX_BUFFER_SIZE] {};

 	uint16_t _raw_rc_values[input_rc_s::RC_INPUT_MAX_CHANNELS] {};
 	uint16_t _raw_rc_count{};
@@ -168,6 +168,7 @@ private:
 	DEFINE_PARAMETERS(
 		(ParamInt<px4::params::RC_RSSI_PWM_CHAN>) _param_rc_rssi_pwm_chan,
 		(ParamInt<px4::params::RC_RSSI_PWM_MIN>) _param_rc_rssi_pwm_min,
-		(ParamInt<px4::params::RC_RSSI_PWM_MAX>) _param_rc_rssi_pwm_max
+		(ParamInt<px4::params::RC_RSSI_PWM_MAX>) _param_rc_rssi_pwm_max,
+		(ParamInt<px4::params::RC_INPUT_PROTO>) _param_rc_input_proto
 	)
 };
@@ -1,4 +1,28 @@
 module_name: RC Input Driver
+parameters:
+    - group: RC Input
+      definitions:
+        RC_INPUT_PROTO:
+            description:
+                short: RC input protocol
+                long: |
+                    Select your RC input protocol or auto to scan.
+            category: System
+            type: enum
+            values:
+                -1: Auto
+                0: None
+                1: PPM
+                2: SBUS
+                3: DSM
+                4: ST24
+                5: SUMD
+                6: CRSF
+                7: GHST
+            min: -1
+            max: 7
+            default: -1
+
 serial_config:
    - command: set RC_INPUT_ARGS "-d ${SERIAL_DEV}"
      port_config_param:
@@ -0,0 +1,5 @@
+menuconfig DRIVERS_TATTU_CAN
+	bool "tattu_can"
+	default n
+	---help---
+		Enable support for tattu_can
@@ -189,8 +189,6 @@ static bool dsm_decode_channel(uint16_t raw, unsigned shift, uint8_t &channel, u
 		static constexpr uint16_t offset = 903; // microseconds
 		value = roundf(servo_position * 0.583f) + offset;

-		PX4_DEBUG(stderr, "CH%d=%d(0x%02x), ", channel, value, raw);
-
 		return true;
 	}

@@ -470,6 +468,9 @@ int dsm_config(int fd)

 void dsm_proto_init()
 {
+	memset(&dsm_frame, 0, sizeof(dsm_frame_t));
+	memset(&dsm_buf, 0, sizeof(dsm_buf_t));
+
 	dsm_channel_shift = 0;
 	dsm_frame_drops = 0;
 	dsm_chan_count = 0;
@@ -509,11 +510,6 @@ int dsm_init(const char *device)

 void dsm_deinit()
 {
-#ifdef SPEKTRUM_POWER_PASSIVE
-	// Turn power controls to passive
-	SPEKTRUM_POWER_PASSIVE();
-#endif
-
 	if (dsm_fd >= 0) {
 		close(dsm_fd);
 	}
@@ -580,7 +576,9 @@ void dsm_bind(uint16_t cmd, int pulses)
 #if defined(DSM_DEBUG)
 		printf("DSM: DSM_CMD_BIND_REINIT_UART\n");
 #endif
+#if defined(SPEKTRUM_RX_AS_UART)
 		SPEKTRUM_RX_AS_UART();
+#endif // SPEKTRUM_RX_AS_UART
 		break;

 	}
@@ -262,3 +262,14 @@ uint32_t ActuatorEffectivenessRotors::getUpwardsMotors() const

 	return upwards_motors;
 }
+
+bool
+ActuatorEffectivenessRotors::getEffectivenessMatrix(Configuration &configuration,
+		EffectivenessUpdateReason external_update)
+{
+	if (external_update == EffectivenessUpdateReason::NO_EXTERNAL_UPDATE) {
+		return false;
+	}
+
+	return addActuators(configuration);
+}
@@ -81,6 +81,8 @@ public:
 				    bool tilt_support = false);
 	virtual ~ActuatorEffectivenessRotors() = default;

+	bool getEffectivenessMatrix(Configuration &configuration, EffectivenessUpdateReason external_update) override;
+
 	void getDesiredAllocationMethod(AllocationMethod allocation_method_out[MAX_NUM_MATRICES]) const override
 	{
 		allocation_method_out[0] = AllocationMethod::SEQUENTIAL_DESATURATION;
@@ -96,7 +98,7 @@ public:

 	bool addActuators(Configuration &configuration);

-	const char *name() const override { return "Multirotor"; }
+	const char *name() const override { return "Rotors"; }

 	/**
 	 * Sets the motor axis from tilt configurations and current tilt control.
@@ -261,13 +261,13 @@ void EKFGSF_yaw::predictEKF(const uint8_t model_index)
 	// predict covariance - equations generated using EKF/python/gsf_ekf_yaw_estimator/main.py

 	// Local short variable name copies required for readability
-	const float &P00 = _ekf_gsf[model_index].P(0,0);
-	const float &P01 = _ekf_gsf[model_index].P(0,1);
-	const float &P02 = _ekf_gsf[model_index].P(0,2);
-	const float &P11 = _ekf_gsf[model_index].P(1,1);
-	const float &P12 = _ekf_gsf[model_index].P(1,2);
-	const float &P22 = _ekf_gsf[model_index].P(2,2);
-	const float &psi = _ekf_gsf[model_index].X(2);
+	const float P00 = _ekf_gsf[model_index].P(0,0);
+	const float P01 = _ekf_gsf[model_index].P(0,1);
+	const float P02 = _ekf_gsf[model_index].P(0,2);
+	const float P11 = _ekf_gsf[model_index].P(1,1);
+	const float P12 = _ekf_gsf[model_index].P(1,2);
+	const float P22 = _ekf_gsf[model_index].P(2,2);
+	const float psi = _ekf_gsf[model_index].X(2);

 	// Use fixed values for delta velocity and delta angle process noise variances
 	const float dvxVar = sq(_accel_noise * _delta_vel_dt); // variance of forward delta velocity - (m/s)^2
@@ -317,12 +317,12 @@ bool EKFGSF_yaw::updateEKF(const uint8_t model_index)
 	_ekf_gsf[model_index].innov(1) = _ekf_gsf[model_index].X(1) - _vel_NE(1);

 	// Use temporary variables for covariance elements to reduce verbosity of auto-code expressions
-	const float &P00 = _ekf_gsf[model_index].P(0,0);
-	const float &P01 = _ekf_gsf[model_index].P(0,1);
-	const float &P02 = _ekf_gsf[model_index].P(0,2);
-	const float &P11 = _ekf_gsf[model_index].P(1,1);
-	const float &P12 = _ekf_gsf[model_index].P(1,2);
-	const float &P22 = _ekf_gsf[model_index].P(2,2);
+	const float P00 = _ekf_gsf[model_index].P(0,0);
+	const float P01 = _ekf_gsf[model_index].P(0,1);
+	const float P02 = _ekf_gsf[model_index].P(0,2);
+	const float P11 = _ekf_gsf[model_index].P(1,1);
+	const float P12 = _ekf_gsf[model_index].P(1,2);
+	const float P22 = _ekf_gsf[model_index].P(2,2);

 	// optimized auto generated code from SymPy script src/lib/ecl/EKF/python/ekf_derivation/main.py
 	const float t0 = ecl::powf(P01, 2);
@@ -47,11 +47,11 @@

 void Ekf::fuseAirspeed()
 {
-	const float &vn = _state.vel(0); // Velocity in north direction
-	const float &ve = _state.vel(1); // Velocity in east direction
-	const float &vd = _state.vel(2); // Velocity in downwards direction
-	const float &vwn = _state.wind_vel(0); // Wind speed in north direction
-	const float &vwe = _state.wind_vel(1); // Wind speed in east direction
+	const float vn = _state.vel(0); // Velocity in north direction
+	const float ve = _state.vel(1); // Velocity in east direction
+	const float vd = _state.vel(2); // Velocity in downwards direction
+	const float vwn = _state.wind_vel(0); // Wind speed in north direction
+	const float vwe = _state.wind_vel(1); // Wind speed in east direction

 	// Variance for true airspeed measurement - (m/sec)^2
 	const float R_TAS = sq(math::constrain(_params.eas_noise, 0.5f, 5.0f) *
@@ -100,26 +100,26 @@ void Ekf::initialiseCovariance()
 void Ekf::predictCovariance()
 {
 	// assign intermediate state variables
-	const float &q0 = _state.quat_nominal(0);
-	const float &q1 = _state.quat_nominal(1);
-	const float &q2 = _state.quat_nominal(2);
-	const float &q3 = _state.quat_nominal(3);
+	const float q0 = _state.quat_nominal(0);
+	const float q1 = _state.quat_nominal(1);
+	const float q2 = _state.quat_nominal(2);
+	const float q3 = _state.quat_nominal(3);

-	const float &dax = _imu_sample_delayed.delta_ang(0);
-	const float &day = _imu_sample_delayed.delta_ang(1);
-	const float &daz = _imu_sample_delayed.delta_ang(2);
+	const float dax = _imu_sample_delayed.delta_ang(0);
+	const float day = _imu_sample_delayed.delta_ang(1);
+	const float daz = _imu_sample_delayed.delta_ang(2);

-	const float &dvx = _imu_sample_delayed.delta_vel(0);
-	const float &dvy = _imu_sample_delayed.delta_vel(1);
-	const float &dvz = _imu_sample_delayed.delta_vel(2);
+	const float dvx = _imu_sample_delayed.delta_vel(0);
+	const float dvy = _imu_sample_delayed.delta_vel(1);
+	const float dvz = _imu_sample_delayed.delta_vel(2);

-	const float &dax_b = _state.delta_ang_bias(0);
-	const float &day_b = _state.delta_ang_bias(1);
-	const float &daz_b = _state.delta_ang_bias(2);
+	const float dax_b = _state.delta_ang_bias(0);
+	const float day_b = _state.delta_ang_bias(1);
+	const float daz_b = _state.delta_ang_bias(2);

-	const float &dvx_b = _state.delta_vel_bias(0);
-	const float &dvy_b = _state.delta_vel_bias(1);
-	const float &dvz_b = _state.delta_vel_bias(2);
+	const float dvx_b = _state.delta_vel_bias(0);
+	const float dvy_b = _state.delta_vel_bias(1);
+	const float dvz_b = _state.delta_vel_bias(2);

 	// Use average update interval to reduce accumulated covariance prediction errors due to small single frame dt values
 	const float dt = _dt_ekf_avg;
@@ -66,19 +66,19 @@ void Ekf::fuseDrag(const dragSample &drag_sample)
 	}

 	// get latest estimated orientation
-	const float &q0 = _state.quat_nominal(0);
-	const float &q1 = _state.quat_nominal(1);
-	const float &q2 = _state.quat_nominal(2);
-	const float &q3 = _state.quat_nominal(3);
+	const float q0 = _state.quat_nominal(0);
+	const float q1 = _state.quat_nominal(1);
+	const float q2 = _state.quat_nominal(2);
+	const float q3 = _state.quat_nominal(3);

 	// get latest velocity in earth frame
-	const float &vn = _state.vel(0);
-	const float &ve = _state.vel(1);
-	const float &vd = _state.vel(2);
+	const float vn = _state.vel(0);
+	const float ve = _state.vel(1);
+	const float vd = _state.vel(2);

 	// get latest wind velocity in earth frame
-	const float &vwn = _state.wind_vel(0);
-	const float &vwe = _state.wind_vel(1);
+	const float vwn = _state.wind_vel(0);
+	const float vwe = _state.wind_vel(1);

 	// predicted specific forces
 	// calculate relative wind velocity in earth frame and rotate into body frame
@@ -48,10 +48,10 @@
 void Ekf::fuseGpsYaw()
 {
 	// assign intermediate state variables
-	const float &q0 = _state.quat_nominal(0);
-	const float &q1 = _state.quat_nominal(1);
-	const float &q2 = _state.quat_nominal(2);
-	const float &q3 = _state.quat_nominal(3);
+	const float q0 = _state.quat_nominal(0);
+	const float q1 = _state.quat_nominal(1);
+	const float q2 = _state.quat_nominal(2);
+	const float q3 = _state.quat_nominal(3);

 	// calculate the observed yaw angle of antenna array, converting a from body to antenna yaw measurement
 	const float measured_hdg = wrap_pi(_gps_sample_delayed.yaw + _gps_yaw_offset);
@@ -48,14 +48,14 @@
 void Ekf::fuseMag(const Vector3f &mag)
 {
 	// assign intermediate variables
-	const float &q0 = _state.quat_nominal(0);
-	const float &q1 = _state.quat_nominal(1);
-	const float &q2 = _state.quat_nominal(2);
-	const float &q3 = _state.quat_nominal(3);
+	const float q0 = _state.quat_nominal(0);
+	const float q1 = _state.quat_nominal(1);
+	const float q2 = _state.quat_nominal(2);
+	const float q3 = _state.quat_nominal(3);

-	const float &magN = _state.mag_I(0);
-	const float &magE = _state.mag_I(1);
-	const float &magD = _state.mag_I(2);
+	const float magN = _state.mag_I(0);
+	const float magE = _state.mag_I(1);
+	const float magD = _state.mag_I(2);

 	// XYZ Measurement uncertainty. Need to consider timing errors for fast rotations
 	const float R_MAG = sq(fmaxf(_params.mag_noise, 0.0f));
@@ -424,10 +424,10 @@ void Ekf::fuseMag(const Vector3f &mag)
 bool Ekf::fuseYaw321(float yaw, float yaw_variance, bool zero_innovation)
 {
 	// assign intermediate state variables
-	const float &q0 = _state.quat_nominal(0);
-	const float &q1 = _state.quat_nominal(1);
-	const float &q2 = _state.quat_nominal(2);
-	const float &q3 = _state.quat_nominal(3);
+	const float q0 = _state.quat_nominal(0);
+	const float q1 = _state.quat_nominal(1);
+	const float q2 = _state.quat_nominal(2);
+	const float q3 = _state.quat_nominal(3);

 	const float R_YAW = fmaxf(yaw_variance, 1.0e-4f);
 	const float measurement = wrap_pi(yaw);
@@ -779,8 +779,8 @@ void Ekf::fuseHeading(float measured_hdg, float obs_var)
 void Ekf::fuseDeclination(float decl_sigma)
 {
 	// assign intermediate state variables
-	const float &magN = _state.mag_I(0);
-	const float &magE = _state.mag_I(1);
+	const float magN = _state.mag_I(0);
+	const float magE = _state.mag_I(1);

 	// minimum North field strength before calculation becomes badly conditioned (T)
 	constexpr float N_field_min = 0.001f;
@@ -48,19 +48,19 @@
 void Ekf::fuseSideslip()
 {
 	// get latest estimated orientation
-	const float &q0 = _state.quat_nominal(0);
-	const float &q1 = _state.quat_nominal(1);
-	const float &q2 = _state.quat_nominal(2);
-	const float &q3 = _state.quat_nominal(3);
+	const float q0 = _state.quat_nominal(0);
+	const float q1 = _state.quat_nominal(1);
+	const float q2 = _state.quat_nominal(2);
+	const float q3 = _state.quat_nominal(3);

 	// get latest velocity in earth frame
-	const float &vn = _state.vel(0);
-	const float &ve = _state.vel(1);
-	const float &vd = _state.vel(2);
+	const float vn = _state.vel(0);
+	const float ve = _state.vel(1);
+	const float vd = _state.vel(2);

 	// get latest wind velocity in earth frame
-	const float &vwn = _state.wind_vel(0);
-	const float &vwe = _state.wind_vel(1);
+	const float vwn = _state.wind_vel(0);
+	const float vwe = _state.wind_vel(1);

 	// calculate relative wind velocity in earth frame and rotate into body frame
 	const Vector3f rel_wind_earth(vn - vwn, ve - vwe, vd);
@@ -188,12 +188,13 @@ void LoggedTopics::add_default_topics()
 	int32_t sys_ctrl_alloc = 0;
 	param_get(param_find("SYS_CTRL_ALLOC"), &sys_ctrl_alloc);

-	if (sys_ctrl_alloc >= 1) {
+	_dynamic_control_allocation = sys_ctrl_alloc >= 1;
+
+	if (_dynamic_control_allocation) {
 		add_topic("actuator_motors", 100);
 		add_topic("actuator_servos", 100);
 		add_topic("vehicle_angular_acceleration", 20);
-		add_topic("vehicle_angular_acceleration_setpoint", 20);
-		add_topic_multi("vehicle_thrust_setpoint", 20, 2);
+		add_topic_multi("vehicle_thrust_setpoint", 20, 1);
 		add_topic_multi("vehicle_torque_setpoint", 20, 2);
 	}

@@ -244,8 +245,6 @@ void LoggedTopics::add_default_topics()
 void LoggedTopics::add_high_rate_topics()
 {
 	// maximum rate to analyze fast maneuvers (e.g. for racing)
-	add_topic("actuator_controls_0");
-	add_topic("actuator_outputs");
 	add_topic("manual_control_setpoint");
 	add_topic("rate_ctrl_status", 20);
 	add_topic("sensor_combined");
@@ -254,6 +253,16 @@ void LoggedTopics::add_high_rate_topics()
 	add_topic("vehicle_attitude");
 	add_topic("vehicle_attitude_setpoint");
 	add_topic("vehicle_rates_setpoint");
+
+	if (_dynamic_control_allocation) {
+		add_topic("actuator_motors");
+		add_topic("vehicle_thrust_setpoint");
+		add_topic("vehicle_torque_setpoint");
+
+	} else {
+		add_topic("actuator_controls_0");
+		add_topic("actuator_outputs");
+	}
 }

 void LoggedTopics::add_debug_topics()
@@ -329,7 +338,6 @@ void LoggedTopics::add_system_identification_topics()
 	add_topic("actuator_controls_1");
 	add_topic("sensor_combined");
 	add_topic("vehicle_angular_acceleration");
-	add_topic("vehicle_angular_acceleration_setpoint");
 	add_topic("vehicle_torque_setpoint");
 }

@@ -185,6 +185,8 @@ private:
 	RequestedSubscriptionArray _subscriptions;
 	int _num_mission_subs{0};
 	float _rate_factor{1.0f};
+
+	bool _dynamic_control_allocation{false};
 };

 } //namespace logger
@@ -97,19 +97,24 @@ static void timer_callback(void *arg)

 	timer_callback_data_s *data = (timer_callback_data_s *)arg;

-	if (watchdog_update(data->watchdog_data)) {
-		data->watchdog_triggered = true;
-	}
+	int semaphore_value = 0;
+
+	px4_sem_getvalue(&data->semaphore, &semaphore_value);

 	/* check the value of the semaphore: if the logger cannot keep up with running it's main loop as fast
 	 * as the timer_callback here increases the semaphore count, the counter would increase unbounded,
 	 * leading to an overflow at some point. This case we want to avoid here, so we check the current
 	 * value against a (somewhat arbitrary) threshold, and avoid calling sem_post() if it's exceeded.
 	 * (it's not a problem if the threshold is a bit too large, it just means the logger will do
-	 * multiple iterations at once, the next time it's scheduled). */
-	int semaphore_value;
+	 * multiple iterations at once, the next time it's scheduled).
+	 * As the watchdog also uses the counter we use a conservatively high value */
+	bool semaphore_value_saturated = semaphore_value > 100;

-	if (px4_sem_getvalue(&data->semaphore, &semaphore_value) == 0 && semaphore_value > 1) {
+	if (watchdog_update(data->watchdog_data, semaphore_value_saturated)) {
+		data->watchdog_triggered = true;
+	}
+
+	if (semaphore_value_saturated) {
 		return;
 	}

@@ -46,7 +46,7 @@ namespace px4
 namespace logger
 {

-bool watchdog_update(watchdog_data_t &watchdog_data)
+bool watchdog_update(watchdog_data_t &watchdog_data, bool semaphore_value_saturated)
 {

 #if defined(__PX4_NUTTX) && defined(CONFIG_BUILD_FLAT)
@@ -62,7 +62,9 @@ bool watchdog_update(watchdog_data_t &watchdog_data)
 			// When the writer is waiting for an SD transfer, it is not in ready state, thus a long dropout
 			// will not trigger it. The longest period in ready state I measured was around 70ms,
 			// after a param change.
-			// We only check the log writer because it runs at lower priority than the main thread.
+			// Additionally we need to check the main thread as well, because if the main thread gets stalled as well
+			// while the writer is idle (no active write), it would not trigger.
+			// We do that by checking if the scheduling semaphore counter is saturated for a certain duration.
 			// No need to lock the tcb access, since we are in IRQ context

 			// update the timestamp if it has been scheduled recently
@@ -100,7 +102,11 @@ bool watchdog_update(watchdog_data_t &watchdog_data)

 #endif

-			if (now - watchdog_data.ready_to_run_timestamp > 1_s) {
+			if (!semaphore_value_saturated) {
+				watchdog_data.sem_counter_saturated_start = now;
+			}
+
+			if (now - watchdog_data.sem_counter_saturated_start > 3_s || now - watchdog_data.ready_to_run_timestamp > 1_s) {
 				// boost the priority to make sure the logger continues to write to the log.
 				// Note that we never restore the priority, to keep the logic simple and because it is
 				// an event that must not occur under normal circumstances (if it does, there's a bug
@@ -50,6 +50,7 @@ struct watchdog_data_t {
 	int logger_main_task_index = -1;
 	int logger_writer_task_index = -1;
 	hrt_abstime ready_to_run_timestamp = hrt_absolute_time();
+	hrt_abstime sem_counter_saturated_start = hrt_absolute_time();
 	uint8_t last_state = TSTATE_TASK_INVALID;
 #endif /* __PX4_NUTTX */
 };
@@ -70,9 +71,10 @@ void watchdog_initialize(const pid_t pid_logger_main, const pthread_t writer_thr
 * Expected to be called from IRQ context.
 *
 * @param watchdog_data
+ * @param semaphore_value_saturated if the scheduling semaphore counter is saturated
 * @return true if watchdog is triggered, false otherwise
 */
-bool watchdog_update(watchdog_data_t &watchdog_data);
+bool watchdog_update(watchdog_data_t &watchdog_data, bool semaphore_value_saturated);

 } //namespace logger
 } //namespace px4
Author	SHA1	Message	Date
Daniel Agar	9acd39521d	rc_input: correctly init SBUS inversion for other RC protocols	2022-04-30 13:36:20 -04:00
Daniel Agar	7784cd1f40	boards: mro_ctrl-zero-classic updates to sync with ctrl-zero-h7-oem	2022-04-30 09:40:52 -04:00
Daniel Agar	6fc857772d	ekf2: remove unnecessary const references	2022-04-29 21:39:02 -04:00
ShiauweiZhao	4a6e958100	add tattu_can smart battery kconfig	2022-04-29 13:49:32 -04:00
Vincent Poon	080ba4458a	holybro/kakuteh7: change Product ID to show board name properly in QGC https://github.com/mavlink/qgroundcontrol/pull/10270	2022-04-29 07:21:58 +02:00
Beat Küng	d94ec84e46	logged_topics: remove unused vehicle_angular_acceleration_setpoint, extend add_high_rate_topics	2022-04-28 19:51:28 -04:00
Beat Küng	4338976247	ActuatorEffectivenessRotors: add missing getEffectivenessMatrix Fixes MOTORS_6DOF	2022-04-28 19:51:28 -04:00
Beat Küng	efdf5b8fce	vehicle_{thrust,torque}_setpoint.msg: fix comment	2022-04-28 19:51:28 -04:00
Beat Küng	a9129ea003	logger watchdog: also check main thread There was a time window where if a task with higher priority than the main logger thread would busy-loop, it would block both logging threads and the watchdog would not trigger if the writer was in idle state. This can happen for fast SD card writes.	2022-04-28 19:49:24 -04:00
Beat Küng	9e0a8050a9	fix dshot: remove setAllFailsafeValues Fixes a regression from `c1e5e666f0`, where with static mixers the dshot outputs would go to max instead of 0 in a failsafe case.	2022-04-28 13:29:24 -04:00
Daniel Agar	a15432fac1	drivers/rc_input: RC_INPUT_PROTO parameter minimal implementation (#19539 ) Co-authored-by: chris1seto <chris12892@gmail.com> Co-authored-by: chris1seto <chris12892@gmail.com>	2022-04-27 21:06:43 -06:00