drivers/rc_input: cleanup and simplify data processing per type

- don't store data in class
drivers/rc_input: poll file descriptor to minimize latency
2026-06-01 08:00:05 +08:00 · 2022-02-09 17:11:11 -05:00 · 2022-02-09 16:48:08 -05:00 · 2022-02-09 13:11:52 -05:00 · 2022-02-09 13:11:52 -05:00 · 2022-02-09 11:39:47 -05:00
158 changed files with 3291 additions and 3077 deletions
@@ -73,10 +73,8 @@ 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'
                }
              }
@@ -145,10 +143,8 @@ 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'
                }
              }
@@ -217,10 +213,8 @@ 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'
                }
              }
@@ -288,10 +282,8 @@ 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'
                }
              }
@@ -360,10 +352,8 @@ 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'
                }
              }
@@ -452,10 +442,8 @@ 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'
                }
              }
@@ -536,10 +524,8 @@ 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'
                }
              }
@@ -608,10 +594,8 @@ 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'
                }
              }
@@ -680,10 +664,8 @@ 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'
                }
              }
@@ -793,7 +775,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"'
+  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 "ver all"'
  sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-*` --cmd "work_queue status"'
  // stop logger
@@ -809,7 +791,7 @@ void resetParameters() {
 void runTests() {

  // test loading a range of airframes
-  sh './Tools/HIL/test_airframes.sh `find /dev/serial -name *usb-*` 1000 1001 2100 3000 4001 4018 6001 8001 10016'
+  sh './Tools/HIL/test_airframes.sh `find /dev/serial -name *usb-*` 1000 1001 2100 3000 4001 6001 8001 10016'

  resetParameters()

@@ -862,7 +844,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"'
+  sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-*` --cmd "uorb top -1 -a" || true'

  // 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,6 +44,8 @@ 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
-	vmount start
+	gimbal start
 fi

 if param greater -s TRIG_MODE 0
@@ -24,6 +24,27 @@

 . ${R}etc/init.d/rc.vtol_defaults

+param set-default CA_AIRFRAME 2
+param set-default CA_ROTOR_COUNT 4
+param set-default CA_ROTOR0_PX 0.15
+param set-default CA_ROTOR0_PY 0.15
+param set-default CA_ROTOR1_PX -0.15
+param set-default CA_ROTOR1_PY -0.15
+param set-default CA_ROTOR2_PX 0.15
+param set-default CA_ROTOR2_PY -0.15
+param set-default CA_ROTOR2_KM -0.05
+param set-default CA_ROTOR3_PX -0.15
+param set-default CA_ROTOR3_PY 0.15
+param set-default CA_ROTOR3_KM -0.05
+param set-default CA_SV_CS_COUNT 4
+param set-default CA_SV_CS0_TYPE 1
+param set-default CA_SV_CS0_TRQ_R -0.5
+param set-default CA_SV_CS1_TYPE 2
+param set-default CA_SV_CS1_TRQ_R 0.5
+param set-default CA_SV_CS2_TYPE 3
+param set-default CA_SV_CS2_TRQ_P 1.0
+param set-default CA_SV_CS3_TRQ_Y 1.0
+param set-default CA_SV_CS3_TYPE 4

 param set-default PWM_AUX_DIS5 950

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

 . ${R}etc/init.d/rc.vtol_defaults

+param set-default CA_AIRFRAME 4
+param set-default CA_ROTOR_COUNT 4
+param set-default CA_ROTOR0_PX 0.15
+param set-default CA_ROTOR0_PY 0.15
+param set-default CA_ROTOR1_PX -0.15
+param set-default CA_ROTOR1_PY -0.15
+param set-default CA_ROTOR2_PX 0.15
+param set-default CA_ROTOR2_PY -0.15
+param set-default CA_ROTOR2_KM -0.05
+param set-default CA_ROTOR3_PX -0.15
+param set-default CA_ROTOR3_PY 0.15
+param set-default CA_ROTOR3_KM -0.05
+param set-default CA_SV_CS_COUNT 2
+param set-default CA_SV_CS0_TRQ_P 0.5
+param set-default CA_SV_CS0_TRQ_Y 0.5
+param set-default CA_SV_CS0_TYPE 5
+param set-default CA_SV_CS1_TRQ_P 0.5
+param set-default CA_SV_CS1_TRQ_Y -0.5
+param set-default CA_SV_CS1_TYPE 6

 param set-default PWM_MAIN_MAX 2000

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

 set MIXER quad_x
@@ -23,6 +23,20 @@ param set-default VT_ELEV_MC_LOCK 0
 param set-default VT_MOT_COUNT 2
 param set-default VT_TYPE 0

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

 . ${R}etc/init.d/rc.fw_defaults

+param set-default CA_AIRFRAME 1
+param set-default CA_ROTOR_COUNT 1
+param set-default CA_ROTOR0_PX 0.3
+param set-default CA_SV_CS_COUNT 4
+param set-default CA_SV_CS0_TRQ_R -0.5
+param set-default CA_SV_CS1_TRQ_R 0.5
+param set-default CA_SV_CS1_TYPE 2
+param set-default CA_SV_CS2_TRQ_P 1.0
+param set-default CA_SV_CS2_TYPE 3
+param set-default CA_SV_CS3_TRQ_Y 1.0
+param set-default CA_SV_CS3_TYPE 4
+
 param set-default PWM_AUX_RATE 50
 param set-default PWM_MAIN_RATE 50

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

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


 # gimbal configuration
-param set-default MNT_MODE_IN 1
+param set-default MNT_MODE_IN 0
 param set-default MNT_MODE_OUT 1
 param set-default MNT_MAN_PITCH 2
 param set-default MNT_RC_IN_MODE 1
@@ -23,6 +23,23 @@

 . ${R}etc/init.d/rc.mc_defaults

+param set-default CA_ROTOR_COUNT 6
+param set-default CA_ROTOR0_PX 0.0
+param set-default CA_ROTOR0_PY 0.5
+param set-default CA_ROTOR0_KM -0.05
+param set-default CA_ROTOR1_PX 0.0
+param set-default CA_ROTOR1_PY -0.5
+param set-default CA_ROTOR2_PX 0.43
+param set-default CA_ROTOR2_PY -0.25
+param set-default CA_ROTOR2_KM -0.05
+param set-default CA_ROTOR3_PX -0.43
+param set-default CA_ROTOR3_PY 0.25
+param set-default CA_ROTOR4_PX 0.43
+param set-default CA_ROTOR4_PY 0.25
+param set-default CA_ROTOR5_PX -0.43
+param set-default CA_ROTOR5_PY -0.25
+param set-default CA_ROTOR5_KM -0.05
+
 set MIXER hexa_x

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

 . ${R}etc/init.d/rc.mc_defaults

+param set-default CA_ROTOR_COUNT 6
+param set-default CA_ROTOR0_PX 0.5
+param set-default CA_ROTOR0_PY 0.0
+param set-default CA_ROTOR0_KM -0.05
+param set-default CA_ROTOR1_PX -0.5
+param set-default CA_ROTOR1_PY 0.0
+param set-default CA_ROTOR2_PX -0.25
+param set-default CA_ROTOR2_PY -0.43
+param set-default CA_ROTOR2_KM -0.05
+param set-default CA_ROTOR3_PX 0.25
+param set-default CA_ROTOR3_PY 0.43
+param set-default CA_ROTOR4_PX 0.25
+param set-default CA_ROTOR4_PY -0.43
+param set-default CA_ROTOR5_PX -0.25
+param set-default CA_ROTOR5_PY 0.43
+param set-default CA_ROTOR5_KM -0.05
+
 set MIXER hexa_+

 # Need to set all 8 channels
@@ -25,6 +25,28 @@

 . ${R}etc/init.d/rc.mc_defaults

+param set-default CA_ROTOR_COUNT 8
+param set-default CA_ROTOR0_KM -0.05
+param set-default CA_ROTOR0_PX 0.46
+param set-default CA_ROTOR0_PY 0.19
+param set-default CA_ROTOR1_KM -0.05
+param set-default CA_ROTOR1_PX -0.46
+param set-default CA_ROTOR1_PY -0.19
+param set-default CA_ROTOR2_PX 0.19
+param set-default CA_ROTOR2_PY 0.46
+param set-default CA_ROTOR3_PX -0.46
+param set-default CA_ROTOR3_PY 0.19
+param set-default CA_ROTOR4_PX 0.46
+param set-default CA_ROTOR4_PY -0.19
+param set-default CA_ROTOR5_PX -0.19
+param set-default CA_ROTOR5_PY -0.46
+param set-default CA_ROTOR6_KM -0.05
+param set-default CA_ROTOR6_PX 0.19
+param set-default CA_ROTOR6_PY -0.46
+param set-default CA_ROTOR7_KM -0.05
+param set-default CA_ROTOR7_PX -0.19
+param set-default CA_ROTOR7_PY 0.46
+
 set MIXER octo_x

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

 	# [7000, 7999] Hexarotor +"
 	7001_hexa_+
@@ -486,11 +486,11 @@ else
 	. ${R}etc/init.d/rc.thermal_cal

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

 	# Check for flow sensor
