ekf2: remove const references

2026-06-05 06:10:04 +08:00 · 2022-02-01 21:57:57 -05:00
159 changed files with 3098 additions and 3312 deletions
@@ -73,8 +73,10 @@ pipeline {
                }
              }
              post {
+                failure {
+                  sh 'cat /tmp/pyserial_spy_file.txt'
+                }
                always {
-                  sh 'cat /tmp/pyserial_spy_file.txt || true'
                  sh './platforms/nuttx/Debug/jlink_gdb_backtrace_simple.sh build/cubepilot_cubeorange_test/cubepilot_cubeorange_test.elf || true'
                }
              }
@@ -143,8 +145,10 @@ pipeline {
                }
              }
              post {
+                failure {
+                  sh 'cat /tmp/pyserial_spy_file.txt'
+                }
                always {
-                  sh 'cat /tmp/pyserial_spy_file.txt || true'
                  sh './platforms/nuttx/Debug/jlink_gdb_backtrace_simple.sh build/cuav_x7pro_test/cuav_x7pro_test.elf || true'
                }
              }
@@ -213,8 +217,10 @@ pipeline {
                }
              }
              post {
+                failure {
+                  sh 'cat /tmp/pyserial_spy_file.txt'
+                }
                always {
-                  sh 'cat /tmp/pyserial_spy_file.txt || true'
                  sh './platforms/nuttx/Debug/jlink_gdb_backtrace_simple.sh build/px4_fmu-v3_test/px4_fmu-v3_test.elf || true'
                }
              }
@@ -282,8 +288,10 @@ pipeline {
                }
              }
              post {
+                failure {
+                  sh 'cat /tmp/pyserial_spy_file.txt'
+                }
                always {
-                  sh 'cat /tmp/pyserial_spy_file.txt || true'
                  sh './platforms/nuttx/Debug/jlink_gdb_backtrace_simple.sh build/px4_fmu-v4_test/px4_fmu-v4_test.elf || true'
                }
              }
@@ -352,8 +360,10 @@ pipeline {
                }
              }
              post {
+                failure {
+                  sh 'cat /tmp/pyserial_spy_file.txt'
+                }
                always {
-                  sh 'cat /tmp/pyserial_spy_file.txt || true'
                  sh './platforms/nuttx/Debug/jlink_gdb_backtrace_simple.sh build/px4_fmu-v4pro_test/px4_fmu-v4pro_test.elf || true'
                }
              }
@@ -442,8 +452,10 @@ pipeline {
                }
              }
              post {
+                failure {
+                  sh 'cat /tmp/pyserial_spy_file.txt'
+                }
                always {
-                  sh 'cat /tmp/pyserial_spy_file.txt || true'
                  sh './platforms/nuttx/Debug/jlink_gdb_backtrace_simple.sh build/px4_fmu-v5_debug/px4_fmu-v5_debug.elf || true'
                }
              }
@@ -524,8 +536,10 @@ pipeline {
                }
              }
              post {
+                failure {
+                  sh 'cat /tmp/pyserial_spy_file.txt'
+                }
                always {
-                  sh 'cat /tmp/pyserial_spy_file.txt || true'
                  sh './platforms/nuttx/Debug/jlink_gdb_backtrace_simple.sh build/px4_fmu-v5_stackcheck/px4_fmu-v5_stackcheck.elf || true'
                }
              }
@@ -594,8 +608,10 @@ pipeline {
                }
              }
              post {
+                failure {
+                  sh 'cat /tmp/pyserial_spy_file.txt'
+                }
                always {
-                  sh 'cat /tmp/pyserial_spy_file.txt || true'
                  sh './platforms/nuttx/Debug/jlink_gdb_backtrace_simple.sh build/px4_fmu-v5_test/px4_fmu-v5_test.elf || true'
                }
              }
@@ -664,8 +680,10 @@ pipeline {
                }
              }
              post {
+                failure {
+                  sh 'cat /tmp/pyserial_spy_file.txt'
+                }
                always {
-                  sh 'cat /tmp/pyserial_spy_file.txt || true'
                  sh './platforms/nuttx/Debug/jlink_gdb_backtrace_simple.sh build/nxp_fmuk66-v3_test/nxp_fmuk66-v3_test.elf || true'
                }
              }
