WIP: drivers/imu/bosch output data changes

2026-06-19 12:50:35 +08:00 · 2022-02-07 20:34:08 -05:00
53 changed files with 701 additions and 822 deletions
@@ -73,8 +73,10 @@ pipeline {
                }
              }
              post {
-                always {
+                failure {
                  sh 'cat /tmp/pyserial_spy_file.txt || true'
+                }
+                always {
                  sh './platforms/nuttx/Debug/jlink_gdb_backtrace_simple.sh build/cubepilot_cubeorange_test/cubepilot_cubeorange_test.elf || true'
                }
              }
@@ -143,8 +145,10 @@ pipeline {
                }
              }
              post {
-                always {
+                failure {
                  sh 'cat /tmp/pyserial_spy_file.txt || true'
+                }
+                always {
                  sh './platforms/nuttx/Debug/jlink_gdb_backtrace_simple.sh build/cuav_x7pro_test/cuav_x7pro_test.elf || true'
                }
              }
@@ -213,8 +217,10 @@ pipeline {
                }
              }
              post {
-                always {
+                failure {
                  sh 'cat /tmp/pyserial_spy_file.txt || true'
+                }
+                always {
                  sh './platforms/nuttx/Debug/jlink_gdb_backtrace_simple.sh build/px4_fmu-v3_test/px4_fmu-v3_test.elf || true'
                }
              }
@@ -282,8 +288,10 @@ pipeline {
                }
              }
              post {
-                always {
+                failure {
                  sh 'cat /tmp/pyserial_spy_file.txt || true'
+                }
+                always {
                  sh './platforms/nuttx/Debug/jlink_gdb_backtrace_simple.sh build/px4_fmu-v4_test/px4_fmu-v4_test.elf || true'
                }
              }
@@ -352,8 +360,10 @@ pipeline {
                }
              }
              post {
-                always {
+                failure {
                  sh 'cat /tmp/pyserial_spy_file.txt || true'
+                }
+                always {
                  sh './platforms/nuttx/Debug/jlink_gdb_backtrace_simple.sh build/px4_fmu-v4pro_test/px4_fmu-v4pro_test.elf || true'
                }
              }
@@ -442,8 +452,10 @@ pipeline {
                }
              }
              post {
-                always {
+                failure {
                  sh 'cat /tmp/pyserial_spy_file.txt || true'
+                }
+                always {
                  sh './platforms/nuttx/Debug/jlink_gdb_backtrace_simple.sh build/px4_fmu-v5_debug/px4_fmu-v5_debug.elf || true'
                }
              }
@@ -524,8 +536,10 @@ pipeline {
                }
              }
              post {
-                always {
+                failure {
                  sh 'cat /tmp/pyserial_spy_file.txt || true'
+                }
+                always {
                  sh './platforms/nuttx/Debug/jlink_gdb_backtrace_simple.sh build/px4_fmu-v5_stackcheck/px4_fmu-v5_stackcheck.elf || true'
                }
              }
@@ -594,8 +608,10 @@ pipeline {
                }
              }
              post {
-                always {
+                failure {
                  sh 'cat /tmp/pyserial_spy_file.txt || true'
+                }
+                always {
                  sh './platforms/nuttx/Debug/jlink_gdb_backtrace_simple.sh build/px4_fmu-v5_test/px4_fmu-v5_test.elf || true'
                }
              }
@@ -664,8 +680,10 @@ pipeline {
                }
              }
              post {
-                always {
+                failure {
                  sh 'cat /tmp/pyserial_spy_file.txt || true'
+                }
+                always {
                  sh './platforms/nuttx/Debug/jlink_gdb_backtrace_simple.sh build/nxp_fmuk66-v3_test/nxp_fmuk66-v3_test.elf || true'
                }
              }
@@ -791,7 +809,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 6001 8001 10016'
+  sh './Tools/HIL/test_airframes.sh `find /dev/serial -name *usb-*` 1000 1001 2100 3000 4001 4018 6001 8001 10016'

  resetParameters()

@@ -24,27 +24,6 @@

 . ${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

@@ -13,25 +13,6 @@

 . ${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

@@ -31,32 +31,6 @@ 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
@@ -23,20 +23,6 @@ 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
@@ -23,18 +23,6 @@

 . ${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

@@ -21,16 +21,3 @@
 #

 . ${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
-
@@ -0,0 +1,39 @@
+#!/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
@@ -23,23 +23,6 @@

 . ${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
@@ -0,0 +1,52 @@
+#!/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
@@ -23,23 +23,6 @@

 . ${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
@@ -25,28 +25,6 @@

 . ${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
@@ -72,6 +72,7 @@ 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
@@ -99,6 +100,7 @@ px4_add_romfs_files(
 	# [6000, 6999] Hexarotor x"
 	6001_hexa_x
 	6002_draco_r
+	6003_hexa_x_ctrlalloc

 	# [7000, 7999] Hexarotor +"
 	7001_hexa_+
@@ -2,7 +2,6 @@ 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
@@ -16,7 +15,6 @@ 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
@@ -3,6 +3,8 @@
 # Matek M9NF4 CAN specific board sensors init
 #------------------------------------------------------------------------------

+gps start -d /dev/ttyS2
+
 icm20602 -s start

 rm3100 -b 2 -s start
@@ -0,0 +1,23 @@
+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
@@ -32,5 +32,5 @@ then
 	mpu6500 -s -R 0 start
 else
 	# mpu9250 internal SPI bus mpu9250
-	mpu9250 -s -R 8 -M start
+	mpu9250 -s -R 8 start
 fi
@@ -7,6 +7,8 @@ float32 x                 # acceleration in the FRD board frame X-axis in m/s^2
 float32 y                 # acceleration in the FRD board frame Y-axis in m/s^2
 float32 z                 # acceleration in the FRD board frame Z-axis in m/s^2

+float32 range # TODO
+
 float32 temperature       # temperature in degrees Celsius

 uint32 error_count
@@ -7,6 +7,8 @@ float32 x                 # angular velocity in the FRD board frame X-axis in ra
 float32 y                 # angular velocity in the FRD board frame Y-axis in rad/s
 float32 z                 # angular velocity in the FRD board frame Z-axis in rad/s

+float32 range # TODO
+
 float32 temperature       # temperature in degrees Celsius

 uint32 error_count
@@ -84,7 +84,7 @@ uORB::DeviceNode::DeviceNode(const struct orb_metadata *meta, const uint8_t inst

 uORB::DeviceNode::~DeviceNode()
 {
-	free(_data);
+	delete[] _data;

 	const char *devname = get_devname();

@@ -186,9 +186,7 @@ uORB::DeviceNode::write(cdev::file_t *filp, const char *buffer, size_t buflen)

 			/* re-check size */
 			if (nullptr == _data) {
-				const size_t data_size = _meta->o_size * _queue_size;
-				_data = (uint8_t *) px4_cache_aligned_alloc(data_size);
-				memset(_data, 0, data_size);
+				_data = new uint8_t[_meta->o_size * _queue_size];
 			}

 			unlock();
@@ -50,24 +50,6 @@ __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;
@@ -222,7 +204,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", SERIAL_PASSTHRU_UBLOX_DEV,   nullptr};
+								static const char *passthru_argv[] {"serial_passthru", "start", "-t", "-b", baudstring, "-e", USB_DEVICE_PATH, "-d", CONFIG_SERIAL_PASSTHRU_UBLOX_DEV,   nullptr};
 #endif
 								char **exec_argv = nullptr;

@@ -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_telemetry_disable         = 32,
+	DShot_cmd_signal_line_telemeetry_disable        = 32,
 	DShot_cmd_signal_line_continuous_erpm_telemetry = 33,
 	DShot_cmd_MAX          = 47,     // >47 are throttle values
 	DShot_cmd_MIN_throttle = 48,
@@ -1,6 +1,6 @@
 /****************************************************************************
 *
- *   Copyright (c) 2020-2021 PX4 Development Team. All rights reserved.
+ *   Copyright (c) 2020-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
@@ -42,13 +42,13 @@ namespace Bosch::BMI055::Accelerometer

 BMI055_Accelerometer::BMI055_Accelerometer(const I2CSPIDriverConfig &config) :
 	BMI055(config),
-	_px4_accel(get_device_id(), config.rotation)
+	_rotation(config.rotation)
 {
 	if (config.drdy_gpio != 0) {
 		_drdy_missed_perf = perf_alloc(PC_COUNT, MODULE_NAME"_accel: DRDY missed");
 	}

-	ConfigureSampleRate(_px4_accel.get_max_rate_hz());
+	ConfigureSampleRate(RATE);
 }

 BMI055_Accelerometer::~BMI055_Accelerometer()
@@ -202,16 +202,6 @@ void BMI055_Accelerometer::RunImpl()

 					uint8_t samples = fifo_frame_counter;

-					// tolerate minor jitter, leave sample to next iteration if behind by only 1
-					if (samples == _fifo_samples + 1) {
-						// sample timestamp set from data ready already corresponds to _fifo_samples
-						if (timestamp_sample == 0) {
-							timestamp_sample = now - static_cast<int>(FIFO_SAMPLE_DT);
-						}
-
-						samples--;
-					}
-
 					if (FIFORead((timestamp_sample == 0) ? now : timestamp_sample, samples)) {
 						success = true;

@@ -263,23 +253,23 @@ void BMI055_Accelerometer::ConfigureAccel()

 	switch (PMU_RANGE) {
 	case range_2g_set:
-		_px4_accel.set_scale(CONSTANTS_ONE_G / 1024.f); // 1024 LSB/g, 0.98mg/LSB
-		_px4_accel.set_range(2.f * CONSTANTS_ONE_G);
+		_accel_scale = CONSTANTS_ONE_G / 1024.f; // 1024 LSB/g, 0.98mg/LSB
+		_accel_range = 2.f * CONSTANTS_ONE_G;
 		break;

 	case range_4g_set:
-		_px4_accel.set_scale(CONSTANTS_ONE_G / 512.f); // 512 LSB/g, 1.95mg/LSB
-		_px4_accel.set_range(4.f * CONSTANTS_ONE_G);
+		_accel_scale = CONSTANTS_ONE_G / 512.f; // 512 LSB/g, 1.95mg/LSB
+		_accel_range = 4.f * CONSTANTS_ONE_G;
 		break;

 	case range_8g_set:
-		_px4_accel.set_scale(CONSTANTS_ONE_G / 256.f); // 256 LSB/g, 3.91mg/LSB
-		_px4_accel.set_range(8.f * CONSTANTS_ONE_G);
+		_accel_scale = CONSTANTS_ONE_G / 256.f; // 256 LSB/g, 3.91mg/LSB
+		_accel_range = 8.f * CONSTANTS_ONE_G;
 		break;

 	case range_16g_set:
-		_px4_accel.set_scale(CONSTANTS_ONE_G / 128.f); // 128 LSB/g, 7.81mg/LSB
-		_px4_accel.set_range(16.f * CONSTANTS_ONE_G);
+		_accel_scale = CONSTANTS_ONE_G / 128.f; // 128 LSB/g, 7.81mg/LSB
+		_accel_range = 16.f * CONSTANTS_ONE_G;
 		break;
 	}
 }
@@ -415,32 +405,51 @@ bool BMI055_Accelerometer::FIFORead(const hrt_abstime &timestamp_sample, uint8_t
 		return false;
 	}

-	sensor_accel_fifo_s accel{};
-	accel.timestamp_sample = timestamp_sample;
-	accel.samples = samples;
-	accel.dt = FIFO_SAMPLE_DT;
+	sensor_accel_s sensor_accel{};
+	sensor_accel.timestamp_sample = timestamp_sample;
+	sensor_accel.device_id = get_device_id();
+
+	float accel_sum[3] {};

 	for (int i = 0; i < samples; i++) {
 		const FIFO::DATA &fifo_sample = buffer.f[i];

 		// acc_x_msb<11:4> + acc_x_lsb<3:0>
-		const int16_t accel_x = combine(fifo_sample.ACCD_X_MSB, fifo_sample.ACCD_X_LSB) >> 4;
-		const int16_t accel_y = combine(fifo_sample.ACCD_Y_MSB, fifo_sample.ACCD_Y_LSB) >> 4;
-		const int16_t accel_z = combine(fifo_sample.ACCD_Z_MSB, fifo_sample.ACCD_Z_LSB) >> 4;
+		int16_t accel_x = combine(fifo_sample.ACCD_X_MSB, fifo_sample.ACCD_X_LSB) >> 4;
+		int16_t accel_y = combine(fifo_sample.ACCD_Y_MSB, fifo_sample.ACCD_Y_LSB) >> 4;
+		int16_t accel_z = combine(fifo_sample.ACCD_Z_MSB, fifo_sample.ACCD_Z_LSB) >> 4;

 		// sensor's frame is +x forward, +y left, +z up
 		//  flip y & z to publish right handed with z down (x forward, y right, z down)
-		accel.x[i] = accel_x;
-		accel.y[i] = (accel_y == INT16_MIN) ? INT16_MAX : -accel_y;
-		accel.z[i] = (accel_z == INT16_MIN) ? INT16_MAX : -accel_z;
+		// accel_x = accel_x;
+		accel_y = (accel_y == INT16_MIN) ? INT16_MAX : -accel_y;
+		accel_z = (accel_z == INT16_MIN) ? INT16_MAX : -accel_z;
+
+		rotate_3i(_rotation, accel_x, accel_y, accel_z);
+
+		accel_sum[0] += accel_x;
+		accel_sum[1] += accel_y;
+		accel_sum[2] += accel_z;
 	}

-	_px4_accel.set_error_count(perf_event_count(_bad_register_perf) + perf_event_count(_bad_transfer_perf) +
-				   perf_event_count(_fifo_empty_perf) + perf_event_count(_fifo_overflow_perf));
+	if (samples > 0) {
+		sensor_accel.x = (accel_sum[0] / samples) * _accel_scale;
+		sensor_accel.y = (accel_sum[1] / samples) * _accel_scale;
+		sensor_accel.z = (accel_sum[2] / samples) * _accel_scale;

-	_px4_accel.updateFIFO(accel);
+		sensor_accel.range = _accel_range;
+		sensor_accel.samples = samples;
+		sensor_accel.temperature = _temperature;
+		sensor_accel.error_count = perf_event_count(_bad_register_perf) + perf_event_count(_bad_transfer_perf) +
+					   perf_event_count(_fifo_empty_perf) + perf_event_count(_fifo_overflow_perf);

-	return true;
+		sensor_accel.timestamp = hrt_absolute_time();
+		_sensor_accel_pub.publish(sensor_accel);
+
+		return true;
+	}
+
+	return false;
 }

 void BMI055_Accelerometer::FIFOReset()
@@ -472,7 +481,7 @@ void BMI055_Accelerometer::UpdateTemperature()
 	float temperature = static_cast<int8_t>(RegisterRead(Register::ACCD_TEMP)) * 0.5f + 23.f;

 	if (PX4_ISFINITE(temperature)) {
-		_px4_accel.set_temperature(temperature);
+		_temperature = temperature;

 	} else {
 		perf_count(_bad_transfer_perf);
@@ -1,6 +1,6 @@
 /****************************************************************************
 *
- *   Copyright (c) 2020-2021 PX4 Development Team. All rights reserved.
+ *   Copyright (c) 2020-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
@@ -35,10 +35,11 @@

 #include "BMI055.hpp"

-#include <lib/drivers/accelerometer/PX4Accelerometer.hpp>
-
 #include "Bosch_BMI055_Accelerometer_Registers.hpp"

+#include <uORB/PublicationMulti.hpp>
+#include <uORB/topics/sensor_accel.h>
+
 namespace Bosch::BMI055::Accelerometer
 {

@@ -58,7 +59,7 @@ private:
 	static constexpr uint32_t RATE{2000}; // 2000 Hz
 	static constexpr float FIFO_SAMPLE_DT{1e6f / RATE};

-	static constexpr int32_t FIFO_MAX_SAMPLES{math::min(FIFO::SIZE / sizeof(FIFO::DATA), sizeof(sensor_accel_fifo_s::x) / sizeof(sensor_accel_fifo_s::x[0]))};
+	static constexpr int32_t FIFO_MAX_SAMPLES{FIFO::SIZE / sizeof(FIFO::DATA)};

 	// Transfer data
 	struct FIFOTransferBuffer {
@@ -97,7 +98,13 @@ private:

 	void UpdateTemperature();

-	PX4Accelerometer _px4_accel;
+	uORB::PublicationMulti<sensor_accel_s> _sensor_accel_pub{ORB_ID(sensor_accel)};
+
+	const enum Rotation _rotation;
+	float _accel_scale{0.f};
+	float _accel_range{0.f};
+
+	float _temperature{NAN};

 	perf_counter_t _bad_register_perf{perf_alloc(PC_COUNT, MODULE_NAME"_accel: bad register")};
 	perf_counter_t _bad_transfer_perf{perf_alloc(PC_COUNT, MODULE_NAME"_accel: bad transfer")};
@@ -1,6 +1,6 @@
 /****************************************************************************
 *
- *   Copyright (c) 2020-2021 PX4 Development Team. All rights reserved.
+ *   Copyright (c) 2020-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,8 +33,6 @@

 #include "BMI055_Gyroscope.hpp"

-#include <px4_platform/board_dma_alloc.h>
-
 using namespace time_literals;

 namespace Bosch::BMI055::Gyroscope
@@ -42,13 +40,13 @@ namespace Bosch::BMI055::Gyroscope

 BMI055_Gyroscope::BMI055_Gyroscope(const I2CSPIDriverConfig &config) :
 	BMI055(config),
-	_px4_gyro(get_device_id(), config.rotation)
+	_rotation(config.rotation)
 {
 	if (config.drdy_gpio != 0) {
 		_drdy_missed_perf = perf_alloc(PC_COUNT, MODULE_NAME"_gyro: DRDY missed");
 	}

-	ConfigureSampleRate(_px4_gyro.get_max_rate_hz());
+	ConfigureSampleRate(RATE);
 }

 BMI055_Gyroscope::~BMI055_Gyroscope()
@@ -202,16 +200,6 @@ void BMI055_Gyroscope::RunImpl()

 					uint8_t samples = fifo_frame_counter;

-					// tolerate minor jitter, leave sample to next iteration if behind by only 1
-					if (samples == _fifo_samples + 1) {
-						// sample timestamp set from data ready already corresponds to _fifo_samples
-						if (timestamp_sample == 0) {
-							timestamp_sample = now - static_cast<int>(FIFO_SAMPLE_DT);
-						}
-
-						samples--;
-					}
-
 					if (FIFORead((timestamp_sample == 0) ? now : timestamp_sample, samples)) {
 						success = true;

@@ -256,28 +244,28 @@ void BMI055_Gyroscope::ConfigureGyro()

 	switch (RANGE) {
 	case gyro_range_2000_dps:
-		_px4_gyro.set_scale(math::radians(1.f / 16.384f));
-		_px4_gyro.set_range(math::radians(2000.f));
+		_gyro_scale = math::radians(1.f / 16.384f);
+		_gyro_range = math::radians(2000.f);
 		break;

 	case gyro_range_1000_dps:
-		_px4_gyro.set_scale(math::radians(1.f / 32.768f));
-		_px4_gyro.set_range(math::radians(1000.f));
+		_gyro_scale = math::radians(1.f / 32.768f);
+		_gyro_range = math::radians(1000.f);
 		break;

 	case gyro_range_500_dps:
-		_px4_gyro.set_scale(math::radians(1.f / 65.536f));
-		_px4_gyro.set_range(math::radians(500.f));
+		_gyro_scale = math::radians(1.f / 65.536f);
+		_gyro_range = math::radians(500.f);
 		break;

 	case gyro_range_250_dps:
-		_px4_gyro.set_scale(math::radians(1.f / 131.072f));
-		_px4_gyro.set_range(math::radians(250.f));
+		_gyro_scale = math::radians(1.f / 131.072f);
+		_gyro_range = math::radians(250.f);
 		break;

 	case gyro_range_125_dps:
-		_px4_gyro.set_scale(math::radians(1.f / 262.144f));
-		_px4_gyro.set_range(math::radians(125.f));
+		_gyro_scale = math::radians(1.f / 262.144f);
+		_gyro_range = math::radians(125.f);
 		break;
 	}
 }
@@ -413,29 +401,44 @@ bool BMI055_Gyroscope::FIFORead(const hrt_abstime &timestamp_sample, uint8_t sam
 		return false;
 	}

-	sensor_gyro_fifo_s gyro{};
-	gyro.timestamp_sample = timestamp_sample;
-	gyro.samples = samples;
-	gyro.dt = FIFO_SAMPLE_DT;
+	sensor_gyro_s sensor_gyro{};
+	sensor_gyro.timestamp_sample = timestamp_sample;
+	sensor_gyro.device_id = get_device_id();
+
+	float gyro_sum[3] {};

 	for (int i = 0; i < samples; i++) {
 		const FIFO::DATA &fifo_sample = buffer.f[i];

-		const int16_t gyro_x = combine(fifo_sample.RATE_X_MSB, fifo_sample.RATE_X_LSB);
-		const int16_t gyro_y = combine(fifo_sample.RATE_Y_MSB, fifo_sample.RATE_Y_LSB);
-		const int16_t gyro_z = combine(fifo_sample.RATE_Z_MSB, fifo_sample.RATE_Z_LSB);
+		int16_t gyro_x = combine(fifo_sample.RATE_X_MSB, fifo_sample.RATE_X_LSB);
+		int16_t gyro_y = combine(fifo_sample.RATE_Y_MSB, fifo_sample.RATE_Y_LSB);
+		int16_t gyro_z = combine(fifo_sample.RATE_Z_MSB, fifo_sample.RATE_Z_LSB);

 		// sensor's frame is +x forward, +y left, +z up
 		//  flip y & z to publish right handed with z down (x forward, y right, z down)
-		gyro.x[i] = gyro_x;
-		gyro.y[i] = (gyro_y == INT16_MIN) ? INT16_MAX : -gyro_y;
-		gyro.z[i] = (gyro_z == INT16_MIN) ? INT16_MAX : -gyro_z;
+		// gyro_x = gyro_x;
+		gyro_y = (gyro_y == INT16_MIN) ? INT16_MAX : -gyro_y;
+		gyro_z = (gyro_z == INT16_MIN) ? INT16_MAX : -gyro_z;
+
+		rotate_3i(_rotation, gyro_x, gyro_y, gyro_z);
+
+		gyro_sum[0] += gyro_x * _gyro_scale;
+		gyro_sum[1] += gyro_y * _gyro_scale;
+		gyro_sum[2] += gyro_z * _gyro_scale;
 	}

-	_px4_gyro.set_error_count(perf_event_count(_bad_register_perf) + perf_event_count(_bad_transfer_perf) +
-				  perf_event_count(_fifo_empty_perf) + perf_event_count(_fifo_overflow_perf));
+	sensor_gyro.x = gyro_sum[0] / samples;
+	sensor_gyro.y = gyro_sum[1] / samples;
+	sensor_gyro.z = gyro_sum[2] / samples;

-	_px4_gyro.updateFIFO(gyro);
+	sensor_gyro.range = _gyro_range;
+	sensor_gyro.samples = samples;
+	sensor_gyro.temperature = NAN;
+	sensor_gyro.error_count = perf_event_count(_bad_register_perf) + perf_event_count(_bad_transfer_perf) +
+				  perf_event_count(_fifo_empty_perf) + perf_event_count(_fifo_overflow_perf);
+
+	sensor_gyro.timestamp = hrt_absolute_time();
+	_sensor_gyro_pub.publish(sensor_gyro);

 	return true;
 }
@@ -1,6 +1,6 @@
 /****************************************************************************
 *
- *   Copyright (c) 2020-2021 PX4 Development Team. All rights reserved.
+ *   Copyright (c) 2020-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
@@ -35,10 +35,11 @@

 #include "BMI055.hpp"

-#include <lib/drivers/gyroscope/PX4Gyroscope.hpp>
-
 #include "Bosch_BMI055_Gyroscope_Registers.hpp"

+#include <uORB/PublicationMulti.hpp>
+#include <uORB/topics/sensor_gyro.h>
+
 namespace Bosch::BMI055::Gyroscope
 {

@@ -58,7 +59,7 @@ private:
 	static constexpr uint32_t RATE{2000}; // 2000 Hz
 	static constexpr float FIFO_SAMPLE_DT{1e6f / RATE};

-	static constexpr int32_t FIFO_MAX_SAMPLES{math::min(FIFO::SIZE / sizeof(FIFO::DATA), sizeof(sensor_gyro_fifo_s::x) / sizeof(sensor_gyro_fifo_s::x[0]))};
+	static constexpr int32_t FIFO_MAX_SAMPLES{FIFO::SIZE / sizeof(FIFO::DATA)};

 	// Transfer data
 	struct FIFOTransferBuffer {
@@ -95,7 +96,11 @@ private:
 	bool FIFORead(const hrt_abstime &timestamp_sample, uint8_t samples);
 	void FIFOReset();

-	PX4Gyroscope _px4_gyro;
+	uORB::PublicationMulti<sensor_gyro_s> _sensor_gyro_pub{ORB_ID(sensor_gyro)};
+
+	const enum Rotation _rotation;
+	float _gyro_scale{0.f};
+	float _gyro_range{0.f};

 	perf_counter_t _bad_register_perf{perf_alloc(PC_COUNT, MODULE_NAME"_gyro: bad register")};
 	perf_counter_t _bad_transfer_perf{perf_alloc(PC_COUNT, MODULE_NAME"_gyro: bad transfer")};
@@ -35,6 +35,7 @@ px4_add_module(
 	MODULE drivers__imu__bosch__bmi055
 	MAIN bmi055
 	COMPILE_FLAGS
+		${MAX_CUSTOM_OPT_LEVEL}
 	SRCS
 		Bosch_BMI055_Accelerometer_Registers.hpp
 		Bosch_BMI055_Gyroscope_Registers.hpp
@@ -48,7 +49,5 @@ px4_add_module(

 		bmi055_main.cpp
 	DEPENDS
-		drivers_accelerometer
-		drivers_gyroscope
 		px4_work_queue
 	)
@@ -1,6 +1,6 @@
 /****************************************************************************
 *
- *   Copyright (c) 2020-2021 PX4 Development Team. All rights reserved.
+ *   Copyright (c) 2020-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
@@ -42,13 +42,13 @@ namespace Bosch::BMI088::Accelerometer

 BMI088_Accelerometer::BMI088_Accelerometer(const I2CSPIDriverConfig &config) :
 	BMI088(config),
-	_px4_accel(get_device_id(), config.rotation)
+	_rotation(config.rotation)
 {
 	if (config.drdy_gpio != 0) {
 		_drdy_missed_perf = perf_alloc(PC_COUNT, MODULE_NAME"_accel: DRDY missed");
 	}

-	ConfigureSampleRate(_px4_accel.get_max_rate_hz());
+	ConfigureSampleRate(RATE);
 }

 BMI088_Accelerometer::~BMI088_Accelerometer()
@@ -218,12 +218,6 @@ void BMI088_Accelerometer::RunImpl()
 				} else {
 					samples = fifo_byte_counter / sizeof(FIFO::DATA);

-					// tolerate minor jitter, leave sample to next iteration if behind by only 1
-					if (samples == _fifo_samples + 1) {
-						timestamp_sample -= static_cast<int>(FIFO_SAMPLE_DT);
-						samples--;
-					}
-
 					if (samples > FIFO_MAX_SAMPLES) {
 						// not technically an overflow, but more samples than we expected or can publish
 						FIFOReset();
@@ -286,23 +280,23 @@ void BMI088_Accelerometer::ConfigureAccel()

 	switch (ACC_RANGE) {
 	case acc_range_3g:
-		_px4_accel.set_scale(CONSTANTS_ONE_G * (powf(2, ACC_RANGE + 1) * 1.5f) / 32768.f);
-		_px4_accel.set_range(3.f * CONSTANTS_ONE_G);
+		_accel_scale = CONSTANTS_ONE_G * (powf(2, ACC_RANGE + 1) * 1.5f) / 32768.f;
+		_accel_range = 3.f * CONSTANTS_ONE_G;
 		break;

 	case acc_range_6g:
-		_px4_accel.set_scale(CONSTANTS_ONE_G * (powf(2, ACC_RANGE + 1) * 1.5f) / 32768.f);
-		_px4_accel.set_range(6.f * CONSTANTS_ONE_G);
+		_accel_scale = CONSTANTS_ONE_G * (powf(2, ACC_RANGE + 1) * 1.5f) / 32768.f;
+		_accel_range = 6.f * CONSTANTS_ONE_G;
 		break;

 	case acc_range_12g:
-		_px4_accel.set_scale(CONSTANTS_ONE_G * (powf(2, ACC_RANGE + 1) * 1.5f) / 32768.f);
-		_px4_accel.set_range(12.f * CONSTANTS_ONE_G);
+		_accel_scale = CONSTANTS_ONE_G * (powf(2, ACC_RANGE + 1) * 1.5f) / 32768.f;
+		_accel_range = 12.f * CONSTANTS_ONE_G;
 		break;

 	case acc_range_24g:
-		_px4_accel.set_scale(CONSTANTS_ONE_G * (powf(2, ACC_RANGE + 1) * 1.5f) / 32768.f);
-		_px4_accel.set_range(24.f * CONSTANTS_ONE_G);
+		_accel_scale = CONSTANTS_ONE_G * (powf(2, ACC_RANGE + 1) * 1.5f) / 32768.f;
+		_accel_range = 24.f * CONSTANTS_ONE_G;
 		break;
 	}
 }
@@ -483,15 +477,17 @@ bool BMI088_Accelerometer::FIFORead(const hrt_abstime &timestamp_sample, uint8_t
 		return false;
 	}

-	sensor_accel_fifo_s accel{};
-	accel.timestamp_sample = timestamp_sample;
-	accel.samples = 0;
-	accel.dt = FIFO_SAMPLE_DT;
+	sensor_accel_s sensor_accel{};
+	sensor_accel.timestamp_sample = timestamp_sample;
+	sensor_accel.device_id = get_device_id();

 	// first find all sensor data frames in the buffer
 	uint8_t *data_buffer = (uint8_t *)&buffer.f[0];
 	unsigned fifo_buffer_index = 0; // start of buffer

+	float accel_sum[3] {};
+	int accel_samples = 0;
+
 	while (fifo_buffer_index < math::min(fifo_byte_counter, transfer_size - 4)) {
 		// look for header signature (first 6 bits) followed by two bits indicating the status of INT1 and INT2
 		switch (data_buffer[fifo_buffer_index] & 0xFC) {
@@ -500,16 +496,23 @@ bool BMI088_Accelerometer::FIFORead(const hrt_abstime &timestamp_sample, uint8_t
 				// Frame length: 7 bytes (1 byte header + 6 bytes payload)

 				FIFO::DATA *fifo_sample = (FIFO::DATA *)&data_buffer[fifo_buffer_index];
-				const int16_t accel_x = combine(fifo_sample->ACC_X_MSB, fifo_sample->ACC_X_LSB);
-				const int16_t accel_y = combine(fifo_sample->ACC_Y_MSB, fifo_sample->ACC_Y_LSB);
-				const int16_t accel_z = combine(fifo_sample->ACC_Z_MSB, fifo_sample->ACC_Z_LSB);
+				int16_t accel_x = combine(fifo_sample->ACC_X_MSB, fifo_sample->ACC_X_LSB);
+				int16_t accel_y = combine(fifo_sample->ACC_Y_MSB, fifo_sample->ACC_Y_LSB);
+				int16_t accel_z = combine(fifo_sample->ACC_Z_MSB, fifo_sample->ACC_Z_LSB);

 				// sensor's frame is +x forward, +y left, +z up
 				//  flip y & z to publish right handed with z down (x forward, y right, z down)
-				accel.x[accel.samples] = accel_x;
-				accel.y[accel.samples] = (accel_y == INT16_MIN) ? INT16_MAX : -accel_y;
-				accel.z[accel.samples] = (accel_z == INT16_MIN) ? INT16_MAX : -accel_z;
-				accel.samples++;
+				// accel_x = accel_x;
+				accel_y = (accel_y == INT16_MIN) ? INT16_MAX : -accel_y;
+				accel_z = (accel_z == INT16_MIN) ? INT16_MAX : -accel_z;
+
+				rotate_3i(_rotation, accel_x, accel_y, accel_z);
+
+				accel_sum[0] += accel_x;
+				accel_sum[1] += accel_y;
+				accel_sum[2] += accel_z;
+
+				accel_samples++;

 				fifo_buffer_index += 7; // move forward to next record
 			}
@@ -549,11 +552,20 @@ bool BMI088_Accelerometer::FIFORead(const hrt_abstime &timestamp_sample, uint8_t
 		}
 	}

-	_px4_accel.set_error_count(perf_event_count(_bad_register_perf) + perf_event_count(_bad_transfer_perf) +
-				   perf_event_count(_fifo_empty_perf) + perf_event_count(_fifo_overflow_perf));
+	if (accel_samples > 0) {
+		sensor_accel.x = (accel_sum[0] / accel_samples) * _accel_scale;
+		sensor_accel.y = (accel_sum[1] / accel_samples) * _accel_scale;
+		sensor_accel.z = (accel_sum[2] / accel_samples) * _accel_scale;
+
+		sensor_accel.range = _accel_range;
+		sensor_accel.samples = accel_samples;
+		sensor_accel.temperature = _temperature;
+		sensor_accel.error_count = perf_event_count(_bad_register_perf) + perf_event_count(_bad_transfer_perf) +
+					   perf_event_count(_fifo_empty_perf) + perf_event_count(_fifo_overflow_perf);
+
+		sensor_accel.timestamp = hrt_absolute_time();
+		_sensor_accel_pub.publish(sensor_accel);

-	if (accel.samples > 0) {
-		_px4_accel.updateFIFO(accel);
 		return true;
 	}

@@ -600,7 +612,7 @@ void BMI088_Accelerometer::UpdateTemperature()
 	float temperature = (Temp_int11 * 0.125f) + 23.f; // Temp_int11 * 0.125°C/LSB + 23°C

 	if (PX4_ISFINITE(temperature)) {
-		_px4_accel.set_temperature(temperature);
+		_temperature = temperature;

 	} else {
 		perf_count(_bad_transfer_perf);
@@ -1,6 +1,6 @@
 /****************************************************************************
 *
- *   Copyright (c) 2020-2021 PX4 Development Team. All rights reserved.
+ *   Copyright (c) 2020-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
@@ -35,10 +35,11 @@

 #include "BMI088.hpp"

-#include <lib/drivers/accelerometer/PX4Accelerometer.hpp>
-
 #include "Bosch_BMI088_Accelerometer_Registers.hpp"

+#include <uORB/PublicationMulti.hpp>
+#include <uORB/topics/sensor_accel.h>
+
 namespace Bosch::BMI088::Accelerometer
 {

@@ -58,7 +59,7 @@ private:
 	static constexpr uint32_t RATE{1600}; // 1600 Hz
 	static constexpr float FIFO_SAMPLE_DT{1e6f / RATE};

-	static constexpr int32_t FIFO_MAX_SAMPLES{math::min(FIFO::SIZE / sizeof(FIFO::DATA), sizeof(sensor_accel_fifo_s::x) / sizeof(sensor_accel_fifo_s::x[0]))};
+	static constexpr int32_t FIFO_MAX_SAMPLES{FIFO::SIZE / sizeof(FIFO::DATA)};

 	// Transfer data
 	struct FIFOTransferBuffer {
@@ -101,7 +102,13 @@ private:

 	void UpdateTemperature();

-	PX4Accelerometer _px4_accel;
+	uORB::PublicationMulti<sensor_accel_s> _sensor_accel_pub{ORB_ID(sensor_accel)};
+
+	const enum Rotation _rotation;
+	float _accel_scale{0.f};
+	float _accel_range{0.f};
+
+	float _temperature{NAN};

 	perf_counter_t _bad_register_perf{perf_alloc(PC_COUNT, MODULE_NAME"_accel: bad register")};
 	perf_counter_t _bad_transfer_perf{perf_alloc(PC_COUNT, MODULE_NAME"_accel: bad transfer")};
@@ -1,6 +1,6 @@
 /****************************************************************************
 *
- *   Copyright (c) 2020-2021 PX4 Development Team. All rights reserved.
+ *   Copyright (c) 2020-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,8 +33,6 @@

 #include "BMI088_Gyroscope.hpp"

-#include <px4_platform/board_dma_alloc.h>
-
 using namespace time_literals;

 namespace Bosch::BMI088::Gyroscope
@@ -42,13 +40,13 @@ namespace Bosch::BMI088::Gyroscope

 BMI088_Gyroscope::BMI088_Gyroscope(const I2CSPIDriverConfig &config) :
 	BMI088(config),
-	_px4_gyro(get_device_id(), config.rotation)
+	_rotation(config.rotation)
 {
 	if (config.drdy_gpio != 0) {
 		_drdy_missed_perf = perf_alloc(PC_COUNT, MODULE_NAME"_gyro: DRDY missed");
 	}

-	ConfigureSampleRate(_px4_gyro.get_max_rate_hz());
+	ConfigureSampleRate(RATE);
 }

 BMI088_Gyroscope::~BMI088_Gyroscope()
@@ -203,16 +201,6 @@ void BMI088_Gyroscope::RunImpl()

 					uint8_t samples = fifo_frame_counter;

-					// tolerate minor jitter, leave sample to next iteration if behind by only 1
-					if (samples == _fifo_samples + 1) {
-						// sample timestamp set from data ready already corresponds to _fifo_samples
-						if (timestamp_sample == 0) {
-							timestamp_sample = now - static_cast<int>(FIFO_SAMPLE_DT);
-						}
-
-						samples--;
-					}
-
 					if (FIFORead((timestamp_sample == 0) ? now : timestamp_sample, samples)) {
 						success = true;

@@ -257,28 +245,28 @@ void BMI088_Gyroscope::ConfigureGyro()

 	switch (GYRO_RANGE) {
 	case gyro_range_2000_dps:
-		_px4_gyro.set_scale(math::radians(1.f / 16.384f));
-		_px4_gyro.set_range(math::radians(2000.f));
+		_gyro_scale = math::radians(1.f / 16.384f);
+		_gyro_range = math::radians(2000.f);
 		break;

 	case gyro_range_1000_dps:
-		_px4_gyro.set_scale(math::radians(1.f / 32.768f));
-		_px4_gyro.set_range(math::radians(1000.f));
+		_gyro_scale = math::radians(1.f / 32.768f);
+		_gyro_range = math::radians(1000.f);
 		break;

 	case gyro_range_500_dps:
-		_px4_gyro.set_scale(math::radians(1.f / 65.536f));
-		_px4_gyro.set_range(math::radians(500.f));
+		_gyro_scale = math::radians(1.f / 65.536f);
+		_gyro_range = math::radians(500.f);
 		break;

 	case gyro_range_250_dps:
-		_px4_gyro.set_scale(math::radians(1.f / 131.072f));
-		_px4_gyro.set_range(math::radians(250.f));
+		_gyro_scale = math::radians(1.f / 131.072f);
+		_gyro_range = math::radians(250.f);
 		break;

 	case gyro_range_125_dps:
-		_px4_gyro.set_scale(math::radians(1.f / 262.144f));
-		_px4_gyro.set_range(math::radians(125.f));
+		_gyro_scale = math::radians(1.f / 262.144f);
+		_gyro_range = math::radians(125.f);
 		break;
 	}
 }
@@ -414,29 +402,44 @@ bool BMI088_Gyroscope::FIFORead(const hrt_abstime &timestamp_sample, uint8_t sam
 		return false;
 	}

-	sensor_gyro_fifo_s gyro{};
-	gyro.timestamp_sample = timestamp_sample;
-	gyro.samples = samples;
-	gyro.dt = FIFO_SAMPLE_DT;
+	sensor_gyro_s sensor_gyro{};
+	sensor_gyro.timestamp_sample = timestamp_sample;
+	sensor_gyro.device_id = get_device_id();
+
+	float gyro_sum[3] {};

 	for (int i = 0; i < samples; i++) {
 		const FIFO::DATA &fifo_sample = buffer.f[i];

-		const int16_t gyro_x = combine(fifo_sample.RATE_X_MSB, fifo_sample.RATE_X_LSB);
-		const int16_t gyro_y = combine(fifo_sample.RATE_Y_MSB, fifo_sample.RATE_Y_LSB);
-		const int16_t gyro_z = combine(fifo_sample.RATE_Z_MSB, fifo_sample.RATE_Z_LSB);
+		int16_t gyro_x = combine(fifo_sample.RATE_X_MSB, fifo_sample.RATE_X_LSB);
+		int16_t gyro_y = combine(fifo_sample.RATE_Y_MSB, fifo_sample.RATE_Y_LSB);
+		int16_t gyro_z = combine(fifo_sample.RATE_Z_MSB, fifo_sample.RATE_Z_LSB);

 		// sensor's frame is +x forward, +y left, +z up
 		//  flip y & z to publish right handed with z down (x forward, y right, z down)
-		gyro.x[i] = gyro_x;
-		gyro.y[i] = (gyro_y == INT16_MIN) ? INT16_MAX : -gyro_y;
-		gyro.z[i] = (gyro_z == INT16_MIN) ? INT16_MAX : -gyro_z;
+		// gyro_x = gyro_x;
+		gyro_y = (gyro_y == INT16_MIN) ? INT16_MAX : -gyro_y;
+		gyro_z = (gyro_z == INT16_MIN) ? INT16_MAX : -gyro_z;
+
+		rotate_3i(_rotation, gyro_x, gyro_y, gyro_z);
+
+		gyro_sum[0] += gyro_x * _gyro_scale;
+		gyro_sum[1] += gyro_y * _gyro_scale;
+		gyro_sum[2] += gyro_z * _gyro_scale;
 	}

-	_px4_gyro.set_error_count(perf_event_count(_bad_register_perf) + perf_event_count(_bad_transfer_perf) +
-				  perf_event_count(_fifo_empty_perf) + perf_event_count(_fifo_overflow_perf));
+	sensor_gyro.x = gyro_sum[0] / samples;
+	sensor_gyro.y = gyro_sum[1] / samples;
+	sensor_gyro.z = gyro_sum[2] / samples;

-	_px4_gyro.updateFIFO(gyro);
+	sensor_gyro.range = _gyro_range;
+	sensor_gyro.samples = samples;
+	sensor_gyro.temperature = NAN;
+	sensor_gyro.error_count = perf_event_count(_bad_register_perf) + perf_event_count(_bad_transfer_perf) +
+				  perf_event_count(_fifo_empty_perf) + perf_event_count(_fifo_overflow_perf);
+
+	sensor_gyro.timestamp = hrt_absolute_time();
+	_sensor_gyro_pub.publish(sensor_gyro);

 	return true;
 }
@@ -1,6 +1,6 @@
 /****************************************************************************
 *
- *   Copyright (c) 2020-2021 PX4 Development Team. All rights reserved.
+ *   Copyright (c) 2020-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
@@ -35,10 +35,11 @@

 #include "BMI088.hpp"

-#include <lib/drivers/gyroscope/PX4Gyroscope.hpp>
-
 #include "Bosch_BMI088_Gyroscope_Registers.hpp"

+#include <uORB/PublicationMulti.hpp>
+#include <uORB/topics/sensor_gyro.h>
+
 namespace Bosch::BMI088::Gyroscope
 {

@@ -58,7 +59,7 @@ private:
 	static constexpr uint32_t RATE{2000}; // 2000 Hz
 	static constexpr float FIFO_SAMPLE_DT{1e6f / RATE};

-	static constexpr int32_t FIFO_MAX_SAMPLES{math::min(FIFO::SIZE / sizeof(FIFO::DATA), sizeof(sensor_gyro_fifo_s::x) / sizeof(sensor_gyro_fifo_s::x[0]))};
+	static constexpr int32_t FIFO_MAX_SAMPLES{FIFO::SIZE / sizeof(FIFO::DATA)};

 	// Transfer data
 	struct FIFOTransferBuffer {
@@ -95,7 +96,11 @@ private:
 	bool FIFORead(const hrt_abstime &timestamp_sample, uint8_t samples);
 	void FIFOReset();

-	PX4Gyroscope _px4_gyro;
+	uORB::PublicationMulti<sensor_gyro_s> _sensor_gyro_pub{ORB_ID(sensor_gyro)};
+
+	const enum Rotation _rotation;
+	float _gyro_scale{0.f};
+	float _gyro_range{0.f};

 	perf_counter_t _bad_register_perf{perf_alloc(PC_COUNT, MODULE_NAME"_gyro: bad register")};
 	perf_counter_t _bad_transfer_perf{perf_alloc(PC_COUNT, MODULE_NAME"_gyro: bad transfer")};
@@ -35,6 +35,7 @@ px4_add_module(
 	MODULE drivers__imu__bosch__bmi088
 	MAIN bmi088
 	COMPILE_FLAGS
+		${MAX_CUSTOM_OPT_LEVEL}
 	SRCS
 		Bosch_BMI088_Accelerometer_Registers.hpp
 		Bosch_BMI088_Gyroscope_Registers.hpp
@@ -48,7 +49,5 @@ px4_add_module(

 		bmi088_main.cpp
 	DEPENDS
-		drivers_accelerometer
-		drivers_gyroscope
 		px4_work_queue
 	)
@@ -1,6 +1,6 @@
 /****************************************************************************
 *
- *   Copyright (c) 2020-2021 PX4 Development Team. All rights reserved.
+ *   Copyright (c) 2020-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
@@ -41,13 +41,13 @@ namespace Bosch::BMI088::Accelerometer
 {
 BMI088_Accelerometer::BMI088_Accelerometer(const I2CSPIDriverConfig &config) :
 	BMI088(config),
-	_px4_accel(get_device_id(), config.rotation)
+	_rotation(config.rotation)
 {
 	if (config.drdy_gpio != 0) {
 		_drdy_missed_perf = perf_alloc(PC_COUNT, MODULE_NAME"_accel: DRDY missed");
 	}

-	ConfigureSampleRate(1600);
+	ConfigureSampleRate(RATE);
 }

 BMI088_Accelerometer::~BMI088_Accelerometer()
@@ -204,23 +204,23 @@ void BMI088_Accelerometer::ConfigureAccel()

 	switch (ACC_RANGE) {
 	case acc_range_3g:
-		_px4_accel.set_scale(CONSTANTS_ONE_G * (powf(2, ACC_RANGE + 1) * 1.5f) / 32768.f);
-		_px4_accel.set_range(3.f * CONSTANTS_ONE_G);
+		_accel_scale = CONSTANTS_ONE_G * (powf(2, ACC_RANGE + 1) * 1.5f) / 32768.f;
+		_accel_range = 3.f * CONSTANTS_ONE_G;
 		break;

 	case acc_range_6g:
-		_px4_accel.set_scale(CONSTANTS_ONE_G * (powf(2, ACC_RANGE + 1) * 1.5f) / 32768.f);
-		_px4_accel.set_range(6.f * CONSTANTS_ONE_G);
+		_accel_scale = CONSTANTS_ONE_G * (powf(2, ACC_RANGE + 1) * 1.5f) / 32768.f;
+		_accel_range = 6.f * CONSTANTS_ONE_G;
 		break;

 	case acc_range_12g:
-		_px4_accel.set_scale(CONSTANTS_ONE_G * (powf(2, ACC_RANGE + 1) * 1.5f) / 32768.f);
-		_px4_accel.set_range(12.f * CONSTANTS_ONE_G);
+		_accel_scale = CONSTANTS_ONE_G * (powf(2, ACC_RANGE + 1) * 1.5f) / 32768.f;
+		_accel_range = 12.f * CONSTANTS_ONE_G;
 		break;

 	case acc_range_24g:
-		_px4_accel.set_scale(CONSTANTS_ONE_G * (powf(2, ACC_RANGE + 1) * 1.5f) / 32768.f);
-		_px4_accel.set_range(24.f * CONSTANTS_ONE_G);
+		_accel_scale = CONSTANTS_ONE_G * (powf(2, ACC_RANGE + 1) * 1.5f) / 32768.f;
+		_accel_range = 24.f * CONSTANTS_ONE_G;
 		break;
 	}
 }
@@ -386,10 +386,6 @@ bool BMI088_Accelerometer::FIFORead(const hrt_abstime &timestamp_sample, uint8_t
 		return false;
 	}

-	sensor_accel_fifo_s accel{};
-	accel.timestamp_sample = timestamp_sample;
-	accel.samples = 0;
-	accel.dt = FIFO_SAMPLE_DT;

 	// first find all sensor data frames in the buffer
 	uint8_t *data_buffer = (uint8_t *)&buffer.f[0];
@@ -1,6 +1,6 @@
 /****************************************************************************
 *
- *   Copyright (c) 2020-2021 PX4 Development Team. All rights reserved.
+ *   Copyright (c) 2020-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
@@ -35,8 +35,6 @@

 #include "BMI088.hpp"

-#include <lib/drivers/accelerometer/PX4Accelerometer.hpp>
-
 #include "Bosch_BMI088_Accelerometer_Registers.hpp"

 namespace Bosch::BMI088::Accelerometer
@@ -59,7 +57,7 @@ private:
 	static constexpr uint32_t RATE{1600}; // 1600 Hz
 	static constexpr float FIFO_SAMPLE_DT{1e6f / RATE};

-	static constexpr int32_t FIFO_MAX_SAMPLES{math::min(FIFO::SIZE / sizeof(FIFO::DATA), sizeof(sensor_accel_fifo_s::x) / sizeof(sensor_accel_fifo_s::x[0]))};
+	static constexpr int32_t FIFO_MAX_SAMPLES{FIFO::SIZE / sizeof(FIFO::DATA)};

 	// Transfer data
 	struct FIFOTransferBuffer {
@@ -114,8 +112,6 @@ private:
 	bool SimpleFIFORead(const hrt_abstime &timestamp_sample);
 	bool NormalRead(const hrt_abstime &timestamp_sample);

-	PX4Accelerometer _px4_accel;
-
 	perf_counter_t _bad_register_perf{perf_alloc(PC_COUNT, MODULE_NAME"_accel: bad register")};
 	perf_counter_t _bad_transfer_perf{perf_alloc(PC_COUNT, MODULE_NAME"_accel: bad transfer")};
 	perf_counter_t _fifo_empty_perf{perf_alloc(PC_COUNT, MODULE_NAME"_accel: FIFO empty")};
@@ -1,6 +1,6 @@
 /****************************************************************************
 *
- *   Copyright (c) 2020-2021 PX4 Development Team. All rights reserved.
+ *   Copyright (c) 2020-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,8 +33,6 @@

 #include "BMI088_Gyroscope.hpp"

-#include <px4_platform/board_dma_alloc.h>
-
 using namespace time_literals;

 namespace Bosch::BMI088::Gyroscope
@@ -42,7 +40,7 @@ namespace Bosch::BMI088::Gyroscope

 BMI088_Gyroscope::BMI088_Gyroscope(const I2CSPIDriverConfig &config) :
 	BMI088(config),
-	_px4_gyro(get_device_id(), config.rotation)
+	_rotation(config.rotation)
 {
 	if (config.drdy_gpio != 0) {
 		_drdy_missed_perf = perf_alloc(PC_COUNT, MODULE_NAME"_gyro: DRDY missed");
@@ -35,8 +35,6 @@

 #include "BMI088.hpp"

-#include <lib/drivers/gyroscope/PX4Gyroscope.hpp>
-
 #include "Bosch_BMI088_Gyroscope_Registers.hpp"

 namespace Bosch::BMI088::Gyroscope
@@ -58,7 +56,7 @@ private:
 	static constexpr uint32_t RATE{400}; // 2000 Hz
 	static constexpr float FIFO_SAMPLE_DT{1e6f / RATE};

-	static constexpr int32_t FIFO_MAX_SAMPLES{math::min(FIFO::SIZE / sizeof(FIFO::DATA), sizeof(sensor_gyro_fifo_s::x) / sizeof(sensor_gyro_fifo_s::x[0]))};
+	static constexpr int32_t FIFO_MAX_SAMPLES{FIFO::SIZE / sizeof(FIFO::DATA)};

 	// Transfer data
 	struct FIFOTransferBuffer {
@@ -98,7 +96,6 @@ private:
 	bool SelfTest();
 	bool NormalRead(const hrt_abstime &timestamp_sample);
 	bool SimpleFIFORead(const hrt_abstime &timestamp_sample);
-	PX4Gyroscope _px4_gyro;

 	perf_counter_t _bad_register_perf{perf_alloc(PC_COUNT, MODULE_NAME"_gyro: bad register")};
 	perf_counter_t _bad_transfer_perf{perf_alloc(PC_COUNT, MODULE_NAME"_gyro: bad transfer")};
@@ -48,7 +48,5 @@ px4_add_module(

 		bmi088_i2c_main.cpp
 	DEPENDS
-		drivers_accelerometer
-		drivers_gyroscope
 		px4_work_queue
 	)
@@ -1,6 +1,6 @@
 /****************************************************************************
 *
- *   Copyright (c) 2012-2022 PX4 Development Team. All rights reserved.
+ *   Copyright (c) 2012-2021 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,8 +44,21 @@ constexpr char const *RCInput::RC_SCAN_STRING[];

 RCInput::RCInput(const char *device) :
 	ModuleParams(nullptr),
-	ScheduledWorkItem(MODULE_NAME, px4::serial_port_to_wq(device))
+	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"))
 {
+	// 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';
@@ -64,7 +77,6 @@ RCInput::~RCInput()
 	delete _ghst_telemetry;

 	perf_free(_cycle_perf);
-	perf_free(_cycle_interval_perf);
 	perf_free(_publish_interval_perf);
 }

@@ -76,11 +88,6 @@ 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);
@@ -157,60 +164,89 @@ RCInput::task_spawn(int argc, char *argv[])
 	_object.store(instance);
 	_task_id = task_id_is_work_queue;

-	instance->ScheduleOnInterval(_backup_update_interval);
+	instance->ScheduleOnInterval(_current_update_interval);

 	return PX4_OK;
 }

-void RCInput::FillRssi(input_rc_s &input_rc)
+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)
 {
-	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)) {
+	// 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)) {
 			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 = ((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)
+			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);

 		} 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;

-			input_rc.rssi = math::constrain((int)roundf(rssi_analog), 0, (int)input_rc_s::RSSI_MAX);
-#endif // ADC_RC_RSSI_CHANNEL
+			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;
 		}
-	}
-}

-void RCInput::PublishInputRc(input_rc_s &input_rc)
-{
-	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;
+	} else {
+		_rc_in.rssi = rssi;
 	}

-	PX4_DEBUG("RC scan: %s failed, trying %s", RCInput::RC_SCAN_STRING[_rc_scan_state], RCInput::RC_SCAN_STRING[new_state]);
+	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)
+{
+	PX4_DEBUG("RCscan: %s failed, trying %s", RCInput::RC_SCAN_STRING[_rc_scan_state], RCInput::RC_SCAN_STRING[newState]);
 	_rc_scan_begin = 0;
-	_rc_scan_state = static_cast<RC_SCAN>(new_state);
+	_rc_scan_state = newState;
 	_rc_scan_locked = false;

 	_report_lock = true;
@@ -252,6 +288,8 @@ void RCInput::Run()

 	} else {

+		perf_begin(_cycle_perf);
+
 		// Check if parameters have changed
 		if (_parameter_update_sub.updated()) {
 			// clear update
@@ -269,10 +307,13 @@ void RCInput::Run()
 			}
 		}

+		const hrt_abstime cycle_timestamp = hrt_absolute_time();
+
+
 		/* vehicle command */
 		vehicle_command_s vcmd;

-		while (_vehicle_cmd_sub.update(&vcmd)) {
+		if (_vehicle_cmd_sub.update(&vcmd)) {
 			// Check for a pairing command
 			if (vcmd.command == vehicle_command_s::VEHICLE_CMD_START_RX_PAIR) {

@@ -329,8 +370,9 @@ 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;
@@ -349,25 +391,18 @@ 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
-		static constexpr hrt_abstime rc_scan_max = 300_ms;
+		constexpr hrt_abstime rc_scan_max = 300_ms;

-		// poll with 1 second timeout
-		pollfd fds[1];
-		fds[0].fd = _rcs_fd;
-		fds[0].events = POLLIN;
-		int ret = poll(fds, 1, 1000);
+		unsigned frame_drops = 0;

-		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();
+		// 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

 		// read all available data from the serial RC input UART
 		int newBytes = ::read(_rcs_fd, &_rcs_buf[0], RC_MAX_BUFFER_SIZE);
@@ -376,48 +411,7 @@ 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;
@@ -429,40 +423,30 @@ 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) {
-					// 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.
-						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);
+						_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;
 					}
 				}

 			} else {
 				// Scan the next protocol
 				rc_io_invert(false);
-				set_next_rc_scan_state();
+				set_rc_scan_state(RC_SCAN_DSM);
 			}

 			break;
@@ -477,38 +461,29 @@ 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)) {
-				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 dsm_11_bit = false;
-					unsigned frame_drops = 0;
-					int8_t dsm_rssi = 0;
+			} else if (_rc_scan_locked
+				   || cycle_timestamp - _rc_scan_begin < rc_scan_max) {

-					rc_updated = dsm_parse(cycle_timestamp, &_rcs_buf[0], newBytes, &raw_rc_values[0], &raw_rc_count,
+				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,
 							       &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.
-						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);
+						_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;
 					}
 				}

 			} else {
 				// Scan the next protocol
-				set_next_rc_scan_state();
+				set_rc_scan_state(RC_SCAN_ST24);
 			}

 			break;
@@ -523,43 +498,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 = 0;
-					uint8_t lost_count = 0;
-					uint16_t raw_rc_count = 0;
-					uint16_t raw_rc_values[input_rc_s::RC_INPUT_MAX_CHANNELS];
+					uint8_t st24_rssi, lost_count;
+
+					rc_updated = 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 */
 						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) {
-						// 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;
+						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;

-						for (int i = 0; i < input_rc.channel_count; i++) {
-							input_rc.values[i] = raw_rc_values[i];
+						} else {
+							// if the lost count > 0 means that there is an RC loss
+							_rc_in.rc_lost = true;
 						}
-
-						PublishInputRc(input_rc);
 					}
 				}

 			} else {
 				// Scan the next protocol
-				set_next_rc_scan_state();
+				set_rc_scan_state(RC_SCAN_SUMD);
 			}

 			break;
@@ -574,47 +549,74 @@ 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 = 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;
+					uint8_t sumd_rssi, rx_count;
+					bool sumd_failsafe;
+
+					rc_updated = 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.
-						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);
+						_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;
 					}
 				}

 			} else {
 				// Scan the next protocol
-				set_next_rc_scan_state();
+				set_rc_scan_state(RC_SCAN_PPM);
 			}

 			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;
