drivers/rc_input: cleanup and simplify data processing per type

- don't store data in class

.ci/Jenkinsfile-hardware

@@ -73,10 +73,8 @@ pipeline {
                 }
               }
               post {
-                failure {
-                  sh 'cat /tmp/pyserial_spy_file.txt || true'
-                }
                 always {
+                  sh 'cat /tmp/pyserial_spy_file.txt || true'
                   sh './platforms/nuttx/Debug/jlink_gdb_backtrace_simple.sh build/cubepilot_cubeorange_test/cubepilot_cubeorange_test.elf || true'
                 }
               }
@@ -145,10 +143,8 @@ pipeline {
                 }
               }
               post {
-                failure {
-                  sh 'cat /tmp/pyserial_spy_file.txt || true'
-                }
                 always {
+                  sh 'cat /tmp/pyserial_spy_file.txt || true'
                   sh './platforms/nuttx/Debug/jlink_gdb_backtrace_simple.sh build/cuav_x7pro_test/cuav_x7pro_test.elf || true'
                 }
               }
@@ -217,10 +213,8 @@ pipeline {
                 }
               }
               post {
-                failure {
-                  sh 'cat /tmp/pyserial_spy_file.txt || true'
-                }
                 always {
+                  sh 'cat /tmp/pyserial_spy_file.txt || true'
                   sh './platforms/nuttx/Debug/jlink_gdb_backtrace_simple.sh build/px4_fmu-v3_test/px4_fmu-v3_test.elf || true'
                 }
               }
@@ -288,10 +282,8 @@ pipeline {
                 }
               }
               post {
-                failure {
-                  sh 'cat /tmp/pyserial_spy_file.txt || true'
-                }
                 always {
+                  sh 'cat /tmp/pyserial_spy_file.txt || true'
                   sh './platforms/nuttx/Debug/jlink_gdb_backtrace_simple.sh build/px4_fmu-v4_test/px4_fmu-v4_test.elf || true'
                 }
               }
@@ -360,10 +352,8 @@ pipeline {
                 }
               }
               post {
-                failure {
-                  sh 'cat /tmp/pyserial_spy_file.txt || true'
-                }
                 always {
+                  sh 'cat /tmp/pyserial_spy_file.txt || true'
                   sh './platforms/nuttx/Debug/jlink_gdb_backtrace_simple.sh build/px4_fmu-v4pro_test/px4_fmu-v4pro_test.elf || true'
                 }
               }
@@ -452,10 +442,8 @@ pipeline {
                 }
               }
               post {
-                failure {
-                  sh 'cat /tmp/pyserial_spy_file.txt || true'
-                }
                 always {
+                  sh 'cat /tmp/pyserial_spy_file.txt || true'
                   sh './platforms/nuttx/Debug/jlink_gdb_backtrace_simple.sh build/px4_fmu-v5_debug/px4_fmu-v5_debug.elf || true'
                 }
               }
@@ -536,10 +524,8 @@ pipeline {
                 }
               }
               post {
-                failure {
-                  sh 'cat /tmp/pyserial_spy_file.txt || true'
-                }
                 always {
+                  sh 'cat /tmp/pyserial_spy_file.txt || true'
                   sh './platforms/nuttx/Debug/jlink_gdb_backtrace_simple.sh build/px4_fmu-v5_stackcheck/px4_fmu-v5_stackcheck.elf || true'
                 }
               }
@@ -608,10 +594,8 @@ pipeline {
                 }
               }
               post {
-                failure {
-                  sh 'cat /tmp/pyserial_spy_file.txt || true'
-                }
                 always {
+                  sh 'cat /tmp/pyserial_spy_file.txt || true'
                   sh './platforms/nuttx/Debug/jlink_gdb_backtrace_simple.sh build/px4_fmu-v5_test/px4_fmu-v5_test.elf || true'
                 }
               }
@@ -680,10 +664,8 @@ pipeline {
                 }
               }
               post {
-                failure {
-                  sh 'cat /tmp/pyserial_spy_file.txt || true'
-                }
                 always {
+                  sh 'cat /tmp/pyserial_spy_file.txt || true'
                   sh './platforms/nuttx/Debug/jlink_gdb_backtrace_simple.sh build/nxp_fmuk66-v3_test/nxp_fmuk66-v3_test.elf || true'
                 }
               }