@@ -775,7 +793,7 @@ void checkStatus() {
  sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-*` --cmd "top once"'
  sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-*` --cmd "uavcan status" || true'
  sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-*` --cmd "uorb status" || true'
-  sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-*` --cmd "uorb top -1 -a" || true'
+  sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-*` --cmd "uorb top -1 -a"'
  sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-*` --cmd "ver all"'
  sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-*` --cmd "work_queue status"'
  // stop logger
@@ -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()

@@ -844,7 +862,7 @@ void runTests() {
 void printTopics() {
  sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-*` --cmd "ls /obj"'
  sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-*` --cmd "uorb status"'
-  sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-*` --cmd "uorb top -1 -a" || true'
+  sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-*` --cmd "uorb top -1 -a"'

  // these are for casually inspecting the system, output failure doesn't matter
  sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-*` --cmd "listener actuator_armed" || true'
@@ -44,8 +44,6 @@ param set-default MIS_TAKEOFF_ALT 30
 param set-default NAV_ACC_RAD 15
 param set-default NAV_DLL_ACT 2

-param set-default FW_USE_NPFG 1
-
 param set-default RWTO_TKOFF 1

 set MIXER_FILE etc/mixers-sitl/plane_sitl.main.mix
@@ -234,7 +234,7 @@ fi

 if param greater -s MNT_MODE_IN -1
 then
-	gimbal start
+	vmount start
 fi

 if param greater -s TRIG_MODE 0
@@ -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
@@ -147,7 +147,7 @@ param set-default MPC_Z_VEL_P 0.27


 # gimbal configuration
-param set-default MNT_MODE_IN 0
+param set-default MNT_MODE_IN 1
 param set-default MNT_MODE_OUT 1
 param set-default MNT_MAN_PITCH 2
 param set-default MNT_RC_IN_MODE 1
@@ -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_+
@@ -486,11 +486,11 @@ else
 	. ${R}etc/init.d/rc.thermal_cal

 	#
-	# Start gimbal to control mounts such as gimbals, disabled by default.
+	# Start vmount to control mounts such as gimbals, disabled by default.
 	#
 	if param greater -s MNT_MODE_IN -1
 	then
-		gimbal start
+		vmount start
 	fi

 	# Check for flow sensor
@@ -35,6 +35,7 @@ do
 	${DIR}/run_nsh_cmd.py --device ${SERIAL_DEVICE} --cmd 'param set MAV_0_CONFIG 101' || true
 	${DIR}/run_nsh_cmd.py --device ${SERIAL_DEVICE} --cmd 'param set MAV_1_CONFIG 102' || true
 	${DIR}/run_nsh_cmd.py --device ${SERIAL_DEVICE} --cmd 'param set MAV_2_CONFIG 103' || true
+	${DIR}/run_nsh_cmd.py --device ${SERIAL_DEVICE} --cmd 'param set MAV_3_CONFIG 104' || true

 	# enable all GPS
 	${DIR}/run_nsh_cmd.py --device ${SERIAL_DEVICE} --cmd 'param set GPS_1_CONFIG 201' || true
@@ -150,7 +150,7 @@ simulator,CONFIG_MODULES_SIMULATOR=y
 temperature_compensation,CONFIG_MODULES_TEMPERATURE_COMPENSATION=y
 uuv_att_control,CONFIG_MODULES_UUV_ATT_CONTROL=y
 uuv_pos_control,CONFIG_MODULES_UUV_POS_CONTROL=y
-gimbal,CONFIG_MODULES_GIMBAL=y
+vmount,CONFIG_MODULES_VMOUNT=y
 vtol_att_control,CONFIG_MODULES_VTOL_ATT_CONTROL=y
 bl_update,CONFIG_SYSTEMCMDS_BL_UPDATE=y
 dmesg,CONFIG_SYSTEMCMDS_DMESG=y
@@ -74,7 +74,7 @@ CONFIG_MODULES_SIH=y
 CONFIG_MODULES_TEMPERATURE_COMPENSATION=y
 CONFIG_MODULES_UUV_ATT_CONTROL=y
 CONFIG_MODULES_UUV_POS_CONTROL=y
-CONFIG_MODULES_GIMBAL=y
+CONFIG_MODULES_VMOUNT=y
 CONFIG_MODULES_VTOL_ATT_CONTROL=y
 CONFIG_SYSTEMCMDS_BL_UPDATE=y
 CONFIG_SYSTEMCMDS_ACTUATOR_TEST=y
@@ -31,7 +31,7 @@ CONFIG_MODULES_NAVIGATOR=y
 CONFIG_MODULES_RC_UPDATE=y
 CONFIG_MODULES_SENSORS=y
 CONFIG_MODULES_SIH=y
-CONFIG_MODULES_GIMBAL=y
+CONFIG_MODULES_VMOUNT=y
 CONFIG_SYSTEMCMDS_BL_UPDATE=y
 CONFIG_SYSTEMCMDS_ACTUATOR_TEST=y
 CONFIG_SYSTEMCMDS_DMESG=y
@@ -71,7 +71,7 @@ CONFIG_MODULES_SIH=y
 CONFIG_MODULES_TEMPERATURE_COMPENSATION=y
 CONFIG_MODULES_UUV_ATT_CONTROL=y
 CONFIG_MODULES_UUV_POS_CONTROL=y
-CONFIG_MODULES_GIMBAL=y
+CONFIG_MODULES_VMOUNT=y
 CONFIG_MODULES_VTOL_ATT_CONTROL=y
 CONFIG_SYSTEMCMDS_BL_UPDATE=y
 CONFIG_SYSTEMCMDS_ACTUATOR_TEST=y
@@ -56,7 +56,7 @@ CONFIG_MODULES_SIH=y
 CONFIG_MODULES_TEMPERATURE_COMPENSATION=y
 CONFIG_MODULES_UUV_ATT_CONTROL=y
 CONFIG_MODULES_UUV_POS_CONTROL=y
-CONFIG_MODULES_GIMBAL=y
+CONFIG_MODULES_VMOUNT=y
 CONFIG_MODULES_VTOL_ATT_CONTROL=y
 CONFIG_SYSTEMCMDS_DYN=y
 CONFIG_SYSTEMCMDS_LED_CONTROL=y
@@ -17,6 +17,7 @@ CONFIG_MODULES_DATAMAN=y
 CONFIG_MODULES_EKF2=y
 CONFIG_MODULES_EVENTS=y
 CONFIG_MODULES_FLIGHT_MODE_MANAGER=y
+CONFIG_MODULES_GYRO_FFT=y
 CONFIG_MODULES_LAND_DETECTOR=y
 CONFIG_MODULES_LOAD_MON=y
 CONFIG_MODULES_LOGGER=y
@@ -76,7 +76,7 @@ CONFIG_MODULES_ROVER_POS_CONTROL=y
 CONFIG_MODULES_SENSORS=y
 CONFIG_MODULES_SIH=y
 CONFIG_MODULES_TEMPERATURE_COMPENSATION=y
-CONFIG_MODULES_GIMBAL=y
+CONFIG_MODULES_VMOUNT=y
 CONFIG_MODULES_VTOL_ATT_CONTROL=y
 CONFIG_SYSTEMCMDS_BL_UPDATE=y
 CONFIG_SYSTEMCMDS_ACTUATOR_TEST=y
@@ -77,7 +77,7 @@ CONFIG_MODULES_ROVER_POS_CONTROL=y
 CONFIG_MODULES_SENSORS=y
 CONFIG_MODULES_SIH=y
 CONFIG_MODULES_TEMPERATURE_COMPENSATION=y
-CONFIG_MODULES_GIMBAL=y
+CONFIG_MODULES_VMOUNT=y
 CONFIG_MODULES_VTOL_ATT_CONTROL=y
 CONFIG_SYSTEMCMDS_BL_UPDATE=y
 CONFIG_SYSTEMCMDS_ACTUATOR_TEST=y
@@ -72,7 +72,7 @@ CONFIG_MODULES_ROVER_POS_CONTROL=y
 CONFIG_MODULES_SENSORS=y
 CONFIG_MODULES_SIH=y
 CONFIG_MODULES_TEMPERATURE_COMPENSATION=y
-CONFIG_MODULES_GIMBAL=y
+CONFIG_MODULES_VMOUNT=y
 CONFIG_MODULES_VTOL_ATT_CONTROL=y
 CONFIG_SYSTEMCMDS_BL_UPDATE=y
 CONFIG_SYSTEMCMDS_ACTUATOR_TEST=y
@@ -74,7 +74,7 @@ CONFIG_MODULES_SIH=y
 CONFIG_MODULES_TEMPERATURE_COMPENSATION=y
 CONFIG_MODULES_UUV_ATT_CONTROL=y
 CONFIG_MODULES_UUV_POS_CONTROL=y
-CONFIG_MODULES_GIMBAL=y
+CONFIG_MODULES_VMOUNT=y
 CONFIG_MODULES_VTOL_ATT_CONTROL=y
 CONFIG_SYSTEMCMDS_BL_UPDATE=y
 CONFIG_SYSTEMCMDS_ACTUATOR_TEST=y
@@ -58,7 +58,7 @@ CONFIG_MODULES_SIH=y
 CONFIG_MODULES_TEMPERATURE_COMPENSATION=y
 CONFIG_MODULES_UUV_ATT_CONTROL=y
 CONFIG_MODULES_UUV_POS_CONTROL=y
-CONFIG_MODULES_GIMBAL=y
+CONFIG_MODULES_VMOUNT=y
 CONFIG_MODULES_VTOL_ATT_CONTROL=y
 CONFIG_SYSTEMCMDS_DYN=y
 CONFIG_SYSTEMCMDS_LED_CONTROL=y
@@ -69,7 +69,7 @@ CONFIG_MODULES_ROVER_POS_CONTROL=y
 CONFIG_MODULES_SENSORS=y
 CONFIG_MODULES_SIH=y
 CONFIG_MODULES_TEMPERATURE_COMPENSATION=y
-CONFIG_MODULES_GIMBAL=y
+CONFIG_MODULES_VMOUNT=y
 CONFIG_MODULES_VTOL_ATT_CONTROL=y
 CONFIG_SYSTEMCMDS_BL_UPDATE=y
 CONFIG_SYSTEMCMDS_ACTUATOR_TEST=y
@@ -21,6 +21,7 @@ CONFIG_MODULES_BATTERY_STATUS=y
 CONFIG_MODULES_COMMANDER=y
 CONFIG_MODULES_CONTROL_ALLOCATOR=y
 CONFIG_MODULES_DATAMAN=y
+CONFIG_MODULES_EVENTS=y
 CONFIG_MODULES_FLIGHT_MODE_MANAGER=y
 CONFIG_MODULES_LAND_DETECTOR=y
 CONFIG_MODULES_LOGGER=y
@@ -62,7 +62,7 @@ CONFIG_MODULES_ROVER_POS_CONTROL=y
 CONFIG_MODULES_SENSORS=y
 CONFIG_MODULES_SIH=y
 CONFIG_MODULES_TEMPERATURE_COMPENSATION=y
-CONFIG_MODULES_GIMBAL=y
+CONFIG_MODULES_VMOUNT=y
 CONFIG_MODULES_VTOL_ATT_CONTROL=y
 CONFIG_SYSTEMCMDS_BL_UPDATE=y
 CONFIG_SYSTEMCMDS_ACTUATOR_TEST=y
@@ -79,7 +79,7 @@ CONFIG_MODULES_SIH=y
 CONFIG_MODULES_TEMPERATURE_COMPENSATION=y
 CONFIG_MODULES_UUV_ATT_CONTROL=y
 CONFIG_MODULES_UUV_POS_CONTROL=y
-CONFIG_MODULES_GIMBAL=y
+CONFIG_MODULES_VMOUNT=y
 CONFIG_MODULES_VTOL_ATT_CONTROL=y
 CONFIG_SYSTEMCMDS_BL_UPDATE=y
 CONFIG_SYSTEMCMDS_ACTUATOR_TEST=y
@@ -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
@@ -52,7 +52,7 @@ CONFIG_MODULES_NAVIGATOR=y
 CONFIG_MODULES_RC_UPDATE=y
 CONFIG_MODULES_SENSORS=y
 CONFIG_MODULES_TEMPERATURE_COMPENSATION=y
-CONFIG_MODULES_GIMBAL=y
+CONFIG_MODULES_VMOUNT=y
 CONFIG_SYSTEMCMDS_BL_UPDATE=y
 CONFIG_SYSTEMCMDS_ACTUATOR_TEST=y
 CONFIG_SYSTEMCMDS_DMESG=y
@@ -1,10 +1,10 @@
 {
-    "board_id": 1013,
+    "board_id": 139,
    "magic": "PX4FWv1",
-    "description": "Firmware for the MatekH743 board",
+    "description": "Firmware for the MatekH743-slim board",
    "image": "",
    "build_time": 0,
-    "summary": "MatekH743",
+    "summary": "MatekH743-slim",
    "version": "0.1",
    "image_size": 0,
    "image_maxsize": 1966080,
@@ -30,11 +30,11 @@ CONFIG_BOARD_RESET_ON_ASSERT=2
 CONFIG_C99_BOOL8=y
 CONFIG_CDCACM=y
 CONFIG_CDCACM_IFLOWCONTROL=y
-CONFIG_CDCACM_PRODUCTID=0x1013
-CONFIG_CDCACM_PRODUCTSTR="MatekH743"
+CONFIG_CDCACM_PRODUCTID=0x004b
+CONFIG_CDCACM_PRODUCTSTR="Matek H743-slim"
 CONFIG_CDCACM_RXBUFSIZE=600
 CONFIG_CDCACM_TXBUFSIZE=12000
-CONFIG_CDCACM_VENDORID=0x1209
+CONFIG_CDCACM_VENDORID=0x3162
 CONFIG_CDCACM_VENDORSTR="Matek"
 CONFIG_CLOCK_MONOTONIC=y
 CONFIG_DEBUG_FULLOPT=y
@@ -46,12 +46,12 @@ CONFIG_BUILTIN=y
 CONFIG_C99_BOOL8=y
 CONFIG_CDCACM=y
 CONFIG_CDCACM_IFLOWCONTROL=y
-CONFIG_CDCACM_PRODUCTID=0x1013
-CONFIG_CDCACM_PRODUCTSTR="MatekH743"
+CONFIG_CDCACM_PRODUCTID=0x0036
+CONFIG_CDCACM_PRODUCTSTR="MatekH743-slim"
 CONFIG_CDCACM_RXBUFSIZE=600
 CONFIG_CDCACM_TXBUFSIZE=12000
-CONFIG_CDCACM_VENDORID=0x1209
-CONFIG_CDCACM_VENDORSTR="Matek"
+CONFIG_CDCACM_VENDORID=0x1B8C
+CONFIG_CDCACM_VENDORSTR="PX4"
 CONFIG_CLOCK_MONOTONIC=y
 CONFIG_DEBUG_FULLOPT=y
 CONFIG_DEBUG_HARDFAULT_ALERT=y
@@ -119,9 +119,9 @@

 /* Spare GPIO */

-#define GPIO_VIDEO_PWR          /* PD10 */  (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_2MHz|GPIO_OUTPUT_CLEAR|GPIO_PORTD|GPIO_PIN10)
-#define GPIO_VIDEO_CAM          /* PD11 */  (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_2MHz|GPIO_OUTPUT_CLEAR|GPIO_PORTD|GPIO_PIN11)
-// #define GPIO_PG15               /* PG15 */  (GPIO_INPUT|GPIO_PULLUP|GPIO_PORTG|GPIO_PIN15)
+// #define GPIO_PG6                        /* PG6  */  (GPIO_INPUT|GPIO_PULLUP|GPIO_PORTG|GPIO_PIN6)
+// #define GPIO_PD15                       /* PD15 */  (GPIO_INPUT|GPIO_FLOAT|GPIO_PORTD|GPIO_PIN15)
+// #define GPIO_PG15                       /* PG15 */  (GPIO_INPUT|GPIO_PULLUP|GPIO_PORTG|GPIO_PIN15)


 /* Tone alarm output */
@@ -174,8 +174,6 @@
 		GPIO_nLED_BLUE, \
 		GPIO_nLED_GREEN, \
 		GPIO_TONE_ALARM_IDLE, \
-		GPIO_VIDEO_PWR, \
-		GPIO_VIDEO_CAM, \
 	}

 #define BOARD_ENABLE_CONSOLE_BUFFER
@@ -96,7 +96,7 @@
 #define INTERFACE_USART                1
 #define INTERFACE_USART_CONFIG         "/dev/ttyS0,57600"
 #define BOOT_DELAY_ADDRESS             0x000001a0
-#define BOARD_TYPE                     1013
+#define BOARD_TYPE                     139
 #define _FLASH_KBYTES                  (*(uint32_t *)0x1FF1E880)
 #define BOARD_FLASH_SECTORS            (15)
 #define BOARD_FLASH_SIZE               (_FLASH_KBYTES * 1024)
@@ -60,10 +60,6 @@
 #include <px4_platform/gpio.h>
 #include <px4_platform/board_dma_alloc.h>

-// # if defined(FLASH_BASED_PARAMS)
-// #  include <parameters/flashparams/flashfs.h>
-// #endif
-
 __BEGIN_DECLS
 extern void led_init(void);
 extern void led_on(int led);
@@ -178,25 +174,6 @@ __EXPORT int board_app_initialize(uintptr_t arg)

 #endif

-
-// #if defined(FLASH_BASED_PARAMS)
-// 	static sector_descriptor_t params_sector_map[] = {
-// 		{6, 128 * 1024, 0x081C0000},
-// 		{7, 128 * 1024, 0x081E0000},
-// 		{0, 0, 0},
-// 	};
-
-// 	/* Initialize the flashfs layer to use heap allocated memory */
-// 	int result = parameter_flashfs_init(params_sector_map, NULL, 0);
-
-// 	if (result != OK) {
-// 		syslog(LOG_ERR, "[boot] FAILED to init params in FLASH %d\n", result);
-// 		led_on(LED_BLUE);
-// 		return -ENODEV;
-// 	}
-
-// #endif
-
 	/* Configure the HW based on the manifest */
 	px4_platform_configure();

@@ -74,7 +74,7 @@ CONFIG_MODULES_SIH=y
 CONFIG_MODULES_TEMPERATURE_COMPENSATION=y
 CONFIG_MODULES_UUV_ATT_CONTROL=y
 CONFIG_MODULES_UUV_POS_CONTROL=y
-CONFIG_MODULES_GIMBAL=y
+CONFIG_MODULES_VMOUNT=y
 CONFIG_MODULES_VTOL_ATT_CONTROL=y
 CONFIG_SYSTEMCMDS_BL_UPDATE=y
 CONFIG_SYSTEMCMDS_ACTUATOR_TEST=y
@@ -70,7 +70,7 @@ CONFIG_MODULES_ROVER_POS_CONTROL=y
 CONFIG_MODULES_SENSORS=y
 CONFIG_MODULES_SIH=y
 CONFIG_MODULES_TEMPERATURE_COMPENSATION=y
-CONFIG_MODULES_GIMBAL=y
+CONFIG_MODULES_VMOUNT=y
 CONFIG_MODULES_VTOL_ATT_CONTROL=y
 CONFIG_SYSTEMCMDS_BL_UPDATE=y
 CONFIG_SYSTEMCMDS_ACTUATOR_TEST=y
@@ -72,7 +72,7 @@ CONFIG_MODULES_SIH=y
 CONFIG_MODULES_TEMPERATURE_COMPENSATION=y
 CONFIG_MODULES_UUV_ATT_CONTROL=y
 CONFIG_MODULES_UUV_POS_CONTROL=y
-CONFIG_MODULES_GIMBAL=y
+CONFIG_MODULES_VMOUNT=y
 CONFIG_MODULES_VTOL_ATT_CONTROL=y
 CONFIG_SYSTEMCMDS_BL_UPDATE=y
 CONFIG_SYSTEMCMDS_ACTUATOR_TEST=y
@@ -72,7 +72,7 @@ CONFIG_MODULES_SIH=y
 CONFIG_MODULES_TEMPERATURE_COMPENSATION=y
 CONFIG_MODULES_UUV_ATT_CONTROL=y
 CONFIG_MODULES_UUV_POS_CONTROL=y
-CONFIG_MODULES_GIMBAL=y
+CONFIG_MODULES_VMOUNT=y
 CONFIG_MODULES_VTOL_ATT_CONTROL=y
 CONFIG_SYSTEMCMDS_BL_UPDATE=y
 CONFIG_SYSTEMCMDS_ACTUATOR_TEST=y
@@ -71,7 +71,7 @@ CONFIG_MODULES_SIH=y
 CONFIG_MODULES_TEMPERATURE_COMPENSATION=y
 CONFIG_MODULES_UUV_ATT_CONTROL=y
 CONFIG_MODULES_UUV_POS_CONTROL=y
-CONFIG_MODULES_GIMBAL=y
+CONFIG_MODULES_VMOUNT=y
 CONFIG_MODULES_VTOL_ATT_CONTROL=y
 CONFIG_SYSTEMCMDS_ACTUATOR_TEST=y
 CONFIG_SYSTEMCMDS_BL_UPDATE=y
@@ -72,7 +72,7 @@ CONFIG_MODULES_SIH=y
 CONFIG_MODULES_TEMPERATURE_COMPENSATION=y
 CONFIG_MODULES_UUV_ATT_CONTROL=y
 CONFIG_MODULES_UUV_POS_CONTROL=y
-CONFIG_MODULES_GIMBAL=y
+CONFIG_MODULES_VMOUNT=y
 CONFIG_MODULES_VTOL_ATT_CONTROL=y
 CONFIG_SYSTEMCMDS_ACTUATOR_TEST=y
 CONFIG_SYSTEMCMDS_BL_UPDATE=y
@@ -70,7 +70,7 @@ CONFIG_MODULES_SIH=y
 CONFIG_MODULES_TEMPERATURE_COMPENSATION=y
 CONFIG_MODULES_UUV_ATT_CONTROL=y
 CONFIG_MODULES_UUV_POS_CONTROL=y
-CONFIG_MODULES_GIMBAL=y
+CONFIG_MODULES_VMOUNT=y
 CONFIG_MODULES_VTOL_ATT_CONTROL=y
 CONFIG_SYSTEMCMDS_BL_UPDATE=y
 CONFIG_SYSTEMCMDS_ACTUATOR_TEST=y
@@ -73,7 +73,7 @@ CONFIG_MODULES_SIH=y
 CONFIG_MODULES_TEMPERATURE_COMPENSATION=y
 CONFIG_MODULES_UUV_ATT_CONTROL=y
 CONFIG_MODULES_UUV_POS_CONTROL=y
-CONFIG_MODULES_GIMBAL=y
+CONFIG_MODULES_VMOUNT=y
 CONFIG_MODULES_VTOL_ATT_CONTROL=y
 CONFIG_SYSTEMCMDS_BL_UPDATE=y
 CONFIG_SYSTEMCMDS_ACTUATOR_TEST=y
@@ -74,7 +74,7 @@ CONFIG_MODULES_SIH=y
 CONFIG_MODULES_TEMPERATURE_COMPENSATION=y
 CONFIG_MODULES_UUV_ATT_CONTROL=y
 CONFIG_MODULES_UUV_POS_CONTROL=y
-CONFIG_MODULES_GIMBAL=y
+CONFIG_MODULES_VMOUNT=y
 CONFIG_MODULES_VTOL_ATT_CONTROL=y
 CONFIG_SYSTEMCMDS_BL_UPDATE=y
 CONFIG_SYSTEMCMDS_ACTUATOR_TEST=y
@@ -72,7 +72,7 @@ CONFIG_MODULES_ROVER_POS_CONTROL=y
 CONFIG_MODULES_SENSORS=y
 CONFIG_MODULES_SIH=y
 CONFIG_MODULES_TEMPERATURE_COMPENSATION=y
-CONFIG_MODULES_GIMBAL=y
+CONFIG_MODULES_VMOUNT=y
 CONFIG_MODULES_VTOL_ATT_CONTROL=y
 CONFIG_SYSTEMCMDS_DUMPFILE=y
 CONFIG_SYSTEMCMDS_I2CDETECT=y
@@ -76,7 +76,7 @@ CONFIG_MODULES_ROVER_POS_CONTROL=y
 CONFIG_MODULES_SENSORS=y
 CONFIG_MODULES_SIH=y
 CONFIG_MODULES_TEMPERATURE_COMPENSATION=y
-CONFIG_MODULES_GIMBAL=y
+CONFIG_MODULES_VMOUNT=y
 CONFIG_MODULES_VTOL_ATT_CONTROL=y
 CONFIG_SYSTEMCMDS_DUMPFILE=y
 CONFIG_SYSTEMCMDS_I2CDETECT=y
@@ -48,7 +48,7 @@ CONFIG_MODULES_MC_RATE_CONTROL=y
 CONFIG_MODULES_NAVIGATOR=y
 CONFIG_MODULES_RC_UPDATE=y
 CONFIG_MODULES_SENSORS=y
-CONFIG_MODULES_GIMBAL=y
+CONFIG_MODULES_VMOUNT=y
 CONFIG_SYSTEMCMDS_DMESG=y
 CONFIG_SYSTEMCMDS_DUMPFILE=y
 CONFIG_SYSTEMCMDS_I2CDETECT=y
@@ -77,7 +77,7 @@ CONFIG_MODULES_SIH=y
 CONFIG_MODULES_TEMPERATURE_COMPENSATION=y
 CONFIG_MODULES_UUV_ATT_CONTROL=y
 CONFIG_MODULES_UUV_POS_CONTROL=y
-CONFIG_MODULES_GIMBAL=y
+CONFIG_MODULES_VMOUNT=y
 CONFIG_MODULES_VTOL_ATT_CONTROL=y
 CONFIG_SYSTEMCMDS_BL_UPDATE=y
 CONFIG_SYSTEMCMDS_ACTUATOR_TEST=y
@@ -78,7 +78,7 @@ CONFIG_MODULES_SIH=y
 CONFIG_MODULES_TEMPERATURE_COMPENSATION=y
 CONFIG_MODULES_UUV_ATT_CONTROL=y
 CONFIG_MODULES_UUV_POS_CONTROL=y
-CONFIG_MODULES_GIMBAL=y
+CONFIG_MODULES_VMOUNT=y
 CONFIG_MODULES_VTOL_ATT_CONTROL=y
 CONFIG_SYSTEMCMDS_BL_UPDATE=y
 CONFIG_SYSTEMCMDS_ACTUATOR_TEST=y
@@ -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
@@ -75,7 +75,7 @@ CONFIG_MODULES_SIH=y
 CONFIG_MODULES_TEMPERATURE_COMPENSATION=y
 CONFIG_MODULES_UUV_ATT_CONTROL=y
 CONFIG_MODULES_UUV_POS_CONTROL=y
-CONFIG_MODULES_GIMBAL=y
+CONFIG_MODULES_VMOUNT=y
 CONFIG_MODULES_VTOL_ATT_CONTROL=y
 CONFIG_SYSTEMCMDS_BL_UPDATE=y
 CONFIG_SYSTEMCMDS_ACTUATOR_TEST=y
@@ -32,7 +32,7 @@ CONFIG_MODULES_SIH=n
 CONFIG_MODULES_TEMPERATURE_COMPENSATION=n
 CONFIG_MODULES_UUV_ATT_CONTROL=n
 CONFIG_MODULES_UUV_POS_CONTROL=n
-CONFIG_MODULES_GIMBAL=n
+CONFIG_MODULES_VMOUNT=n
 CONFIG_SYSTEMCMDS_SERIAL_TEST=n
 CONFIG_BOARD_CONSTRAINED_FLASH=y
 CONFIG_BOARD_TESTING=y
@@ -80,7 +80,7 @@ CONFIG_MODULES_SIH=y
 CONFIG_MODULES_TEMPERATURE_COMPENSATION=y
 CONFIG_MODULES_UUV_ATT_CONTROL=y
 CONFIG_MODULES_UUV_POS_CONTROL=y
-CONFIG_MODULES_GIMBAL=y
+CONFIG_MODULES_VMOUNT=y
 CONFIG_MODULES_VTOL_ATT_CONTROL=y
 CONFIG_SYSTEMCMDS_ACTUATOR_TEST=y
 CONFIG_SYSTEMCMDS_BL_UPDATE=y
@@ -28,6 +28,6 @@ CONFIG_MODULES_LOCAL_POSITION_ESTIMATOR=n
 CONFIG_MODULES_ROVER_POS_CONTROL=n
 CONFIG_MODULES_SIH=n
 CONFIG_MODULES_TEMPERATURE_COMPENSATION=n
-CONFIG_MODULES_GIMBAL=n
+CONFIG_MODULES_VMOUNT=n
 CONFIG_BOARD_CONSTRAINED_FLASH=y
 CONFIG_BOARD_TESTING=y
@@ -80,7 +80,7 @@ CONFIG_MODULES_ROVER_POS_CONTROL=y
 CONFIG_MODULES_SENSORS=y
 CONFIG_MODULES_SIH=y
 CONFIG_MODULES_TEMPERATURE_COMPENSATION=y
-CONFIG_MODULES_GIMBAL=y
+CONFIG_MODULES_VMOUNT=y
 CONFIG_MODULES_VTOL_ATT_CONTROL=y
 CONFIG_SYSTEMCMDS_ACTUATOR_TEST=y
 CONFIG_SYSTEMCMDS_BL_UPDATE=y
@@ -72,7 +72,7 @@ CONFIG_MODULES_ROVER_POS_CONTROL=y
 CONFIG_MODULES_SENSORS=y
 CONFIG_MODULES_SIH=y
 CONFIG_MODULES_TEMPERATURE_COMPENSATION=y
-CONFIG_MODULES_GIMBAL=y
+CONFIG_MODULES_VMOUNT=y
 CONFIG_MODULES_VTOL_ATT_CONTROL=y
 CONFIG_SYSTEMCMDS_BL_UPDATE=y
 CONFIG_SYSTEMCMDS_ACTUATOR_TEST=y
@@ -68,7 +68,7 @@ CONFIG_MODULES_ROVER_POS_CONTROL=y
 CONFIG_MODULES_SENSORS=y
 CONFIG_MODULES_SIH=y
 CONFIG_MODULES_TEMPERATURE_COMPENSATION=y
-CONFIG_MODULES_GIMBAL=y
+CONFIG_MODULES_VMOUNT=y
 CONFIG_MODULES_VTOL_ATT_CONTROL=y
 CONFIG_SYSTEMCMDS_BL_UPDATE=y
 CONFIG_SYSTEMCMDS_ACTUATOR_TEST=y
@@ -55,7 +55,7 @@ CONFIG_MODULES_SIH=y
 CONFIG_MODULES_TEMPERATURE_COMPENSATION=y
 CONFIG_MODULES_UUV_ATT_CONTROL=y
 CONFIG_MODULES_UUV_POS_CONTROL=y
-CONFIG_MODULES_GIMBAL=y
+CONFIG_MODULES_VMOUNT=y
 CONFIG_MODULES_VTOL_ATT_CONTROL=y
 CONFIG_SYSTEMCMDS_DYN=y
 CONFIG_SYSTEMCMDS_LED_CONTROL=y
@@ -43,7 +43,7 @@ CONFIG_MODULES_SIMULATOR=y
 CONFIG_MODULES_TEMPERATURE_COMPENSATION=y
 CONFIG_MODULES_UUV_ATT_CONTROL=y
 CONFIG_MODULES_UUV_POS_CONTROL=y
-CONFIG_MODULES_GIMBAL=y
+CONFIG_MODULES_VMOUNT=y
 CONFIG_MODULES_VTOL_ATT_CONTROL=y
 CONFIG_SYSTEMCMDS_ACTUATOR_TEST=y
 CONFIG_SYSTEMCMDS_DYN=y
@@ -53,7 +53,7 @@ CONFIG_MODULES_ROVER_POS_CONTROL=y
 CONFIG_MODULES_SENSORS=y
 CONFIG_MODULES_SIH=y
 CONFIG_MODULES_TEMPERATURE_COMPENSATION=y
-CONFIG_MODULES_GIMBAL=y
+CONFIG_MODULES_VMOUNT=y
 CONFIG_MODULES_VTOL_ATT_CONTROL=y
 CONFIG_SYSTEMCMDS_DYN=y
 CONFIG_SYSTEMCMDS_LED_CONTROL=y
@@ -39,7 +39,7 @@ CONFIG_MODULES_CONTROL_ALLOCATOR=y
 CONFIG_MODULES_NAVIGATOR=y
 CONFIG_MODULES_RC_UPDATE=y
 CONFIG_MODULES_SENSORS=y
-CONFIG_MODULES_GIMBAL=y
+CONFIG_MODULES_VMOUNT=y
 CONFIG_SYSTEMCMDS_DMESG=y
 CONFIG_SYSTEMCMDS_DUMPFILE=y
 CONFIG_SYSTEMCMDS_HARDFAULT_LOG=y
@@ -59,7 +59,7 @@ CONFIG_MODULES_RC_UPDATE=y
 CONFIG_MODULES_SENSORS=y
 CONFIG_MODULES_SIH=y
 CONFIG_MODULES_TEMPERATURE_COMPENSATION=y
-CONFIG_MODULES_GIMBAL=y
+CONFIG_MODULES_VMOUNT=y
 CONFIG_SYSTEMCMDS_BL_UPDATE=y
 CONFIG_SYSTEMCMDS_ACTUATOR_TEST=y
 CONFIG_SYSTEMCMDS_DMESG=y
@@ -11,7 +11,6 @@ set(MENUCONFIG_PATH ${PYTHON_EXECUTABLE} -m menuconfig CACHE INTERNAL "menuconfi
 set(GUICONFIG_PATH ${PYTHON_EXECUTABLE} -m guiconfig CACHE INTERNAL "guiconfig program" FORCE)
 set(DEFCONFIG_PATH ${PYTHON_EXECUTABLE} -m defconfig CACHE INTERNAL "defconfig program" FORCE)
 set(SAVEDEFCONFIG_PATH ${PYTHON_EXECUTABLE} -m savedefconfig CACHE INTERNAL "savedefconfig program" FORCE)
-set(GENCONFIG_PATH ${PYTHON_EXECUTABLE} -m genconfig CACHE INTERNAL "genconfig program" FORCE)

 set(COMMON_KCONFIG_ENV_SETTINGS
 	PYTHON_EXECUTABLE=${PYTHON_EXECUTABLE}
@@ -48,11 +47,6 @@ if(EXISTS ${BOARD_DEFCONFIG})
                        OUTPUT_VARIABLE DUMMY_RESULTS)
    endif()

-    # Generate header file for C/C++ preprocessor
-    execute_process(COMMAND ${CMAKE_COMMAND} -E env ${COMMON_KCONFIG_ENV_SETTINGS}
-                    ${GENCONFIG_PATH} --header-path ${PX4_BINARY_DIR}/px4_boardconfig.h
-                    WORKING_DIRECTORY ${PX4_SOURCE_DIR}
-                    OUTPUT_VARIABLE DUMMY_RESULTS)

    # parse board config options for cmake
    file(STRINGS ${BOARD_CONFIG} ConfigContents)
@@ -16,5 +16,3 @@ float32 angular_velocity_y
 float32 angular_velocity_z

 uint32 failure_flags
-
-bool received_from_mavlink
@@ -175,5 +175,3 @@ uint8 source_system		# System sending the command
 uint8 source_component		# Component sending the command
 uint8 confirmation		# 0: First transmission of this command. 1-255: Confirmation transmissions (e.g. for kill command)
 bool from_external
-
-# TOPICS vehicle_command gimbal_v1_command
@@ -52,6 +52,3 @@
 #include <board_config.h>

 #endif
-
-/* PX4 board kconfig symbols */
-#include <px4_boardconfig.h>
@@ -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,
@@ -101,10 +101,10 @@ PARAM_DEFINE_INT32(GPS_UBX_MODE, 0);
 *
 * Heading offset angle for dual antenna GPS setups that support heading estimation.
 *
- * Set this to 0 if the antennas are parallel to the forward-facing direction of the vehicle and the rover antenna is in
+ * Set this to 0 if the antennas are parallel to the forward-facing direction of the vehicle and the first antenna is in
 * front. The offset angle increases clockwise.
 *
- * Set this to 90 if the rover antenna is placed on the right side of the vehicle and the moving base antenna is on the left side.
+ * Set this to 90 if the first antenna is placed on the right side and the second on the left side of the vehicle.
 *
 * @min 0
 * @max 360
@@ -436,8 +436,8 @@ uint8_t MPU6000::RegisterRead(Register reg)
 	uint8_t cmd[2] {};
 	cmd[0] = static_cast<uint8_t>(reg) | DIR_READ;
 	set_frequency(SPI_SPEED); // low speed for regular registers
-	px4_udelay(10);
 	transfer(cmd, cmd, sizeof(cmd));
+	px4_udelay(10);
 	return cmd[1];
 }

@@ -445,8 +445,8 @@ void MPU6000::RegisterWrite(Register reg, uint8_t value)
 {
 	uint8_t cmd[2] { (uint8_t)reg, value };
 	set_frequency(SPI_SPEED); // low speed for regular registers
-	px4_udelay(10);
 	transfer(cmd, cmd, sizeof(cmd));
+	px4_udelay(10);
 }

 void MPU6000::RegisterSetAndClearBits(Register reg, uint8_t setbits, uint8_t clearbits)
@@ -108,18 +108,15 @@ float BMM150::compensate_x(int16_t mag_data_x, uint16_t data_rhall)
 {
 	float retval = 0;

-	// Overflow condition check
-	if ((mag_data_x != OVERFLOW_XYAXES) && (data_rhall != 0) && (_trim_data.dig_xyz1 != 0)) {
-		// Processing compensation equations
-		//  not documented, but derived from https://github.com/BoschSensortec/BMM150-Sensor-API/blob/a20641f216057f0c54de115fe81b57368e119c01/bmm150.c#L1624-L1633 as of 2020-09-25
-		float process_comp_x0 = (((float)_trim_data.dig_xyz1) * 16384.0f / data_rhall);
-		retval = (process_comp_x0 - 16384.0f);
-		float process_comp_x1 = ((float)_trim_data.dig_xy2) * (retval * retval / 268435456.0f);
-		float process_comp_x2 = process_comp_x1 + retval * ((float)_trim_data.dig_xy1) / 16384.0f;
-		float process_comp_x3 = ((float)_trim_data.dig_x2) + 160.0f;
-		float process_comp_x4 = mag_data_x * ((process_comp_x2 + 256.0f) * process_comp_x3);
-		retval = ((process_comp_x4 / 8192.0f) + (((float)_trim_data.dig_x1) * 8.0f)) / 16.0f;
-	}
+	// Processing compensation equations
+	//  not documented, but derived from https://github.com/BoschSensortec/BMM150-Sensor-API/blob/a20641f216057f0c54de115fe81b57368e119c01/bmm150.c#L1624-L1633 as of 2020-09-25
+	float process_comp_x0 = (((float)_trim_data.dig_xyz1) * 16384.f / data_rhall);
+	retval = (process_comp_x0 - 16384.f);
+	float process_comp_x1 = ((float)_trim_data.dig_xy2) * (retval * retval / 268435456.f);
+	float process_comp_x2 = process_comp_x1 + retval * ((float)_trim_data.dig_xy1) / 16384.f;
+	float process_comp_x3 = ((float)_trim_data.dig_x2) + 160.f;
+	float process_comp_x4 = mag_data_x * ((process_comp_x2 + 256.f) * process_comp_x3);
+	retval = ((process_comp_x4 / 8192.f) + (((float)_trim_data.dig_x1) * 8.f)) / 16.f;

 	return retval;
 }
@@ -128,17 +125,15 @@ float BMM150::compensate_y(int16_t mag_data_y, uint16_t data_rhall)
 {
 	float retval = 0;

-	// Overflow condition check
-	if ((mag_data_y != OVERFLOW_XYAXES) && (data_rhall != 0) && (_trim_data.dig_xyz1 != 0)) {
-		// Processing compensation equations
-		float process_comp_y0 = ((float)_trim_data.dig_xyz1) * 16384.0f / data_rhall;
-		retval = process_comp_y0 - 16384.0f;
-		float process_comp_y1 = ((float)_trim_data.dig_xy2) * (retval * retval / 268435456.0f);
-		float process_comp_y2 = process_comp_y1 + retval * ((float)_trim_data.dig_xy1) / 16384.0f;
-		float process_comp_y3 = ((float)_trim_data.dig_y2) + 160.0f;
-		float process_comp_y4 = mag_data_y * (((process_comp_y2) + 256.0f) * process_comp_y3);
-		retval = ((process_comp_y4 / 8192.0f) + (((float)_trim_data.dig_y1) * 8.0f)) / 16.0f;
-	}
+	// Processing compensation equations
+	//  not documented, but derived from https://github.com/BoschSensortec/BMM150-Sensor-API/blob/a20641f216057f0c54de115fe81b57368e119c01/bmm150.c#L1660-L1667 as of 2020-09-25
+	float process_comp_y0 = ((float)_trim_data.dig_xyz1) * 16384.f / data_rhall;
+	retval = process_comp_y0 - 16384.f;
+	float process_comp_y1 = ((float)_trim_data.dig_xy2) * (retval * retval / 268435456.f);
+	float process_comp_y2 = process_comp_y1 + retval * ((float)_trim_data.dig_xy1) / 16384.f;
+	float process_comp_y3 = ((float)_trim_data.dig_y2) + 160.0f;
+	float process_comp_y4 = mag_data_y * (((process_comp_y2) + 256.f) * process_comp_y3);
+	retval = ((process_comp_y4 / 8192.f) + (((float)_trim_data.dig_y1) * 8.f)) / 16.f;

 	return retval;
 }
@@ -147,52 +142,35 @@ float BMM150::compensate_z(int16_t mag_data_z, uint16_t data_rhall)
 {
 	float retval = 0;

-	// Overflow condition check
-	if ((mag_data_z != OVERFLOW_ZAXIS)
-	    && (_trim_data.dig_z2 != 0) && (_trim_data.dig_z1 != 0) && (_trim_data.dig_xyz1 != 0)
-	    && (data_rhall != 0)) {
-		// Processing compensation equations
-		//  not documented, but derived from https://github.com/BoschSensortec/BMM150-Sensor-API/blob/a20641f216057f0c54de115fe81b57368e119c01/bmm150.c#L1696-L1703 as of 2020-09-25
-		float process_comp_z0 = ((float)mag_data_z) - ((float)_trim_data.dig_z4);
-		float process_comp_z1 = ((float)data_rhall) - ((float)_trim_data.dig_xyz1);
-		float process_comp_z2 = (((float)_trim_data.dig_z3) * process_comp_z1);
-		float process_comp_z3 = ((float)_trim_data.dig_z1) * ((float)data_rhall) / 32768.0f;
-		float process_comp_z4 = ((float)_trim_data.dig_z2) + process_comp_z3;
-		float process_comp_z5 = (process_comp_z0 * 131072.0f) - process_comp_z2;
-		retval = (process_comp_z5 / ((process_comp_z4) * 4.0f)) / 16.0f;
-	}
+	// Processing compensation equations
+	//  not documented, but derived from https://github.com/BoschSensortec/BMM150-Sensor-API/blob/a20641f216057f0c54de115fe81b57368e119c01/bmm150.c#L1696-L1703 as of 2020-09-25
+	float process_comp_z0 = ((float)mag_data_z) - ((float)_trim_data.dig_z4);
+	float process_comp_z1 = ((float)data_rhall) - ((float)_trim_data.dig_xyz1);
+	float process_comp_z2 = (((float)_trim_data.dig_z3) * process_comp_z1);
+	float process_comp_z3 = ((float)_trim_data.dig_z1) * ((float)data_rhall) / 32768.f;
+	float process_comp_z4 = ((float)_trim_data.dig_z2) + process_comp_z3;
+	float process_comp_z5 = (process_comp_z0 * 131072.f) - process_comp_z2;
+	retval = (process_comp_z5 / ((process_comp_z4) * 4.f)) / 16.f;

 	return retval;
 }

 static constexpr int16_t combine_xy_int13(const uint8_t msb, const uint8_t lsb)
 {
-	// msb: 8-bit MSB part [12:5] of the 13 bit output data
-	// lsb: 5-bit LSB part [4:0] of the 13 bit output data
-	int16_t msb_data = ((int16_t)((int8_t)msb)) << 5;
-	int16_t lsb_data = ((lsb & 0xF8) >> 3);
-
-	return (int16_t)(msb_data | lsb_data);
+	int16_t x = ((msb << 8) | lsb);
+	return x / 8; // arithmetic shift by 3 (13 bit signed integer)
 }

 static constexpr int16_t combine_z_int15(const uint8_t msb, const uint8_t lsb)
 {
-	// msb: 8-bit MSB part [12:5] of the 13 bit output data
-	// lsb: 7-bit LSB part [6:0] of the 15 bit output data
-	int16_t msb_data = ((int16_t)((int8_t)msb)) << 7;
-	int16_t lsb_data = ((lsb & 0xFE) >> 1);
-
-	return (int16_t)(msb_data | lsb_data);
+	int16_t z = ((msb << 8) | lsb);
+	return z / 2; // arithmetic shift by 1 (15 bit signed integer)
 }

 static constexpr uint16_t combine_rhall_uint14(const uint8_t msb, const uint8_t lsb)
 {
-	// msb: 8-bit MSB part [13:6] of the 14 bit output data
-	// lsb: 6-bit LSB part [5:0] of the 14 bit output data
-	uint16_t msb_data = ((uint16_t)((uint16_t)msb)) << 6;
-	uint16_t lsb_data = ((lsb & 0xFC) >> 2);
-
-	return (uint16_t)(msb_data | lsb_data);
+	uint16_t rhall = ((msb << 8) | lsb);
+	return (rhall >> 2) & 0x3FFF; // 14 bit unsigned integer
 }

 void BMM150::RunImpl()
@@ -128,7 +128,7 @@ private:
 	register_config_t _register_cfg[size_register_cfg] {
 		// Register                | Set bits, Clear bits
 		{ Register::POWER_CONTROL, POWER_CONTROL_BIT::PowerControl, POWER_CONTROL_BIT::SoftReset },
-		{ Register::OP_MODE,       OP_MODE_BIT::ODR_20HZ_SET, OP_MODE_BIT::ODR_20HZ_CLEAR | OP_MODE_BIT::Opmode_Sleep | OP_MODE_BIT::Self_Test },
+		{ Register::OP_MODE,       OP_MODE_BIT::ODR_20Hz, OP_MODE_BIT::Opmode_Sleep | OP_MODE_BIT::Self_Test },
 		{ Register::REPXY,         REPXY_BIT::XY_HA_SET, REPXY_BIT::XY_HA_CLEAR },
 		{ Register::REPZ,          REPZ_BIT::Z_HA_SET, REPZ_BIT::Z_HA_CLEAR },
 	};
@@ -96,12 +96,11 @@ enum POWER_CONTROL_BIT : uint8_t {
 // OP_MODE
 enum OP_MODE_BIT : uint8_t {
 	// 5:3 Data rate control
-	ODR_20HZ_SET   = Bit5 | Bit3,
-	ODR_20HZ_CLEAR = Bit4,
+	ODR_20Hz     = Bit5 | Bit3, // ODR 20 Hz

 	// 2:1 Operation mode control
-	Opmode_Sleep   = Bit2 | Bit1, // Sleep mode
-	Self_Test      = Bit0,
+	Opmode_Sleep = Bit2 | Bit1, // Sleep mode
+	Self_Test    = Bit0,
 };

 // STATUS
@@ -548,7 +548,7 @@ void DevCommon::scan_for_packets()

 		perf_set_count(header_bytes_received_count, _read_buffer->header_bytes_received);

-		i += Sp2HeaderSize + payload_len;
+		i += payload_len;

 	}
 }