@@ -625,44 +627,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)) {
-				if (newBytes > 0) {
-					// parse new data
-					uint16_t raw_rc_values[input_rc_s::RC_INPUT_MAX_CHANNELS];
-					uint16_t raw_rc_count = 0;
+			} else if (_rc_scan_locked
+				   || cycle_timestamp - _rc_scan_begin < rc_scan_max) {

-					rc_updated = crsf_parse(cycle_timestamp, &_rcs_buf[0], newBytes, &raw_rc_values[0], &raw_rc_count,
+				// parse new data
+				if (newBytes > 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.
-						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;
+						_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);

-						for (int i = 0; i < input_rc.channel_count; i++) {
-							input_rc.values[i] = raw_rc_values[i];
+						// 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);
 						}

-						PublishInputRc(input_rc);
+#endif /* BOARD_SUPPORTS_RC_SERIAL_PORT_OUTPUT */

-						if (_rc_serial_port_output) {
-							if (!_rc_scan_locked && !_crsf_telemetry) {
-								_crsf_telemetry = new CRSFTelemetry(_rcs_fd);
-							}
+						_rc_scan_locked = true;

-							if (_crsf_telemetry) {
-								_crsf_telemetry->update(cycle_timestamp);
-							}
+						if (_crsf_telemetry) {
+							_crsf_telemetry->update(cycle_timestamp);
 						}
 					}
 				}

 			} else {
 				// Scan the next protocol
-				set_next_rc_scan_state();
+				set_rc_scan_state(RC_SCAN_GHST);
 			}

 			break;
@@ -677,55 +675,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.
-						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;
+						_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);