@@ -35,7 +35,6 @@ 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
-vmount,CONFIG_MODULES_VMOUNT=y
+gimbal,CONFIG_MODULES_GIMBAL=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_VMOUNT=y
+CONFIG_MODULES_GIMBAL=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_VMOUNT=y
+CONFIG_MODULES_GIMBAL=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_VMOUNT=y
+CONFIG_MODULES_GIMBAL=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_VMOUNT=y
+CONFIG_MODULES_GIMBAL=y
 CONFIG_MODULES_VTOL_ATT_CONTROL=y
 CONFIG_SYSTEMCMDS_DYN=y
 CONFIG_SYSTEMCMDS_LED_CONTROL=y
@@ -17,7 +17,6 @@ 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_VMOUNT=y
+CONFIG_MODULES_GIMBAL=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_VMOUNT=y
+CONFIG_MODULES_GIMBAL=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_VMOUNT=y
+CONFIG_MODULES_GIMBAL=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_VMOUNT=y
+CONFIG_MODULES_GIMBAL=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_VMOUNT=y
+CONFIG_MODULES_GIMBAL=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_VMOUNT=y
+CONFIG_MODULES_GIMBAL=y
 CONFIG_MODULES_VTOL_ATT_CONTROL=y
 CONFIG_SYSTEMCMDS_BL_UPDATE=y
 CONFIG_SYSTEMCMDS_ACTUATOR_TEST=y
@@ -21,7 +21,6 @@ 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_VMOUNT=y
+CONFIG_MODULES_GIMBAL=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_VMOUNT=y
+CONFIG_MODULES_GIMBAL=y
 CONFIG_MODULES_VTOL_ATT_CONTROL=y
 CONFIG_SYSTEMCMDS_BL_UPDATE=y
 CONFIG_SYSTEMCMDS_ACTUATOR_TEST=y
@@ -2,6 +2,7 @@ CONFIG_BOARD_TOOLCHAIN="arm-none-eabi"
 CONFIG_BOARD_ARCHITECTURE="cortex-m4"
 CONFIG_BOARD_ROMFSROOT="cannode"
 CONFIG_BOARD_CONSTRAINED_MEMORY=y
+CONFIG_BOARD_SERIAL_GPS1="/dev/ttyS2"
 CONFIG_DRIVERS_BAROMETER_DPS310=y
 CONFIG_DRIVERS_BOOTLOADERS=y
 CONFIG_DRIVERS_GPS=y
@@ -15,6 +16,7 @@ CONFIG_SYSTEMCMDS_NSHTERM=y
 CONFIG_SYSTEMCMDS_PARAM=y
 CONFIG_SYSTEMCMDS_REBOOT=y
 CONFIG_SYSTEMCMDS_SERIAL_PASSTHRU=y
+CONFIG_SERIAL_PASSTHRU_UBLOX=y
 CONFIG_SYSTEMCMDS_TOP=y
 CONFIG_SYSTEMCMDS_TOPIC_LISTENER=y
 CONFIG_SYSTEMCMDS_UORB=y
@@ -3,8 +3,6 @@
 # Matek M9NF4 CAN specific board sensors init
 #------------------------------------------------------------------------------

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

 rm3100 -b 2 -s start
@@ -1,23 +0,0 @@
-menu "SERIAL_PASSTHRU Configuration"
-config SERIAL_PASSTHRU_UBLOX
-	bool "Detect and Auto Connect on U-Center messages"
-	default n
-	---help---
-		This option will enable the cdc_acm_check to launch
-		The passthru driver.
-
-config SERIAL_PASSTHRU_UBLOX_DEV
-	string "Device path of the GPS"
-	depends on SERIAL_PASSTHRU_UBLOX
-	default "/dev/ttyS2"
-	---help---
-		This is the path of the device used as the right side
-		of the passthru.
-
-config SERIAL_PASSTHRU_UBLOX_BAUDRATE
-	string "baudrate"
-	depends on SERIAL_PASSTHRU_UBLOX
-	default "115200"
-	---help---
-		This option sets the baudrate for the passthru.
-endmenu
@@ -52,7 +52,7 @@ CONFIG_MODULES_NAVIGATOR=y
 CONFIG_MODULES_RC_UPDATE=y
 CONFIG_MODULES_SENSORS=y
 CONFIG_MODULES_TEMPERATURE_COMPENSATION=y
-CONFIG_MODULES_VMOUNT=y
+CONFIG_MODULES_GIMBAL=y
 CONFIG_SYSTEMCMDS_BL_UPDATE=y
 CONFIG_SYSTEMCMDS_ACTUATOR_TEST=y
 CONFIG_SYSTEMCMDS_DMESG=y
@@ -1,10 +1,10 @@
 {
-    "board_id": 139,
+    "board_id": 1013,
    "magic": "PX4FWv1",
-    "description": "Firmware for the MatekH743-slim board",
+    "description": "Firmware for the MatekH743 board",
    "image": "",
    "build_time": 0,
-    "summary": "MatekH743-slim",
+    "summary": "MatekH743",
    "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=0x004b
-CONFIG_CDCACM_PRODUCTSTR="Matek H743-slim"
+CONFIG_CDCACM_PRODUCTID=0x1013
+CONFIG_CDCACM_PRODUCTSTR="MatekH743"
 CONFIG_CDCACM_RXBUFSIZE=600
 CONFIG_CDCACM_TXBUFSIZE=12000
-CONFIG_CDCACM_VENDORID=0x3162
+CONFIG_CDCACM_VENDORID=0x1209
 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=0x0036
-CONFIG_CDCACM_PRODUCTSTR="MatekH743-slim"
+CONFIG_CDCACM_PRODUCTID=0x1013
+CONFIG_CDCACM_PRODUCTSTR="MatekH743"
 CONFIG_CDCACM_RXBUFSIZE=600
 CONFIG_CDCACM_TXBUFSIZE=12000
-CONFIG_CDCACM_VENDORID=0x1B8C
-CONFIG_CDCACM_VENDORSTR="PX4"
+CONFIG_CDCACM_VENDORID=0x1209
+CONFIG_CDCACM_VENDORSTR="Matek"
 CONFIG_CLOCK_MONOTONIC=y
 CONFIG_DEBUG_FULLOPT=y
 CONFIG_DEBUG_HARDFAULT_ALERT=y
@@ -119,9 +119,9 @@

 /* Spare GPIO */

-// #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)
+#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)


 /* Tone alarm output */
@@ -174,6 +174,8 @@
 		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                     139
+#define BOARD_TYPE                     1013
 #define _FLASH_KBYTES                  (*(uint32_t *)0x1FF1E880)
 #define BOARD_FLASH_SECTORS            (15)
 #define BOARD_FLASH_SIZE               (_FLASH_KBYTES * 1024)
@@ -60,6 +60,10 @@
 #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);
@@ -174,6 +178,25 @@ __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_VMOUNT=y
+CONFIG_MODULES_GIMBAL=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_VMOUNT=y
+CONFIG_MODULES_GIMBAL=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_VMOUNT=y
+CONFIG_MODULES_GIMBAL=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_VMOUNT=y
+CONFIG_MODULES_GIMBAL=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_VMOUNT=y
+CONFIG_MODULES_GIMBAL=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_VMOUNT=y
+CONFIG_MODULES_GIMBAL=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_VMOUNT=y
+CONFIG_MODULES_GIMBAL=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_VMOUNT=y
+CONFIG_MODULES_GIMBAL=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_VMOUNT=y
+CONFIG_MODULES_GIMBAL=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_VMOUNT=y
+CONFIG_MODULES_GIMBAL=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_VMOUNT=y
+CONFIG_MODULES_GIMBAL=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_VMOUNT=y
+CONFIG_MODULES_GIMBAL=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_VMOUNT=y
+CONFIG_MODULES_GIMBAL=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_VMOUNT=y
+CONFIG_MODULES_GIMBAL=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 start
+	mpu9250 -s -R 8 -M 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_VMOUNT=y
+CONFIG_MODULES_GIMBAL=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_VMOUNT=n
+CONFIG_MODULES_GIMBAL=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_VMOUNT=y
+CONFIG_MODULES_GIMBAL=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_VMOUNT=n
+CONFIG_MODULES_GIMBAL=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_VMOUNT=y
+CONFIG_MODULES_GIMBAL=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_VMOUNT=y
+CONFIG_MODULES_GIMBAL=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_VMOUNT=y
+CONFIG_MODULES_GIMBAL=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_VMOUNT=y
+CONFIG_MODULES_GIMBAL=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_VMOUNT=y
+CONFIG_MODULES_GIMBAL=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_VMOUNT=y
+CONFIG_MODULES_GIMBAL=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_VMOUNT=y
+CONFIG_MODULES_GIMBAL=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_VMOUNT=y
+CONFIG_MODULES_GIMBAL=y
 CONFIG_SYSTEMCMDS_BL_UPDATE=y
 CONFIG_SYSTEMCMDS_ACTUATOR_TEST=y
 CONFIG_SYSTEMCMDS_DMESG=y
@@ -11,6 +11,7 @@ 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}
@@ -47,6 +48,11 @@ 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,3 +16,5 @@ float32 angular_velocity_y
 float32 angular_velocity_z

 uint32 failure_flags