@@ -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);
@@ -617,25 +617,6 @@ void NPFG::navigateLoiter(const Vector2d &loiter_center, const Vector2d &vehicle
 	updateRollSetpoint();
 } // navigateLoiter

-
-void NPFG::navigatePathTangent(const matrix::Vector2d &vehicle_pos, const matrix::Vector2d &position_setpoint,
-			       const matrix::Vector2f &tangent_setpoint,
-			       const matrix::Vector2f &ground_vel, const matrix::Vector2f &wind_vel, const float &curvature)
-{
-	path_type_loiter_ = false;
-
-	// set unit tangent directly
-	unit_path_tangent_ = tangent_setpoint.normalized();
-
-	// closest point to vehicle
-	matrix::Vector2f error_vector = getLocalPlanarVector(position_setpoint, vehicle_pos);
-	signed_track_error_ = cross2D(unit_path_tangent_, error_vector);
-
-	guideToPath(ground_vel, wind_vel, unit_path_tangent_, signed_track_error_, curvature);
-
-	updateRollSetpoint();
-} // navigatePathTangent
-
 void NPFG::navigateHeading(float heading_ref, const Vector2f &ground_vel, const Vector2f &wind_vel)
 {
 	path_type_loiter_ = false;
@@ -248,21 +248,6 @@ public:
 			    float radius, int8_t loiter_direction, const matrix::Vector2f &ground_vel,
 			    const matrix::Vector2f &wind_vel);