-						for (int i = 0; i < input_rc.channel_count; i++) {
-							input_rc.values[i] = raw_rc_values[i];
+						// 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);
 						}

-						PublishInputRc(input_rc);
+#endif /* BOARD_SUPPORTS_RC_SERIAL_PORT_OUTPUT */

-						if (_rc_serial_port_output) {
-							if (!_rc_scan_locked && !_ghst_telemetry) {
-								_ghst_telemetry = new GHSTTelemetry(_rcs_fd);
-							}
+						_rc_scan_locked = true;

-							if (_ghst_telemetry) {
-								_ghst_telemetry->update(cycle_timestamp);
-							}
+						if (_ghst_telemetry) {
+							_ghst_telemetry->update(cycle_timestamp);
 						}
 					}
 				}

 			} else {
 				// Scan the next protocol
-				set_next_rc_scan_state();
+				set_rc_scan_state(RC_SCAN_SBUS);
 			}

 			break;
-
-		default:
-			// Scan the next protocol
-			set_next_rc_scan_state();
 		}

-		if (!rc_updated && !_armed && (hrt_elapsed_time(&_last_publish_time) > 1_s)) {
+		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)) {
 			_rc_scan_locked = false;
 		}

@@ -733,16 +731,6 @@ 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);
 	}
 }