@@ -809,7 +791,7 @@ void resetParameters() {
 void runTests() {
 
   // test loading a range of airframes
-  sh './Tools/HIL/test_airframes.sh `find /dev/serial -name *usb-*` 1000 1001 2100 3000 4001 4018 6001 8001 10016'
+  sh './Tools/HIL/test_airframes.sh `find /dev/serial -name *usb-*` 1000 1001 2100 3000 4001 6001 8001 10016'
 
   resetParameters()
 

ROMFS/px4fmu_common/init.d/airframes/13000_generic_vtol_standard

@@ -24,6 +24,27 @@
 
 . ${R}etc/init.d/rc.vtol_defaults
 
+param set-default CA_AIRFRAME 2
+param set-default CA_ROTOR_COUNT 4
+param set-default CA_ROTOR0_PX 0.15
+param set-default CA_ROTOR0_PY 0.15
+param set-default CA_ROTOR1_PX -0.15
+param set-default CA_ROTOR1_PY -0.15
+param set-default CA_ROTOR2_PX 0.15
+param set-default CA_ROTOR2_PY -0.15
+param set-default CA_ROTOR2_KM -0.05
+param set-default CA_ROTOR3_PX -0.15
+param set-default CA_ROTOR3_PY 0.15
+param set-default CA_ROTOR3_KM -0.05
+param set-default CA_SV_CS_COUNT 4
+param set-default CA_SV_CS0_TYPE 1
+param set-default CA_SV_CS0_TRQ_R -0.5
+param set-default CA_SV_CS1_TYPE 2
+param set-default CA_SV_CS1_TRQ_R 0.5
+param set-default CA_SV_CS2_TYPE 3
+param set-default CA_SV_CS2_TRQ_P 1.0
+param set-default CA_SV_CS3_TRQ_Y 1.0
+param set-default CA_SV_CS3_TYPE 4
 
 param set-default PWM_AUX_DIS5 950
 

ROMFS/px4fmu_common/init.d/airframes/13003_quad_tailsitter

@@ -13,6 +13,25 @@
 
 . ${R}etc/init.d/rc.vtol_defaults
 
+param set-default CA_AIRFRAME 4
+param set-default CA_ROTOR_COUNT 4
+param set-default CA_ROTOR0_PX 0.15
+param set-default CA_ROTOR0_PY 0.15
+param set-default CA_ROTOR1_PX -0.15
+param set-default CA_ROTOR1_PY -0.15
+param set-default CA_ROTOR2_PX 0.15
+param set-default CA_ROTOR2_PY -0.15
+param set-default CA_ROTOR2_KM -0.05
+param set-default CA_ROTOR3_PX -0.15
+param set-default CA_ROTOR3_PY 0.15
+param set-default CA_ROTOR3_KM -0.05
+param set-default CA_SV_CS_COUNT 2
+param set-default CA_SV_CS0_TRQ_P 0.5
+param set-default CA_SV_CS0_TRQ_Y 0.5
+param set-default CA_SV_CS0_TYPE 5
+param set-default CA_SV_CS1_TRQ_P 0.5
+param set-default CA_SV_CS1_TRQ_Y -0.5
+param set-default CA_SV_CS1_TYPE 6
 
 param set-default PWM_MAIN_MAX 2000
 

ROMFS/px4fmu_common/init.d/airframes/13030_generic_vtol_quad_tiltrotor

@@ -31,6 +31,32 @@ param set-default VT_IDLE_PWM_MC 1100
 param set-default VT_TYPE 1
 param set-default VT_MOT_ID 1234
 param set-default VT_FW_MOT_OFFID 24
+
+param set-default CA_AIRFRAME 3
+param set-default CA_ROTOR_COUNT 4
+param set-default CA_ROTOR0_PX 0.15
+param set-default CA_ROTOR0_PY 0.15
+param set-default CA_ROTOR0_TILT 1
+param set-default CA_ROTOR1_PX -0.15
+param set-default CA_ROTOR1_PY -0.15
+param set-default CA_ROTOR1_TILT 2
+param set-default CA_ROTOR2_PX 0.15
+param set-default CA_ROTOR2_PY -0.15
+param set-default CA_ROTOR2_KM -0.05
+param set-default CA_ROTOR2_TILT 3
+param set-default CA_ROTOR3_PX -0.15
+param set-default CA_ROTOR3_PY 0.15
+param set-default CA_ROTOR3_KM -0.05
+param set-default CA_ROTOR3_TILT 4
+param set-default CA_SV_CS_COUNT 4
+param set-default CA_SV_CS1_TRQ_R 0.5
+param set-default CA_SV_CS1_TYPE 2
+param set-default CA_SV_CS2_TRQ_P 1.0
+param set-default CA_SV_CS2_TYPE 3
+param set-default CA_SV_CS3_TRQ_Y 1.0
+param set-default CA_SV_CS3_TYPE 4
+param set-default CA_SV_TL_COUNT 4
+
 set MAV_TYPE 21
 
 set MIXER quad_x

ROMFS/px4fmu_common/init.d/airframes/13200_generic_vtol_tailsitter

@@ -23,6 +23,20 @@ param set-default VT_ELEV_MC_LOCK 0
 param set-default VT_MOT_COUNT 2
 param set-default VT_TYPE 0
 
+param set-default CA_AIRFRAME 4
+param set-default CA_ROTOR_COUNT 2
+param set-default CA_ROTOR0_KM -0.05
+param set-default CA_ROTOR0_PY 0.2
+param set-default CA_ROTOR1_KM -0.05
+param set-default CA_ROTOR1_PY -0.2
+param set-default CA_SV_CS_COUNT 2
+param set-default CA_SV_CS0_TRQ_P 0.5
+param set-default CA_SV_CS0_TRQ_Y 0.5
+param set-default CA_SV_CS0_TYPE 5
+param set-default CA_SV_CS1_TRQ_P 0.5
+param set-default CA_SV_CS1_TRQ_Y -0.5
+param set-default CA_SV_CS1_TYPE 6
+
 param set-default MAV_TYPE 19
 set MAV_TYPE 19
 set MIXER vtol_tailsitter_duo

ROMFS/px4fmu_common/init.d/airframes/2100_standard_plane

@@ -23,6 +23,18 @@
 
 . ${R}etc/init.d/rc.fw_defaults
 
+param set-default CA_AIRFRAME 1
+param set-default CA_ROTOR_COUNT 1
+param set-default CA_ROTOR0_PX 0.3
+param set-default CA_SV_CS_COUNT 4
+param set-default CA_SV_CS0_TRQ_R -0.5
+param set-default CA_SV_CS1_TRQ_R 0.5
+param set-default CA_SV_CS1_TYPE 2
+param set-default CA_SV_CS2_TRQ_P 1.0
+param set-default CA_SV_CS2_TYPE 3
+param set-default CA_SV_CS3_TRQ_Y 1.0
+param set-default CA_SV_CS3_TYPE 4
+
 param set-default PWM_AUX_RATE 50
 param set-default PWM_MAIN_RATE 50
 

ROMFS/px4fmu_common/init.d/airframes/4001_quad_x

@@ -21,3 +21,16 @@
 #
 
 . ${R}etc/init.d/rc.mc_defaults
+
+param set-default CA_ROTOR_COUNT 4
+param set-default CA_ROTOR0_PX 0.15
+param set-default CA_ROTOR0_PY 0.15
+param set-default CA_ROTOR1_PX -0.15
+param set-default CA_ROTOR1_PY -0.15
+param set-default CA_ROTOR2_PX 0.15
+param set-default CA_ROTOR2_PY -0.15
+param set-default CA_ROTOR2_KM -0.05
+param set-default CA_ROTOR3_PX -0.15
+param set-default CA_ROTOR3_PY 0.15
+param set-default CA_ROTOR3_KM -0.05
+

ROMFS/px4fmu_common/init.d/airframes/4018_s500_ctrlalloc

@@ -1,39 +0,0 @@
-#!/bin/sh
-#
-# @name S500 with control allocation
-#
-# @type Quadrotor x
-# @class Copter
-#
-# @maintainer Silvan Fuhrer
-#
-# @board px4_fmu-v2 exclude
-#
-
-. ${R}etc/init.d/rc.mc_defaults
-
-set MIXER skip
-set MIXER_AUX none
-
-param set-default SYS_CTRL_ALLOC 1
-
-param set-default CA_AIRFRAME 0
-
-param set-default CA_ROTOR_COUNT 4
-param set-default CA_ROTOR0_PX 0.177
-param set-default CA_ROTOR0_PY 0.177
-param set-default CA_ROTOR0_KM 0.05
-param set-default CA_ROTOR1_PX -0.177
-param set-default CA_ROTOR1_PY -0.177
-param set-default CA_ROTOR1_KM 0.05
-param set-default CA_ROTOR2_PX 0.177
-param set-default CA_ROTOR2_PY -0.177
-param set-default CA_ROTOR2_KM -0.05
-param set-default CA_ROTOR3_PX -0.177
-param set-default CA_ROTOR3_PY 0.177
-param set-default CA_ROTOR3_KM -0.05
-
-param set-default PWM_MAIN_FUNC1 101
-param set-default PWM_MAIN_FUNC2 102
-param set-default PWM_MAIN_FUNC3 103
-param set-default PWM_MAIN_FUNC4 104

ROMFS/px4fmu_common/init.d/airframes/6001_hexa_x

@@ -23,6 +23,23 @@
 
 . ${R}etc/init.d/rc.mc_defaults
 
+param set-default CA_ROTOR_COUNT 6
+param set-default CA_ROTOR0_PX 0.0
+param set-default CA_ROTOR0_PY 0.5
+param set-default CA_ROTOR0_KM -0.05
+param set-default CA_ROTOR1_PX 0.0
+param set-default CA_ROTOR1_PY -0.5
+param set-default CA_ROTOR2_PX 0.43
+param set-default CA_ROTOR2_PY -0.25
+param set-default CA_ROTOR2_KM -0.05
+param set-default CA_ROTOR3_PX -0.43
+param set-default CA_ROTOR3_PY 0.25
+param set-default CA_ROTOR4_PX 0.43
+param set-default CA_ROTOR4_PY 0.25
+param set-default CA_ROTOR5_PX -0.43
+param set-default CA_ROTOR5_PY -0.25
+param set-default CA_ROTOR5_KM -0.05
+
 set MIXER hexa_x
 
 # Need to set all 8 channels

ROMFS/px4fmu_common/init.d/airframes/6003_hexa_x_ctrlalloc

@@ -1,52 +0,0 @@
-#!/bin/sh
-#
-# @name Hex X with control allocation
-#
-# @type Hexarotor x
-# @class Copter
-#
-# @maintainer Silvan Fuhrer
-#
-# @board px4_fmu-v2 exclude
-#
-
-. ${R}etc/init.d/rc.mc_defaults
-
-param set-default SYS_CTRL_ALLOC 1
-
-param set-default CA_AIRFRAME 0
-param set-default CA_ROTOR_COUNT 6
-
-param set-default CA_ROTOR0_PX 0.0
-param set-default CA_ROTOR0_PY 0.275
-param set-default CA_ROTOR0_KM -0.05
-
-param set-default CA_ROTOR1_PX 0.0
-param set-default CA_ROTOR1_PY -0.275
-param set-default CA_ROTOR1_KM 0.05
-
-param set-default CA_ROTOR2_PX 0.238
-param set-default CA_ROTOR2_PY -0.1375
-param set-default CA_ROTOR2_KM -0.05
-
-param set-default CA_ROTOR3_PX -0.238
-param set-default CA_ROTOR3_PY 0.1375
-param set-default CA_ROTOR3_KM 0.05
-
-param set-default CA_ROTOR4_PX 0.238
-param set-default CA_ROTOR4_PY 0.1375
-param set-default CA_ROTOR4_KM 0.05
-
-param set-default CA_ROTOR5_PX -0.238
-param set-default CA_ROTOR5_PY -0.1375
-param set-default CA_ROTOR5_KM -0.05
-
-param set-default PWM_MAIN_FUNC1 101
-param set-default PWM_MAIN_FUNC2 102
-param set-default PWM_MAIN_FUNC3 103
-param set-default PWM_MAIN_FUNC4 104
-param set-default PWM_MAIN_FUNC5 105
-param set-default PWM_MAIN_FUNC6 106
-
-set MIXER skip
-set MIXER_AUX none

ROMFS/px4fmu_common/init.d/airframes/7001_hexa_+

@@ -23,6 +23,23 @@
 
 . ${R}etc/init.d/rc.mc_defaults
 
+param set-default CA_ROTOR_COUNT 6
+param set-default CA_ROTOR0_PX 0.5
+param set-default CA_ROTOR0_PY 0.0
+param set-default CA_ROTOR0_KM -0.05
+param set-default CA_ROTOR1_PX -0.5
+param set-default CA_ROTOR1_PY 0.0
+param set-default CA_ROTOR2_PX -0.25
+param set-default CA_ROTOR2_PY -0.43
+param set-default CA_ROTOR2_KM -0.05
+param set-default CA_ROTOR3_PX 0.25
+param set-default CA_ROTOR3_PY 0.43
+param set-default CA_ROTOR4_PX 0.25
+param set-default CA_ROTOR4_PY -0.43
+param set-default CA_ROTOR5_PX -0.25
+param set-default CA_ROTOR5_PY 0.43
+param set-default CA_ROTOR5_KM -0.05
+
 set MIXER hexa_+
 
 # Need to set all 8 channels

ROMFS/px4fmu_common/init.d/airframes/8001_octo_x

@@ -25,6 +25,28 @@
 
 . ${R}etc/init.d/rc.mc_defaults
 
+param set-default CA_ROTOR_COUNT 8
+param set-default CA_ROTOR0_KM -0.05
+param set-default CA_ROTOR0_PX 0.46
+param set-default CA_ROTOR0_PY 0.19
+param set-default CA_ROTOR1_KM -0.05
+param set-default CA_ROTOR1_PX -0.46
+param set-default CA_ROTOR1_PY -0.19
+param set-default CA_ROTOR2_PX 0.19
+param set-default CA_ROTOR2_PY 0.46
+param set-default CA_ROTOR3_PX -0.46
+param set-default CA_ROTOR3_PY 0.19
+param set-default CA_ROTOR4_PX 0.46
+param set-default CA_ROTOR4_PY -0.19
+param set-default CA_ROTOR5_PX -0.19
+param set-default CA_ROTOR5_PY -0.46
+param set-default CA_ROTOR6_KM -0.05
+param set-default CA_ROTOR6_PX 0.19
+param set-default CA_ROTOR6_PY -0.46
+param set-default CA_ROTOR7_KM -0.05
+param set-default CA_ROTOR7_PX -0.19
+param set-default CA_ROTOR7_PY 0.46
+
 set MIXER octo_x
 
 set PWM_OUT 12345678

ROMFS/px4fmu_common/init.d/airframes/CMakeLists.txt

@@ -72,7 +72,6 @@ px4_add_romfs_files(
 	4015_holybro_s500
 	4016_holybro_px4vision
 	4017_nxp_hovergames
-	4018_s500_ctrlalloc
 	4019_x500_v2
 	4030_3dr_solo
 	4031_3dr_quad
@@ -100,7 +99,6 @@ px4_add_romfs_files(
 	# [6000, 6999] Hexarotor x"
 	6001_hexa_x
 	6002_draco_r
-	6003_hexa_x_ctrlalloc
 
 	# [7000, 7999] Hexarotor +"
 	7001_hexa_+

boards/matek/gnss-m9n-f4/default.px4board

@@ -2,6 +2,7 @@ CONFIG_BOARD_TOOLCHAIN="arm-none-eabi"
 CONFIG_BOARD_ARCHITECTURE="cortex-m4"
 CONFIG_BOARD_ROMFSROOT="cannode"
 CONFIG_BOARD_CONSTRAINED_MEMORY=y
+CONFIG_BOARD_SERIAL_GPS1="/dev/ttyS2"
 CONFIG_DRIVERS_BAROMETER_DPS310=y
 CONFIG_DRIVERS_BOOTLOADERS=y
 CONFIG_DRIVERS_GPS=y
@@ -15,6 +16,7 @@ CONFIG_SYSTEMCMDS_NSHTERM=y
 CONFIG_SYSTEMCMDS_PARAM=y
 CONFIG_SYSTEMCMDS_REBOOT=y
 CONFIG_SYSTEMCMDS_SERIAL_PASSTHRU=y
+CONFIG_SERIAL_PASSTHRU_UBLOX=y
 CONFIG_SYSTEMCMDS_TOP=y
 CONFIG_SYSTEMCMDS_TOPIC_LISTENER=y
 CONFIG_SYSTEMCMDS_UORB=y

boards/matek/gnss-m9n-f4/init/rc.board_sensors

@@ -3,8 +3,6 @@
 # Matek M9NF4 CAN specific board sensors init
 #------------------------------------------------------------------------------
 
-gps start -d /dev/ttyS2
-
 icm20602 -s start
 
 rm3100 -b 2 -s start

boards/matek/gnss-m9n-f4/nuttx-config/Kconfig

@@ -1,23 +0,0 @@
-menu "SERIAL_PASSTHRU Configuration"
-config SERIAL_PASSTHRU_UBLOX
-	bool "Detect and Auto Connect on U-Center messages"
-	default n
-	---help---
-		This option will enable the cdc_acm_check to launch
-		The passthru driver.
-
-config SERIAL_PASSTHRU_UBLOX_DEV
-	string "Device path of the GPS"
-	depends on SERIAL_PASSTHRU_UBLOX
-	default "/dev/ttyS2"
-	---help---
-		This is the path of the device used as the right side
-		of the passthru.
-
-config SERIAL_PASSTHRU_UBLOX_BAUDRATE
-	string "baudrate"
-	depends on SERIAL_PASSTHRU_UBLOX
-	default "115200"
-	---help---
-		This option sets the baudrate for the passthru.
-endmenu

boards/px4/fmu-v4/init/rc.board_sensors

@@ -32,5 +32,5 @@ then
 	mpu6500 -s -R 0 start
 else
 	# mpu9250 internal SPI bus mpu9250
-	mpu9250 -s -R 8 start
+	mpu9250 -s -R 8 -M start
 fi

platforms/common/uORB/uORBDeviceNode.cpp

@@ -84,7 +84,7 @@ uORB::DeviceNode::DeviceNode(const struct orb_metadata *meta, const uint8_t inst
 
 uORB::DeviceNode::~DeviceNode()
 {
-	delete[] _data;
+	free(_data);
 
 	const char *devname = get_devname();
 
@@ -186,7 +186,9 @@ uORB::DeviceNode::write(cdev::file_t *filp, const char *buffer, size_t buflen)
 
 			/* re-check size */
 			if (nullptr == _data) {
-				_data = new uint8_t[_meta->o_size * _queue_size];
+				const size_t data_size = _meta->o_size * _queue_size;
+				_data = (uint8_t *) px4_cache_aligned_alloc(data_size);
+				memset(_data, 0, data_size);
 			}
 
 			unlock();

platforms/nuttx/src/px4/common/cdc_acm_check.cpp

@@ -50,6 +50,24 @@ __END_DECLS
 
 #define USB_DEVICE_PATH "/dev/ttyACM0"
 
+#if defined(CONFIG_SERIAL_PASSTHRU_UBLOX)
+#  undef SERIAL_PASSTHRU_UBLOX_DEV
+#  if defined(CONFIG_SERIAL_PASSTHRU_GPS1) && defined(CONFIG_BOARD_SERIAL_GPS1)
+#    define SERIAL_PASSTHRU_UBLOX_DEV CONFIG_BOARD_SERIAL_GPS1
+#  elif defined(CONFIG_SERIAL_PASSTHRU_GPS2)&& defined(CONFIG_BOARD_SERIAL_GPS2)
+#    define SERIAL_PASSTHRU_UBLOX_DEV CONFIG_BOARD_SERIAL_GPS2
+#  elif defined(CONFIG_SERIAL_PASSTHRU_GPS3)&& defined(CONFIG_BOARD_SERIAL_GPS3)
+#    define SERIAL_PASSTHRU_UBLOX_DEV CONFIG_BOARD_SERIAL_GPS3
+#  elif defined(CONFIG_SERIAL_PASSTHRU_GPS4)&& defined(CONFIG_BOARD_SERIAL_GPS4)
+#    define SERIAL_PASSTHRU_UBLOX_DEV CONFIG_BOARD_SERIAL_GPS4
+#  elif defined(CONFIG_SERIAL_PASSTHRU_GPS5) && defined(CONFIG_BOARD_SERIAL_GPS5)
+#    define SERIAL_PASSTHRU_UBLOX_DEV CONFIG_BOARD_SERIAL_GPS5
+#  endif
+#  if !defined(SERIAL_PASSTHRU_UBLOX_DEV)
+#    error "CONFIG_SERIAL_PASSTHRU_GPSn and CONFIG_BOARD_SERIAL_GPSn must be defined"
+#  endif
+#endif
+
 static struct work_s usb_serial_work;
 static bool vbus_present_prev = false;
 static int ttyacm_fd = -1;
@@ -204,7 +222,7 @@ static void mavlink_usb_check(void *arg)
 								snprintf(baudstring, sizeof(baudstring), "%d", baudrate);
 								static const char *gps_argv[] {"gps", "stop", nullptr};
 
-								static const char *passthru_argv[] {"serial_passthru", "start", "-t", "-b", baudstring, "-e", USB_DEVICE_PATH, "-d", CONFIG_SERIAL_PASSTHRU_UBLOX_DEV,   nullptr};
+								static const char *passthru_argv[] {"serial_passthru", "start", "-t", "-b", baudstring, "-e", USB_DEVICE_PATH, "-d", SERIAL_PASSTHRU_UBLOX_DEV,   nullptr};
 #endif
 								char **exec_argv = nullptr;
 

src/drivers/drv_pwm_output.h

@@ -241,7 +241,7 @@ typedef enum {
 	DShot_cmd_led4_off,     // BLHeli32 only
 	DShot_cmd_audio_stream_mode_on_off              = 30, // KISS audio Stream mode on/off
 	DShot_cmd_silent_mode_on_off                    = 31, // KISS silent Mode on/off
-	DShot_cmd_signal_line_telemeetry_disable        = 32,
+	DShot_cmd_signal_line_telemetry_disable         = 32,
 	DShot_cmd_signal_line_continuous_erpm_telemetry = 33,
 	DShot_cmd_MAX          = 47,     // >47 are throttle values
 	DShot_cmd_MIN_throttle = 48,

src/drivers/rc_input/RCInput.cpp

@@ -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
@@ -33,9 +33,9 @@
 
 #include "RCInput.hpp"
 
-#include "crsf_telemetry.h"
 #include <uORB/topics/vehicle_command_ack.h>
 
+#include <poll.h>
 #include <termios.h>
 
 using namespace time_literals;
@@ -44,21 +44,8 @@ constexpr char const *RCInput::RC_SCAN_STRING[];
 
 RCInput::RCInput(const char *device) :
 	ModuleParams(nullptr),
-	ScheduledWorkItem(MODULE_NAME, px4::serial_port_to_wq(device)),
-	_cycle_perf(perf_alloc(PC_ELAPSED, MODULE_NAME": cycle time")),
-	_publish_interval_perf(perf_alloc(PC_INTERVAL, MODULE_NAME": publish interval"))
+	ScheduledWorkItem(MODULE_NAME, px4::serial_port_to_wq(device))
 {
-	// rc input, published to ORB
-	_rc_in.input_source = input_rc_s::RC_INPUT_SOURCE_PX4FMU_PPM;
-
-	// initialize it as RC lost
-	_rc_in.rc_lost = true;
-
-	// initialize raw_rc values and count
-	for (unsigned i = 0; i < input_rc_s::RC_INPUT_MAX_CHANNELS; i++) {
-		_raw_rc_values[i] = UINT16_MAX;
-	}
-
 	if (device) {
 		strncpy(_device, device, sizeof(_device) - 1);
 		_device[sizeof(_device) - 1] = '\0';
@@ -77,6 +64,7 @@ RCInput::~RCInput()
 	delete _ghst_telemetry;
 
 	perf_free(_cycle_perf);
+	perf_free(_cycle_interval_perf);
 	perf_free(_publish_interval_perf);
 }
 
@@ -88,6 +76,11 @@ RCInput::init()
 	RF_RADIO_POWER_CONTROL(true);
 #endif // RF_RADIO_POWER_CONTROL
 
+
+#if defined(RC_SERIAL_PORT)
+	_rc_serial_port_output = (strcmp(_device, RC_SERIAL_PORT) != 0);
+#endif // RC_SERIAL_PORT
+
 	// 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);
@@ -164,89 +157,60 @@ RCInput::task_spawn(int argc, char *argv[])
 	_object.store(instance);
 	_task_id = task_id_is_work_queue;
 
-	instance->ScheduleOnInterval(_current_update_interval);
+	instance->ScheduleOnInterval(_backup_update_interval);
 
 	return PX4_OK;
 }
 
-void
-RCInput::fill_rc_in(uint16_t raw_rc_count_local,
-		    uint16_t raw_rc_values_local[input_rc_s::RC_INPUT_MAX_CHANNELS],
-		    hrt_abstime now, bool frame_drop, bool failsafe,
-		    unsigned frame_drops, int rssi = -1)
+void RCInput::FillRssi(input_rc_s &input_rc)
 {
-	// fill rc_in struct for publishing
-	_rc_in.channel_count = raw_rc_count_local;
-
-	if (_rc_in.channel_count > input_rc_s::RC_INPUT_MAX_CHANNELS) {
-		_rc_in.channel_count = input_rc_s::RC_INPUT_MAX_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) {
-		if ((_param_rc_rssi_pwm_chan.get() > 0) && (_param_rc_rssi_pwm_chan.get() < _rc_in.channel_count)) {
+	if (input_rc.rssi < 0 || input_rc.rssi > input_rc_s::RSSI_MAX) {
+		if ((_param_rc_rssi_pwm_chan.get() > 0) && (_param_rc_rssi_pwm_chan.get() < input_rc.channel_count)) {
 			const int32_t rssi_pwm_chan = _param_rc_rssi_pwm_chan.get();
 			const int32_t rssi_pwm_min = _param_rc_rssi_pwm_min.get();
 			const int32_t rssi_pwm_max = _param_rc_rssi_pwm_max.get();
 
 			// get RSSI from input channel
-			int rc_rssi = ((_rc_in.values[rssi_pwm_chan - 1] - rssi_pwm_min) * 100) / (rssi_pwm_max - rssi_pwm_min);
-			_rc_in.rssi = math::constrain(rc_rssi, 0, 100);
+			int rc_rssi = ((input_rc.values[rssi_pwm_chan - 1] - rssi_pwm_min) * 100) / (rssi_pwm_max - rssi_pwm_min);
+
+			input_rc.rssi = math::constrain(rc_rssi, 0, (int)input_rc_s::RSSI_MAX);
+
+#if defined(ADC_RC_RSSI_CHANNEL)
 
 		} else if (_analog_rc_rssi_stable) {
 			// set RSSI if analog RSSI input is present
 			float rssi_analog = ((_analog_rc_rssi_volt - 0.2f) / 3.0f) * 100.0f;
 
-			if (rssi_analog > 100.0f) {
-				rssi_analog = 100.0f;
-			}
-
-			if (rssi_analog < 0.0f) {
-				rssi_analog = 0.0f;
-			}
-
-			_rc_in.rssi = rssi_analog;
-
-		} else {
-			_rc_in.rssi = 255;
+			input_rc.rssi = math::constrain((int)roundf(rssi_analog), 0, (int)input_rc_s::RSSI_MAX);
+#endif // ADC_RC_RSSI_CHANNEL
 		}
-
-	} 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 RCInput::set_rc_scan_state(RC_SCAN newState)
+void RCInput::PublishInputRc(input_rc_s &input_rc)
 {
-	PX4_DEBUG("RCscan: %s failed, trying %s", RCInput::RC_SCAN_STRING[_rc_scan_state], RCInput::RC_SCAN_STRING[newState]);
+	FillRssi(input_rc); // requires input_rc.values[]
+
+	input_rc.timestamp = hrt_absolute_time();
+	_input_rc_pub.publish(input_rc);
+
+	perf_count(_publish_interval_perf);
+
+	_last_publish_time = input_rc.timestamp;
+	_rc_scan_locked = true;
+}
+
+void RCInput::set_next_rc_scan_state()
+{
+	int new_state = _rc_scan_state + 1;
+
+	if (new_state >= RC_SCAN::RC_SCAN_MAX) {
+		new_state = 0;
+	}
+
+	PX4_DEBUG("RC scan: %s failed, trying %s", RCInput::RC_SCAN_STRING[_rc_scan_state], RCInput::RC_SCAN_STRING[new_state]);
 	_rc_scan_begin = 0;
-	_rc_scan_state = newState;
+	_rc_scan_state = static_cast<RC_SCAN>(new_state);
 	_rc_scan_locked = false;
 
 	_report_lock = true;
@@ -288,8 +252,6 @@ void RCInput::Run()
 
 	} else {
 
-		perf_begin(_cycle_perf);
-
 		// Check if parameters have changed
 		if (_parameter_update_sub.updated()) {
 			// clear update
@@ -307,13 +269,10 @@ void RCInput::Run()
 			}
 		}
 
-		const hrt_abstime cycle_timestamp = hrt_absolute_time();
-
-
 		/* vehicle command */
 		vehicle_command_s vcmd;
 
-		if (_vehicle_cmd_sub.update(&vcmd)) {
+		while (_vehicle_cmd_sub.update(&vcmd)) {
 			// Check for a pairing command
 			if (vcmd.command == vehicle_command_s::VEHICLE_CMD_START_RX_PAIR) {
 
@@ -370,9 +329,8 @@ void RCInput::Run()
 			if (_adc_report_sub.copy(&adc)) {
 				for (unsigned i = 0; i < PX4_MAX_ADC_CHANNELS; ++i) {
 					if (adc.channel_id[i] == ADC_RC_RSSI_CHANNEL) {
-						float adc_volt = adc.raw_data[i] *
-								 adc.v_ref /
-								 adc.resolution;
+
+						float adc_volt = adc.raw_data[i] * adc.v_ref / adc.resolution;
 
 						if (_analog_rc_rssi_volt < 0.0f) {
 							_analog_rc_rssi_volt = adc_volt;
@@ -391,18 +349,25 @@ void RCInput::Run()
 
 #endif // ADC_RC_RSSI_CHANNEL
 
-		bool rc_updated = false;
 
 		// 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;
+		static constexpr hrt_abstime rc_scan_max = 300_ms;
 
-		unsigned frame_drops = 0;
+		// poll with 1 second timeout
+		pollfd fds[1];
+		fds[0].fd = _rcs_fd;
+		fds[0].events = POLLIN;
+		int ret = poll(fds, 1, 1000);
 
-		// TODO: needs work (poll _rcs_fd)
-		// int ret = poll(fds, sizeof(fds) / sizeof(fds[0]), 100);
-		// then update priority to SCHED_PRIORITY_FAST_DRIVER
-		// read all available data from the serial RC input UART
+		perf_begin(_cycle_perf);
+		perf_count(_cycle_interval_perf);
+
+		if (ret < 0) {
+			PX4_DEBUG("poll error %d", ret);
+		}
+
+		const hrt_abstime cycle_timestamp = hrt_absolute_time();
 
 		// read all available data from the serial RC input UART
 		int newBytes = ::read(_rcs_fd, &_rcs_buf[0], RC_MAX_BUFFER_SIZE);
@@ -411,7 +376,48 @@ void RCInput::Run()
 			_bytes_rx += newBytes;
 		}
 
+		bool rc_updated = false;
+
 		switch (_rc_scan_state) {
+#if defined(HRT_PPM_CHANNEL)
+
+		case RC_SCAN_PPM:
+			if (_rc_scan_begin == 0) {
+				_rc_scan_begin = cycle_timestamp;
+				// Configure timer input pin for CPPM
+				px4_arch_configgpio(GPIO_PPM_IN);
+
+			} else if (_rc_scan_locked || (cycle_timestamp - _rc_scan_begin < rc_scan_max)) {
+				// see if we have new PPM input data
+				if ((ppm_last_valid_decode != _last_publish_time) && (ppm_decoded_channels >= 4)) {
+					// we have a new PPM frame. Publish it.
+					rc_updated = true;
+
+					input_rc_s input_rc{};
+					input_rc.timestamp_last_signal = ppm_last_valid_decode;
+					input_rc.channel_count = math::max(ppm_decoded_channels, (unsigned)input_rc_s::RC_INPUT_MAX_CHANNELS);
+					input_rc.rc_lost = (ppm_decoded_channels == 0);
+					input_rc.input_source = input_rc_s::RC_INPUT_SOURCE_PX4FMU_PPM;
+					input_rc.rc_ppm_frame_length = ppm_frame_length;
+
+					for (int i = 0; i < input_rc.channel_count; i++) {
+						input_rc.values[i] = ppm_buffer[i];
+					}
+
+					PublishInputRc(input_rc);
+				}
+
+			} else {
+				// disable CPPM input by mapping it away from the timer capture input
+				px4_arch_unconfiggpio(GPIO_PPM_IN);
+
+				// Scan the next protocol
+				set_next_rc_scan_state();
+			}
+
+			break;
+#endif  // HRT_PPM_CHANNEL
+
 		case RC_SCAN_SBUS:
 			if (_rc_scan_begin == 0) {
 				_rc_scan_begin = cycle_timestamp;
@@ -423,30 +429,40 @@ void RCInput::Run()
 				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
+			} else if (_rc_scan_locked || (cycle_timestamp - _rc_scan_begin < rc_scan_max)) {
 				if (newBytes > 0) {
+					// parse new data
+					uint16_t raw_rc_values[input_rc_s::RC_INPUT_MAX_CHANNELS];
+					uint16_t raw_rc_count = 0;
 					bool sbus_failsafe = false;
 					bool sbus_frame_drop = false;
+					unsigned frame_drops = 0;
 
-					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) {
 						// we have a new SBUS frame. Publish it.
-						_rc_in.input_source = input_rc_s::RC_INPUT_SOURCE_PX4FMU_SBUS;
-						fill_rc_in(_raw_rc_count, _raw_rc_values, cycle_timestamp,
-							   sbus_frame_drop, sbus_failsafe, frame_drops);
-						_rc_scan_locked = true;
+						input_rc_s input_rc{};
+						input_rc.timestamp_last_signal = cycle_timestamp;
+						input_rc.channel_count = math::max(raw_rc_count, (uint16_t)input_rc_s::RC_INPUT_MAX_CHANNELS);
+						input_rc.rc_failsafe = sbus_failsafe;
+						input_rc.rc_lost = (raw_rc_count == 0);
+						input_rc.rc_lost_frame_count = frame_drops;
+						input_rc.input_source = input_rc_s::RC_INPUT_SOURCE_PX4FMU_SBUS;
+
+						for (int i = 0; i < input_rc.channel_count; i++) {
+							input_rc.values[i] = raw_rc_values[i];
+						}
+
+						PublishInputRc(input_rc);
 					}
 				}
 
 			} else {
 				// Scan the next protocol
 				rc_io_invert(false);
-				set_rc_scan_state(RC_SCAN_DSM);
+				set_next_rc_scan_state();
 			}
 
 			break;
@@ -461,29 +477,38 @@ void RCInput::Run()
 				tcflush(_rcs_fd, TCIOFLUSH);
 				memset(_rcs_buf, 0, sizeof(_rcs_buf));
 
-			} else if (_rc_scan_locked
-				   || cycle_timestamp - _rc_scan_begin < rc_scan_max) {
-
+			} else if (_rc_scan_locked || (cycle_timestamp - _rc_scan_begin < rc_scan_max)) {
 				if (newBytes > 0) {
-					int8_t dsm_rssi = 0;
-					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,
+					uint16_t raw_rc_values[input_rc_s::RC_INPUT_MAX_CHANNELS];
+					uint16_t raw_rc_count = 0;
+					bool dsm_11_bit = false;
+					unsigned frame_drops = 0;
+					int8_t dsm_rssi = 0;
+
+					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) {
 						// we have a new DSM frame. Publish it.
-						_rc_in.input_source = input_rc_s::RC_INPUT_SOURCE_PX4FMU_DSM;
-						fill_rc_in(_raw_rc_count, _raw_rc_values, cycle_timestamp,
-							   false, false, frame_drops, dsm_rssi);
-						_rc_scan_locked = true;
+						input_rc_s input_rc{};
+						input_rc.timestamp_last_signal = cycle_timestamp;
+						input_rc.channel_count = math::max(raw_rc_count, (uint16_t)input_rc_s::RC_INPUT_MAX_CHANNELS);
+						input_rc.rc_lost = (raw_rc_count == 0);
+						input_rc.rc_lost_frame_count = frame_drops;
+						input_rc.input_source = input_rc_s::RC_INPUT_SOURCE_PX4FMU_DSM;
+
+						for (int i = 0; i < input_rc.channel_count; i++) {
+							input_rc.values[i] = raw_rc_values[i];
+						}
+
+						PublishInputRc(input_rc);
 					}
 				}
 
 			} else {
 				// Scan the next protocol
-				set_rc_scan_state(RC_SCAN_ST24);
+				set_next_rc_scan_state();
 			}
 
 			break;
@@ -498,43 +523,43 @@ void RCInput::Run()
 				tcflush(_rcs_fd, TCIOFLUSH);
 				memset(_rcs_buf, 0, sizeof(_rcs_buf));
 
-			} else if (_rc_scan_locked
-				   || cycle_timestamp - _rc_scan_begin < rc_scan_max) {
-
+			} else if (_rc_scan_locked || (cycle_timestamp - _rc_scan_begin < rc_scan_max)) {
 				if (newBytes > 0) {
 					// parse new data
-					uint8_t st24_rssi, lost_count;
-
-					rc_updated = false;
+					uint8_t st24_rssi = 0;
+					uint8_t lost_count = 0;
+					uint16_t raw_rc_count = 0;
+					uint16_t raw_rc_values[input_rc_s::RC_INPUT_MAX_CHANNELS];
 
 					for (unsigned i = 0; i < (unsigned)newBytes; i++) {
-						/* set updated flag if one complete packet was parsed */
+						// 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,
-										&_raw_rc_count, _raw_rc_values, input_rc_s::RC_INPUT_MAX_CHANNELS));
+						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));
 					}
 
 					// The st24 will keep outputting RC channels and RSSI even if RC has been lost.
 					// The only way to detect RC loss is therefore to look at the lost_count.
-
 					if (rc_updated) {
-						if (lost_count == 0) {
-							// we have a new ST24 frame. Publish it.
-							_rc_in.input_source = input_rc_s::RC_INPUT_SOURCE_PX4FMU_ST24;
-							fill_rc_in(_raw_rc_count, _raw_rc_values, cycle_timestamp,
-								   false, false, frame_drops, st24_rssi);
-							_rc_scan_locked = true;
+						// we have a new ST24 frame. Publish it.
+						input_rc_s input_rc{};
+						input_rc.timestamp_last_signal = cycle_timestamp;
+						input_rc.channel_count = math::max(raw_rc_count, (uint16_t)input_rc_s::RC_INPUT_MAX_CHANNELS);
+						input_rc.rssi = st24_rssi;
+						input_rc.rc_lost = (raw_rc_count == 0) || (lost_count > 0);
+						input_rc.input_source = input_rc_s::RC_INPUT_SOURCE_PX4FMU_ST24;
 
-						} else {
-							// if the lost count > 0 means that there is an RC loss
-							_rc_in.rc_lost = true;
+						for (int i = 0; i < input_rc.channel_count; i++) {
+							input_rc.values[i] = raw_rc_values[i];
 						}
+
+						PublishInputRc(input_rc);
 					}
 				}
 
 			} else {
 				// Scan the next protocol
-				set_rc_scan_state(RC_SCAN_SUMD);
+				set_next_rc_scan_state();
 			}
 
 			break;
@@ -549,74 +574,47 @@ void RCInput::Run()
 				tcflush(_rcs_fd, TCIOFLUSH);
 				memset(_rcs_buf, 0, sizeof(_rcs_buf));
 
-			} else if (_rc_scan_locked
-				   || cycle_timestamp - _rc_scan_begin < rc_scan_max) {
-
+			} else if (_rc_scan_locked || (cycle_timestamp - _rc_scan_begin < rc_scan_max)) {
 				if (newBytes > 0) {
 					// parse new data
-					uint8_t sumd_rssi, rx_count;
-					bool sumd_failsafe;
-
-					rc_updated = false;
+					uint8_t sumd_rssi = 0;
+					uint8_t rx_count = 0;
+					uint16_t raw_rc_count = 0;
+					uint16_t raw_rc_values[input_rc_s::RC_INPUT_MAX_CHANNELS];
+					bool sumd_failsafe = false;
 
 					for (unsigned i = 0; i < (unsigned)newBytes; i++) {
-						/* set updated flag if one complete packet was parsed */
+						// 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,
-										&_raw_rc_count, _raw_rc_values, input_rc_s::RC_INPUT_MAX_CHANNELS, &sumd_failsafe));
+						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));
 					}
 
 					if (rc_updated) {
 						// we have a new SUMD frame. Publish it.
-						_rc_in.input_source = input_rc_s::RC_INPUT_SOURCE_PX4FMU_SUMD;
-						fill_rc_in(_raw_rc_count, _raw_rc_values, cycle_timestamp,
-							   false, sumd_failsafe, frame_drops, sumd_rssi);
-						_rc_scan_locked = true;
+						input_rc_s input_rc{};
+						input_rc.timestamp_last_signal = cycle_timestamp;
+						input_rc.channel_count = math::max(raw_rc_count, (uint16_t)input_rc_s::RC_INPUT_MAX_CHANNELS);
+						input_rc.rssi = sumd_rssi;
+						input_rc.rc_failsafe = sumd_failsafe;
+						input_rc.rc_lost = (raw_rc_count == 0);
+						input_rc.input_source = input_rc_s::RC_INPUT_SOURCE_PX4FMU_SUMD;
+
+						for (int i = 0; i < input_rc.channel_count; i++) {
+							input_rc.values[i] = raw_rc_values[i];
+						}
+
+						PublishInputRc(input_rc);
 					}
 				}
 
 			} else {
 				// Scan the next protocol
-				set_rc_scan_state(RC_SCAN_PPM);
+				set_next_rc_scan_state();
 			}
 
 			break;
 
-		case RC_SCAN_PPM:
-			// skip PPM if it's not supported
-#ifdef HRT_PPM_CHANNEL
-			if (_rc_scan_begin == 0) {
-				_rc_scan_begin = cycle_timestamp;
-				// Configure timer input pin for CPPM
-				px4_arch_configgpio(GPIO_PPM_IN);
-
-			} else if (_rc_scan_locked || cycle_timestamp - _rc_scan_begin < rc_scan_max) {
-
-				// see if we have new PPM input data
-				if ((ppm_last_valid_decode != _rc_in.timestamp_last_signal) && ppm_decoded_channels > 3) {
-					// we have a new PPM frame. Publish it.
-					rc_updated = true;
-					_rc_in.input_source = input_rc_s::RC_INPUT_SOURCE_PX4FMU_PPM;
-					fill_rc_in(ppm_decoded_channels, ppm_buffer, cycle_timestamp, false, false, 0);
-					_rc_scan_locked = true;
-					_rc_in.rc_ppm_frame_length = ppm_frame_length;
-					_rc_in.timestamp_last_signal = ppm_last_valid_decode;
-				}
-
-			} else {
-				// disable CPPM input by mapping it away from the timer capture input
-				px4_arch_unconfiggpio(GPIO_PPM_IN);
-				// Scan the next protocol
-				set_rc_scan_state(RC_SCAN_CRSF);
-			}
-
-#else   // skip PPM if it's not supported
-			set_rc_scan_state(RC_SCAN_CRSF);
-
-#endif  // HRT_PPM_CHANNEL
-
-			break;
-
 		case RC_SCAN_CRSF:
 			if (_rc_scan_begin == 0) {
 				_rc_scan_begin = cycle_timestamp;
@@ -627,40 +625,44 @@ void RCInput::Run()
 				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
+			} else if (_rc_scan_locked || (cycle_timestamp - _rc_scan_begin < rc_scan_max)) {
 				if (newBytes > 0) {
-					rc_updated = crsf_parse(cycle_timestamp, &_rcs_buf[0], newBytes, &_raw_rc_values[0], &_raw_rc_count,
+					// parse new data
+					uint16_t raw_rc_values[input_rc_s::RC_INPUT_MAX_CHANNELS];
+					uint16_t raw_rc_count = 0;
+
+					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) {
 						// we have a new CRSF frame. Publish it.
-						_rc_in.input_source = input_rc_s::RC_INPUT_SOURCE_PX4FMU_CRSF;
-						fill_rc_in(_raw_rc_count, _raw_rc_values, cycle_timestamp, false, false, 0);
+						input_rc_s input_rc{};
+						input_rc.timestamp_last_signal = cycle_timestamp;
+						input_rc.channel_count = math::max(raw_rc_count, (uint16_t)input_rc_s::RC_INPUT_MAX_CHANNELS);
+						input_rc.rc_lost = (raw_rc_count == 0);
+						input_rc.input_source = input_rc_s::RC_INPUT_SOURCE_PX4FMU_CRSF;
 
-						// on Pixhawk (-related) boards we cannot write to the RC UART
-						// another option is to use a different UART port
-#ifdef BOARD_SUPPORTS_RC_SERIAL_PORT_OUTPUT
-
-						if (!_rc_scan_locked && !_crsf_telemetry) {
-							_crsf_telemetry = new CRSFTelemetry(_rcs_fd);
+						for (int i = 0; i < input_rc.channel_count; i++) {
+							input_rc.values[i] = raw_rc_values[i];
 						}
 
-#endif /* BOARD_SUPPORTS_RC_SERIAL_PORT_OUTPUT */
+						PublishInputRc(input_rc);
 
-						_rc_scan_locked = true;
+						if (_rc_serial_port_output) {
+							if (!_rc_scan_locked && !_crsf_telemetry) {
+								_crsf_telemetry = new CRSFTelemetry(_rcs_fd);
+							}
 
-						if (_crsf_telemetry) {
-							_crsf_telemetry->update(cycle_timestamp);
+							if (_crsf_telemetry) {
+								_crsf_telemetry->update(cycle_timestamp);
+							}
 						}
 					}
 				}
 
 			} else {
 				// Scan the next protocol
-				set_rc_scan_state(RC_SCAN_GHST);
+				set_next_rc_scan_state();
 			}
 
 			break;
@@ -675,55 +677,55 @@ void RCInput::Run()
 				tcflush(_rcs_fd, TCIOFLUSH);
 				memset(_rcs_buf, 0, sizeof(_rcs_buf));
 
-			} else if (_rc_scan_locked
-				   || cycle_timestamp - _rc_scan_begin < rc_scan_max) {
-
+			} else if (_rc_scan_locked || (cycle_timestamp - _rc_scan_begin < rc_scan_max)) {
 				// parse new data
 				if (newBytes > 0) {
+					uint16_t raw_rc_values[input_rc_s::RC_INPUT_MAX_CHANNELS];
+					uint16_t raw_rc_count = 0;
 					int8_t ghst_rssi = -1;
-					rc_updated = ghst_parse(cycle_timestamp, &_rcs_buf[0], newBytes, &_raw_rc_values[0], &ghst_rssi,
-								&_raw_rc_count, input_rc_s::RC_INPUT_MAX_CHANNELS);
+					rc_updated = ghst_parse(cycle_timestamp, &_rcs_buf[0], newBytes, &raw_rc_values[0], &ghst_rssi, &raw_rc_count,
+								input_rc_s::RC_INPUT_MAX_CHANNELS);
 
 					if (rc_updated) {
 						// we have a new GHST frame. Publish it.
-						_rc_in.input_source = input_rc_s::RC_INPUT_SOURCE_PX4FMU_GHST;
-						fill_rc_in(_raw_rc_count, _raw_rc_values, cycle_timestamp, false, false, 0, ghst_rssi);
+						input_rc_s input_rc{};
+						input_rc.timestamp_last_signal = cycle_timestamp;
+						input_rc.channel_count = math::max(raw_rc_count, (uint16_t)input_rc_s::RC_INPUT_MAX_CHANNELS);
+						input_rc.rssi = ghst_rssi;
+						input_rc.rc_lost = (raw_rc_count == 0);
+						input_rc.input_source = input_rc_s::RC_INPUT_SOURCE_PX4FMU_GHST;
 
-						// ghst telemetry works on fmu-v5
-						// on other Pixhawk (-related) boards we cannot write to the RC UART
-						// another option is to use a different UART port
-#ifdef BOARD_SUPPORTS_RC_SERIAL_PORT_OUTPUT
-
-						if (!_rc_scan_locked && !_ghst_telemetry) {
-							_ghst_telemetry = new GHSTTelemetry(_rcs_fd);
+						for (int i = 0; i < input_rc.channel_count; i++) {
+							input_rc.values[i] = raw_rc_values[i];
 						}
 
-#endif /* BOARD_SUPPORTS_RC_SERIAL_PORT_OUTPUT */
+						PublishInputRc(input_rc);
 
-						_rc_scan_locked = true;
+						if (_rc_serial_port_output) {
+							if (!_rc_scan_locked && !_ghst_telemetry) {
+								_ghst_telemetry = new GHSTTelemetry(_rcs_fd);
+							}
 
-						if (_ghst_telemetry) {
-							_ghst_telemetry->update(cycle_timestamp);
+							if (_ghst_telemetry) {
+								_ghst_telemetry->update(cycle_timestamp);
+							}
 						}
 					}
 				}
 
 			} else {
 				// Scan the next protocol
-				set_rc_scan_state(RC_SCAN_SBUS);
+				set_next_rc_scan_state();
 			}
 
 			break;
+
+		default:
+			// Scan the next protocol
+			set_next_rc_scan_state();
 		}
 
-		perf_end(_cycle_perf);
-
-		if (rc_updated) {
-			perf_count(_publish_interval_perf);
-
-			_to_input_rc.publish(_rc_in);
-
-		} else if (!rc_updated && !_armed && (hrt_elapsed_time(&_rc_in.timestamp_last_signal) > 1_s)) {
+		if (!rc_updated && !_armed && (hrt_elapsed_time(&_last_publish_time) > 1_s)) {
 			_rc_scan_locked = false;
 		}
 
@@ -731,6 +733,16 @@ void RCInput::Run()
 			_report_lock = false;
 			PX4_INFO("RC scan: %s RC input locked", RC_SCAN_STRING[_rc_scan_state]);
 		}
+
+		// reschedule immediately if RC is locked
+		if (_rc_scan_locked) {
+			ScheduleNow();
+
+		} else {
+			ScheduleDelayed(_backup_update_interval);
+		}
+
+		perf_end(_cycle_perf);
 	}
 }
 
@@ -805,8 +817,6 @@ int RCInput::custom_command(int argc, char *argv[])
 
 int RCInput::print_status()
 {
-	PX4_INFO("Max update rate: %u Hz", 1000000 / _current_update_interval);
-
 	if (_device[0] != '\0') {
 		PX4_INFO("UART device: %s", _device);
 		PX4_INFO("UART RX bytes: %"  PRIu32, _bytes_rx);
@@ -828,42 +838,23 @@ int RCInput::print_status()
 			PX4_INFO("SBUS frame drops: %u", sbus_dropped_frames());
 			break;
 
-
-		case RC_SCAN_DSM:
-			// DSM status output
-#if defined(SPEKTRUM_POWER)
-#endif
-			break;
-
-		case RC_SCAN_PPM:
-			// PPM status output
-			break;
-
-		case RC_SCAN_SUMD:
-			// SUMD status output
-			break;
-
-		case RC_SCAN_ST24:
-			// SUMD status output
+		default:
 			break;
 		}
 	}
 
-#if ADC_RC_RSSI_CHANNEL
+#if defined(ADC_RC_RSSI_CHANNEL)
 
 	if (_analog_rc_rssi_stable) {
 		PX4_INFO("vrssi: %dmV", (int)(_analog_rc_rssi_volt * 1000.0f));
 	}
 
-#endif
+#endif // ADC_RC_RSSI_CHANNEL
 
 	perf_print_counter(_cycle_perf);
+	perf_print_counter(_cycle_interval_perf);
 	perf_print_counter(_publish_interval_perf);
 
-	if (hrt_elapsed_time(&_rc_in.timestamp) < 1_s) {
-		print_message(ORB_ID(input_rc), _rc_in);
-	}
-
 	return 0;
 }
 

src/drivers/rc_input/RCInput.hpp

@@ -91,23 +91,31 @@ public:
 private:
 
 	enum RC_SCAN {
-		RC_SCAN_PPM = 0,
+#if defined(HRT_PPM_CHANNEL)
+		RC_SCAN_PPM,
+#endif // HRT_PPM_CHANNEL
 		RC_SCAN_SBUS,
 		RC_SCAN_DSM,
 		RC_SCAN_SUMD,
 		RC_SCAN_ST24,
 		RC_SCAN_CRSF,
-		RC_SCAN_GHST
+		RC_SCAN_GHST,
+
+		RC_SCAN_MAX
 	} _rc_scan_state{RC_SCAN_SBUS};
 
-	static constexpr char const *RC_SCAN_STRING[7] {
+	static constexpr char const *RC_SCAN_STRING[] {
+#if defined(HRT_PPM_CHANNEL)
 		"PPM",
+#endif // HRT_PPM_CHANNEL
 		"SBUS",
 		"DSM",
 		"SUMD",
 		"ST24",
 		"CRSF",
-		"GHST"
+		"GHST",
+
+		"NONE"
 	};
 
 	void Run() override;
@@ -116,38 +124,36 @@ private:
 	bool bind_spektrum(int arg = DSMX8_BIND_PULSES) const;
 #endif // SPEKTRUM_POWER
 
-	void fill_rc_in(uint16_t raw_rc_count_local,
-			uint16_t raw_rc_values_local[input_rc_s::RC_INPUT_MAX_CHANNELS],
-			hrt_abstime now, bool frame_drop, bool failsafe,
-			unsigned frame_drops, int rssi);
-
-	void set_rc_scan_state(RC_SCAN _rc_scan_state);
+	void FillRssi(input_rc_s &input_rc);
+	void PublishInputRc(input_rc_s &input_rc);
 
 	void rc_io_invert(bool invert);
+	void set_next_rc_scan_state();
 
 	hrt_abstime _rc_scan_begin{0};
+	hrt_abstime _last_publish_time{0};
 
 	bool _initialized{false};
 	bool _rc_scan_locked{false};
 	bool _report_lock{true};
 
-	static constexpr unsigned	_current_update_interval{4000}; // 250 Hz
+	static constexpr unsigned	_backup_update_interval{100000}; // 10 Hz (backup schedule)
 
 	uORB::SubscriptionInterval _parameter_update_sub{ORB_ID(parameter_update), 1_s};
 
-	uORB::Subscription	_adc_report_sub{ORB_ID(adc_report)};
+#if defined(ADC_RC_RSSI_CHANNEL)
+	uORB::Subscription	_adc_report_sub {ORB_ID(adc_report)};
+	float _analog_rc_rssi_volt{-1.0f};
+	bool _analog_rc_rssi_stable{false};
+#endif // ADC_RC_RSSI_CHANNEL
+
 	uORB::Subscription	_vehicle_cmd_sub{ORB_ID(vehicle_command)};
 	uORB::Subscription	_vehicle_status_sub{ORB_ID(vehicle_status)};
 
-	input_rc_s	_rc_in{};
-
-	float		_analog_rc_rssi_volt{-1.0f};
-	bool		_analog_rc_rssi_stable{false};
-
+	bool _rc_serial_port_output{true};
 	bool _armed{false};
 
-
-	uORB::PublicationMulti<input_rc_s>	_to_input_rc{ORB_ID(input_rc)};
+	uORB::PublicationMulti<input_rc_s> _input_rc_pub{ORB_ID(input_rc)};
 
 	int		_rcs_fd{-1};
 	char		_device[20] {};					///< device / serial port path
@@ -155,14 +161,13 @@ private:
 	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{};
-
 	CRSFTelemetry *_crsf_telemetry{nullptr};
 	GHSTTelemetry *_ghst_telemetry{nullptr};
 
-	perf_counter_t	_cycle_perf;
-	perf_counter_t	_publish_interval_perf;
+	perf_counter_t	_cycle_perf{perf_alloc(PC_ELAPSED, MODULE_NAME": cycle time")};
+	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")};
+
 	uint32_t	_bytes_rx{0};
 
 	DEFINE_PARAMETERS(

src/drivers/sw_crypto/crypto.c

@@ -64,6 +64,12 @@ extern void libtomcrypt_init(void);
  */
 static int crypto_open_count = 0;
 
+/*
+ * Status of libtomcrypt initialization. This is a large library, which
+ * is initialized & pulled in by linker only when it is actually used
+ */
+static bool tomcrypt_initialized = false;
+
 typedef struct {
 	size_t key_size;
 	uint8_t *key;
@@ -76,6 +82,14 @@ typedef struct {
 	uint64_t ctr;
 } chacha20_context_t;
 
+static inline void initialize_tomcrypt(void)
+{
+	if (!tomcrypt_initialized) {
+		libtomcrypt_init();
+		tomcrypt_initialized = true;
+	}
+}
+
 /* Clear key cache */
 static void clear_key_cache(void)
 {
@@ -135,7 +149,6 @@ void crypto_init()
 {
 	keystore_init();
 	clear_key_cache();
-	libtomcrypt_init();
 }
 
 crypto_session_handle_t crypto_open(px4_crypto_algorithm_t algorithm)
@@ -269,6 +282,8 @@ bool crypto_encrypt_data(crypto_session_handle_t handle,
 			uint8_t *public_key = (uint8_t *)crypto_get_key_ptr(handle.keystore_handle, key_idx, &key_sz);
 			*cipher_size = 0;
 
+			initialize_tomcrypt();
+
 			if (public_key &&
 			    rsa_import(public_key, key_sz, &key) == CRYPT_OK) {
 				if (outlen >= ltc_mp.unsigned_size(key.N) &&
@@ -413,6 +428,8 @@ size_t crypto_get_min_blocksize(crypto_session_handle_t handle, uint8_t key_idx)
 			size_t pub_key_sz;
 			uint8_t *pub_key = (uint8_t *)crypto_get_key_ptr(handle.keystore_handle, key_idx, &pub_key_sz);
 
+			initialize_tomcrypt();
+
 			if (pub_key &&
 			    rsa_import(pub_key, pub_key_sz, &enc_key) == CRYPT_OK) {
 				ret = ltc_mp.unsigned_size(enc_key.N);

src/modules/fw_pos_control_l1/FixedwingPositionControl.cpp

@@ -1093,9 +1093,9 @@ FixedwingPositionControl::handle_setpoint_type(const uint8_t setpoint_type, cons
 		}
 	}
 
-	// set to type loiter during VTOL transitions in Land mode (to not start FW landing logic)
+	// set to type position during VTOL transitions in Land mode (to not start FW landing logic)
 	if (pos_sp_curr.type == position_setpoint_s::SETPOINT_TYPE_LAND && _vehicle_status.in_transition_mode) {
-		position_sp_type = position_setpoint_s::SETPOINT_TYPE_LOITER;
+		position_sp_type = position_setpoint_s::SETPOINT_TYPE_POSITION;
 	}
 
 	return position_sp_type;

src/modules/local_position_estimator/BlockLocalPositionEstimator.cpp

@@ -19,7 +19,7 @@ static const char *msg_label = "[lpe] ";	// rate of land detector correction
 
 BlockLocalPositionEstimator::BlockLocalPositionEstimator() :
 	ModuleParams(nullptr),
-	WorkItem(MODULE_NAME, px4::wq_configurations::nav_and_controllers),
+	WorkItem(MODULE_NAME, px4::wq_configurations::INS0),
 
 	// this block has no parent, and has name LPE
 	SuperBlock(nullptr, "LPE"),

src/modules/micrortps_bridge/micrortps_client/microRTPS_client_main.cpp

@@ -122,7 +122,17 @@ static int parse_options(int argc, char *argv[])
 				break;
 			}
 
-		case 'm': _options.datarate        = strtoul(myoptarg, nullptr, 10);    break;
+		case 'm': {
+				int datarate = 0;
+
+				if (px4_get_parameter_value(myoptarg, datarate) != 0) {
+					PX4_ERR("datarate parsing failed");
+				}
+
+				_options.datarate = datarate;
+
+				break;
+			}
 
 		case 'p': _options.poll_ms         = strtoul(myoptarg, nullptr, 10);    break;
 

src/modules/micrortps_bridge/micrortps_client/module.yaml

@@ -3,15 +3,36 @@ serial_config:
     - command: |
         protocol_splitter start ${SERIAL_DEV}
         mavlink start -d /dev/mavlink -b p:${BAUD_PARAM} -m onboard -r 5000 -x
-        micrortps_client start -d /dev/rtps -b p:${BAUD_PARAM} -l -1
+        micrortps_client start -d /dev/rtps -b p:${BAUD_PARAM} -m p:RTPS_RATE -l -1
       port_config_param:
         name: RTPS_MAV_CONFIG
         group: RTPS
       label: MAVLink + FastRTPS
     - command: |
-        micrortps_client start -d ${SERIAL_DEV} -b p:${BAUD_PARAM} -l -1
+        micrortps_client start -d ${SERIAL_DEV} -b p:${BAUD_PARAM} -m p:RTPS_RATE -l -1
       port_config_param:
         name: RTPS_CONFIG
         group: RTPS
       label: FastRTPS
 
+parameters:
+    - group: RTPS
+      definitions:
+        RTPS_RATE: 
+            description:
+                short: Maximum RTPS data rate
+                long: |
+                    Configure the maximum sending rate for the RTPS streams in Bytes/sec.
+                    If the configured streams exceed the maximum rate, the sending rate of
+                    each stream is automatically decreased.
+
+                    0 is unlimited. Note this can cause reliability issues
+                    if enough RTPS topics are selected that exceed the
+                    serial bus limit.
+
+            type: int32
+            min: 0
+            unit: B/s
+            reboot_required: true
+            default: 0
+

src/modules/navigator/land.cpp

@@ -74,8 +74,12 @@ Land::on_active()
 	if (_navigator->get_vstatus()->is_vtol &&
 	    _navigator->get_vstatus()->in_transition_mode) {
 		struct position_setpoint_triplet_s *pos_sp_triplet = _navigator->get_position_setpoint_triplet();
-		pos_sp_triplet->current.lat = _navigator->get_global_position()->lat;
-		pos_sp_triplet->current.lon = _navigator->get_global_position()->lon;
+
+		// create a virtual wp 1m in front of the vehicle to track during the backtransition
+		waypoint_from_heading_and_distance(_navigator->get_global_position()->lat, _navigator->get_global_position()->lon,
+						   _navigator->get_position_setpoint_triplet()->current.yaw, 1.f,
+						   &pos_sp_triplet->current.lat, &pos_sp_triplet->current.lon);
+
 		_navigator->set_position_setpoint_triplet_updated();
 	}
 

src/modules/navigator/mission_block.cpp

@@ -795,7 +795,7 @@ MissionBlock::set_vtol_transition_item(struct mission_item_s *item, const uint8_
 	item->nav_cmd = NAV_CMD_DO_VTOL_TRANSITION;
 	item->params[0] = (float) new_mode;
 	item->params[1] = 0.0f;
-	item->yaw = _navigator->get_local_position()->heading;
+	item->yaw = _navigator->get_local_position()->heading; // ideally that would be course and not heading
 	item->autocontinue = true;
 }
 

src/modules/sensors/sensors.cpp

@@ -344,7 +344,7 @@ int Sensors::parameters_update()
 		// sensor_accel
 		uORB::SubscriptionData<sensor_accel_s> sensor_accel_sub{ORB_ID(sensor_accel), i};
 
-		if (sensor_accel_sub.get().device_id != 0) {
+		if (sensor_accel_sub.advertised() && (sensor_accel_sub.get().device_id != 0)) {
 			calibration::Accelerometer cal;
 			cal.set_calibration_index(i);
 			cal.ParametersLoad();
@@ -353,7 +353,7 @@ int Sensors::parameters_update()
 		// sensor_gyro
 		uORB::SubscriptionData<sensor_gyro_s> sensor_gyro_sub{ORB_ID(sensor_gyro), i};
 
-		if (sensor_gyro_sub.get().device_id != 0) {
+		if (sensor_gyro_sub.advertised() && (sensor_gyro_sub.get().device_id != 0)) {
 			calibration::Gyroscope cal;
 			cal.set_calibration_index(i);
 			cal.ParametersLoad();
@@ -362,7 +362,7 @@ int Sensors::parameters_update()
 		// sensor_mag
 		uORB::SubscriptionData<sensor_mag_s> sensor_mag_sub{ORB_ID(sensor_mag), i};
 
-		if (sensor_mag_sub.get().device_id != 0) {
+		if (sensor_mag_sub.advertised() && (sensor_mag_sub.get().device_id != 0)) {
 			calibration::Magnetometer cal;
 			cal.set_calibration_index(i);
 			cal.ParametersLoad();

src/modules/sensors/vehicle_acceleration/VehicleAcceleration.cpp

@@ -91,8 +91,10 @@ void VehicleAcceleration::CheckAndUpdateFilters()
 
 		const float sample_rate_hz = imu_status.get().accel_rate_hz;
 
-		if ((imu_status.get().accel_device_id != 0) && (imu_status.get().accel_device_id == _calibration.device_id())
+		if (imu_status.advertised() && (imu_status.get().timestamp != 0)
+		    && (imu_status.get().accel_device_id != 0) && (imu_status.get().accel_device_id == _calibration.device_id())
 		    && PX4_ISFINITE(sample_rate_hz) && (sample_rate_hz > 0)) {
+
 			// check if sample rate error is greater than 1%
 			if (!PX4_ISFINITE(_filter_sample_rate) || (fabsf(sample_rate_hz - _filter_sample_rate) / _filter_sample_rate) > 0.01f) {
 				PX4_DEBUG("sample rate changed: %.3f Hz -> %.3f Hz", (double)_filter_sample_rate, (double)sample_rate_hz);
@@ -161,7 +163,8 @@ bool VehicleAcceleration::SensorSelectionUpdate(bool force)
 
 				const uint32_t device_id = sensor_accel_sub.get().device_id;
 
-				if ((device_id != 0) && (device_id == sensor_selection.accel_device_id)) {
+				if (sensor_accel_sub.advertised() && (sensor_accel_sub.get().timestamp != 0)
+				    && (device_id != 0) && (device_id == sensor_selection.accel_device_id)) {
 
 					if (_sensor_sub.ChangeInstance(i) && _sensor_sub.registerCallback()) {
 						PX4_DEBUG("selected sensor changed %" PRIu32 " -> %" PRIu32 "", _calibration.device_id(), device_id);

src/modules/sensors/vehicle_angular_velocity/VehicleAngularVelocity.cpp

@@ -230,19 +230,20 @@ bool VehicleAngularVelocity::SensorSelectionUpdate(const hrt_abstime &time_now_u
 		// use vehicle_imu_status to do basic sensor selection validation
 		for (uint8_t i = 0; i < MAX_SENSOR_COUNT; i++) {
 			uORB::SubscriptionData<vehicle_imu_status_s> imu_status{ORB_ID(vehicle_imu_status), i};
-			bool imu_status_gyro_valid = false;
 
-			if ((imu_status.get().gyro_device_id != 0) && (time_now_us < imu_status.get().timestamp + 1_s)) {
-				imu_status_gyro_valid = true;
-			}
+			if (imu_status.advertised()
+			    && (imu_status.get().timestamp != 0) && (time_now_us < imu_status.get().timestamp + 1_s)
+			    && (imu_status.get().gyro_device_id != 0)) {
+				// vehicle_imu_status gyro valid
 
-			if ((device_id != 0) && (imu_status.get().gyro_device_id == device_id) && imu_status_gyro_valid) {
-				selected_device_id_valid = true;
-			}
+				if ((device_id != 0) && (imu_status.get().gyro_device_id == device_id)) {
+					selected_device_id_valid = true;
+				}
 
-			// record first valid IMU as a backup option
-			if ((device_id_first_valid_imu == 0) && imu_status_gyro_valid) {
-				device_id_first_valid_imu = imu_status.get().gyro_device_id;
+				// record first valid IMU as a backup option
+				if (device_id_first_valid_imu == 0) {
+					device_id_first_valid_imu = imu_status.get().gyro_device_id;
+				}
 			}
 		}
 
@@ -259,7 +260,11 @@ bool VehicleAngularVelocity::SensorSelectionUpdate(const hrt_abstime &time_now_u
 			for (uint8_t i = 0; i < MAX_SENSOR_COUNT; i++) {
 				uORB::SubscriptionData<sensor_gyro_fifo_s> sensor_gyro_fifo_sub{ORB_ID(sensor_gyro_fifo), i};
 
-				if ((time_now_us < sensor_gyro_fifo_sub.get().timestamp + 1_s) && (sensor_gyro_fifo_sub.get().device_id != 0)) {
+				if (sensor_gyro_fifo_sub.advertised()
+				    && (sensor_gyro_fifo_sub.get().timestamp != 0)
+				    && (sensor_gyro_fifo_sub.get().device_id != 0)
+				    && (time_now_us < sensor_gyro_fifo_sub.get().timestamp + 1_s)) {
+
 					// if no gyro was selected use the first valid sensor_gyro_fifo
 					if (!device_id_valid) {
 						device_id = sensor_gyro_fifo_sub.get().device_id;
@@ -297,7 +302,11 @@ bool VehicleAngularVelocity::SensorSelectionUpdate(const hrt_abstime &time_now_u
 			for (uint8_t i = 0; i < MAX_SENSOR_COUNT; i++) {
 				uORB::SubscriptionData<sensor_gyro_s> sensor_gyro_sub{ORB_ID(sensor_gyro), i};
 
-				if ((time_now_us < sensor_gyro_sub.get().timestamp + 1_s) && (sensor_gyro_sub.get().device_id != 0)) {
+				if (sensor_gyro_sub.advertised()
+				    && (sensor_gyro_sub.get().timestamp != 0)
+				    && (sensor_gyro_sub.get().device_id != 0)
+				    && (time_now_us < sensor_gyro_sub.get().timestamp + 1_s)) {
+
 					// if no gyro was selected use the first valid sensor_gyro
 					if (!device_id_valid) {
 						device_id = sensor_gyro_sub.get().device_id;

src/modules/sensors/voted_sensors_update.cpp

@@ -87,7 +87,8 @@ void VotedSensorsUpdate::parametersUpdate()
 		uORB::SubscriptionData<vehicle_imu_s> imu{ORB_ID(vehicle_imu), uorb_index};
 		imu.update();
 
-		if (imu.get().timestamp > 0 && imu.get().accel_device_id > 0 && imu.get().gyro_device_id > 0) {
+		if (imu.advertised() && (imu.get().timestamp != 0)
+		    && (imu.get().accel_device_id != 0) && (imu.get().gyro_device_id != 0)) {
 
 			// find corresponding configured accel priority
 			int8_t accel_cal_index = calibration::FindCurrentCalibrationIndex("ACC", imu.get().accel_device_id);

src/systemcmds/serial_passthru/Kconfig

@@ -4,3 +4,54 @@ menuconfig SYSTEMCMDS_SERIAL_PASSTHRU
 	---help---
 		Enable support for passing one sevice to another
 		i.e ttyACM to ttyS5
+
+if SYSTEMCMDS_SERIAL_PASSTHRU
+
+config SERIAL_PASSTHRU_UBLOX
+	bool "Detect and Auto Connect on U-Center messages"
+	default n
+	---help---
+		This option will enable the cdc_acm_check to launch
+		The passthru driver.
+
+#
+# GPS Passthru device selection
+#
+
+choice
+	prompt "Passthru device"
+	default SERIAL_PASSTHRU_GPS1 if BOARD_SERIAL_GPS1 != ""
+	depends on SERIAL_PASSTHRU_UBLOX
+	---help---
+		This is the GPS device used as the right side of the passthru.
+		the path is provided by BOARD_SERIAL_GPSn
+
+config SERIAL_PASSTHRU_GPS1
+	bool "GPS1"
+	depends on BOARD_SERIAL_GPS1 !=""
+
+config SERIAL_PASSTHRU_GPS2
+	bool "GPS2"
+	depends on BOARD_SERIAL_GPS2 !=""
+
+config SERIAL_PASSTHRU_GPS3
+	bool "GPS3"
+	depends on BOARD_SERIAL_GPS3 !=""
+
+config SERIAL_PASSTHRU_GPS4
+	bool "GPS4"
+	depends on BOARD_SERIAL_GPS4 !=""
+
+config SERIAL_PASSTHRU_GPS5
+	bool "GPS5"
+	depends on BOARD_SERIAL_GPS5 !=""
+endchoice
+
+config SERIAL_PASSTHRU_UBLOX_BAUDRATE
+	string "baudrate"
+	depends on SERIAL_PASSTHRU_UBLOX
+	default "115200"
+	---help---
+		This option sets the baudrate for the passthru.
+
+endif #SYSTEMCMDS_SERIAL_PASSTHRU