-	/*
-	 * Path following logic. Takes poisiton, path tangent, curvature and
-	 * then updates control setpoints to follow a path setpoint.
-	 *
-	 * @param[in] vehicle_pos vehicle_pos Vehicle position in WGS84 coordinates (lat,lon) [deg]
-	 * @param[in] position_setpoint closest point on a path in WGS84 coordinates (lat,lon) [deg]
-	 * @param[in] tangent_setpoint unit tangent vector of the path [m]
-	 * @param[in] ground_vel Vehicle ground velocity vector [m/s]
-	 * @param[in] wind_vel Wind velocity vector [m/s]
-	 * @param[in] curvature of the path setpoint [1/m]
-	 */
-	void navigatePathTangent(const matrix::Vector2d &vehicle_pos, const matrix::Vector2d &position_setpoint,
-				 const matrix::Vector2f &tangent_setpoint,
-				 const matrix::Vector2f &ground_vel, const matrix::Vector2f &wind_vel, const float &curvature);
-
 	/*
 	 * Navigate on a fixed heading.
 	 *
@@ -629,10 +629,6 @@ bool set_nav_state(vehicle_status_s &status, actuator_armed_s &armed, commander_

 		} else if (is_armed && check_invalid_pos_nav_state(status, old_failsafe, mavlink_log_pub, status_flags, false, true)) {
 			// nothing to do - everything done in check_invalid_pos_nav_state
-
-		} else if (status_flags.vtol_transition_failure) {
-			status.nav_state = vehicle_status_s::NAVIGATION_STATE_AUTO_RTL;
-
 		} else if (status.data_link_lost && data_link_loss_act_configured && !landed && is_armed) {
 			// Data link lost, data link loss reaction configured -> do configured reaction
 			enable_failsafe(status, old_failsafe, mavlink_log_pub, event_failsafe_reason_t::no_datalink);
@@ -743,10 +739,6 @@ bool set_nav_state(vehicle_status_s &status, actuator_armed_s &armed, commander_

 		} else if (is_armed && check_invalid_pos_nav_state(status, old_failsafe, mavlink_log_pub, status_flags, false, false)) {
 			// nothing to do - everything done in check_invalid_pos_nav_state
-
-		} else if (status_flags.vtol_transition_failure) {
-			status.nav_state = vehicle_status_s::NAVIGATION_STATE_AUTO_RTL;
-
 		} else if (status.data_link_lost && data_link_loss_act_configured && !landed && is_armed) {
 			// Data link lost, data link loss reaction configured -> do configured reaction
 			enable_failsafe(status, old_failsafe, mavlink_log_pub, event_failsafe_reason_t::no_datalink);
@@ -245,7 +245,10 @@ void EKFGSF_yaw::predictEKF(const uint8_t model_index)
 	}

 	// Calculate the yaw state using a projection onto the horizontal that avoids gimbal lock
-	_ekf_gsf[model_index].X(2) = getEulerYaw(_ahrs_ekf_gsf[model_index].R);
+	const Dcmf &R = _ahrs_ekf_gsf[model_index].R;
+	_ekf_gsf[model_index].X(2) = shouldUse321RotationSequence(R) ?
+				     getEuler321Yaw(R) :
+				     getEuler312Yaw(R);

 	// calculate delta velocity in a horizontal front-right frame
 	const Vector3f del_vel_NED = _ahrs_ekf_gsf[model_index].R * _delta_vel;
@@ -261,13 +264,13 @@ void EKFGSF_yaw::predictEKF(const uint8_t model_index)
 	// predict covariance - equations generated using EKF/python/gsf_ekf_yaw_estimator/main.py

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

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

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

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

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

 	// Variance for true airspeed measurement - (m/sec)^2
 	const float R_TAS = sq(math::constrain(_params.eas_noise, 0.5f, 5.0f) *
@@ -187,7 +187,9 @@ void Ekf::resetWind()
 */
 void Ekf::resetWindUsingAirspeed()
 {
-	const float euler_yaw = getEulerYaw(_R_to_earth);
+	const float euler_yaw = shouldUse321RotationSequence(_R_to_earth)
+				? getEuler321Yaw(_state.quat_nominal)
+				: getEuler312Yaw(_state.quat_nominal);

 	// estimate wind using zero sideslip assumption and airspeed measurement if airspeed available
 	_state.wind_vel(0) = _state.vel(0) - _airspeed_sample_delayed.true_airspeed * cosf(euler_yaw);
@@ -243,6 +243,7 @@ void Ekf::controlExternalVisionFusion()
 			}

 			Vector3f ev_pos_obs_var;