@@ -817,6 +805,8 @@ 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);
@@ -838,23 +828,42 @@ int RCInput::print_status()
 			PX4_INFO("SBUS frame drops: %u", sbus_dropped_frames());
 			break;

-		default:
+
+		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
 			break;
 		}
 	}

-#if defined(ADC_RC_RSSI_CHANNEL)
+#if ADC_RC_RSSI_CHANNEL

 	if (_analog_rc_rssi_stable) {
 		PX4_INFO("vrssi: %dmV", (int)(_analog_rc_rssi_volt * 1000.0f));
 	}

-#endif // ADC_RC_RSSI_CHANNEL
+#endif

 	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;
 }

@@ -91,31 +91,23 @@ public:
 private:

 	enum RC_SCAN {
-#if defined(HRT_PPM_CHANNEL)
-		RC_SCAN_PPM,
-#endif // HRT_PPM_CHANNEL
+		RC_SCAN_PPM = 0,
 		RC_SCAN_SBUS,
 		RC_SCAN_DSM,
 		RC_SCAN_SUMD,
 		RC_SCAN_ST24,
 		RC_SCAN_CRSF,
-		RC_SCAN_GHST,
-
-		RC_SCAN_MAX
+		RC_SCAN_GHST
 	} _rc_scan_state{RC_SCAN_SBUS};