+
+bool received_from_mavlink
@@ -175,3 +175,5 @@ 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,3 +52,6 @@
 #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()
 {
-	delete[] _data;
+	free(_data);

 	const char *devname = get_devname();

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

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

 			unlock();
@@ -50,6 +50,24 @@ __END_DECLS

 #define USB_DEVICE_PATH "/dev/ttyACM0"

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

-								static const char *passthru_argv[] {"serial_passthru", "start", "-t", "-b", baudstring, "-e", USB_DEVICE_PATH, "-d", CONFIG_SERIAL_PASSTHRU_UBLOX_DEV,   nullptr};
+								static const char *passthru_argv[] {"serial_passthru", "start", "-t", "-b", baudstring, "-e", USB_DEVICE_PATH, "-d", SERIAL_PASSTHRU_UBLOX_DEV,   nullptr};
 #endif
 								char **exec_argv = nullptr;

@@ -241,7 +241,7 @@ typedef enum {
 	DShot_cmd_led4_off,     // BLHeli32 only
 	DShot_cmd_audio_stream_mode_on_off              = 30, // KISS audio Stream mode on/off
 	DShot_cmd_silent_mode_on_off                    = 31, // KISS silent Mode on/off
-	DShot_cmd_signal_line_telemeetry_disable        = 32,
+	DShot_cmd_signal_line_telemetry_disable         = 32,
 	DShot_cmd_signal_line_continuous_erpm_telemetry = 33,
 	DShot_cmd_MAX          = 47,     // >47 are throttle values
 	DShot_cmd_MIN_throttle = 48,
@@ -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 first antenna is in
+ * Set this to 0 if the antennas are parallel to the forward-facing direction of the vehicle and the rover antenna is in
 * front. The offset angle increases clockwise.
 *
- * Set this to 90 if the first antenna is placed on the right side and the second on the left side of the vehicle.
+ * 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.
 *
 * @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
-	transfer(cmd, cmd, sizeof(cmd));
 	px4_udelay(10);
+	transfer(cmd, cmd, sizeof(cmd));
 	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
-	transfer(cmd, cmd, sizeof(cmd));
 	px4_udelay(10);
+	transfer(cmd, cmd, sizeof(cmd));
 }

 void MPU6000::RegisterSetAndClearBits(Register reg, uint8_t setbits, uint8_t clearbits)
@@ -108,15 +108,18 @@ float BMM150::compensate_x(int16_t mag_data_x, uint16_t data_rhall)
 {
 	float retval = 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.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;
+	// 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;
+	}

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

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

 	return retval;
 }
@@ -142,35 +147,52 @@ float BMM150::compensate_z(int16_t mag_data_z, uint16_t data_rhall)
 {
 	float retval = 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.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;
+	// 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;
+	}

 	return retval;
 }

 static constexpr int16_t combine_xy_int13(const uint8_t msb, const uint8_t lsb)
 {
-	int16_t x = ((msb << 8) | lsb);
-	return x / 8; // arithmetic shift by 3 (13 bit signed integer)
+	// 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);
 }

 static constexpr int16_t combine_z_int15(const uint8_t msb, const uint8_t lsb)
 {
-	int16_t z = ((msb << 8) | lsb);
-	return z / 2; // arithmetic shift by 1 (15 bit signed integer)
+	// 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);
 }

 static constexpr uint16_t combine_rhall_uint14(const uint8_t msb, const uint8_t lsb)
 {
-	uint16_t rhall = ((msb << 8) | lsb);
-	return (rhall >> 2) & 0x3FFF; // 14 bit unsigned integer
+	// 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);
 }

 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, OP_MODE_BIT::Opmode_Sleep | OP_MODE_BIT::Self_Test },
+		{ 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::REPXY,         REPXY_BIT::XY_HA_SET, REPXY_BIT::XY_HA_CLEAR },
 		{ Register::REPZ,          REPZ_BIT::Z_HA_SET, REPZ_BIT::Z_HA_CLEAR },
 	};
@@ -96,11 +96,12 @@ enum POWER_CONTROL_BIT : uint8_t {
 // OP_MODE
 enum OP_MODE_BIT : uint8_t {
 	// 5:3 Data rate control
-	ODR_20Hz     = Bit5 | Bit3, // ODR 20 Hz
+	ODR_20HZ_SET   = Bit5 | Bit3,
+	ODR_20HZ_CLEAR = Bit4,

 	// 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 += payload_len;
+		i += Sp2HeaderSize + payload_len;

 	}
 }