+			Vector2f ev_pos_innov_gates;

 			// correct position and height for offset relative to IMU
 			const Vector3f pos_offset_body = _params.ev_pos_body - _params.imu_pos_body;
@@ -306,9 +307,9 @@ void Ekf::controlExternalVisionFusion()
 			}

 			// innovation gate size
-			const float ev_pos_innov_gate = fmaxf(_params.ev_pos_innov_gate, 1.0f);
+			ev_pos_innov_gates(0) = fmaxf(_params.ev_pos_innov_gate, 1.0f);

-			fuseHorizontalPosition(_ev_pos_innov, ev_pos_innov_gate, ev_pos_obs_var, _ev_pos_innov_var, _ev_pos_test_ratio);
+			fuseHorizontalPosition(_ev_pos_innov, ev_pos_innov_gates, ev_pos_obs_var, _ev_pos_innov_var, _ev_pos_test_ratio);
 		}

 		// determine if we should use the velocity observations
@@ -317,7 +318,10 @@ void Ekf::controlExternalVisionFusion()
 				resetVelocity();
 			}

-			_ev_vel_innov = _state.vel - getVisionVelocityInEkfFrame();
+			Vector2f ev_vel_innov_gates;
+
+			_last_vel_obs = getVisionVelocityInEkfFrame();
+			_ev_vel_innov = _state.vel - _last_vel_obs;

 			// check if we have been deadreckoning too long
 			if (isTimedOut(_time_last_hor_vel_fuse, _params.reset_timeout_max)) {
@@ -328,12 +332,12 @@ void Ekf::controlExternalVisionFusion()
 				}
 			}