-	static constexpr char const *RC_SCAN_STRING[] {
-#if defined(HRT_PPM_CHANNEL)
+	static constexpr char const *RC_SCAN_STRING[7] {
 		"PPM",
-#endif // HRT_PPM_CHANNEL
 		"SBUS",
 		"DSM",
 		"SUMD",
 		"ST24",
 		"CRSF",
-		"GHST",
-
-		"NONE"
+		"GHST"
 	};

 	void Run() override;
@@ -124,36 +116,38 @@ private:
 	bool bind_spektrum(int arg = DSMX8_BIND_PULSES) const;
 #endif // SPEKTRUM_POWER

-	void FillRssi(input_rc_s &input_rc);
-	void PublishInputRc(input_rc_s &input_rc);
+	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 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	_backup_update_interval{100000}; // 10 Hz (backup schedule)
+	static constexpr unsigned	_current_update_interval{4000}; // 250 Hz

 	uORB::SubscriptionInterval _parameter_update_sub{ORB_ID(parameter_update), 1_s};

-#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	_adc_report_sub{ORB_ID(adc_report)};
 	uORB::Subscription	_vehicle_cmd_sub{ORB_ID(vehicle_command)};
 	uORB::Subscription	_vehicle_status_sub{ORB_ID(vehicle_status)};

-	bool _rc_serial_port_output{true};
+	input_rc_s	_rc_in{};
+
+	float		_analog_rc_rssi_volt{-1.0f};
+	bool		_analog_rc_rssi_stable{false};
+
 	bool _armed{false};

-	uORB::PublicationMulti<input_rc_s> _input_rc_pub{ORB_ID(input_rc)};
+
+	uORB::PublicationMulti<input_rc_s>	_to_input_rc{ORB_ID(input_rc)};

 	int		_rcs_fd{-1};
 	char		_device[20] {};					///< device / serial port path
@@ -161,13 +155,14 @@ 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_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")};
-
+	perf_counter_t	_cycle_perf;
+	perf_counter_t	_publish_interval_perf;
 	uint32_t	_bytes_rx{0};

 	DEFINE_PARAMETERS(
@@ -64,12 +64,6 @@ 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;
@@ -82,14 +76,6 @@ 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)
 {
@@ -149,6 +135,7 @@ void crypto_init()
 {
 	keystore_init();
 	clear_key_cache();
+	libtomcrypt_init();
 }

 crypto_session_handle_t crypto_open(px4_crypto_algorithm_t algorithm)
@@ -282,8 +269,6 @@ 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) &&
@@ -428,8 +413,6 @@ 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);
@@ -1093,9 +1093,9 @@ FixedwingPositionControl::handle_setpoint_type(const uint8_t setpoint_type, cons
 		}
 	}