@@ -1,6 +1,6 @@
 /****************************************************************************
 *
- *   Copyright (c) 2012-2021 PX4 Development Team. All rights reserved.
+ *   Copyright (c) 2012-2022 PX4 Development Team. All rights reserved.
 *
 * Redistribution and use in source and binary forms, with or without
 * modification, are permitted provided that the following conditions
@@ -33,9 +33,9 @@

 #include "RCInput.hpp"

-#include "crsf_telemetry.h"
 #include <uORB/topics/vehicle_command_ack.h>

+#include <poll.h>
 #include <termios.h>

 using namespace time_literals;
@@ -44,21 +44,8 @@ constexpr char const *RCInput::RC_SCAN_STRING[];

 RCInput::RCInput(const char *device) :
 	ModuleParams(nullptr),
-	ScheduledWorkItem(MODULE_NAME, px4::serial_port_to_wq(device)),
-	_cycle_perf(perf_alloc(PC_ELAPSED, MODULE_NAME": cycle time")),
-	_publish_interval_perf(perf_alloc(PC_INTERVAL, MODULE_NAME": publish interval"))
+	ScheduledWorkItem(MODULE_NAME, px4::serial_port_to_wq(device))
 {
-	// rc input, published to ORB
-	_rc_in.input_source = input_rc_s::RC_INPUT_SOURCE_PX4FMU_PPM;
-
-	// initialize it as RC lost
-	_rc_in.rc_lost = true;
-
-	// initialize raw_rc values and count
-	for (unsigned i = 0; i < input_rc_s::RC_INPUT_MAX_CHANNELS; i++) {
-		_raw_rc_values[i] = UINT16_MAX;
-	}
-
 	if (device) {
 		strncpy(_device, device, sizeof(_device) - 1);
 		_device[sizeof(_device) - 1] = '\0';
@@ -77,6 +64,7 @@ RCInput::~RCInput()
 	delete _ghst_telemetry;

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

@@ -88,6 +76,11 @@ RCInput::init()
 	RF_RADIO_POWER_CONTROL(true);
 #endif // RF_RADIO_POWER_CONTROL

+
+#if defined(RC_SERIAL_PORT)
+	_rc_serial_port_output = (strcmp(_device, RC_SERIAL_PORT) != 0);
+#endif // RC_SERIAL_PORT
+
 	// dsm_init sets some file static variables and returns a file descriptor
 	// it also powers on the radio if needed
 	_rcs_fd = dsm_init(_device);
@@ -164,89 +157,60 @@ RCInput::task_spawn(int argc, char *argv[])
 	_object.store(instance);
 	_task_id = task_id_is_work_queue;

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

 	return PX4_OK;
 }

-void
-RCInput::fill_rc_in(uint16_t raw_rc_count_local,
-		    uint16_t raw_rc_values_local[input_rc_s::RC_INPUT_MAX_CHANNELS],
-		    hrt_abstime now, bool frame_drop, bool failsafe,
-		    unsigned frame_drops, int rssi = -1)
+void RCInput::FillRssi(input_rc_s &input_rc)
 {
-	// fill rc_in struct for publishing
-	_rc_in.channel_count = raw_rc_count_local;
-
-	if (_rc_in.channel_count > input_rc_s::RC_INPUT_MAX_CHANNELS) {
-		_rc_in.channel_count = input_rc_s::RC_INPUT_MAX_CHANNELS;
-	}
-
-	unsigned valid_chans = 0;
-
-	for (unsigned i = 0; i < _rc_in.channel_count; i++) {
-		_rc_in.values[i] = raw_rc_values_local[i];
-
-		if (raw_rc_values_local[i] != UINT16_MAX) {
-			valid_chans++;
-		}
-
-		// once filled, reset values back to default
-		_raw_rc_values[i] = UINT16_MAX;
-	}
-
-	_rc_in.timestamp = now;
-	_rc_in.timestamp_last_signal = _rc_in.timestamp;
-	_rc_in.rc_ppm_frame_length = 0;
-
-	/* fake rssi if no value was provided */
-	if (rssi == -1) {
-		if ((_param_rc_rssi_pwm_chan.get() > 0) && (_param_rc_rssi_pwm_chan.get() < _rc_in.channel_count)) {
+	if (input_rc.rssi < 0 || input_rc.rssi > input_rc_s::RSSI_MAX) {
+		if ((_param_rc_rssi_pwm_chan.get() > 0) && (_param_rc_rssi_pwm_chan.get() < input_rc.channel_count)) {
 			const int32_t rssi_pwm_chan = _param_rc_rssi_pwm_chan.get();
 			const int32_t rssi_pwm_min = _param_rc_rssi_pwm_min.get();
 			const int32_t rssi_pwm_max = _param_rc_rssi_pwm_max.get();

 			// get RSSI from input channel
-			int rc_rssi = ((_rc_in.values[rssi_pwm_chan - 1] - rssi_pwm_min) * 100) / (rssi_pwm_max - rssi_pwm_min);
-			_rc_in.rssi = math::constrain(rc_rssi, 0, 100);
+			int rc_rssi = ((input_rc.values[rssi_pwm_chan - 1] - rssi_pwm_min) * 100) / (rssi_pwm_max - rssi_pwm_min);
+
+			input_rc.rssi = math::constrain(rc_rssi, 0, (int)input_rc_s::RSSI_MAX);
+
+#if defined(ADC_RC_RSSI_CHANNEL)

 		} else if (_analog_rc_rssi_stable) {
 			// set RSSI if analog RSSI input is present
 			float rssi_analog = ((_analog_rc_rssi_volt - 0.2f) / 3.0f) * 100.0f;

-			if (rssi_analog > 100.0f) {
-				rssi_analog = 100.0f;
-			}
-
-			if (rssi_analog < 0.0f) {
-				rssi_analog = 0.0f;
-			}
-
-			_rc_in.rssi = rssi_analog;
-
-		} else {
-			_rc_in.rssi = 255;
+			input_rc.rssi = math::constrain((int)roundf(rssi_analog), 0, (int)input_rc_s::RSSI_MAX);
+#endif // ADC_RC_RSSI_CHANNEL
 		}
-
-	} else {
-		_rc_in.rssi = rssi;
 	}
-
-	if (valid_chans == 0) {
-		_rc_in.rssi = 0;
-	}
-
-	_rc_in.rc_failsafe = failsafe;
-	_rc_in.rc_lost = (valid_chans == 0);
-	_rc_in.rc_lost_frame_count = frame_drops;
-	_rc_in.rc_total_frame_count = 0;
 }

-void RCInput::set_rc_scan_state(RC_SCAN newState)
+void RCInput::PublishInputRc(input_rc_s &input_rc)
 {
-	PX4_DEBUG("RCscan: %s failed, trying %s", RCInput::RC_SCAN_STRING[_rc_scan_state], RCInput::RC_SCAN_STRING[newState]);
+	FillRssi(input_rc); // requires input_rc.values[]
+
+	input_rc.timestamp = hrt_absolute_time();
+	_input_rc_pub.publish(input_rc);
+
+	perf_count(_publish_interval_perf);
+
+	_last_publish_time = input_rc.timestamp;
+	_rc_scan_locked = true;
+}
+
+void RCInput::set_next_rc_scan_state()
+{
+	int new_state = _rc_scan_state + 1;
+
+	if (new_state >= RC_SCAN::RC_SCAN_MAX) {
+		new_state = 0;
+	}
+
+	PX4_DEBUG("RC scan: %s failed, trying %s", RCInput::RC_SCAN_STRING[_rc_scan_state], RCInput::RC_SCAN_STRING[new_state]);
 	_rc_scan_begin = 0;
-	_rc_scan_state = newState;
+	_rc_scan_state = static_cast<RC_SCAN>(new_state);
 	_rc_scan_locked = false;

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

 	} else {

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

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

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

@@ -370,9 +329,8 @@ void RCInput::Run()
 			if (_adc_report_sub.copy(&adc)) {
 				for (unsigned i = 0; i < PX4_MAX_ADC_CHANNELS; ++i) {
 					if (adc.channel_id[i] == ADC_RC_RSSI_CHANNEL) {
-						float adc_volt = adc.raw_data[i] *
-								 adc.v_ref /
-								 adc.resolution;
+
+						float adc_volt = adc.raw_data[i] * adc.v_ref / adc.resolution;

 						if (_analog_rc_rssi_volt < 0.0f) {
 							_analog_rc_rssi_volt = adc_volt;
@@ -391,18 +349,25 @@ void RCInput::Run()

 #endif // ADC_RC_RSSI_CHANNEL

-		bool rc_updated = false;

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

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

-		// TODO: needs work (poll _rcs_fd)
-		// int ret = poll(fds, sizeof(fds) / sizeof(fds[0]), 100);
-		// then update priority to SCHED_PRIORITY_FAST_DRIVER
-		// read all available data from the serial RC input UART
+		perf_begin(_cycle_perf);
+		perf_count(_cycle_interval_perf);
+
+		if (ret < 0) {
+			PX4_DEBUG("poll error %d", ret);
+		}
+
+		const hrt_abstime cycle_timestamp = hrt_absolute_time();

 		// read all available data from the serial RC input UART
 		int newBytes = ::read(_rcs_fd, &_rcs_buf[0], RC_MAX_BUFFER_SIZE);
@@ -411,7 +376,48 @@ void RCInput::Run()
 			_bytes_rx += newBytes;
 		}

+		bool rc_updated = false;
+
 		switch (_rc_scan_state) {
+#if defined(HRT_PPM_CHANNEL)
+
+		case RC_SCAN_PPM:
+			if (_rc_scan_begin == 0) {
+				_rc_scan_begin = cycle_timestamp;
+				// Configure timer input pin for CPPM
+				px4_arch_configgpio(GPIO_PPM_IN);
+
+			} else if (_rc_scan_locked || (cycle_timestamp - _rc_scan_begin < rc_scan_max)) {
+				// see if we have new PPM input data
+				if ((ppm_last_valid_decode != _last_publish_time) && (ppm_decoded_channels >= 4)) {
+					// we have a new PPM frame. Publish it.
+					rc_updated = true;
+
+					input_rc_s input_rc{};
+					input_rc.timestamp_last_signal = ppm_last_valid_decode;
+					input_rc.channel_count = math::max(ppm_decoded_channels, (unsigned)input_rc_s::RC_INPUT_MAX_CHANNELS);
+					input_rc.rc_lost = (ppm_decoded_channels == 0);
+					input_rc.input_source = input_rc_s::RC_INPUT_SOURCE_PX4FMU_PPM;
+					input_rc.rc_ppm_frame_length = ppm_frame_length;
+
+					for (int i = 0; i < input_rc.channel_count; i++) {
+						input_rc.values[i] = ppm_buffer[i];
+					}
+
+					PublishInputRc(input_rc);
+				}
+
+			} else {
+				// disable CPPM input by mapping it away from the timer capture input
+				px4_arch_unconfiggpio(GPIO_PPM_IN);
+
+				// Scan the next protocol
+				set_next_rc_scan_state();
+			}
+
+			break;
+#endif  // HRT_PPM_CHANNEL
+
 		case RC_SCAN_SBUS:
 			if (_rc_scan_begin == 0) {
 				_rc_scan_begin = cycle_timestamp;
@@ -423,30 +429,40 @@ void RCInput::Run()
 				tcflush(_rcs_fd, TCIOFLUSH);
 				memset(_rcs_buf, 0, sizeof(_rcs_buf));

-			} else if (_rc_scan_locked
-				   || cycle_timestamp - _rc_scan_begin < rc_scan_max) {
-
-				// parse new data
+			} else if (_rc_scan_locked || (cycle_timestamp - _rc_scan_begin < rc_scan_max)) {
 				if (newBytes > 0) {
+					// parse new data
+					uint16_t raw_rc_values[input_rc_s::RC_INPUT_MAX_CHANNELS];
+					uint16_t raw_rc_count = 0;
 					bool sbus_failsafe = false;
 					bool sbus_frame_drop = false;
+					unsigned frame_drops = 0;

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

 					if (rc_updated) {
 						// we have a new SBUS frame. Publish it.
-						_rc_in.input_source = input_rc_s::RC_INPUT_SOURCE_PX4FMU_SBUS;
-						fill_rc_in(_raw_rc_count, _raw_rc_values, cycle_timestamp,
-							   sbus_frame_drop, sbus_failsafe, frame_drops);
-						_rc_scan_locked = true;
+						input_rc_s input_rc{};
+						input_rc.timestamp_last_signal = cycle_timestamp;
+						input_rc.channel_count = math::max(raw_rc_count, (uint16_t)input_rc_s::RC_INPUT_MAX_CHANNELS);
+						input_rc.rc_failsafe = sbus_failsafe;
+						input_rc.rc_lost = (raw_rc_count == 0);
+						input_rc.rc_lost_frame_count = frame_drops;
+						input_rc.input_source = input_rc_s::RC_INPUT_SOURCE_PX4FMU_SBUS;
+
+						for (int i = 0; i < input_rc.channel_count; i++) {
+							input_rc.values[i] = raw_rc_values[i];
+						}
+
+						PublishInputRc(input_rc);
 					}
 				}

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

 			break;
@@ -461,29 +477,38 @@ void RCInput::Run()
 				tcflush(_rcs_fd, TCIOFLUSH);
 				memset(_rcs_buf, 0, sizeof(_rcs_buf));

-			} else if (_rc_scan_locked
-				   || cycle_timestamp - _rc_scan_begin < rc_scan_max) {
-
+			} else if (_rc_scan_locked || (cycle_timestamp - _rc_scan_begin < rc_scan_max)) {
 				if (newBytes > 0) {
-					int8_t dsm_rssi = 0;
-					bool dsm_11_bit = false;
-
 					// parse new data
-					rc_updated = dsm_parse(cycle_timestamp, &_rcs_buf[0], newBytes, &_raw_rc_values[0], &_raw_rc_count,
+					uint16_t raw_rc_values[input_rc_s::RC_INPUT_MAX_CHANNELS];
+					uint16_t raw_rc_count = 0;
+					bool dsm_11_bit = false;
+					unsigned frame_drops = 0;
+					int8_t dsm_rssi = 0;
+
+					rc_updated = dsm_parse(cycle_timestamp, &_rcs_buf[0], newBytes, &raw_rc_values[0], &raw_rc_count,
 							       &dsm_11_bit, &frame_drops, &dsm_rssi, input_rc_s::RC_INPUT_MAX_CHANNELS);

 					if (rc_updated) {
 						// we have a new DSM frame. Publish it.
-						_rc_in.input_source = input_rc_s::RC_INPUT_SOURCE_PX4FMU_DSM;
-						fill_rc_in(_raw_rc_count, _raw_rc_values, cycle_timestamp,
-							   false, false, frame_drops, dsm_rssi);
-						_rc_scan_locked = true;
+						input_rc_s input_rc{};
+						input_rc.timestamp_last_signal = cycle_timestamp;
+						input_rc.channel_count = math::max(raw_rc_count, (uint16_t)input_rc_s::RC_INPUT_MAX_CHANNELS);
+						input_rc.rc_lost = (raw_rc_count == 0);
+						input_rc.rc_lost_frame_count = frame_drops;
+						input_rc.input_source = input_rc_s::RC_INPUT_SOURCE_PX4FMU_DSM;
+
+						for (int i = 0; i < input_rc.channel_count; i++) {
+							input_rc.values[i] = raw_rc_values[i];
+						}
+
+						PublishInputRc(input_rc);
 					}
 				}

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

 			break;
@@ -498,43 +523,43 @@ void RCInput::Run()
 				tcflush(_rcs_fd, TCIOFLUSH);
 				memset(_rcs_buf, 0, sizeof(_rcs_buf));

-			} else if (_rc_scan_locked
-				   || cycle_timestamp - _rc_scan_begin < rc_scan_max) {
-
+			} else if (_rc_scan_locked || (cycle_timestamp - _rc_scan_begin < rc_scan_max)) {
 				if (newBytes > 0) {
 					// parse new data
-					uint8_t st24_rssi, lost_count;
-
-					rc_updated = false;
+					uint8_t st24_rssi = 0;
+					uint8_t lost_count = 0;
+					uint16_t raw_rc_count = 0;
+					uint16_t raw_rc_values[input_rc_s::RC_INPUT_MAX_CHANNELS];

 					for (unsigned i = 0; i < (unsigned)newBytes; i++) {
-						/* set updated flag if one complete packet was parsed */
+						// set updated flag if one complete packet was parsed
 						st24_rssi = input_rc_s::RSSI_MAX;
-						rc_updated = (OK == st24_decode(_rcs_buf[i], &st24_rssi, &lost_count,
-										&_raw_rc_count, _raw_rc_values, input_rc_s::RC_INPUT_MAX_CHANNELS));
+						rc_updated = (OK == st24_decode(_rcs_buf[i], &st24_rssi, &lost_count, &raw_rc_count, raw_rc_values,
+										input_rc_s::RC_INPUT_MAX_CHANNELS));
 					}

 					// The st24 will keep outputting RC channels and RSSI even if RC has been lost.
 					// The only way to detect RC loss is therefore to look at the lost_count.
-
 					if (rc_updated) {
-						if (lost_count == 0) {
-							// we have a new ST24 frame. Publish it.
-							_rc_in.input_source = input_rc_s::RC_INPUT_SOURCE_PX4FMU_ST24;
-							fill_rc_in(_raw_rc_count, _raw_rc_values, cycle_timestamp,
-								   false, false, frame_drops, st24_rssi);
-							_rc_scan_locked = true;
+						// we have a new ST24 frame. Publish it.
+						input_rc_s input_rc{};
+						input_rc.timestamp_last_signal = cycle_timestamp;
+						input_rc.channel_count = math::max(raw_rc_count, (uint16_t)input_rc_s::RC_INPUT_MAX_CHANNELS);
+						input_rc.rssi = st24_rssi;
+						input_rc.rc_lost = (raw_rc_count == 0) || (lost_count > 0);
+						input_rc.input_source = input_rc_s::RC_INPUT_SOURCE_PX4FMU_ST24;

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

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

 			break;
@@ -549,74 +574,47 @@ void RCInput::Run()
 				tcflush(_rcs_fd, TCIOFLUSH);
 				memset(_rcs_buf, 0, sizeof(_rcs_buf));

-			} else if (_rc_scan_locked
-				   || cycle_timestamp - _rc_scan_begin < rc_scan_max) {
-
+			} else if (_rc_scan_locked || (cycle_timestamp - _rc_scan_begin < rc_scan_max)) {
 				if (newBytes > 0) {
 					// parse new data
-					uint8_t sumd_rssi, rx_count;
-					bool sumd_failsafe;
-
-					rc_updated = false;
+					uint8_t sumd_rssi = 0;
+					uint8_t rx_count = 0;
+					uint16_t raw_rc_count = 0;
+					uint16_t raw_rc_values[input_rc_s::RC_INPUT_MAX_CHANNELS];
+					bool sumd_failsafe = false;

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

 					if (rc_updated) {
 						// we have a new SUMD frame. Publish it.
-						_rc_in.input_source = input_rc_s::RC_INPUT_SOURCE_PX4FMU_SUMD;
-						fill_rc_in(_raw_rc_count, _raw_rc_values, cycle_timestamp,
-							   false, sumd_failsafe, frame_drops, sumd_rssi);
-						_rc_scan_locked = true;
+						input_rc_s input_rc{};
+						input_rc.timestamp_last_signal = cycle_timestamp;
+						input_rc.channel_count = math::max(raw_rc_count, (uint16_t)input_rc_s::RC_INPUT_MAX_CHANNELS);
+						input_rc.rssi = sumd_rssi;
+						input_rc.rc_failsafe = sumd_failsafe;
+						input_rc.rc_lost = (raw_rc_count == 0);
+						input_rc.input_source = input_rc_s::RC_INPUT_SOURCE_PX4FMU_SUMD;
+
+						for (int i = 0; i < input_rc.channel_count; i++) {
+							input_rc.values[i] = raw_rc_values[i];
+						}
+
+						PublishInputRc(input_rc);
 					}
 				}

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

 			break;

-		case RC_SCAN_PPM:
-			// skip PPM if it's not supported
-#ifdef HRT_PPM_CHANNEL
-			if (_rc_scan_begin == 0) {
-				_rc_scan_begin = cycle_timestamp;
-				// Configure timer input pin for CPPM
-				px4_arch_configgpio(GPIO_PPM_IN);
-
-			} else if (_rc_scan_locked || cycle_timestamp - _rc_scan_begin < rc_scan_max) {
-
-				// see if we have new PPM input data
-				if ((ppm_last_valid_decode != _rc_in.timestamp_last_signal) && ppm_decoded_channels > 3) {
-					// we have a new PPM frame. Publish it.
-					rc_updated = true;
-					_rc_in.input_source = input_rc_s::RC_INPUT_SOURCE_PX4FMU_PPM;
-					fill_rc_in(ppm_decoded_channels, ppm_buffer, cycle_timestamp, false, false, 0);
-					_rc_scan_locked = true;
-					_rc_in.rc_ppm_frame_length = ppm_frame_length;
-					_rc_in.timestamp_last_signal = ppm_last_valid_decode;
-				}
-
-			} else {
-				// disable CPPM input by mapping it away from the timer capture input
-				px4_arch_unconfiggpio(GPIO_PPM_IN);
-				// Scan the next protocol
-				set_rc_scan_state(RC_SCAN_CRSF);
-			}
-
-#else   // skip PPM if it's not supported
-			set_rc_scan_state(RC_SCAN_CRSF);
-
-#endif  // HRT_PPM_CHANNEL
-
-			break;
-
 		case RC_SCAN_CRSF:
 			if (_rc_scan_begin == 0) {
 				_rc_scan_begin = cycle_timestamp;
@@ -627,40 +625,44 @@ void RCInput::Run()
 				tcflush(_rcs_fd, TCIOFLUSH);
 				memset(_rcs_buf, 0, sizeof(_rcs_buf));

-			} else if (_rc_scan_locked
-				   || cycle_timestamp - _rc_scan_begin < rc_scan_max) {
-
-				// parse new data
+			} else if (_rc_scan_locked || (cycle_timestamp - _rc_scan_begin < rc_scan_max)) {
 				if (newBytes > 0) {
-					rc_updated = crsf_parse(cycle_timestamp, &_rcs_buf[0], newBytes, &_raw_rc_values[0], &_raw_rc_count,
+					// parse new data
+					uint16_t raw_rc_values[input_rc_s::RC_INPUT_MAX_CHANNELS];
+					uint16_t raw_rc_count = 0;
+
+					rc_updated = crsf_parse(cycle_timestamp, &_rcs_buf[0], newBytes, &raw_rc_values[0], &raw_rc_count,
 								input_rc_s::RC_INPUT_MAX_CHANNELS);

 					if (rc_updated) {
 						// we have a new CRSF frame. Publish it.
-						_rc_in.input_source = input_rc_s::RC_INPUT_SOURCE_PX4FMU_CRSF;
-						fill_rc_in(_raw_rc_count, _raw_rc_values, cycle_timestamp, false, false, 0);
+						input_rc_s input_rc{};
+						input_rc.timestamp_last_signal = cycle_timestamp;
+						input_rc.channel_count = math::max(raw_rc_count, (uint16_t)input_rc_s::RC_INPUT_MAX_CHANNELS);
+						input_rc.rc_lost = (raw_rc_count == 0);
+						input_rc.input_source = input_rc_s::RC_INPUT_SOURCE_PX4FMU_CRSF;

-						// on Pixhawk (-related) boards we cannot write to the RC UART
-						// another option is to use a different UART port
-#ifdef BOARD_SUPPORTS_RC_SERIAL_PORT_OUTPUT
-
-						if (!_rc_scan_locked && !_crsf_telemetry) {
-							_crsf_telemetry = new CRSFTelemetry(_rcs_fd);
+						for (int i = 0; i < input_rc.channel_count; i++) {
+							input_rc.values[i] = raw_rc_values[i];
 						}

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

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

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

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

 			break;
@@ -675,55 +677,55 @@ void RCInput::Run()
 				tcflush(_rcs_fd, TCIOFLUSH);
 				memset(_rcs_buf, 0, sizeof(_rcs_buf));

-			} else if (_rc_scan_locked
-				   || cycle_timestamp - _rc_scan_begin < rc_scan_max) {
-
+			} else if (_rc_scan_locked || (cycle_timestamp - _rc_scan_begin < rc_scan_max)) {
 				// parse new data
 				if (newBytes > 0) {
+					uint16_t raw_rc_values[input_rc_s::RC_INPUT_MAX_CHANNELS];
+					uint16_t raw_rc_count = 0;
 					int8_t ghst_rssi = -1;
-					rc_updated = ghst_parse(cycle_timestamp, &_rcs_buf[0], newBytes, &_raw_rc_values[0], &ghst_rssi,
-								&_raw_rc_count, input_rc_s::RC_INPUT_MAX_CHANNELS);
+					rc_updated = ghst_parse(cycle_timestamp, &_rcs_buf[0], newBytes, &raw_rc_values[0], &ghst_rssi, &raw_rc_count,
+								input_rc_s::RC_INPUT_MAX_CHANNELS);

 					if (rc_updated) {
 						// we have a new GHST frame. Publish it.
-						_rc_in.input_source = input_rc_s::RC_INPUT_SOURCE_PX4FMU_GHST;
-						fill_rc_in(_raw_rc_count, _raw_rc_values, cycle_timestamp, false, false, 0, ghst_rssi);
+						input_rc_s input_rc{};
+						input_rc.timestamp_last_signal = cycle_timestamp;
+						input_rc.channel_count = math::max(raw_rc_count, (uint16_t)input_rc_s::RC_INPUT_MAX_CHANNELS);
+						input_rc.rssi = ghst_rssi;
+						input_rc.rc_lost = (raw_rc_count == 0);
+						input_rc.input_source = input_rc_s::RC_INPUT_SOURCE_PX4FMU_GHST;

-						// ghst telemetry works on fmu-v5
-						// on other Pixhawk (-related) boards we cannot write to the RC UART
-						// another option is to use a different UART port
-#ifdef BOARD_SUPPORTS_RC_SERIAL_PORT_OUTPUT
-
-						if (!_rc_scan_locked && !_ghst_telemetry) {
-							_ghst_telemetry = new GHSTTelemetry(_rcs_fd);
+						for (int i = 0; i < input_rc.channel_count; i++) {
+							input_rc.values[i] = raw_rc_values[i];
 						}

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

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

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

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

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

-		perf_end(_cycle_perf);
-
-		if (rc_updated) {
-			perf_count(_publish_interval_perf);
-
-			_to_input_rc.publish(_rc_in);
-
-		} else if (!rc_updated && !_armed && (hrt_elapsed_time(&_rc_in.timestamp_last_signal) > 1_s)) {
+		if (!rc_updated && !_armed && (hrt_elapsed_time(&_last_publish_time) > 1_s)) {
 			_rc_scan_locked = false;
 		}

@@ -731,6 +733,16 @@ void RCInput::Run()
 			_report_lock = false;
 			PX4_INFO("RC scan: %s RC input locked", RC_SCAN_STRING[_rc_scan_state]);
 		}
+
+		// reschedule immediately if RC is locked
+		if (_rc_scan_locked) {
+			ScheduleNow();
+
+		} else {
+			ScheduleDelayed(_backup_update_interval);
+		}
+
+		perf_end(_cycle_perf);
 	}
 }

@@ -805,8 +817,6 @@ int RCInput::custom_command(int argc, char *argv[])

 int RCInput::print_status()
 {
-	PX4_INFO("Max update rate: %u Hz", 1000000 / _current_update_interval);
-
 	if (_device[0] != '\0') {
 		PX4_INFO("UART device: %s", _device);
 		PX4_INFO("UART RX bytes: %"  PRIu32, _bytes_rx);
@@ -828,42 +838,23 @@ int RCInput::print_status()
 			PX4_INFO("SBUS frame drops: %u", sbus_dropped_frames());
 			break;

-
-		case RC_SCAN_DSM:
-			// DSM status output
-#if defined(SPEKTRUM_POWER)
-#endif
-			break;
-
-		case RC_SCAN_PPM:
-			// PPM status output
-			break;
-
-		case RC_SCAN_SUMD:
-			// SUMD status output
-			break;
-
-		case RC_SCAN_ST24:
-			// SUMD status output
+		default:
 			break;
 		}
 	}

-#if ADC_RC_RSSI_CHANNEL
+#if defined(ADC_RC_RSSI_CHANNEL)

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

-#endif
+#endif // ADC_RC_RSSI_CHANNEL

 	perf_print_counter(_cycle_perf);
+	perf_print_counter(_cycle_interval_perf);
 	perf_print_counter(_publish_interval_perf);

-	if (hrt_elapsed_time(&_rc_in.timestamp) < 1_s) {
-		print_message(ORB_ID(input_rc), _rc_in);
-	}
-
 	return 0;
 }

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

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

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

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

-	void fill_rc_in(uint16_t raw_rc_count_local,
-			uint16_t raw_rc_values_local[input_rc_s::RC_INPUT_MAX_CHANNELS],
-			hrt_abstime now, bool frame_drop, bool failsafe,
-			unsigned frame_drops, int rssi);
-
-	void set_rc_scan_state(RC_SCAN _rc_scan_state);
+	void FillRssi(input_rc_s &input_rc);
+	void PublishInputRc(input_rc_s &input_rc);

 	void rc_io_invert(bool invert);
+	void set_next_rc_scan_state();

 	hrt_abstime _rc_scan_begin{0};
+	hrt_abstime _last_publish_time{0};

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

-	static constexpr unsigned	_current_update_interval{4000}; // 250 Hz
+	static constexpr unsigned	_backup_update_interval{100000}; // 10 Hz (backup schedule)

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

-	uORB::Subscription	_adc_report_sub{ORB_ID(adc_report)};
+#if defined(ADC_RC_RSSI_CHANNEL)
+	uORB::Subscription	_adc_report_sub {ORB_ID(adc_report)};
+	float _analog_rc_rssi_volt{-1.0f};
+	bool _analog_rc_rssi_stable{false};
+#endif // ADC_RC_RSSI_CHANNEL
+
 	uORB::Subscription	_vehicle_cmd_sub{ORB_ID(vehicle_command)};
 	uORB::Subscription	_vehicle_status_sub{ORB_ID(vehicle_status)};

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

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

 	int		_rcs_fd{-1};
 	char		_device[20] {};					///< device / serial port path
@@ -155,14 +161,13 @@ private:
 	static constexpr size_t RC_MAX_BUFFER_SIZE{SBUS_BUFFER_SIZE};
 	uint8_t _rcs_buf[RC_MAX_BUFFER_SIZE] {};

-	uint16_t _raw_rc_values[input_rc_s::RC_INPUT_MAX_CHANNELS] {};
-	uint16_t _raw_rc_count{};
-
 	CRSFTelemetry *_crsf_telemetry{nullptr};
 	GHSTTelemetry *_ghst_telemetry{nullptr};

-	perf_counter_t	_cycle_perf;
-	perf_counter_t	_publish_interval_perf;
+	perf_counter_t	_cycle_perf{perf_alloc(PC_ELAPSED, MODULE_NAME": cycle time")};
+	perf_counter_t	_cycle_interval_perf{perf_alloc(PC_INTERVAL, MODULE_NAME": cycle interval")};
+	perf_counter_t	_publish_interval_perf{perf_alloc(PC_INTERVAL, MODULE_NAME": publish interval")};
+
 	uint32_t	_bytes_rx{0};

 	DEFINE_PARAMETERS(
@@ -64,6 +64,12 @@ extern void libtomcrypt_init(void);
 */
 static int crypto_open_count = 0;

+/*
+ * Status of libtomcrypt initialization. This is a large library, which
+ * is initialized & pulled in by linker only when it is actually used
+ */
+static bool tomcrypt_initialized = false;
+
 typedef struct {
 	size_t key_size;
 	uint8_t *key;
@@ -76,6 +82,14 @@ typedef struct {
 	uint64_t ctr;
 } chacha20_context_t;

+static inline void initialize_tomcrypt(void)
+{
+	if (!tomcrypt_initialized) {
+		libtomcrypt_init();
+		tomcrypt_initialized = true;
+	}
+}
+
 /* Clear key cache */
 static void clear_key_cache(void)
 {
@@ -135,7 +149,6 @@ void crypto_init()
 {
 	keystore_init();
 	clear_key_cache();
-	libtomcrypt_init();
 }

 crypto_session_handle_t crypto_open(px4_crypto_algorithm_t algorithm)
@@ -269,6 +282,8 @@ bool crypto_encrypt_data(crypto_session_handle_t handle,
 			uint8_t *public_key = (uint8_t *)crypto_get_key_ptr(handle.keystore_handle, key_idx, &key_sz);
 			*cipher_size = 0;

+			initialize_tomcrypt();
+
 			if (public_key &&
 			    rsa_import(public_key, key_sz, &key) == CRYPT_OK) {
 				if (outlen >= ltc_mp.unsigned_size(key.N) &&
@@ -413,6 +428,8 @@ size_t crypto_get_min_blocksize(crypto_session_handle_t handle, uint8_t key_idx)
 			size_t pub_key_sz;
 			uint8_t *pub_key = (uint8_t *)crypto_get_key_ptr(handle.keystore_handle, key_idx, &pub_key_sz);

+			initialize_tomcrypt();
+
 			if (pub_key &&
 			    rsa_import(pub_key, pub_key_sz, &enc_key) == CRYPT_OK) {
 				ret = ltc_mp.unsigned_size(enc_key.N);
@@ -617,6 +617,25 @@ 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,6 +248,21 @@ 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,6 +629,10 @@ 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);
@@ -739,6 +743,10 @@ 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,10 +245,7 @@ void EKFGSF_yaw::predictEKF(const uint8_t model_index)
 	}

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

 	// calculate delta velocity in a horizontal front-right frame
 	const Vector3f del_vel_NED = _ahrs_ekf_gsf[model_index].R * _delta_vel;
@@ -187,9 +187,7 @@ void Ekf::resetWind()
 */
 void Ekf::resetWindUsingAirspeed()
 {
-	const float euler_yaw = shouldUse321RotationSequence(_R_to_earth)
-				? getEuler321Yaw(_state.quat_nominal)
-				: getEuler312Yaw(_state.quat_nominal);
+	const float euler_yaw = getEulerYaw(_R_to_earth);

 	// 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,7 +243,6 @@ 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;
@@ -307,9 +306,9 @@ void Ekf::controlExternalVisionFusion()
 			}

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

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

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

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

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

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

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

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

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

 void Ekf::controlDragFusion()
 {
-	if ((_params.fusion_mode & MASK_USE_DRAG) &&
-	    !_using_synthetic_position &&
-	    _control_status.flags.in_air &&
-	    !_mag_inhibit_yaw_reset_req) {
+	if ((_params.fusion_mode & MASK_USE_DRAG) && _drag_buffer &&
+	    !_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();

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

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

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

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

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

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

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

@@ -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 = getEuler321Yaw(_state.quat_nominal);
+	const float euler_yaw = getEulerYaw(_R_to_earth);
 	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()
+void Ekf::fuseDrag(const dragSample &drag_sample)
 {
 	SparseVector24f<0,1,2,3,4,5,6,22,23> Hfusion;  // Observation Jacobians
 	Vector24f Kfusion; // Kalman gain vector
@@ -89,7 +89,7 @@ void Ekf::fuseDrag()
 	// 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_delayed.accelXY(axis_index)  - _state.delta_vel_bias(axis_index) / _dt_ekf_avg;
+		const float mea_acc = drag_sample.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,7 +261,6 @@ 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,9 +420,6 @@ 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
@@ -622,7 +619,7 @@ private:
 	void fuseSideslip();

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

 	void fuseBaroHgt();
 	void fuseGpsHgt();
@@ -636,43 +633,43 @@ private:

 	void resetVelocityToGps();

-	inline void resetHorizontalVelocityToOpticalFlow();
+	void resetHorizontalVelocityToOpticalFlow();

-	inline void resetVelocityToVision();
+	void resetVelocityToVision();

-	inline void resetHorizontalVelocityToZero();
+	void resetHorizontalVelocityToZero();

-	inline void resetVelocityTo(const Vector3f &vel);
+	void resetVelocityTo(const Vector3f &vel);

-	inline void resetHorizontalVelocityTo(const Vector2f &new_horz_vel);
+	void resetHorizontalVelocityTo(const Vector2f &new_horz_vel);

-	inline void resetVerticalVelocityTo(float new_vert_vel);
+	void resetVerticalVelocityTo(float new_vert_vel);

 	void resetHorizontalPosition();

 	void resetHorizontalPositionToGps();

-	inline void resetHorizontalPositionToVision();
+	void resetHorizontalPositionToVision();

-	inline void resetHorizontalPositionTo(const Vector2f &new_horz_pos);
+	void resetHorizontalPositionTo(const Vector2f &new_horz_pos);

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

 	void resetHeight();

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

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

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

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

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

 	void fuseGpsVelPos();
@@ -229,7 +229,7 @@ void Ekf::resetHorizontalPositionTo(const Vector2f &new_horz_pos)
 	_state_reset_status.posNE_counter++;
 }

-void Ekf::resetVerticalPositionTo(const float &new_vert_pos)
+void Ekf::resetVerticalPositionTo(const float new_vert_pos)
 {
 	const float old_vert_pos = _state.pos(2);
 	_state.pos(2) = new_vert_pos;
@@ -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 = getEuler321Yaw(_state.quat_nominal);
+			const float current_yaw = getEulerYaw(_R_to_earth);
 			yaw_new = current_yaw + courseYawError;
 			_control_status.flags.mag_aligned_in_flight = true;

@@ -454,7 +454,6 @@ 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();
@@ -541,7 +540,7 @@ bool Ekf::resetMagHeading(bool increase_yaw_var, bool update_buffer)

 bool Ekf::resetYawToEv()
 {
-	const float yaw_new = getEuler312Yaw(_ev_sample_delayed.quat);
+	const float yaw_new = getEulerYaw(_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);
@@ -1695,10 +1694,8 @@ 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, _R_to_earth);
+	_R_to_earth = updateYawInRotMat(yaw, Dcmf(_state.quat_nominal));

 	// calculate the amount that the quaternion has changed by
 	const Quatf quat_after_reset(_R_to_earth);
@@ -295,9 +295,7 @@ 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
Author	SHA1	Message	Date
Daniel Agar	edb9fce9b3	drivers/rc_input: cleanup and simplify data processing per type - don't store data in class	2022-02-09 17:11:11 -05:00
Daniel Agar	c4f9f2980d	drivers/rc_input: poll file descriptor to minimize latency	2022-02-09 16:48:08 -05:00
David Sidrane	fd1aa3cfb9	matek_gnss-m9n-f4:Use CONFIG_BOARD_SERIAL_GPSn for serial_passthru	2022-02-09 13:11:52 -05:00
David Sidrane	0c936e4fd2	serial_passthru:Move CONFIG_xxx to serial_passthru	2022-02-09 13:11:52 -05:00
Silvan Fuhrer	81b08a0168	FW Pos C: set position_sp type to position during VTOL backtransition Signed-off-by: Silvan Fuhrer <silvan@auterion.com>	2022-02-09 11:39:47 -05:00
Silvan Fuhrer	1d6396b418	Navigator: VTOL: track virtual WP 1m ahead of vehicle during backtransition in Land mode Signed-off-by: Silvan Fuhrer <silvan@auterion.com>	2022-02-09 11:39:47 -05:00
Jukka Laitinen	dcde0d0559	src/drivers/sw_crypto: Late initialize tomcypt This saves a lot of flash space, in case functions from libtomcrypt are not used (currently only RSA related). When RSA is not used, the linker can now drop all libtomcrypt related things. This is especially relevant for bootloaders using the SW crypto. Signed-off-by: Jukka Laitinen <jukkax@ssrc.tii.ae>	2022-02-09 10:28:06 -05:00
Daniel Agar	a95da715d5	Jenkins: HIL remove airframe 4018 test	2022-02-09 10:08:59 +01:00
Beat Küng	86860808e3	ROMFS: set CA_* geometry params for some of the generic airframes Not enabled, makes it easier to switch.	2022-02-09 10:08:59 +01:00
Beat Küng	a2ba613254	ROMFS: remove 4018 + 6003 ctrlalloc airframes	2022-02-09 10:08:59 +01:00
PX4 BuildBot	33ce1b9b64	Update submodule mavlink to latest Wed Feb 9 00:39:13 UTC 2022 - mavlink in PX4/Firmware (ed8c1dca2e792e95d9ab1ba72a22bc9658f10b79): https://github.com/mavlink/mavlink/commit/51abf3c82b3d7137406459dc9b337e57637711ae - mavlink current upstream: https://github.com/mavlink/mavlink/commit/311eee010bb82f5fb2e4e0f64f7961a83212b003 - Changes: https://github.com/mavlink/mavlink/compare/51abf3c82b3d7137406459dc9b337e57637711ae...311eee010bb82f5fb2e4e0f64f7961a83212b003 311eee01 2022-02-04 Peter Barker - common.xml: deprecate SET_HOME_POSITION message (#1791) 63d62522 2022-02-02 Julian Oes - minimal: add MAV_TYPE_WINCH (#1789)	2022-02-08 21:50:55 -05:00
PX4 BuildBot	421ca2fc48	Update submodule sitl_gazebo to latest Wed Feb 9 00:39:03 UTC 2022 - sitl_gazebo in PX4/Firmware (a088277fe866860272f1ef21773f5be1b6832b11): https://github.com/PX4/PX4-SITL_gazebo/commit/2750fe233c6a38dbe60e545ac0ec878d6ac20b1e - sitl_gazebo current upstream: https://github.com/PX4/PX4-SITL_gazebo/commit/25138e803ee8525ee5fe4e6d511506e88e3f819c - Changes: https://github.com/PX4/PX4-SITL_gazebo/compare/2750fe233c6a38dbe60e545ac0ec878d6ac20b1e...25138e803ee8525ee5fe4e6d511506e88e3f819c 25138e8 2021-07-16 Julian Oes - gimbal_controller: fix attitude status	2022-02-08 20:52:12 -05:00
Daniel Agar	b24aa071b6	Jenkins: hardware always dump pyserial debug (silently)	2022-02-08 17:22:57 -05:00
Daniel Agar	6686736cff	drv_pwm_output.h fix dshot cmd typo	2022-02-08 17:11:49 -05:00
Daniel Agar	86f81680fb	sensors: check uORB::SubscriptionData validity before use	2022-02-08 13:19:01 -05:00
Oleg Kalachev	21b78f9d05	Enable mpu9250’s magnetometer on fmu-v4	2022-02-08 13:18:39 -05:00
Steve Nogar	dce067df83	add configurable rtps rate parameter	2022-02-08 19:03:20 +01:00
Daniel Agar	133fe0cfb1	local_position_estimator: move to INS0 work queue (for significantly more stack)	2022-02-08 11:16:49 -05:00
Daniel Agar	be3da5089c	uORB: uORBDeviceNode use px4_cache_aligned_alloc	2022-02-08 10:20:50 -05:00
Viktor Vladic	f4d02a2937	MatekH743 board_id, usb vid/pid changes, and MPU6000 delay before transfer - not after (#18733 ) * Make board_id compatible with ardupilot * Initialize outputs for CAM1/CAM2 and Vsw pad * Correct board type 1013 in bootloader to match AP * Change usb vendor string to "Matek" * Change cdcacm pid to 1013 * Comment out FLASH_BASED_PARAMS because of #15331	2022-02-07 19:55:49 -05:00
Julian Oes	490a0c473b	Rename vmount to gimbal	2022-02-07 19:21:15 -05:00
Julian Oes	853047c643	Mantis: use gimbal auto input This allows RC and mavlink gimbal v2 input.	2022-02-07 19:21:15 -05:00
Julian Oes	39f0e97245	vmount: refactor for v2 auto input, test command This is a bigger refactor of vmount to clean it up and plug some holes in functionality. The changes include: - Fixing/simplifying the test command. - Changing the dependencies of the input and output classes to just the parameter list. - Adding a separate topic to publish gimbal v1 commands to avoid polluting the vehicle_command topic. - Removing outdated comments and author lists. - Extracting the gimbal v2 "in control" notion outside into control_data rather than only the v2 input.	2022-02-07 19:21:15 -05:00
Julian Oes	f2216dff55	mavlink: don't send gimbal_device_attitude_status If we receive gimbal_device_attitude_status by mavlink we should not re-send it as we are already supposed to be forwarding mavlink traffic from the gimbal to the ground station.	2022-02-07 19:21:15 -05:00
Julian Oes	7e7a99b542	mavlink: handle for GIMBAL_DEVICE_ATTITUDE_STATUS That way we can log it later.	2022-02-07 19:21:15 -05:00
Daniel Agar	5b07398b3e	Tools/HIL/test_airframes.sh: only attempt setting first 3 MAV_x_CONFIG - MAV_3_CONFIG doesn't exist	2022-02-07 16:31:27 -05:00
Daniel Agar	f9d87fd97a	sensors/vehicle_angular_velocity: improve error handling (especially during initial selection)	2022-02-07 12:47:02 -05:00
Daniel Agar	3a37fd92e6	Jenkins: HIL temporarily tolerate ERRORs during 'uorb top' (long running command) - on older boards if the sensor auto cal completes while this is running it causes a brief sensor timeout	2022-02-07 10:26:48 -05:00
Daniel Agar	052adfbfd9	borads: holybro_kakutef7 disable modules/events to save flash	2022-02-06 16:53:13 -05:00
Daniel Agar	3a741cb9c9	boards: bitcrazy_crazyflie disable gyro_fft to save flash	2022-02-06 16:53:13 -05:00
Thomas Debrunner	b4087ebd2b	mc_pos_control: increase the default crawl speed for three-stage-landing approach for a more robust landing detection	2022-02-04 21:56:20 -05:00
Matthias Grob	4ef8cead3d	mc_pos_control_params: correct crawl speed reference	2022-02-04 21:56:20 -05:00
Matthias Grob	1df9d6fca6	MulticopterLandDetector: fix crawl speed parameter fetching	2022-02-04 21:56:20 -05:00
Thomas Debrunner	fb8b9b647a	land-detector: switch to crawl speed for intent detection	2022-02-04 21:56:20 -05:00
Thomas Debrunner	2a6d9bc1dd	fligh-mode-manager: First implementation of a three-stage-landing for multirotos, in case LIDAR is available	2022-02-04 21:56:20 -05:00
Daniel Agar	893cdf8f38	ekf2: test external vision adjustments after lowering fake position fusion variance	2022-02-04 21:48:32 -05:00
Daniel Agar	6b1750d8be	ekf2: lower fake position observation variance when at rest	2022-02-04 21:48:32 -05:00
Daniel Agar	c028b964e2	bmm150: minor changes to match reference driver	2022-02-04 19:57:24 -05:00
Beat Küng	8b2016b4ed	fix protocol_splitter: increment i properly in scan_for_packets This also guarantees that i is increased in every loop iteration. Before it was possible to enter a busy loop.	2022-02-04 09:42:52 -05:00
Daniel Agar	cf3db0d313	mavlink: don't send_mission_current if mission invalid	2022-02-04 09:40:25 -05:00
Silvan Fuhrer	26dba1407b	Commander: VTOL quadchuate also triggers a RTL in Loiter and VTOL_Takeoff modes, not just in Mission (#19123 ) Signed-off-by: Silvan Fuhrer <silvan@auterion.com>	2022-02-03 14:18:28 +01:00
JaeyoungLim	f82c722653	Publish correct orbit status with goto (#19102 ) For fixedwings, the orbit status was publishing zero radius when a goto waypoint was being passed. This commit corrects this by passing the default loiter radius as the guidance logic was using	2022-02-03 10:16:39 +01:00
bresch	917910f3e2	ekf: pass a float by copy instead of a constant ref	2022-02-02 17:43:25 -05:00
bresch	3077f27821	ekf: use float instead of Vector2f for innov gate in pos/vel fusion We always use the same innovation gate for X and Y anyway	2022-02-02 17:43:25 -05:00
Daniel Agar	5fb0084524	ekf2: remove unused shared fields for last velocity observation and variance	2022-02-02 17:43:25 -05:00
Daniel Agar	8a0581516c	ekf2: don't store _auxvel_sample_delayed	2022-02-02 17:43:25 -05:00
Daniel Agar	452b5e94b4	ekf2: don't store _drag_sample_delayed	2022-02-02 17:43:25 -05:00
Daniel Agar	138ff7a316	sensors: ensure that best accel/gyro is in bounds	2022-02-02 17:14:46 -05:00
Daniel Agar	07c273fc31	sensors/vehicle_angular_velocity: print full sensor_selection if device id not found	2022-02-02 17:14:46 -05:00
Daniel Agar	94604ff21a	Jenkins: hardware pyserial cat shouldn't fail the build	2022-02-02 14:17:02 -05:00
Peter van der Perk	9f97793491	Generate C/C++ header to expose px4board kconfig symbols to the preprocessor	2022-02-02 13:23:21 -05:00
Jaeyoung Lim	2fc95bb369	Rebase fix Use getnavspeed_2d for groundspeed	2022-02-02 17:16:19 +01:00
Jaeyoung Lim	3dd5c1fbaf	Enable NPFG by default on the believer	2022-02-02 17:16:19 +01:00
Jaeyoung Lim	3a9c5c3178	Use acceleration to pass path curvature	2022-02-02 17:16:19 +01:00
Jaeyoung Lim	e2741f988a	Add interface for passing path tangent and closest point directly This commit adds an interface to pass the path tangent and closest point directly to NPFG using the offboard interface	2022-02-02 17:16:19 +01:00
David Sidrane	f460611098	Fix serial_test stack warning	2022-02-02 11:12:53 -05:00
Peter van der Perk	efd3bc1794	Serial_test move g_cl and g_mod from heap to stack to save memory by not preallocating them	2022-02-02 07:21:43 -08:00
Daniel Agar	88ffc177f7	ekf2: utils add getEulerYaw() that uses the best rotation sequence	2022-02-02 08:54:03 -05:00
Daniel Agar	c3e0b93fc8	ekf2: remove unnecessary inlines	2022-02-02 08:50:49 -05:00
Hamish Willee	26d5ac4f58	GPS_YAW_OFFSET param docs: use rover and moving base terminology	2022-02-02 07:39:01 +01:00