-			const Vector3f obs_var = matrix::max(getVisionVelocityVarianceInEkfFrame(), sq(0.05f));
+			_last_vel_obs_var = matrix::max(getVisionVelocityVarianceInEkfFrame(), sq(0.05f));

-			const float innov_gate = fmaxf(_params.ev_vel_innov_gate, 1.f);
+			ev_vel_innov_gates.setAll(fmaxf(_params.ev_vel_innov_gate, 1.0f));

-			fuseHorizontalVelocity(_ev_vel_innov, innov_gate, obs_var, _ev_vel_innov_var, _ev_vel_test_ratio);
-			fuseVerticalVelocity(_ev_vel_innov, innov_gate, obs_var, _ev_vel_innov_var, _ev_vel_test_ratio);
+			fuseHorizontalVelocity(_ev_vel_innov, ev_vel_innov_gates, _last_vel_obs_var, _ev_vel_innov_var, _ev_vel_test_ratio);
+			fuseVerticalVelocity(_ev_vel_innov, ev_vel_innov_gates, _last_vel_obs_var, _ev_vel_innov_var, _ev_vel_test_ratio);
 		}

 		// determine if we should use the yaw observation
@@ -1043,46 +1047,45 @@ void Ekf::controlBetaFusion()

 void Ekf::controlDragFusion()
 {
-	if ((_params.fusion_mode & MASK_USE_DRAG) && _drag_buffer &&
-	    !_using_synthetic_position && _control_status.flags.in_air && !_mag_inhibit_yaw_reset_req) {
+	if ((_params.fusion_mode & MASK_USE_DRAG) &&
+	    !_using_synthetic_position &&
+	    _control_status.flags.in_air &&
+	    !_mag_inhibit_yaw_reset_req) {

 		if (!_control_status.flags.wind) {
 			// reset the wind states and covariances when starting drag accel fusion
 			_control_status.flags.wind = true;
 			resetWind();

-		}
-
-
-		dragSample drag_sample;
-
-		if (_drag_buffer->pop_first_older_than(_imu_sample_delayed.time_us, &drag_sample)) {
-			fuseDrag(drag_sample);
+		} else if (_drag_buffer && _drag_buffer->pop_first_older_than(_imu_sample_delayed.time_us, &_drag_sample_delayed)) {
+			fuseDrag();
 		}
 	}
 }

 void Ekf::controlAuxVelFusion()
 {
+	bool data_ready = false;
+
 	if (_auxvel_buffer) {
-		auxVelSample auxvel_sample_delayed;
+		data_ready = _auxvel_buffer->pop_first_older_than(_imu_sample_delayed.time_us, &_auxvel_sample_delayed);
+	}

-		if (_auxvel_buffer->pop_first_older_than(_imu_sample_delayed.time_us, &auxvel_sample_delayed)) {
+	if (data_ready && isHorizontalAidingActive()) {

-			if (isHorizontalAidingActive()) {
+		const Vector2f aux_vel_innov_gate(_params.auxvel_gate, _params.auxvel_gate);

-				const float aux_vel_innov_gate = fmaxf(_params.auxvel_gate, 1.f);
+		_last_vel_obs = _auxvel_sample_delayed.vel;
+		_aux_vel_innov = _state.vel - _last_vel_obs;
+		_last_vel_obs_var = _aux_vel_innov_var;

-				_aux_vel_innov = _state.vel - auxvel_sample_delayed.vel;
+		fuseHorizontalVelocity(_aux_vel_innov, aux_vel_innov_gate, _auxvel_sample_delayed.velVar,
+				       _aux_vel_innov_var, _aux_vel_test_ratio);

-				fuseHorizontalVelocity(_aux_vel_innov, aux_vel_innov_gate, auxvel_sample_delayed.velVar,
-						       _aux_vel_innov_var, _aux_vel_test_ratio);
+		// Can be enabled after bit for this is added to EKF_AID_MASK
+		// fuseVerticalVelocity(_aux_vel_innov, aux_vel_innov_gate, _auxvel_sample_delayed.velVar,
+		//		_aux_vel_innov_var, _aux_vel_test_ratio);

-				// Can be enabled after bit for this is added to EKF_AID_MASK
-				// fuseVerticalVelocity(_aux_vel_innov, aux_vel_innov_gate, auxvel_sample_delayed.velVar,
-				//		_aux_vel_innov_var, _aux_vel_test_ratio);
-			}
-		}
 	}
 }

@@ -100,26 +100,26 @@ void Ekf::initialiseCovariance()
 void Ekf::predictCovariance()
 {
 	// assign intermediate state variables
-	const float &q0 = _state.quat_nominal(0);
-	const float &q1 = _state.quat_nominal(1);
-	const float &q2 = _state.quat_nominal(2);
-	const float &q3 = _state.quat_nominal(3);
+	const float q0 = _state.quat_nominal(0);
+	const float q1 = _state.quat_nominal(1);
+	const float q2 = _state.quat_nominal(2);
+	const float q3 = _state.quat_nominal(3);

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

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

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

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

 	// Use average update interval to reduce accumulated covariance prediction errors due to small single frame dt values
 	const float dt = _dt_ekf_avg;
@@ -1137,7 +1137,7 @@ void Ekf::resetWindCovarianceUsingAirspeed()
 {
 	// Derived using EKF/matlab/scripts/Inertial Nav EKF/wind_cov.py
 	// TODO: explicitly include the sideslip angle in the derivation
-	const float euler_yaw = getEulerYaw(_R_to_earth);
+	const float euler_yaw = getEuler321Yaw(_state.quat_nominal);
 	const float R_TAS = sq(math::constrain(_params.eas_noise, 0.5f, 5.0f) * math::constrain(_airspeed_sample_delayed.eas2tas, 0.9f, 10.0f));
 	constexpr float initial_sideslip_uncertainty = math::radians(15.0f);
 	const float initial_wind_var_body_y = sq(_airspeed_sample_delayed.true_airspeed * sinf(initial_sideslip_uncertainty));
@@ -44,7 +44,7 @@

 #include <mathlib/mathlib.h>

-void Ekf::fuseDrag(const dragSample &drag_sample)
+void Ekf::fuseDrag()
 {
 	SparseVector24f<0,1,2,3,4,5,6,22,23> Hfusion;  // Observation Jacobians
 	Vector24f Kfusion; // Kalman gain vector
@@ -66,19 +66,19 @@ void Ekf::fuseDrag(const dragSample &drag_sample)
 	}

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

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

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

 	// predicted specific forces
 	// calculate relative wind velocity in earth frame and rotate into body frame
@@ -89,7 +89,7 @@ void Ekf::fuseDrag(const dragSample &drag_sample)
 	// perform sequential fusion of XY specific forces
 	for (uint8_t axis_index = 0; axis_index < 2; axis_index++) {
 		// measured drag acceleration corrected for sensor bias
-		const float mea_acc = drag_sample.accelXY(axis_index)  - _state.delta_vel_bias(axis_index) / _dt_ekf_avg;
+		const float mea_acc = _drag_sample_delayed.accelXY(axis_index)  - _state.delta_vel_bias(axis_index) / _dt_ekf_avg;

 		// predicted drag force sign is opposite to predicted wind relative velocity
 		const float drag_sign = (rel_wind_body(axis_index) >= 0.f) ? -1.f : 1.f;
@@ -261,6 +261,7 @@ void Ekf::predictState()

 	// rotate the previous quaternion by the delta quaternion using a quaternion multiplication
 	_state.quat_nominal = (_state.quat_nominal * dq).normalized();
+
 	_R_to_earth = Dcmf(_state.quat_nominal);

 	// Calculate an earth frame delta velocity
@@ -420,6 +420,9 @@ private:
 	Vector3f _delta_vel_bias_var_accum{};		///< kahan summation algorithm accumulator for delta velocity bias variance
 	Vector3f _delta_angle_bias_var_accum{};	///< kahan summation algorithm accumulator for delta angle bias variance

+	Vector3f _last_vel_obs{};			///< last velocity observation (m/s)
+	Vector3f _last_vel_obs_var{};		///< last velocity observation variance (m/s)**2
+	Vector2f _last_fail_hvel_innov{};		///< last failed horizontal velocity innovation (m/s)**2
 	float _vert_pos_innov_ratio{0.f};	///< vertical position innovation divided by estimated standard deviation of innovation
 	uint64_t _vert_pos_fuse_attempt_time_us{0};	///< last system time in usec vertical position measurement fuson was attempted
 	float _vert_vel_innov_ratio{0.f};		///< standard deviation of vertical velocity innovation
@@ -619,7 +622,7 @@ private:
 	void fuseSideslip();

 	// fuse body frame drag specific forces for multi-rotor wind estimation
-	void fuseDrag(const dragSample &drag_sample);
+	void fuseDrag();

 	void fuseBaroHgt();
 	void fuseGpsHgt();
@@ -653,23 +656,23 @@ private:

 	void resetHorizontalPositionTo(const Vector2f &new_horz_pos);

-	void resetVerticalPositionTo(float new_vert_pos);
+	void resetVerticalPositionTo(const float new_vert_pos);

 	void resetHeight();

 	// fuse optical flow line of sight rate measurements
 	void fuseOptFlow();

-	bool fuseHorizontalVelocity(const Vector3f &innov, float innov_gate, const Vector3f &obs_var,
+	bool fuseHorizontalVelocity(const Vector3f &innov, const Vector2f &innov_gate, const Vector3f &obs_var,
 				    Vector3f &innov_var, Vector2f &test_ratio);

-	bool fuseVerticalVelocity(const Vector3f &innov, float innov_gate, const Vector3f &obs_var,
+	bool fuseVerticalVelocity(const Vector3f &innov, const Vector2f &innov_gate, const Vector3f &obs_var,
 				  Vector3f &innov_var, Vector2f &test_ratio);

-	bool fuseHorizontalPosition(const Vector3f &innov, float innov_gate, const Vector3f &obs_var,
+	bool fuseHorizontalPosition(const Vector3f &innov, const Vector2f &innov_gate, const Vector3f &obs_var,
 				    Vector3f &innov_var, Vector2f &test_ratiov, bool inhibit_gate = false);

-	bool fuseVerticalPosition(float innov, float innov_gate, float obs_var,
+	bool fuseVerticalPosition(const float innov, const float innov_gate, const float obs_var,
 				  float &innov_var, float &test_ratio);

 	void fuseGpsVelPos();
@@ -428,7 +428,7 @@ bool Ekf::realignYawGPS(const Vector3f &mag)
 		if (!_control_status.flags.mag_aligned_in_flight) {
 			// This is our first flight alignment so we can assume that the recent change in velocity has occurred due to a
 			// forward direction takeoff or launch and therefore the inertial and GPS ground course discrepancy is due to yaw error
-			const float current_yaw = getEulerYaw(_R_to_earth);
+			const float current_yaw = getEuler321Yaw(_state.quat_nominal);
 			yaw_new = current_yaw + courseYawError;
 			_control_status.flags.mag_aligned_in_flight = true;

@@ -454,6 +454,7 @@ bool Ekf::realignYawGPS(const Vector3f &mag)

 		// Use the last magnetometer measurements to reset the field states
 		_state.mag_B.zero();
+		_R_to_earth = Dcmf(_state.quat_nominal);
 		_state.mag_I = _R_to_earth * mag;

 		resetMagCov();
@@ -540,7 +541,7 @@ bool Ekf::resetMagHeading(bool increase_yaw_var, bool update_buffer)

 bool Ekf::resetYawToEv()
 {
-	const float yaw_new = getEulerYaw(_ev_sample_delayed.quat);
+	const float yaw_new = getEuler312Yaw(_ev_sample_delayed.quat);
 	const float yaw_new_variance = fmaxf(_ev_sample_delayed.angVar, sq(1.0e-2f));

 	resetQuatStateYaw(yaw_new, yaw_new_variance, true);
@@ -1694,8 +1695,10 @@ void Ekf::resetQuatStateYaw(float yaw, float yaw_variance, bool update_buffer)
 	const Quatf quat_before_reset = _state.quat_nominal;

 	// update transformation matrix from body to world frame using the current estimate
+	_R_to_earth = Dcmf(_state.quat_nominal);
+
 	// update the rotation matrix using the new yaw value
-	_R_to_earth = updateYawInRotMat(yaw, Dcmf(_state.quat_nominal));
+	_R_to_earth = updateYawInRotMat(yaw, _R_to_earth);

 	// calculate the amount that the quaternion has changed by
 	const Quatf quat_after_reset(_R_to_earth);
@@ -295,7 +295,9 @@ protected:
 	flowSample _flow_sample_delayed{};
 	extVisionSample _ev_sample_delayed{};
 	extVisionSample _ev_sample_delayed_prev{};
+	dragSample _drag_sample_delayed{};
 	dragSample _drag_down_sampled{};	// down sampled drag specific force data (filter prediction rate -> observation rate)
+	auxVelSample _auxvel_sample_delayed{};

 	float _air_density{CONSTANTS_AIR_DENSITY_SEA_LEVEL_15C};		// air density (kg/m**3)

--- a/Show More
+++ b/Show More