-	// set to type position during VTOL transitions in Land mode (to not start FW landing logic)
+	// set to type loiter 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_POSITION;
+		position_sp_type = position_setpoint_s::SETPOINT_TYPE_LOITER;
 	}

 	return position_sp_type;
@@ -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::INS0),
+	WorkItem(MODULE_NAME, px4::wq_configurations::nav_and_controllers),

 	// this block has no parent, and has name LPE
 	SuperBlock(nullptr, "LPE"),
@@ -122,17 +122,7 @@ static int parse_options(int argc, char *argv[])
 				break;
 			}

-		case 'm': {
-				int datarate = 0;
-
-				if (px4_get_parameter_value(myoptarg, datarate) != 0) {
-					PX4_ERR("datarate parsing failed");
-				}
-
-				_options.datarate = datarate;
-
-				break;
-			}
+		case 'm': _options.datarate        = strtoul(myoptarg, nullptr, 10);    break;

 		case 'p': _options.poll_ms         = strtoul(myoptarg, nullptr, 10);    break;

@@ -3,36 +3,15 @@ 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} -m p:RTPS_RATE -l -1
+        micrortps_client start -d /dev/rtps -b p:${BAUD_PARAM} -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} -m p:RTPS_RATE -l -1
+        micrortps_client start -d ${SERIAL_DEV} -b p:${BAUD_PARAM} -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
-
@@ -74,12 +74,8 @@ 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();
-
-		// 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);
-
+		pos_sp_triplet->current.lat = _navigator->get_global_position()->lat;
+		pos_sp_triplet->current.lon = _navigator->get_global_position()->lon;
 		_navigator->set_position_setpoint_triplet_updated();
 	}

@@ -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; // ideally that would be course and not heading
+	item->yaw = _navigator->get_local_position()->heading;
 	item->autocontinue = true;
 }

@@ -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.advertised() && (sensor_accel_sub.get().device_id != 0)) {
+		if (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.advertised() && (sensor_gyro_sub.get().device_id != 0)) {
+		if (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.advertised() && (sensor_mag_sub.get().device_id != 0)) {
+		if (sensor_mag_sub.get().device_id != 0) {
 			calibration::Magnetometer cal;
 			cal.set_calibration_index(i);
 			cal.ParametersLoad();
@@ -91,10 +91,8 @@ void VehicleAcceleration::CheckAndUpdateFilters()

 		const float sample_rate_hz = imu_status.get().accel_rate_hz;

-		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())
+		if ((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);
@@ -163,8 +161,7 @@ bool VehicleAcceleration::SensorSelectionUpdate(bool force)

 				const uint32_t device_id = sensor_accel_sub.get().device_id;

-				if (sensor_accel_sub.advertised() && (sensor_accel_sub.get().timestamp != 0)
-				    && (device_id != 0) && (device_id == sensor_selection.accel_device_id)) {
+				if ((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);
@@ -230,20 +230,19 @@ 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.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 ((imu_status.get().gyro_device_id != 0) && (time_now_us < imu_status.get().timestamp + 1_s)) {
+				imu_status_gyro_valid = true;
+			}

-				if ((device_id != 0) && (imu_status.get().gyro_device_id == device_id)) {
-					selected_device_id_valid = true;
-				}
+			if ((device_id != 0) && (imu_status.get().gyro_device_id == device_id) && imu_status_gyro_valid) {
+				selected_device_id_valid = true;
+			}

-				// 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;
-				}
+			// 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;
 			}
 		}

@@ -260,11 +259,7 @@ 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 (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 ((time_now_us < sensor_gyro_fifo_sub.get().timestamp + 1_s) && (sensor_gyro_fifo_sub.get().device_id != 0)) {
 					// 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;
@@ -302,11 +297,7 @@ 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 (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 ((time_now_us < sensor_gyro_sub.get().timestamp + 1_s) && (sensor_gyro_sub.get().device_id != 0)) {
 					// if no gyro was selected use the first valid sensor_gyro
 					if (!device_id_valid) {
 						device_id = sensor_gyro_sub.get().device_id;
@@ -87,8 +87,7 @@ void VotedSensorsUpdate::parametersUpdate()
 		uORB::SubscriptionData<vehicle_imu_s> imu{ORB_ID(vehicle_imu), uorb_index};
 		imu.update();

-		if (imu.advertised() && (imu.get().timestamp != 0)
-		    && (imu.get().accel_device_id != 0) && (imu.get().gyro_device_id != 0)) {
+		if (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);
@@ -4,54 +4,3 @@ 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