uORB Explained Blog Post : Add pasta_information uORB message definition

Add details to pasta_information.msg file and add to logged_topics Add uORBExplained Module (Not complete yet) Add functioning uorb_explained module - Implements a functioning module that works based on ModulBae inheritance - Edits the rcS script to start the module on start-up - Implements Customer / Waiter / Chef distinction inside the code by each function to demonstrate how uORB communication can work Fix timestamp not logged bug + Make temperature draw sine curve - More interesting in PlotJuggler :)
2026-06-03 12:20:05 +08:00 · 2022-06-02 14:43:42 +02:00
321 changed files with 9967 additions and 18060 deletions
@@ -1,13 +0,0 @@
-# Google C++ style as base
-Language: Cpp
-BasedOnStyle: Google
-
-# Indentation using tabs
-UseTab: ForContinuationAndIndentation
-TabWidth: 8
-IndentWidth: 8
-ContinuationIndentWidth: 8
-ConstructorInitializerIndentWidth: 8
-AccessModifierOffset: -8
-
-ColumnLimit: 120 # Allow more collumns
@@ -375,10 +375,6 @@ format:
 	$(call colorecho,'Formatting with astyle')
 	@"$(SRC_DIR)"/Tools/astyle/check_code_style_all.sh --fix

-format_clang:
-	$(call colorecho,'Formatting with clang-format')
-	@"$(SRC_DIR)"/Tools/astyle/files_to_check_code_style.sh | xargs clang-format -i -style=file
-
 # Testing
 # --------------------------------------------------------------------
 .PHONY: tests tests_coverage tests_mission tests_mission_coverage tests_offboard tests_avoidance
@@ -86,28 +86,10 @@ unset BOARD_RC_SENSORS
 . ${R}etc/init.d/rc.serial

 # Check for flow sensor
-if param compare -s SENS_EN_PX4FLOW 1
+if param compare SENS_EN_PX4FLOW 1
 then
 	px4flow start -X &
 fi

-if param compare -s IMU_GYRO_CAL_EN 1
-then
-	gyro_calibration start
-fi
-
-
-if param compare -s MBE_ENABLE 1
-then
-	# conservative mag bias estimation
-	param set-default MBE_LEARN_GAIN 5
-	param set-default IMU_GYRO_CUTOFF 20
-	mag_bias_estimator start
-fi
-
-param set-default SENS_MAG_RATE 100
-
-sensors start
-
 uavcannode start
 unset R
@@ -1,101 +0,0 @@
-#!/bin/sh
-#
-# @name 6DoF Omnicopter SITL
-#
-# @type Quadrotor Wide
-#
-# @maintainer Jaeyoung Lim <jalim@ethz.ch>
-#
-
-. ${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 8
-param set-default CA_R_REV 255
-
-param set-default CA_ROTOR0_PX 0.14435
-param set-default CA_ROTOR0_PY -0.14435
-param set-default CA_ROTOR0_PZ -0.14435
-param set-default CA_ROTOR0_KM 0.05 # CCW
-param set-default CA_ROTOR0_AX -0.788675
-param set-default CA_ROTOR0_AY -0.211325
-param set-default CA_ROTOR0_AZ -0.57735
-
-param set-default CA_ROTOR1_PX -0.14435
-param set-default CA_ROTOR1_PY -0.14435
-param set-default CA_ROTOR1_PZ -0.14435
-param set-default CA_ROTOR1_KM 0.05
-param set-default CA_ROTOR1_AX 0.211325
-param set-default CA_ROTOR1_AY -0.788675
-param set-default CA_ROTOR1_AZ 0.57735
-
-param set-default CA_ROTOR2_PX 0.14435
-param set-default CA_ROTOR2_PY 0.14435
-param set-default CA_ROTOR2_PZ -0.14435
-param set-default CA_ROTOR2_KM 0.05
-param set-default CA_ROTOR2_AX -0.211325
-param set-default CA_ROTOR2_AY 0.788675
-param set-default CA_ROTOR2_AZ 0.57735
-
-param set-default CA_ROTOR3_PX -0.14435
-param set-default CA_ROTOR3_PY 0.14435
-param set-default CA_ROTOR3_PZ -0.14435
-param set-default CA_ROTOR3_KM 0.05
-param set-default CA_ROTOR3_AX 0.788675
-param set-default CA_ROTOR3_AY 0.211325
-param set-default CA_ROTOR3_AZ -0.57735
-
-param set-default CA_ROTOR4_PX 0.14435
-param set-default CA_ROTOR4_PY -0.14435
-param set-default CA_ROTOR4_PZ 0.14435
-param set-default CA_ROTOR4_KM 0.05
-param set-default CA_ROTOR4_AX 0.788675
-param set-default CA_ROTOR4_AY 0.211325
-param set-default CA_ROTOR4_AZ -0.57735
-
-param set-default CA_ROTOR5_PX -0.14435
-param set-default CA_ROTOR5_PY -0.14435
-param set-default CA_ROTOR5_PZ 0.14435
-param set-default CA_ROTOR5_KM 0.05
-param set-default CA_ROTOR5_AX -0.211325
-param set-default CA_ROTOR5_AY 0.788675
-param set-default CA_ROTOR5_AZ 0.57735
-
-param set-default CA_ROTOR6_PX 0.14435
-param set-default CA_ROTOR6_PY 0.14435
-param set-default CA_ROTOR6_PZ 0.14435
-param set-default CA_ROTOR6_KM 0.05
-param set-default CA_ROTOR6_AX 0.211325
-param set-default CA_ROTOR6_AY -0.788675
-param set-default CA_ROTOR6_AZ 0.57735
-
-param set-default CA_ROTOR7_PX -0.14435
-param set-default CA_ROTOR7_PY 0.14435
-param set-default CA_ROTOR7_PZ 0.14435
-param set-default CA_ROTOR7_KM 0.05
-param set-default CA_ROTOR7_AX -0.788675
-param set-default CA_ROTOR7_AY -0.211325
-param set-default CA_ROTOR7_AZ -0.57735
-
-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
-param set-default PWM_MAIN_FUNC7 107
-param set-default PWM_MAIN_FUNC8 108
-
-# disable MC desaturation which improves attitude tracking
-param set-default CA_METHOD 0
-
-# disable attitude failure detection
-param set-default FD_FAIL_P 0
-param set-default FD_FAIL_R 0
-
-set MIXER skip
-set MIXER_AUX none
@@ -1,23 +0,0 @@
-#!/bin/sh
-#
-# @name QuadrotorX SITL for SIH
-#
-# @type Quadrotor
-#
-# @maintainer Romain Chiappinelli <romain.chiap@gmail.com>
-#
-
-. ${R}etc/init.d/rc.mc_defaults
-
-set MIXER quad_x
-
-# disable some checks to allow to fly:
-# - with usb
-param set-default CBRK_USB_CHK 197848
-# - without real battery
-param set-default CBRK_SUPPLY_CHK 894281
-# - without safety switch
-param set-default COM_PREARM_MODE 0
-param set-default CBRK_IO_SAFETY 22027
-
-param set SIH_VEHICLE_TYPE 0
@@ -1,33 +0,0 @@
-#!/bin/sh
-#
-# @name Plane SITL for SIH
-#
-# @type Plane
-#
-# @maintainer Romain Chiappinelli <romain.chiap@gmail.com>
-
-. ${R}etc/init.d/rc.fw_defaults
-
-set MIXER AERT
-
-# disable some checks to allow to fly:
-# - with usb
-param set-default CBRK_USB_CHK 197848
-# - without real battery
-param set-default CBRK_SUPPLY_CHK 894281
-# - without safety switch
-param set-default COM_PREARM_MODE 0
-param set-default CBRK_IO_SAFETY 22027
-
-param set-default BAT_N_CELLS 3
-
-param set SIH_T_MAX 6.0
-param set SIH_MASS 0.3
-param set SIH_IXX 0.00402
-param set SIH_IYY 0.0144
-param set SIH_IZZ 0.0177
-param set SIH_IXZ 0.00046
-param set SIH_KDV 0.2
-
-param set SIH_VEHICLE_TYPE 1 	# sih as fixed wing
-param set RWTO_TKOFF 1  # enable takeoff from runway (as opposed to launched)
@@ -1,45 +0,0 @@
-#!/bin/sh
-#
-# @name SIH Tailsitter Duo
-#
-# @type VTOL
-#
-# @maintainer Romain Chiappinelli <romain.chiap@gmail.com>
-
-. ${R}etc/init.d/rc.vtol_defaults
-
-param set-default VT_ELEV_MC_LOCK 0
-param set-default VT_TYPE 0
-param set-default VT_FW_DIFTHR_EN 1
-param set-default VT_FW_DIFTHR_SC 0.3
-param set-default MPC_MAN_Y_MAX 60
-param set-default MC_PITCH_P 5
-
-param set-default MAV_TYPE 19
-set MAV_TYPE 19
-set MIXER vtol_tailsitter_duo_sat
-
-# disable some checks to allow to fly:
-# - with usb
-param set-default CBRK_USB_CHK 197848
-# - without real battery
-param set-default CBRK_SUPPLY_CHK 894281
-# - without safety switch
-param set-default COM_PREARM_MODE 0
-param set-default CBRK_IO_SAFETY 22027
-
-param set-default BAT_N_CELLS 3
-
-param set SIH_T_MAX 2.0
-param set SIH_Q_MAX 0.0165
-param set SIH_MASS 0.2
-# IXX and IZZ are inverted from the thesis as the body frame is pitched by 90 deg
-param set SIH_IXX 0.00354
-param set SIH_IYY 0.000625
-param set SIH_IZZ 0.00300
-param set SIH_IXZ 0.0
-param set SIH_KDV 0.2
-param set SIH_L_ROLL 0.145
-
-# sih as tailsitter
-param set SIH_VEHICLE_TYPE 2
@@ -18,7 +18,3 @@ param set-default LPE_FAKE_ORIGIN 1

 param set-default MPC_ALT_MODE 2

-param set-default SENS_FLOW_ROT 6
-param set-default SENS_FLOW_MINHGT 0.7
-param set-default SENS_FLOW_MAXHGT 3.0
-param set-default SENS_FLOW_MAXR 2.5
@@ -9,6 +9,7 @@

 # EKF2
 param set-default EKF2_AID_MASK 2
+param set-default SENS_FLOW_ROT 0

 # LPE: Flow-only mode
 param set-default LPE_FUSION 242
@@ -35,12 +35,8 @@ px4_add_romfs_files(
 	10016_iris
 	10017_iris_ctrlalloc
 	10018_iris_foggy_lidar
-	10019_omnicopter
 	10020_if750a
 	10030_px4vision
-	10040_quadx
-	10041_airplane
-	10042_xvert
 	1010_iris_opt_flow
 	1010_iris_opt_flow.post
 	1011_iris_irlock
@@ -30,12 +30,3 @@ mavlink start -x -u $udp_onboard_payload_port_local -r 4000 -f -m onboard -o $ud

 # Onboard link to gimbal
 mavlink start -x -u $udp_onboard_gimbal_port_local -r 400000 -m gimbal -o $udp_onboard_gimbal_port_remote
-
-# To display for SIH sitl
-if [ "$SIM_MODE" = "sihsim" ]; then
-	udp_sihsim_port_local=$((19450+px4_instance))
-	udp_sihsim_port_remote=$((19410+px4_instance))
-	mavlink start -x -u $udp_sihsim_port_local -r 400000 -m custom -o $udp_sihsim_port_remote
-	mavlink stream -r 200 -s HIL_ACTUATOR_CONTROLS -u  $udp_sihsim_port_local
-	mavlink stream -r 25 -s HIL_STATE_QUATERNION -u  $udp_sihsim_port_local
-fi
@@ -154,8 +154,6 @@ param set-default SDLOG_DIRS_MAX 7

 param set-default TRIG_INTERFACE 3

-param set-default SYS_FAILURE_EN 1
-
 # Adapt timeout parameters if simulation runs faster or slower than realtime.
 if [ -n "$PX4_SIM_SPEED_FACTOR" ]; then
 	COM_DL_LOSS_T_LONGER=$(echo "$PX4_SIM_SPEED_FACTOR * 10" | bc)
@@ -204,11 +202,8 @@ param set IMU_INTEG_RATE 250
 . px4-rc.params

 dataman start
-# start sih in sih_sim mode, otherwise simulator module
-if [ "$SIM_MODE" = "sihsim" ]; then
-	sih start
 # only start the simulator if not in replay mode, as both control the lockstep time
-elif ! replay tryapplyparams
+if ! replay tryapplyparams
 then
  . px4-rc.simulator
 fi
@@ -272,3 +267,5 @@ fi

 mavlink boot_complete
 replay trystart
+
+uorb_explained start
@@ -62,7 +62,7 @@ param set-default MPC_JERK_AUTO 4
 param set-default MPC_LAND_SPEED 1
 param set-default MPC_MAN_TILT_MAX 25
 param set-default MPC_MAN_Y_MAX 40
-param set-default COM_SPOOLUP_TIME 1.5
+param set-default MPC_SPOOLUP_TIME 1.5
 param set-default MPC_THR_HOVER 0.45
 param set-default MPC_TILTMAX_AIR 25
 param set-default MPC_TKO_RAMP_T 1.8
@@ -104,6 +104,7 @@ param set-default SDLOG_PROFILE 131
 param set-default SENS_CM8JL65_CFG 104
 param set-default SENS_FLOW_MAXHGT 25
 param set-default SENS_FLOW_MINHGT 0.5
+param set-default SENS_FLOW_ROT 0
 param set-default IMU_GYRO_CUTOFF 100
 param set-default SENS_EN_PMW3901 1

@@ -47,7 +47,6 @@ param set-default COM_RC_LOSS_T 3

 # ekf2
 param set-default EKF2_AID_MASK 33
-param set-default EKF2_TERR_MASK 1
 param set-default EKF2_BARO_DELAY 0
 param set-default EKF2_BARO_NOISE 2.0

@@ -174,7 +173,7 @@ param set-default RC1_TRIM          1000
 param set-default SENS_FLOW_MAXR   7.4
 param set-default SENS_FLOW_MINHGT 0.15
 param set-default SENS_FLOW_MAXHGT 5.0
-param set-default SENS_FLOW_ROT 0
+param set-default SENS_FLOW_ROT    0

 # ignore the SD card errors and use normal startup sound
 set STARTUP_TUNE "1"
@@ -8,24 +8,23 @@ if [ $# -gt 0 ]; then
 fi

 exec find boards msg src platforms test \
-    -not -path "msg/templates/urtps/*" \
-    -not -path "platforms/nuttx/NuttX/*" \
-    -not -path "platforms/qurt/dspal/*" \
-    -not -path "src/drivers/gps/devices/*" \
-    -not -path "src/drivers/uavcan/libuavcan/*" \
-    -not -path "src/drivers/uavcan/uavcan_drivers/kinetis/driver/include/uavcan_kinetis/*" \
-    -not -path "src/drivers/cyphal/libcanard/*" \
-    -not -path "src/lib/crypto/monocypher/*" \
-    -not -path "src/lib/events/libevents/*" \
-    -not -path "src/lib/parameters/uthash/*" \
-    -not -path "src/modules/ekf2/EKF/*" \
-    -not -path "src/modules/gyro_fft/CMSIS_5/*" \
-    -not -path "src/modules/mavlink/mavlink/*" \
-    -not -path "src/modules/micrortps_bridge/micro-CDR/*" \
-    -not -path "src/modules/micrortps_bridge/microRTPS_client/*" \
-    -not -path "test/mavsdk_tests/catch2/*" \
-    -not -path "src/lib/crypto/monocypher/*" \
-    -not -path "src/lib/crypto/libtomcrypt/*" \
-    -not -path "src/lib/crypto/libtommath/*" \
-    -not -path "src/modules/microdds_client/Micro-XRCE-DDS-Client/*" \
+    -path msg/templates/urtps -prune -o \
+    -path platforms/nuttx/NuttX -prune -o \
+    -path platforms/qurt/dspal -prune -o \
+    -path src/drivers/uavcan/libuavcan -prune -o \
+    -path src/drivers/uavcan/uavcan_drivers/kinetis/driver/include/uavcan_kinetis -prune -o \
+    -path src/drivers/cyphal/libcanard -prune -o \
+    -path src/lib/crypto/monocypher -prune -o \
+    -path src/lib/events/libevents -prune -o \
+    -path src/lib/parameters/uthash -prune -o \
+    -path src/modules/ekf2/EKF -prune -o \
+    -path src/modules/gyro_fft/CMSIS_5 -prune -o \
+    -path src/modules/mavlink/mavlink -prune -o \
+    -path src/modules/micrortps_bridge/micro-CDR -prune -o \
+    -path src/modules/micrortps_bridge/microRTPS_client -prune -o \
+    -path test/mavsdk_tests/catch2 -prune -o \
+    -path src/lib/crypto/monocypher -prune -o \
+    -path src/lib/crypto/libtomcrypt -prune -o \
+    -path src/lib/crypto/libtommath -prune -o \
+    -path src/modules/microdds_client/Micro-XRCE-DDS-Client -prune -o \
    -type f \( -name "*.c" -o -name "*.h" -o -name "*.cpp" -o -name "*.hpp" \) | grep $PATTERN
@@ -5,13 +5,12 @@ set -e
 SCRIPT_DIR="$( cd "$( dirname "${BASH_SOURCE[0]}" )" && pwd )"
 cd "$SCRIPT_DIR/jMAVSim"

-port=4560
+tcp_port=4560
 extra_args=
 baudrate=921600
 device=
 ip="127.0.0.1"
-protocol="tcp"
-while getopts ":b:d:u:p:qsr:f:i:loat" opt; do
+while getopts ":b:d:p:qsr:f:i:loat" opt; do
 	case $opt in
 		b)
 			baudrate=$OPTARG
@@ -19,14 +18,11 @@ while getopts ":b:d:u:p:qsr:f:i:loat" opt; do
 		d)
 			device="$OPTARG"
 			;;
-		u)
-			protocol="udp"
-			;;
 		i)
 			ip="$OPTARG"
 			;;
 		p)
-			port=$OPTARG
+			tcp_port=$OPTARG
 			;;
 		q)
 			extra_args="$extra_args -qgc"
@@ -57,11 +53,7 @@ while getopts ":b:d:u:p:qsr:f:i:loat" opt; do
 done

 if [ "$device" == "" ]; then
-	if [ "$protocol" == "tcp" ]; then
-		device="-tcp $ip:$port"
-	else
-		device="-udp $port"
-	fi
+	device="-tcp $ip:$tcp_port"
 else
 	device="-serial $device $baudrate"
 fi
@@ -0,0 +1,72 @@
+#!/bin/python3
+
+import parse_cmake.parsing as cmp
+import glob
+import pprint
+import re
+import os
+
+__location__ = os.path.realpath(
+    os.path.join(os.getcwd(), os.path.dirname(__file__)))
+
+serial_regex = r"(\D\D\D\d):(/dev/ttyS\d+)"
+io_regex = r"IO (.*)"
+romfs_regex = r"ROMFSROOT (.*)"
+arch_regex = r"ARCHITECTURE (.*)"
+toolchain_regex = r"TOOLCHAIN (.*)"
+
+
+
+def stripComments(code):
+    code = str(code)
+    return re.sub(r'(?m) *#.*\n?', '', code)
+
+lut = {}
+with open(os.path.join(__location__, "cmake_kconfig_lut.txt"),'r') as lookup:
+    for line in lookup:
+        if ',' in line:
+            key, value = line.strip().split(',')
+            lut[key] = value
+
+#for name in glob.glob('boards/*/*/*.cmake'):
+px4_dir = os.path.abspath(os.path.join(os.path.dirname(__file__), '../../'))
+
+for name in glob.glob(px4_dir + '/boards/*/*/*.cmake'):
+    print(name)
+    with open(name, 'r') as f:
+        romfs_set = False
+        w = open(name.replace(".cmake",".px4board"), "w")
+        for line in f:
+            clean_line = stripComments(line.strip())
+            value = lut.get(clean_line)
+            if value is not None:
+                print(value, file=w)
+                print(value)
+            else:
+                matches = re.finditer(serial_regex, clean_line, re.MULTILINE)
+                for matchNum, match in enumerate(matches, start=1):
+                    print("CONFIG_BOARD_SERIAL_" + match.groups()[0] + "=\"" + match.groups()[1] + "\"")
+                    print("CONFIG_BOARD_SERIAL_" + match.groups()[0] + "=\"" + match.groups()[1] + "\"", file=w)
+                matches = re.finditer(io_regex, clean_line, re.MULTILINE)
+                for matchNum, match in enumerate(matches, start=1):
+                    print("CONFIG_BOARD_IO=\"" + match.groups()[0] + "\"")
+                    print("CONFIG_BOARD_IO=\"" + match.groups()[0] + "\"", file=w)
+                matches = re.finditer(romfs_regex, clean_line, re.MULTILINE)
+                for matchNum, match in enumerate(matches, start=1):
+                    print("CONFIG_BOARD_ROMFSROOT=\"" + match.groups()[0] + "\"")
+                    print("CONFIG_BOARD_ROMFSROOT=\"" + match.groups()[0] + "\"", file=w)
+                    romfs_set = True
+                matches = re.finditer(arch_regex, clean_line, re.MULTILINE)
+                for matchNum, match in enumerate(matches, start=1):
+                    print("CONFIG_BOARD_ARCHITECTURE=\"" + match.groups()[0] + "\"")
+                    print("CONFIG_BOARD_ARCHITECTURE=\"" + match.groups()[0] + "\"", file=w)
+                matches = re.finditer(toolchain_regex, clean_line, re.MULTILINE)
+                for matchNum, match in enumerate(matches, start=1):
+                    print("CONFIG_BOARD_TOOLCHAIN=\"" + match.groups()[0] + "\"")
+                    print("CONFIG_BOARD_TOOLCHAIN=\"" + match.groups()[0] + "\"", file=w)
+
+    if(romfs_set == False):
+        print("CONFIG_BOARD_ROMFSROOT=\"\"", file=w)
+
+
+    w.close()
@@ -0,0 +1,204 @@
+PLATFORM nuttx,CONFIG_PLATFORM_NUTTX=y
+PLATFORM posix,CONFIG_PLATFORM_POSIX=y
+CONSTRAINED_MEMORY,CONFIG_BOARD_CONSTRAINED_MEMORY=y
+CONSTRAINED_FLASH,CONFIG_BOARD_CONSTRAINED_FLASH=y
+NO_HELP,CONFIG_BOARD_NO_HELP=y
+EXTERNAL_METADATA,CONFIG_BOARD_EXTERNAL_METADATA=y
+BUILD_BOOTLOADER,CONFIG_BOARD_BUILD_BOOTLOADER=y
+UAVCAN_INTERFACES 2,CONFIG_BOARD_UAVCAN_INTERFACES=2
+UAVCAN_INTERFACES 1,CONFIG_BOARD_UAVCAN_INTERFACES=1
+UAVCAN_TIMER_OVERRIDE 2,CONFIG_BOARD_UAVCAN_TIMER_OVERRIDE=2
+UAVCAN_TIMER_OVERRIDE 1,CONFIG_BOARD_UAVCAN_TIMER_OVERRIDE=1
+UAVCAN_TIMER_OVERRIDE 1,CONFIG_BOARD_UAVCAN_TIMER_OVERRIDE=0
+TESTING,CONFIG_BOARD_TESTING=y
+ETHERNET,CONFIG_BOARD_ETHERNET=y
+adc/ads1115,CONFIG_DRIVERS_ADC_ADS1115=y
+adc/board_adc,CONFIG_DRIVERS_ADC_BOARD_ADC=y
+barometer,CONFIG_COMMON_BAROMETERS=y
+barometer/bmp280,CONFIG_DRIVERS_BAROMETER_BMP280=y
+barometer/bmp388,CONFIG_DRIVERS_BAROMETER_BMP388=y
+barometer/dps310,CONFIG_DRIVERS_BAROMETER_DPS310=y
+barometer/lps22hb,CONFIG_DRIVERS_BAROMETER_LPS22HB=y
+barometer/lps25h,CONFIG_DRIVERS_BAROMETER_LPS25H=y
+barometer/lps33hw,CONFIG_DRIVERS_BAROMETER_LPS33HW=y
+barometer/mpl3115a2,CONFIG_DRIVERS_BAROMETER_MPL3115A2=y
+barometer/ms5611,CONFIG_DRIVERS_BAROMETER_MS5611=y
+barometer/tcbp001ta,CONFIG_DRIVERS_BAROMETER_TCBP001TA=y
+batt_smbus,CONFIG_DRIVERS_BATT_SMBUS=y
+bootloaders,CONFIG_DRIVERS_BOOTLOADERS=y
+camera_capture,CONFIG_DRIVERS_CAMERA_CAPTURE=y
+camera_trigger,CONFIG_DRIVERS_CAMERA_TRIGGER=y
+differential_pressure,CONFIG_DRIVERS_DIFFERENTIAL_PRESSURE=y
+distance_sensor,CONFIG_COMMON_DISTANCE_SENSOR=y
+distance_sensor/ll40ls,CONFIG_DRIVERS_DISTANCE_SENSOR_LL40LS=y
+distance_sensor/lightware_laser_serial,CONFIG_DRIVERS_DISTANCE_SENSOR_LIGHTWARE_LASER_SERIAL=y
+distance_sensor/broadcom/afbrs50,CONFIG_DRIVERS_DISTANCE_SENSOR_BROADCOM_AFBRS50=y
+distance_sensor/vl53l0x,CONFIG_DRIVERS_DISTANCE_SENSOR_VL53L0X=y
+distance_sensor/vl53l1x,CONFIG_DRIVERS_DISTANCE_SENSOR_VL53L1X=y
+distance_sensor/srf05,CONFIG_DRIVERS_DISTANCE_SENSOR_SRF05=y
+dshot,CONFIG_DRIVERS_DSHOT=y
+gps,CONFIG_DRIVERS_GPS=y
+heater,CONFIG_DRIVERS_HEATER=y
+imu,CONFIG_COMMON_IMU=y
+imu/adis16477,CONFIG_DRIVERS_IMU_ADIS16477=y
+imu/adis16497,CONFIG_DRIVERS_IMU_ADIS16497=y
+imu/analog_devices/adis16448,CONFIG_DRIVERS_IMU_ANALOG_DEVICES_ADIS16448=y
+imu/analog_devices/adis16470,CONFIG_DRIVERS_IMU_ANALOG_DEVICES_ADIS16470=y
+imu/bosch/bmi055,CONFIG_DRIVERS_IMU_BOSCH_BMI055=y
+imu/bosch/bmi088,CONFIG_DRIVERS_IMU_BOSCH_BMI088=y
+imu/fxas21002c,CONFIG_DRIVERS_IMU_FXAS21002C=y
+imu/fxos8701cq,CONFIG_DRIVERS_IMU_FXOS8701CQ=y
+imu/invensense/icm20602,CONFIG_DRIVERS_IMU_INVENSENSE_ICM20602=y
+imu/invensense/icm20608g,CONFIG_DRIVERS_IMU_INVENSENSE_ICM20608G=y
+imu/invensense/icm20649,CONFIG_DRIVERS_IMU_INVENSENSE_ICM20649=y
+imu/invensense/icm20689,CONFIG_DRIVERS_IMU_INVENSENSE_ICM20689=y
+imu/invensense/icm20948,CONFIG_DRIVERS_IMU_INVENSENSE_ICM20948=y
+imu/invensense/icm40609d,CONFIG_DRIVERS_IMU_INVENSENSE_ICM40609D=y
+imu/invensense/icm42605,CONFIG_DRIVERS_IMU_INVENSENSE_ICM42605=y
+imu/invensense/icm42688p,CONFIG_DRIVERS_IMU_INVENSENSE_ICM42688P=y
+imu/invensense/mpu6000,CONFIG_DRIVERS_IMU_INVENSENSE_MPU6000=y
+imu/invensense/mpu6500,CONFIG_DRIVERS_IMU_INVENSENSE_MPU6500=y
+imu/invensense/mpu9250,CONFIG_DRIVERS_IMU_INVENSENSE_MPU9250=y
+imu/l3gd20,CONFIG_DRIVERS_IMU_L3GD20=y
+imu/lsm303d,CONFIG_DRIVERS_IMU_LSM303D=y
+imu/st,CONFIG_DRIVERS_IMU_ST=y
+irlock,CONFIG_DRIVERS_IRLOCK=y
+lights,CONFIG_COMMON_LIGHT=y
+lights/neopixel,CONFIG_DRIVERS_LIGHTS_NEOPIXEL=y
+lights/rgbled,CONFIG_DRIVERS_LIGHTS_RGBLED=y
+lights/rgbled_ncp5623c,CONFIG_DRIVERS_LIGHTS_RGBLED_NCP5623C=y
+lights/rgbled_pwm,CONFIG_DRIVERS_LIGHTS_RGBLED_PWM=y
+magnetometer,CONFIG_COMMON_MAGNETOMETER=y
+magnetometer/akm/ak09916,CONFIG_DRIVERS_MAGNETOMETER_AKM_AK09916=y
+magnetometer/akm/ak8963,CONFIG_DRIVERS_MAGNETOMETER_AKM_AK8963=y
+magnetometer/bosch/bmm150,CONFIG_DRIVERS_MAGNETOMETER_BOSCH_BMM150=y
+magnetometer/hmc5883,CONFIG_DRIVERS_MAGNETOMETER_HMC5883=y
+magnetometer/isentek/ist8308,CONFIG_DRIVERS_MAGNETOMETER_ISENTEK_IST8308=y
+magnetometer/isentek/ist8310,CONFIG_DRIVERS_MAGNETOMETER_ISENTEK_IST8310=y
+magnetometer/lis2mdl,CONFIG_DRIVERS_MAGNETOMETER_LIS2MDL=y
+magnetometer/lis3mdl,CONFIG_DRIVERS_MAGNETOMETER_LIS3MDL=y
+magnetometer/lsm303agr,CONFIG_DRIVERS_MAGNETOMETER_LSM303AGR=y
+magnetometer/lsm9ds1_mag,CONFIG_DRIVERS_MAGNETOMETER_LSM9DS1_MAG=y
+magnetometer/qmc5883l,CONFIG_DRIVERS_MAGNETOMETER_QMC5883L=y
+magnetometer/rm3100,CONFIG_DRIVERS_MAGNETOMETER_RM3100=y
+magnetometer/vtrantech/vcm1193l,CONFIG_DRIVERS_MAGNETOMETER_VTRANTECH_VCM1193L=y
+optical_flow,CONFIG_COMMON_OPTICAL_FLOW=y
+optical_flow/paw3902,CONFIG_DRIVERS_OPTICAL_FLOW_PAW3902=y
+optical_flow/paw3901,CONFIG_DRIVERS_OPTICAL_FLOW_PMW3901=y
+optical_flow/px4flow,CONFIG_DRIVERS_OPTICAL_FLOW_PX4FLOW=y
+optical_flow/thoneflow,CONFIG_DRIVERS_OPTICAL_FLOW_THONEFLOW=y
+osd,CONFIG_DRIVERS_OSD=y
+pca9685,CONFIG_DRIVERS_PCA9685=y
+pca9685_pwm_out,CONFIG_DRIVERS_PCA9685_PWM_OUT=y
+power_monitor/ina226,CONFIG_DRIVERS_POWER_MONITOR_INA226=y
+power_monitor/voxlpm,CONFIG_DRIVERS_POWER_MONITOR_VOXLPM=y
+pps_capture,CONFIG_DRIVERS_PPS_CAPTURE=y
+protocol_splitter,CONFIG_DRIVERS_PROTOCOL_SPLITTER=y
+pwm_input,CONFIG_DRIVERS_PWM_INPUT=y
+pwm_out_sim,CONFIG_DRIVERS_PWM_OUT_SIM=y
+pwm_out,CONFIG_DRIVERS_PWM_OUT=y
+px4io,CONFIG_DRIVERS_PX4IO=y
+rc_input,CONFIG_DRIVERS_RC_INPUT=y
+roboclaw,CONFIG_DRIVERS_ROBOCLAW=y
+rpi_rc_in,CONFIG_DRIVERS_RPI_RC_IN=y
+rpm,CONFIG_DRIVERS_RPM=y
+safety_button,CONFIG_DRIVERS_SAFETY_BUTTON=y
+smart_battery/batmon,CONFIG_DRIVERS_SMART_BATTERY_BATMON=y
+spektrum_rc,CONFIG_DRIVERS_SPEKTRUM_RC=y
+telemetry,CONFIG_DRIVERS_TELEMETRY=y
+test_ppm,CONFIG_DRIVERS_TEST_PPM=y
+tone_alarm,CONFIG_DRIVERS_TONE_ALARM=y
+uavcan,CONFIG_DRIVERS_UAVCAN=y
+uavcannode,CONFIG_DRIVERS_UAVCANNODE=y
+uavcannode_gps_demo,CONFIG_DRIVERS_UAVCANNODE_GPS_DEMO=y
+airship_att_control,CONFIG_MODULES_AIRSHIP_ATT_CONTROL=y
+airspeed_selector,CONFIG_MODULES_AIRSPEED_SELECTOR=y
+velocity_controller,CONFIG_MODULES_ANGULAR_VELOCITY_CONTROLLER=y
+attitude_estimator_q,CONFIG_MODULES_ATTITUDE_ESTIMATOR_Q=y
+battery_status,CONFIG_MODULES_BATTERY_STATUS=y
+camera_feedback,CONFIG_MODULES_CAMERA_FEEDBACK=y
+commander,CONFIG_MODULES_COMMANDER=y
+control_allocator,CONFIG_MODULES_CONTROL_ALLOCATOR=y
+dataman,CONFIG_MODULES_DATAMAN=y
+ekf2,CONFIG_MODULES_EKF2=y
+esc_battery,CONFIG_MODULES_ESC_BATTERY=y
+events,CONFIG_MODULES_EVENTS=y
+flight_mode_manager,CONFIG_MODULES_FLIGHT_MODE_MANAGER=y
+fw_att_control,CONFIG_MODULES_FW_ATT_CONTROL=y
+fw_pos_control_l1,CONFIG_MODULES_FW_POS_CONTROL_L1=y
+gyro_calibration,CONFIG_MODULES_GYRO_CALIBRATION=y
+gyro_fft,CONFIG_MODULES_GYRO_FFT=y
+land_detector,CONFIG_MODULES_LAND_DETECTOR=y
+landing_target_estimator,CONFIG_MODULES_LANDING_TARGET_ESTIMATOR=y
+load_mon,CONFIG_MODULES_LOAD_MON=y
+local_position_estimator,CONFIG_MODULES_LOCAL_POSITION_ESTIMATOR=y
+logger,CONFIG_MODULES_LOGGER=y
+mavlink,CONFIG_MODULES_MAVLINK=y
+mc_att_control,CONFIG_MODULES_MC_ATT_CONTROL=y
+mc_hover_thrust_estimator,CONFIG_MODULES_MC_HOVER_THRUST_ESTIMATOR=y
+mc_pos_control,CONFIG_MODULES_MC_POS_CONTROL=y
+mc_rate_control,CONFIG_MODULES_MC_RATE_CONTROL=y
+micrortps_bridge,CONFIG_MODULES_MICRORTPS_BRIDGE=y
+microdds_client,CONFIG_MODULES_MICRODDS_CLIENT=y
+navigator,CONFIG_MODULES_NAVIGATOR=y
+px4iofirmware,CONFIG_MODULES_PX4IOFIRMWARE=y
+rc_update,CONFIG_MODULES_RC_UPDATE=y
+replay,CONFIG_MODULES_REPLAY=y
+rover_pos_control,CONFIG_MODULES_ROVER_POS_CONTROL=y
+sensors,CONFIG_MODULES_SENSORS=y
+sih,CONFIG_MODULES_SIH=y
+simulator,CONFIG_MODULES_SIMULATOR=y
+temperature_compensation,CONFIG_MODULES_TEMPERATURE_COMPENSATION=y
+uuv_att_control,CONFIG_MODULES_UUV_ATT_CONTROL=y
+uuv_pos_control,CONFIG_MODULES_UUV_POS_CONTROL=y
+gimbal,CONFIG_MODULES_GIMBAL=y
+vtol_att_control,CONFIG_MODULES_VTOL_ATT_CONTROL=y
+bl_update,CONFIG_SYSTEMCMDS_BL_UPDATE=y
+dmesg,CONFIG_SYSTEMCMDS_DMESG=y
+dumpfile,CONFIG_SYSTEMCMDS_DUMPFILE=y
+dyn,CONFIG_SYSTEMCMDS_DYN=y
+failure,CONFIG_SYSTEMCMDS_FAILURE=y
+gpio,CONFIG_SYSTEMCMDS_GPIO=y
+hardfault_log,CONFIG_SYSTEMCMDS_HARDFAULT_LOG=y
+i2cdetect,CONFIG_SYSTEMCMDS_I2CDETECT=y
+led_control,CONFIG_SYSTEMCMDS_LED_CONTROL=y
+mft,CONFIG_SYSTEMCMDS_MFT=y
+microbench,CONFIG_SYSTEMCMDS_MICROBENCH=y
+mixer,CONFIG_SYSTEMCMDS_MIXER=y
+motor_test,CONFIG_SYSTEMCMDS_MOTOR_TEST=y
+mtd,CONFIG_SYSTEMCMDS_MTD=y
+netman,CONFIG_SYSTEMCMDS_NETMAN=y
+nshterm,CONFIG_SYSTEMCMDS_NSHTERM=y
+param,CONFIG_SYSTEMCMDS_PARAM=y
+perf,CONFIG_SYSTEMCMDS_PERF=y
+pwm,CONFIG_SYSTEMCMDS_PWM=y
+reboot,CONFIG_SYSTEMCMDS_REBOOT=y
+reflect,CONFIG_SYSTEMCMDS_REFLECT=y
+sd_bench,CONFIG_SYSTEMCMDS_SD_BENCH=y
+serial_tet,CONFIG_SYSTEMCMDS_SERIAL_TEST=y
+shutdown,CONFIG_SYSTEMCMDS_SHUTDOWN=y
+system_time,CONFIG_SYSTEMCMDS_SYSTEM_TIME=y
+tests,CONFIG_SYSTEMCMDS_TESTS=y
+top,CONFIG_SYSTEMCMDS_TOP=y
+topic_listener,CONFIG_SYSTEMCMDS_TOPIC_LISTENER=y
+tune_control,CONFIG_SYSTEMCMDS_TUNE_CONTROL=y
+uorb,CONFIG_SYSTEMCMDS_UORB=y
+usb_connected,CONFIG_SYSTEMCMDS_USB_CONNECTED=y
+ver,CONFIG_SYSTEMCMDS_VER=y
+work_queue,CONFIG_SYSTEMCMDS_WORK_QUEUE=y
+dyn_hello,CONFIG_EXAMPLES_DYN_HELLO=y
+fake_gps,CONFIG_EXAMPLES_FAKE_GPS=y
+fake_gyro,CONFIG_EXAMPLES_FAKE_GYRO=y
+fake_imu,CONFIG_EXAMPLES_FAKE_IMU=y
+fake_magnetometer,CONFIG_EXAMPLES_FAKE_MAGNETOMETER=y
+fixedwing_control,CONFIG_EXAMPLES_FIXEDWING_CONTROL=y
+hello,CONFIG_EXAMPLES_HELLO=y
+hwtest,CONFIG_EXAMPLES_HWTEST=y
+matlab_csv_serial,CONFIG_EXAMPLES_MATLAB_CSV_SERIAL=y
+px4_mavlink_debug,CONFIG_EXAMPLES_PX4_MAVLINK_DEBUG=y
+px4_simple_app,CONFIG_EXAMPLES_PX4_SIMPLE_APP=y
+rover_steering_control,CONFIG_EXAMPLES_ROVER_STEERING_CONTROL=y
+uuv_example_app,CONFIG_EXAMPLES_UUV_EXAMPLE_APP=y
+work_item,CONFIG_EXAMPLES_WORK_ITEM=y
+add_compile_options(-Wno-narrowing),CONFIG_BOARD_COMPILE_DEFINITIONS="-Wno-narrowing"
+-D__PX4_LINUX,CONFIG_BOARD_LINUX=y
@@ -16,7 +16,7 @@ class ModuleDocumentation(object):
    valid_categories = ['driver', 'estimator', 'controller', 'system',
                        'communication', 'command', 'template', 'simulation', 'autotune']
    valid_subcategories = ['', 'distance_sensor', 'imu', 'airspeed_sensor',
-                           'magnetometer', 'baro', 'optical_flow', 'rpm_sensor', 'transponder']
+                           'magnetometer', 'baro', 'optical_flow', 'rpm_sensor']

    max_line_length = 80 # wrap lines that are longer than this

@@ -74,9 +74,6 @@ if [ "$model" == "" ] || [ "$model" == "none" ]; then
 	if [ "$program" == "jsbsim" ]; then
 		echo "empty model, setting rascal as default for jsbsim"
 		model="rascal"
-	elif [ "$program" == "sihsim" ]; then
-		echo "empty model, setting quadx as default for sihsim"
-		model="quadx"
 	else
 		echo "empty model, setting iris as default"
 		model="iris"
@@ -217,12 +214,6 @@ elif [ "$program" == "jsbsim" ] && [ -z "$no_sim" ]; then
 	fi
 	"${build_path}/build_jsbsim_bridge/jsbsim_bridge" ${model} -s "${src_path}/Tools/jsbsim_bridge/scene/${world}.xml" 2> /dev/null &
 	JSBSIM_PID=$!
-elif [ "$program" == "sihsim" ] && [ ! -n "$no_sim" ]; then
-	export SIM_MODE="sihsim"
-	if [ "$model" != "airplane" ] && [ "$model" != "quadx" ] && [ "$model" != "xvert" ]; then
-		echo "Model ${model} not compatible with with sih. sih supports [quadx,airplane,xvert]."
-		exit 1
-	fi
 fi

 pushd "$rootfs" >/dev/null
@@ -2,7 +2,6 @@ CONFIG_BOARD_TOOLCHAIN="arm-none-eabi"
 CONFIG_BOARD_ARCHITECTURE="cortex-m4"
 CONFIG_BOARD_ROMFSROOT="cannode"
 CONFIG_BOARD_CONSTRAINED_FLASH=y
-CONFIG_BOARD_NO_HELP=y
 CONFIG_BOARD_CONSTRAINED_MEMORY=y
 CONFIG_DRIVERS_BOOTLOADERS=y
 CONFIG_DRIVERS_DISTANCE_SENSOR_BROADCOM_AFBRS50=y
@@ -11,16 +10,7 @@ CONFIG_DRIVERS_IMU_INVENSENSE_ICM42688P=y
 CONFIG_DRIVERS_OPTICAL_FLOW_PAW3902=y
 CONFIG_BOARD_UAVCAN_INTERFACES=1
 CONFIG_DRIVERS_UAVCANNODE=y
-CONFIG_MODULES_GYRO_CALIBRATION=y
-CONFIG_MODULES_SENSORS=y
-# CONFIG_SENSORS_VEHICLE_AIRSPEED is not set
-# CONFIG_SENSORS_VEHICLE_AIR_DATA is not set
-# CONFIG_SENSORS_VEHICLE_GPS_POSITION is not set
-# CONFIG_SENSORS_VEHICLE_MAGNETOMETER is not set
 CONFIG_SYSTEMCMDS_PARAM=y
 CONFIG_SYSTEMCMDS_PERF=y
 CONFIG_SYSTEMCMDS_TOP=y
 CONFIG_SYSTEMCMDS_TOPIC_LISTENER=y
-CONFIG_SYSTEMCMDS_UORB=y
-CONFIG_SYSTEMCMDS_VER=y
-CONFIG_SYSTEMCMDS_WORK_QUEUE=y
@@ -3,8 +3,6 @@
 # board sensors init
 #------------------------------------------------------------------------------

-param set-default IMU_GYRO_RATEMAX 1000
-
 # Internal SPI
 paw3902 -s start -Y 180

@@ -4,6 +4,7 @@ CONFIG_BOARD_ROMFSROOT="cannode"
 CONFIG_BOARD_CONSTRAINED_FLASH=y
 CONFIG_BOARD_NO_HELP=y
 CONFIG_BOARD_CONSTRAINED_MEMORY=y
+CONFIG_BOARD_EXTERNAL_METADATA=y
 CONFIG_DRIVERS_BAROMETER_BMP388=y
 CONFIG_DRIVERS_BOOTLOADERS=y
 CONFIG_DRIVERS_GPS=y
@@ -13,18 +14,5 @@ CONFIG_DRIVERS_SAFETY_BUTTON=y
 CONFIG_DRIVERS_TONE_ALARM=y
 CONFIG_BOARD_UAVCAN_INTERFACES=1
 CONFIG_DRIVERS_UAVCANNODE=y
-CONFIG_MODULES_GYRO_CALIBRATION=y
-CONFIG_MODULES_MAG_BIAS_ESTIMATOR=y
-CONFIG_MODULES_SENSORS=y
-# CONFIG_SENSORS_VEHICLE_AIRSPEED is not set
-# CONFIG_SENSORS_VEHICLE_AIR_DATA is not set
-# CONFIG_SENSORS_VEHICLE_GPS_POSITION is not set
 CONFIG_SYSTEMCMDS_PARAM=y
-CONFIG_SYSTEMCMDS_PERF=y
-CONFIG_SYSTEMCMDS_REBOOT=y
-CONFIG_SYSTEMCMDS_SYSTEM_TIME=y
-CONFIG_SYSTEMCMDS_TOP=y
 CONFIG_SYSTEMCMDS_TOPIC_LISTENER=y
-CONFIG_SYSTEMCMDS_UORB=y
-CONFIG_SYSTEMCMDS_VER=y
-CONFIG_SYSTEMCMDS_WORK_QUEUE=y
@@ -4,7 +4,6 @@
 #------------------------------------------------------------------------------

 param set-default CBRK_IO_SAFETY 0
-param set-default MBE_ENABLE 1

 safety_button start
 tone_alarm start
@@ -4,6 +4,7 @@ CONFIG_BOARD_ROMFSROOT="cannode"
 CONFIG_BOARD_CONSTRAINED_FLASH=y
 CONFIG_BOARD_NO_HELP=y
 CONFIG_BOARD_CONSTRAINED_MEMORY=y
+CONFIG_BOARD_EXTERNAL_METADATA=y
 CONFIG_DRIVERS_BAROMETER_BMP388=y
 CONFIG_DRIVERS_BOOTLOADERS=y
 CONFIG_DRIVERS_GPS=y
@@ -13,18 +14,5 @@ CONFIG_DRIVERS_SAFETY_BUTTON=y
 CONFIG_DRIVERS_TONE_ALARM=y
 CONFIG_BOARD_UAVCAN_INTERFACES=1
 CONFIG_DRIVERS_UAVCANNODE=y
-CONFIG_MODULES_GYRO_CALIBRATION=y
-CONFIG_MODULES_MAG_BIAS_ESTIMATOR=y
-CONFIG_MODULES_SENSORS=y
-# CONFIG_SENSORS_VEHICLE_AIRSPEED is not set
-# CONFIG_SENSORS_VEHICLE_AIR_DATA is not set
-# CONFIG_SENSORS_VEHICLE_GPS_POSITION is not set
 CONFIG_SYSTEMCMDS_PARAM=y
-CONFIG_SYSTEMCMDS_PERF=y
-CONFIG_SYSTEMCMDS_REBOOT=y
-CONFIG_SYSTEMCMDS_SYSTEM_TIME=y
-CONFIG_SYSTEMCMDS_TOP=y
 CONFIG_SYSTEMCMDS_TOPIC_LISTENER=y
-CONFIG_SYSTEMCMDS_UORB=y
-CONFIG_SYSTEMCMDS_VER=y
-CONFIG_SYSTEMCMDS_WORK_QUEUE=y
@@ -5,7 +5,6 @@

 param set-default CBRK_IO_SAFETY 0
 param set-default CANNODE_GPS_RTCM 1
-param set-default MBE_ENABLE 1

 safety_button start
 tone_alarm start
@@ -30,7 +30,6 @@ CONFIG_MODULES_MC_RATE_CONTROL=y
 CONFIG_MODULES_NAVIGATOR=y
 CONFIG_MODULES_RC_UPDATE=y
 CONFIG_MODULES_SENSORS=y
-# CONFIG_SENSORS_VEHICLE_AIRSPEED is not set
 CONFIG_SYSTEMCMDS_DMESG=y
 CONFIG_SYSTEMCMDS_HARDFAULT_LOG=y
 CONFIG_SYSTEMCMDS_MFT=y
@@ -141,7 +141,6 @@
 #define PX4_PWM_ALTERNATE_RANGES
 #define PWM_LOWEST_MIN 0
 #define PWM_MOTOR_OFF	0
-#define PWM_SERVO_STOP	0
 #define PWM_DEFAULT_MIN 20
 #define PWM_HIGHEST_MIN 0
 #define PWM_HIGHEST_MAX 255
@@ -30,7 +30,6 @@ CONFIG_MODULES_MC_RATE_CONTROL=y
 CONFIG_MODULES_NAVIGATOR=y
 CONFIG_MODULES_RC_UPDATE=y
 CONFIG_MODULES_SENSORS=y
-# CONFIG_SENSORS_VEHICLE_AIRSPEED is not set
 CONFIG_SYSTEMCMDS_DMESG=y
 CONFIG_SYSTEMCMDS_HARDFAULT_LOG=y
 CONFIG_SYSTEMCMDS_MIXER=y
@@ -142,7 +142,6 @@
 #define PX4_PWM_ALTERNATE_RANGES
 #define PWM_LOWEST_MIN 0
 #define PWM_MOTOR_OFF	0
-#define PWM_SERVO_STOP	0
 #define PWM_DEFAULT_MIN 20
 #define PWM_HIGHEST_MIN 0
 #define PWM_HIGHEST_MAX 255
@@ -1,8 +1,6 @@
 CONFIG_BOARD_TOOLCHAIN="arm-none-eabi"
 CONFIG_BOARD_ARCHITECTURE="cortex-m4"
 CONFIG_BOARD_ROMFSROOT="cannode"
-CONFIG_BOARD_CONSTRAINED_FLASH=y
-CONFIG_BOARD_NO_HELP=y
 CONFIG_BOARD_CONSTRAINED_MEMORY=y
 CONFIG_BOARD_COMPILE_DEFINITIONS="-DUSE_S_RGB_LED_DMA"
 CONFIG_DRIVERS_BAROMETER_MS5611=y
@@ -14,16 +12,5 @@ CONFIG_DRIVERS_SAFETY_BUTTON=y
 CONFIG_DRIVERS_TONE_ALARM=y
 CONFIG_BOARD_UAVCAN_INTERFACES=1
 CONFIG_DRIVERS_UAVCANNODE=y
-CONFIG_MODULES_SENSORS=y
-# CONFIG_SENSORS_VEHICLE_AIRSPEED is not set
-# CONFIG_SENSORS_VEHICLE_AIR_DATA is not set
-# CONFIG_SENSORS_VEHICLE_GPS_POSITION is not set
 CONFIG_SYSTEMCMDS_PARAM=y
-CONFIG_SYSTEMCMDS_PERF=y
-CONFIG_SYSTEMCMDS_REBOOT=y
-CONFIG_SYSTEMCMDS_SYSTEM_TIME=y
-CONFIG_SYSTEMCMDS_TOP=y
 CONFIG_SYSTEMCMDS_TOPIC_LISTENER=y
-CONFIG_SYSTEMCMDS_UORB=y
-CONFIG_SYSTEMCMDS_VER=y
-CONFIG_SYSTEMCMDS_WORK_QUEUE=y
@@ -118,8 +118,8 @@
 #define GPIO_nPOWER_IN_CAN              /* PG2  */ (GPIO_INPUT|GPIO_PULLUP|GPIO_PORTG|GPIO_PIN2)
 #define GPIO_nPOWER_IN_C                /* PG0  */ (GPIO_INPUT|GPIO_PULLUP|GPIO_PORTG|GPIO_PIN0)

-#define GPIO_nVDD_BRICK1_VALID          GPIO_nPOWER_IN_ADC /* Brick 1 is Chosen */
-#define GPIO_nVDD_BRICK2_VALID          GPIO_nPOWER_IN_CAN /* Brick 2 is Chosen  */
+#define GPIO_nVDD_BRICK1_VALID          GPIO_nPOWER_IN_CAN /* Brick 1 is Chosen */
+#define GPIO_nVDD_BRICK2_VALID          GPIO_nPOWER_IN_ADC /* Brick 2 is Chosen  */
 #define GPIO_nVDD_USB_VALID             GPIO_nPOWER_IN_C   /* USB     is Chosen */

 #define GPIO_VDD_5V_HIPOWER_EN          /* PD11 */ (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_2MHz|GPIO_OUTPUT_SET|GPIO_PORTD|GPIO_PIN11)
@@ -106,4 +106,3 @@ CONFIG_SYSTEMCMDS_USB_CONNECTED=y
 CONFIG_SYSTEMCMDS_VER=y
 CONFIG_SYSTEMCMDS_WORK_QUEUE=y
 CONFIG_EXAMPLES_FAKE_GPS=y
-#CONFIG_DRIVERS_TRANSPONDER_SAGETECH_MXS=y
@@ -7,7 +7,7 @@ CONFIG_BOARD_SERIAL_TEL1="/dev/ttyS0"
 CONFIG_BOARD_SERIAL_TEL2="/dev/ttyS1"
 CONFIG_DRIVERS_ADC_ADS1115=y
 CONFIG_DRIVERS_ADC_BOARD_ADC=y
-CONFIG_DRIVERS_BAROMETER_MS5611=y
+CONFIG_COMMON_BAROMETERS=y
 CONFIG_DRIVERS_BATT_SMBUS=y
 CONFIG_DRIVERS_CAMERA_CAPTURE=y
 CONFIG_DRIVERS_CAMERA_TRIGGER=y
@@ -24,13 +24,14 @@ CONFIG_COMMON_LIGHT=y
 CONFIG_COMMON_MAGNETOMETER=y
 CONFIG_COMMON_OPTICAL_FLOW=y
 CONFIG_DRIVERS_OSD=y
+CONFIG_DRIVERS_PCA9685=y
 CONFIG_DRIVERS_PCA9685_PWM_OUT=y
 CONFIG_DRIVERS_POWER_MONITOR_INA226=y
 CONFIG_DRIVERS_PWM_INPUT=y
 CONFIG_DRIVERS_PWM_OUT=y
 CONFIG_DRIVERS_PWM_OUT_SIM=y
 CONFIG_DRIVERS_PX4IO=y
-#CONFIG_DRIVERS_ROBOCLAW=y
+CONFIG_DRIVERS_ROBOCLAW=y
 CONFIG_DRIVERS_RPM=y
 CONFIG_DRIVERS_SMART_BATTERY_BATMON=y
 CONFIG_COMMON_TELEMETRY=y
@@ -41,7 +42,6 @@ CONFIG_MODULES_ATTITUDE_ESTIMATOR_Q=y
 CONFIG_MODULES_BATTERY_STATUS=y
 CONFIG_MODULES_CAMERA_FEEDBACK=y
 CONFIG_MODULES_COMMANDER=y
-CONFIG_MODULES_CONTROL_ALLOCATOR=y
 CONFIG_MODULES_DATAMAN=y
 CONFIG_MODULES_EKF2=y
 CONFIG_MODULES_ESC_BATTERY=y
@@ -50,7 +50,6 @@ CONFIG_MODULES_FLIGHT_MODE_MANAGER=y
 CONFIG_MODULES_FW_ATT_CONTROL=y
 CONFIG_MODULES_FW_AUTOTUNE_ATTITUDE_CONTROL=y
 CONFIG_MODULES_FW_POS_CONTROL_L1=y
-CONFIG_MODULES_GIMBAL=y
 CONFIG_MODULES_GYRO_CALIBRATION=y
 CONFIG_MODULES_GYRO_FFT=y
 CONFIG_MODULES_LAND_DETECTOR=y
@@ -66,15 +65,17 @@ CONFIG_MODULES_MC_AUTOTUNE_ATTITUDE_CONTROL=y
 CONFIG_MODULES_MC_HOVER_THRUST_ESTIMATOR=y
 CONFIG_MODULES_MC_POS_CONTROL=y
 CONFIG_MODULES_MC_RATE_CONTROL=y
+CONFIG_MODULES_CONTROL_ALLOCATOR=y
 CONFIG_MODULES_NAVIGATOR=y
 CONFIG_MODULES_RC_UPDATE=y
 CONFIG_MODULES_ROVER_POS_CONTROL=y
 CONFIG_MODULES_SENSORS=y
 CONFIG_MODULES_SIH=y
 CONFIG_MODULES_TEMPERATURE_COMPENSATION=y
+CONFIG_MODULES_GIMBAL=y
 CONFIG_MODULES_VTOL_ATT_CONTROL=y
-CONFIG_SYSTEMCMDS_ACTUATOR_TEST=y
 CONFIG_SYSTEMCMDS_BL_UPDATE=y
+CONFIG_SYSTEMCMDS_ACTUATOR_TEST=y
 CONFIG_SYSTEMCMDS_DMESG=y
 CONFIG_SYSTEMCMDS_DUMPFILE=y
 CONFIG_SYSTEMCMDS_GPIO=y
@@ -92,6 +93,7 @@ CONFIG_SYSTEMCMDS_PWM=y
 CONFIG_SYSTEMCMDS_REBOOT=y
 CONFIG_SYSTEMCMDS_REFLECT=y
 CONFIG_SYSTEMCMDS_SD_BENCH=y
+CONFIG_SYSTEMCMDS_SD_STRESS=y
 CONFIG_SYSTEMCMDS_SYSTEM_TIME=y
 CONFIG_SYSTEMCMDS_TOP=y
 CONFIG_SYSTEMCMDS_TOPIC_LISTENER=y
@@ -100,4 +102,4 @@ CONFIG_SYSTEMCMDS_UORB=y
 CONFIG_SYSTEMCMDS_USB_CONNECTED=y
 CONFIG_SYSTEMCMDS_VER=y
 CONFIG_SYSTEMCMDS_WORK_QUEUE=y
-CONFIG_DRIVERS_TRANSPONDER_SAGETECH_MXS=y
+CONFIG_EXAMPLES_FAKE_GPS=y
@@ -5,6 +5,7 @@ CONFIG_DRIVERS_PCA9685=n
 CONFIG_DRIVERS_PCA9685_PWM_OUT=n
 CONFIG_DRIVERS_RPM=n
 CONFIG_DRIVERS_UAVCAN=n
+CONFIG_EXAMPLES_FAKE_GPS=n
 CONFIG_MODULES_ATTITUDE_ESTIMATOR_Q=n
 CONFIG_MODULES_LOCAL_POSITION_ESTIMATOR=n
 CONFIG_MODULES_ROVER_POS_CONTROL=n
@@ -12,4 +13,3 @@ CONFIG_MODULES_TEMPERATURE_COMPENSATION=n
 CONFIG_BOARD_TESTING=y
 CONFIG_DRIVERS_TEST_PPM=y
 CONFIG_SYSTEMCMDS_MICROBENCH=y
-CONFIG_SYSTEMCMDS_SD_STRESS=y
@@ -175,8 +175,8 @@ CONFIG_SYSTEM_CDCACM=y
 CONFIG_SYSTEM_NSH=y
 CONFIG_TASK_NAME_SIZE=24
 CONFIG_UART4_BAUD=57600
-CONFIG_UART4_RXBUFSIZE=600
-CONFIG_UART4_TXBUFSIZE=600
+CONFIG_UART4_RXBUFSIZE=300
+CONFIG_UART4_TXBUFSIZE=900
 CONFIG_USART1_RXBUFSIZE=300
 CONFIG_USART1_RXDMA=y
 CONFIG_USART1_TXBUFSIZE=300
@@ -1,22 +1,16 @@
 CONFIG_BOARD_TOOLCHAIN="arm-none-eabi"
 CONFIG_BOARD_ARCHITECTURE="cortex-m7"
 CONFIG_BOARD_ROMFSROOT="cannode"
-CONFIG_BOARD_CONSTRAINED_FLASH=y
-CONFIG_BOARD_NO_HELP=y
-CONFIG_BOARD_CONSTRAINED_MEMORY=y
 CONFIG_DRIVERS_BAROMETER_BMP388=y
 CONFIG_DRIVERS_BOOTLOADERS=y
 CONFIG_DRIVERS_GPS=y
+CONFIG_DRIVERS_IMU_ST=y
 CONFIG_DRIVERS_LIGHTS_RGBLED_NCP5623C=y
 CONFIG_DRIVERS_MAGNETOMETER_ISENTEK_IST8310=y
 CONFIG_BOARD_UAVCAN_INTERFACES=1
 CONFIG_DRIVERS_UAVCANNODE=y
-CONFIG_MODULES_GYRO_CALIBRATION=y
-CONFIG_MODULES_MAG_BIAS_ESTIMATOR=y
+CONFIG_MODULES_LOAD_MON=y
 CONFIG_MODULES_SENSORS=y
-# CONFIG_SENSORS_VEHICLE_AIRSPEED is not set
-# CONFIG_SENSORS_VEHICLE_AIR_DATA is not set
-# CONFIG_SENSORS_VEHICLE_GPS_POSITION is not set
 CONFIG_SYSTEMCMDS_LED_CONTROL=y
 CONFIG_SYSTEMCMDS_MFT=y
 CONFIG_SYSTEMCMDS_MTD=y
@@ -25,7 +19,5 @@ CONFIG_SYSTEMCMDS_PERF=y
 CONFIG_SYSTEMCMDS_REBOOT=y
 CONFIG_SYSTEMCMDS_SYSTEM_TIME=y
 CONFIG_SYSTEMCMDS_TOP=y
-CONFIG_SYSTEMCMDS_TOPIC_LISTENER=y
-CONFIG_SYSTEMCMDS_UORB=y
 CONFIG_SYSTEMCMDS_VER=y
 CONFIG_SYSTEMCMDS_WORK_QUEUE=y
@@ -2,7 +2,6 @@ CONFIG_BOARD_TOOLCHAIN="arm-none-eabi"
 CONFIG_BOARD_ARCHITECTURE="cortex-m4"
 CONFIG_BOARD_ROMFSROOT="cannode"
 CONFIG_BOARD_CONSTRAINED_FLASH=y
-CONFIG_BOARD_NO_HELP=y
 CONFIG_BOARD_CONSTRAINED_MEMORY=y
 CONFIG_DRIVERS_ADC_BOARD_ADC=y
 CONFIG_DRIVERS_BAROMETER_BMP388=y
@@ -13,18 +12,6 @@ CONFIG_DRIVERS_LIGHTS_RGBLED_NCP5623C=y
 CONFIG_DRIVERS_MAGNETOMETER_BOSCH_BMM150=y
 CONFIG_BOARD_UAVCAN_INTERFACES=1
 CONFIG_DRIVERS_UAVCANNODE=y
-CONFIG_MODULES_GYRO_CALIBRATION=y
-CONFIG_MODULES_MAG_BIAS_ESTIMATOR=y
 CONFIG_MODULES_SENSORS=y
-# CONFIG_SENSORS_VEHICLE_AIRSPEED is not set
-# CONFIG_SENSORS_VEHICLE_AIR_DATA is not set
-# CONFIG_SENSORS_VEHICLE_GPS_POSITION is not set
 CONFIG_SYSTEMCMDS_PARAM=y
-CONFIG_SYSTEMCMDS_PERF=y
-CONFIG_SYSTEMCMDS_REBOOT=y
-CONFIG_SYSTEMCMDS_SYSTEM_TIME=y
-CONFIG_SYSTEMCMDS_TOP=y
 CONFIG_SYSTEMCMDS_TOPIC_LISTENER=y
-CONFIG_SYSTEMCMDS_UORB=y
-CONFIG_SYSTEMCMDS_VER=y
-CONFIG_SYSTEMCMDS_WORK_QUEUE=y
@@ -1,6 +1,5 @@
 CONFIG_BOARD_TOOLCHAIN="arm-none-eabi"
 CONFIG_BOARD_ARCHITECTURE="cortex-m7"
-CONFIG_BOARD_CONSTRAINED_FLASH=y
 CONFIG_BOARD_NO_HELP=y
 CONFIG_BOARD_EXTERNAL_METADATA=y
 CONFIG_BOARD_SERIAL_GPS1="/dev/ttyS3"
@@ -32,7 +31,6 @@ CONFIG_MODULES_MC_RATE_CONTROL=y
 CONFIG_MODULES_NAVIGATOR=y
 CONFIG_MODULES_RC_UPDATE=y
 CONFIG_MODULES_SENSORS=y
-# CONFIG_SENSORS_VEHICLE_AIRSPEED is not set
 CONFIG_SYSTEMCMDS_MIXER=y
 CONFIG_SYSTEMCMDS_PARAM=y
 CONFIG_SYSTEMCMDS_PWM=y
@@ -15,10 +15,10 @@ CONFIG_DRIVERS_DSHOT=y
 CONFIG_DRIVERS_GPS=y
 CONFIG_DRIVERS_IMU_INVENSENSE_ICM20689=y
 CONFIG_DRIVERS_IMU_INVENSENSE_MPU6000=y
+CONFIG_DRIVERS_OSD=y
 CONFIG_COMMON_LIGHT=y
 CONFIG_COMMON_MAGNETOMETER=y
 CONFIG_COMMON_OPTICAL_FLOW=y
-CONFIG_DRIVERS_OSD=y
 CONFIG_DRIVERS_POWER_MONITOR_INA226=y
 CONFIG_DRIVERS_PWM_OUT=y
 CONFIG_DRIVERS_PWM_OUT_SIM=y
@@ -28,8 +28,8 @@ CONFIG_DRIVERS_RPM=y
 CONFIG_DRIVERS_SMART_BATTERY_BATMON=y
 CONFIG_COMMON_TELEMETRY=y
 CONFIG_DRIVERS_TONE_ALARM=y
-CONFIG_MODULES_AIRSPEED_SELECTOR=y
 CONFIG_MODULES_ATTITUDE_ESTIMATOR_Q=y
+CONFIG_MODULES_AIRSPEED_SELECTOR=y
 CONFIG_MODULES_BATTERY_STATUS=y
 CONFIG_MODULES_CAMERA_FEEDBACK=y
 CONFIG_MODULES_COMMANDER=y
@@ -42,7 +42,6 @@ CONFIG_MODULES_FLIGHT_MODE_MANAGER=y
 CONFIG_MODULES_FW_ATT_CONTROL=y
 CONFIG_MODULES_FW_AUTOTUNE_ATTITUDE_CONTROL=y
 CONFIG_MODULES_FW_POS_CONTROL_L1=y
-CONFIG_MODULES_GIMBAL=y
 CONFIG_MODULES_GYRO_CALIBRATION=y
 CONFIG_MODULES_GYRO_FFT=y
 CONFIG_MODULES_LAND_DETECTOR=y
@@ -61,12 +60,12 @@ CONFIG_MODULES_NAVIGATOR=y
 CONFIG_MODULES_RC_UPDATE=y
 CONFIG_MODULES_ROVER_POS_CONTROL=y
 CONFIG_MODULES_SENSORS=y
-# CONFIG_SENSORS_VEHICLE_AIRSPEED is not set
 CONFIG_MODULES_SIH=y
 CONFIG_MODULES_TEMPERATURE_COMPENSATION=y
+CONFIG_MODULES_GIMBAL=y
 CONFIG_MODULES_VTOL_ATT_CONTROL=y
-CONFIG_SYSTEMCMDS_ACTUATOR_TEST=y
 CONFIG_SYSTEMCMDS_BL_UPDATE=y
+CONFIG_SYSTEMCMDS_ACTUATOR_TEST=y
 CONFIG_SYSTEMCMDS_DMESG=y
 CONFIG_SYSTEMCMDS_DUMPFILE=y
 CONFIG_SYSTEMCMDS_GPIO=y
@@ -1,8 +1,6 @@
 CONFIG_BOARD_TOOLCHAIN="arm-none-eabi"
 CONFIG_BOARD_ARCHITECTURE="cortex-m4"
 CONFIG_BOARD_ROMFSROOT="cannode"
-CONFIG_BOARD_CONSTRAINED_FLASH=y
-CONFIG_BOARD_NO_HELP=y
 CONFIG_BOARD_CONSTRAINED_MEMORY=y
 CONFIG_BOARD_SERIAL_GPS1="/dev/ttyS2"
 CONFIG_DRIVERS_BAROMETER_DPS310=y
@@ -12,20 +10,13 @@ CONFIG_DRIVERS_IMU_INVENSENSE_ICM20602=y
 CONFIG_DRIVERS_MAGNETOMETER_RM3100=y
 CONFIG_BOARD_UAVCAN_INTERFACES=1
 CONFIG_DRIVERS_UAVCANNODE=y
-CONFIG_MODULES_GYRO_CALIBRATION=y
-CONFIG_MODULES_MAG_BIAS_ESTIMATOR=y
 CONFIG_MODULES_SENSORS=y
-# CONFIG_SENSORS_VEHICLE_AIRSPEED is not set
-# CONFIG_SENSORS_VEHICLE_AIR_DATA is not set
-# CONFIG_SENSORS_VEHICLE_GPS_POSITION is not set
 CONFIG_SYSTEMCMDS_I2CDETECT=y
 CONFIG_SYSTEMCMDS_NSHTERM=y
 CONFIG_SYSTEMCMDS_PARAM=y
-CONFIG_SYSTEMCMDS_PERF=y
 CONFIG_SYSTEMCMDS_REBOOT=y
 CONFIG_SYSTEMCMDS_SERIAL_PASSTHRU=y
 CONFIG_SERIAL_PASSTHRU_UBLOX=y
-CONFIG_SYSTEMCMDS_SYSTEM_TIME=y
 CONFIG_SYSTEMCMDS_TOP=y
 CONFIG_SYSTEMCMDS_TOPIC_LISTENER=y
 CONFIG_SYSTEMCMDS_UORB=y
@@ -86,9 +86,9 @@
 #define DIRECT_PWM_OUTPUT_CHANNELS  8

 /* Power supply control and monitoring GPIOs */
-#define GPIO_POWER_IN_A                 /* PB5 */ (GPIO_INPUT|GPIO_PULLUP|GPIO_PORTB|GPIO_PIN5)
+#define GPIO_nPOWER_IN_A                /* PB5 */ (GPIO_INPUT|GPIO_PULLUP|GPIO_PORTB|GPIO_PIN5)

-#define GPIO_VDD_BRICK1_VALID           GPIO_POWER_IN_A /* Brick 1 Is Chosen */
+#define GPIO_nVDD_BRICK1_VALID          GPIO_nPOWER_IN_A /* Brick 1 Is Chosen */
 #define BOARD_NUMBER_BRICKS             1

 #define GPIO_VDD_3V3_SPEKTRUM_POWER_EN  /* PE4  */ (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_2MHz|GPIO_OUTPUT_CLEAR|GPIO_PORTE|GPIO_PIN4)
@@ -143,7 +143,7 @@
 */
 #define BOARD_ADC_USB_CONNECTED (px4_arch_gpioread(GPIO_OTGFS_VBUS))
 #define BOARD_ADC_USB_VALID     BOARD_ADC_USB_CONNECTED
-#define BOARD_ADC_BRICK_VALID   (px4_arch_gpioread(GPIO_VDD_BRICK1_VALID))
+#define BOARD_ADC_BRICK_VALID   (!px4_arch_gpioread(GPIO_nVDD_BRICK1_VALID))

 #define BOARD_NUM_IO_TIMERS 3
 #define BOARD_DMA_ALLOC_POOL_SIZE 5120 /* This board provides a DMA pool and APIs */
@@ -160,7 +160,7 @@
 		GPIO_CAN2_SILENT_S0,              \
 		GPIO_LEVEL_SHIFTER_OE,            \
 		GPIO_PWM_VOLT_SEL,                \
-		GPIO_POWER_IN_A,                  \
+		GPIO_nPOWER_IN_A,                 \
 		GPIO_VDD_3V3_SPEKTRUM_POWER_EN,   \
 		PX4_GPIO_PIN_OFF(GPIO_SDMMC1_D0), \
 		PX4_GPIO_PIN_OFF(GPIO_SDMMC1_D1), \
@@ -198,9 +198,9 @@ CONFIG_SYSTEM_CDCACM=y
 CONFIG_SYSTEM_NSH=y
 CONFIG_TASK_NAME_SIZE=24
 CONFIG_UART4_BAUD=57600
-CONFIG_UART4_RXBUFSIZE=1200
-CONFIG_UART4_RXDMA=n
-CONFIG_UART4_TXBUFSIZE=1200
+CONFIG_UART4_RXBUFSIZE=600
+CONFIG_UART4_RXDMA=y
+CONFIG_UART4_TXBUFSIZE=1500
 CONFIG_UART7_BAUD=57600
 CONFIG_UART7_RXBUFSIZE=600
 CONFIG_UART7_SERIAL_CONSOLE=y
@@ -225,10 +225,10 @@ CONFIG_USART3_RXBUFSIZE=600
 CONFIG_USART3_RXDMA=y
 CONFIG_USART3_TXBUFSIZE=3000
 CONFIG_USART6_BAUD=57600
-CONFIG_USART6_RXBUFSIZE=1200
-CONFIG_USART6_RXDMA=n
-CONFIG_USART6_TXBUFSIZE=1200
-CONFIG_USART6_TXDMA=n
+CONFIG_USART6_RXBUFSIZE=600
+CONFIG_USART6_RXDMA=y
+CONFIG_USART6_TXBUFSIZE=1500
+CONFIG_USART6_TXDMA=y
 CONFIG_USBDEV=y
 CONFIG_USBDEV_BUSPOWERED=y
 CONFIG_USBDEV_MAXPOWER=500
@@ -32,7 +32,6 @@ CONFIG_MODULES_MC_RATE_CONTROL=y
 CONFIG_MODULES_NAVIGATOR=y
 CONFIG_MODULES_RC_UPDATE=y
 CONFIG_MODULES_SENSORS=y
-# CONFIG_SENSORS_VEHICLE_AIRSPEED is not set
 CONFIG_SYSTEMCMDS_HARDFAULT_LOG=y
 CONFIG_SYSTEMCMDS_MIXER=y
 CONFIG_SYSTEMCMDS_PARAM=y
@@ -1,9 +1,11 @@
-# CONFIG_BOARD_UAVCAN_TIMER_OVERRIDE is not set
+CONFIG_BOARD_PROTECTED=y
 CONFIG_COMMON_BAROMETERS=n
 CONFIG_COMMON_DIFFERENTIAL_PRESSURE=n
 CONFIG_COMMON_DISTANCE_SENSOR=n
 CONFIG_COMMON_OPTICAL_FLOW=n
 CONFIG_COMMON_TELEMETRY=n
+CONFIG_DRIVERS_BAROMETER_MS5611=y
+CONFIG_DRIVERS_DISTANCE_SENSOR_LL40LS=y
 CONFIG_DRIVERS_OSD=n
 CONFIG_DRIVERS_ROBOCLAW=n
 CONFIG_DRIVERS_RPM=n
@@ -15,8 +17,8 @@ CONFIG_MODULES_CAMERA_FEEDBACK=n
 CONFIG_MODULES_ESC_BATTERY=n
 CONFIG_MODULES_FW_ATT_CONTROL=n
 CONFIG_MODULES_FW_AUTOTUNE_ATTITUDE_CONTROL=n
+CONFIG_MODULES_FW_POS_CONTROL=n
 CONFIG_MODULES_FW_POS_CONTROL_L1=n
-CONFIG_MODULES_GYRO_FFT=n
 CONFIG_MODULES_ROVER_POS_CONTROL=n
 CONFIG_MODULES_SIH=n
 CONFIG_MODULES_TEMPERATURE_COMPENSATION=n
@@ -27,9 +29,11 @@ CONFIG_SYSTEMCMDS_ACTUATOR_TEST=n
 CONFIG_SYSTEMCMDS_BL_UPDATE=n
 CONFIG_SYSTEMCMDS_DMESG=n
 CONFIG_SYSTEMCMDS_DUMPFILE=n
+CONFIG_SYSTEMCMDS_ESC_CALIB=n
 CONFIG_SYSTEMCMDS_GPIO=n
 CONFIG_SYSTEMCMDS_I2CDETECT=n
 CONFIG_SYSTEMCMDS_LED_CONTROL=n
+CONFIG_SYSTEMCMDS_MOTOR_RAMP=n
 CONFIG_SYSTEMCMDS_MOTOR_TEST=n
 CONFIG_SYSTEMCMDS_MTD=n
 CONFIG_SYSTEMCMDS_NSHTERM=n
@@ -39,6 +43,3 @@ CONFIG_SYSTEMCMDS_SD_STRESS=n
 CONFIG_SYSTEMCMDS_SERIAL_TEST=n
 CONFIG_SYSTEMCMDS_SYSTEM_TIME=n
 CONFIG_SYSTEMCMDS_USB_CONNECTED=n
-CONFIG_BOARD_PROTECTED=y
-CONFIG_DRIVERS_BAROMETER_MS5611=y
-CONFIG_DRIVERS_DISTANCE_SENSOR_LL40LS=y
@@ -4,9 +4,7 @@ CONFIG_DRIVERS_IRLOCK=n
 CONFIG_DRIVERS_PCA9685=n
 CONFIG_DRIVERS_PCA9685_PWM_OUT=n
 CONFIG_DRIVERS_RPM=n
-CONFIG_EXAMPLES_FAKE_GPS=n
 CONFIG_MODULES_ATTITUDE_ESTIMATOR_Q=n
-CONFIG_MODULES_GYRO_FFT=n
 CONFIG_MODULES_LOCAL_POSITION_ESTIMATOR=n
 CONFIG_MODULES_ROVER_POS_CONTROL=n
 CONFIG_BOARD_TESTING=y
@@ -1,5 +1,4 @@
 # CONFIG_BOARD_UAVCAN_TIMER_OVERRIDE is not set
-CONFIG_COMMON_BAROMETERS=n
 CONFIG_COMMON_DIFFERENTIAL_PRESSURE=n
 CONFIG_COMMON_HYGROMETERS=n
 CONFIG_COMMON_TELEMETRY=n
@@ -22,7 +21,6 @@ CONFIG_MODULES_GYRO_FFT=n
 CONFIG_MODULES_LOCAL_POSITION_ESTIMATOR=n
 CONFIG_MODULES_ROVER_POS_CONTROL=n
 CONFIG_MODULES_SIH=n
-CONFIG_MODULES_TEMPERATURE_COMPENSATION=n
 CONFIG_MODULES_UUV_ATT_CONTROL=n
 CONFIG_MODULES_UUV_POS_CONTROL=n
 CONFIG_SYSTEMCMDS_BL_UPDATE=n
@@ -32,7 +30,5 @@ CONFIG_SYSTEMCMDS_REFLECT=n
 CONFIG_SYSTEMCMDS_SD_BENCH=n
 CONFIG_SYSTEMCMDS_SD_STRESS=n
 CONFIG_SYSTEMCMDS_SERIAL_TEST=n
-CONFIG_SYSTEMCMDS_SYSTEM_TIME=n
 CONFIG_SYSTEMCMDS_TOPIC_LISTENER=n
 CONFIG_BOARD_UAVCAN_PERIPHERALS="cuav_can-gps-v1_default"
-CONFIG_DRIVERS_BAROMETER_MS5611=y
@@ -1,4 +1,3 @@
-CONFIG_COMMON_BAROMETERS=n
 CONFIG_COMMON_TELEMETRY=n
 CONFIG_DRIVERS_OSD=n
 CONFIG_EXAMPLES_FAKE_GPS=n
@@ -9,7 +8,5 @@ CONFIG_SYSTEMCMDS_REFLECT=n
 CONFIG_SYSTEMCMDS_SD_BENCH=n
 CONFIG_SYSTEMCMDS_SD_STRESS=n
 CONFIG_SYSTEMCMDS_SERIAL_TEST=n
-CONFIG_DRIVERS_BAROMETER_BMP388=y
-CONFIG_DRIVERS_BAROMETER_MS5611=y
 CONFIG_MODULES_MICRODDS_CLIENT=y
 CONFIG_MODULES_MICRORTPS_BRIDGE=y
@@ -34,6 +34,7 @@ CONFIG_DRIVERS_PWM_OUT_SIM=y
 CONFIG_DRIVERS_PX4IO=y
 CONFIG_DRIVERS_RC_INPUT=y
 CONFIG_DRIVERS_SAFETY_BUTTON=y
+CONFIG_COMMON_TELEMETRY=y
 CONFIG_DRIVERS_TONE_ALARM=y
 CONFIG_DRIVERS_UAVCAN=y
 CONFIG_BOARD_UAVCAN_TIMER_OVERRIDE=2
@@ -47,7 +47,7 @@ then
 	fi
 fi

-if ver hwtypecmp V6X04 V6X14 V6X54
+if ver hwtypecmp V6X04 V6X14
 then
 	# Internal SPI bus ICM20649
 	icm20649 -s -R 6 start
@@ -60,7 +60,7 @@ fi
 # Internal SPI bus ICM42688p
 icm42688p -R 6 -s start

-if ver hwtypecmp V6X03 V6X13 V6X04 V6X14 V6X53 V6X54
+if ver hwtypecmp V6X03 V6X13 V6X04 V6X14
 then
 	# Internal SPI bus ICM-42670-P (hard-mounted)
 	icm42670p -R 10 -s start
@@ -39,7 +39,6 @@ CONFIG_MODULES_RC_UPDATE=y
 CONFIG_MODULES_REPLAY=y
 CONFIG_MODULES_ROVER_POS_CONTROL=y
 CONFIG_MODULES_SENSORS=y
-CONFIG_MODULES_SIH=y
 CONFIG_MODULES_SIMULATOR=y
 CONFIG_MODULES_TEMPERATURE_COMPENSATION=y
 CONFIG_MODULES_UUV_ATT_CONTROL=y
@@ -41,7 +41,6 @@ CONFIG_MODULES_MC_RATE_CONTROL=y
 CONFIG_MODULES_NAVIGATOR=y
 CONFIG_MODULES_RC_UPDATE=y
 CONFIG_MODULES_SENSORS=y
-# CONFIG_SENSORS_VEHICLE_AIRSPEED is not set
 CONFIG_SYSTEMCMDS_DMESG=y
 CONFIG_SYSTEMCMDS_DUMPFILE=y
 CONFIG_SYSTEMCMDS_HARDFAULT_LOG=y
@@ -65,25 +65,24 @@ set(msg_files
 	differential_pressure.msg
 	distance_sensor.msg
 	ekf2_timestamps.msg
+	estimator_gps_status.msg
 	esc_report.msg
 	esc_status.msg
-	estimator_aid_source_1d.msg
-	estimator_aid_source_2d.msg
-	estimator_aid_source_3d.msg
 	estimator_baro_bias.msg
 	estimator_event_flags.msg
-	estimator_gps_status.msg
 	estimator_innovations.msg
+	estimator_optical_flow_vel.msg
 	estimator_selector_status.msg
 	estimator_sensor_bias.msg
 	estimator_states.msg
 	estimator_status.msg
 	estimator_status_flags.msg
+	estimator_aid_source_1d.msg
+	estimator_aid_source_2d.msg
+	estimator_aid_source_3d.msg
 	event.msg
-	failure_detector_status.msg
 	follow_target.msg
-	follow_target_estimator.msg
-	follow_target_status.msg
+	failure_detector_status.msg
 	generator_status.msg
 	geofence_result.msg
 	gimbal_device_attitude_status.msg
@@ -98,8 +97,8 @@ set(msg_files
 	heater_status.msg
 	home_position.msg
 	hover_thrust_estimate.msg
-	input_rc.msg
 	internal_combustion_engine_status.msg
+	input_rc.msg
 	iridiumsbd_status.msg
 	irlock_report.msg
 	landing_gear.msg
@@ -122,16 +121,18 @@ set(msg_files
 	obstacle_distance.msg
 	offboard_control_mode.msg
 	onboard_computer_status.msg
+	optical_flow.msg
 	orbit_status.msg
 	parameter_update.msg
+	pasta_information.msg
 	ping.msg
+	pps_capture.msg
 	position_controller_landing_status.msg
 	position_controller_status.msg
 	position_setpoint.msg
 	position_setpoint_triplet.msg
 	power_button_state.msg
 	power_monitor.msg
-	pps_capture.msg
 	pwm_input.msg
 	px4io_status.msg
 	radio_status.msg
@@ -146,18 +147,17 @@ set(msg_files
 	sensor_baro.msg
 	sensor_combined.msg
 	sensor_correction.msg
-	sensor_gnss_relative.msg
 	sensor_gps.msg
+	sensor_gnss_relative.msg
 	sensor_gyro.msg
 	sensor_gyro_fft.msg
 	sensor_gyro_fifo.msg
 	sensor_hygrometer.msg
 	sensor_mag.msg
-	sensor_optical_flow.msg
 	sensor_preflight_mag.msg
 	sensor_selection.msg
-	sensors_status.msg
 	sensors_status_imu.msg
+	sensors_status.msg
 	system_power.msg
 	takeoff_status.msg
 	task_stack_info.msg
@@ -175,8 +175,8 @@ set(msg_files
 	uavcan_parameter_value.msg
 	ulog_stream.msg
 	ulog_stream_ack.msg
-	uwb_distance.msg
 	uwb_grid.msg
+	uwb_distance.msg
 	vehicle_acceleration.msg
 	vehicle_air_data.msg
 	vehicle_angular_acceleration.msg
@@ -197,8 +197,6 @@ set(msg_files
 	vehicle_local_position_setpoint.msg
 	vehicle_magnetometer.msg
 	vehicle_odometry.msg
-	vehicle_optical_flow.msg
-	vehicle_optical_flow_vel.msg
 	vehicle_rates_setpoint.msg
 	vehicle_roi.msg
 	vehicle_status.msg
@@ -0,0 +1,8 @@
+uint64 timestamp			# time since system start (microseconds)
+uint64 timestamp_sample			# the timestamp of the raw data (microseconds)
+
+float32[2] vel_body			# velocity obtained from gyro-compensated and distance-scaled optical flow raw measurements in body frame(m/s)
+float32[2] vel_ne			# same as vel_body but in local frame (m/s)
+float32[2] flow_uncompensated_integral	# integrated optical flow measurement (rad)
+float32[2] flow_compensated_integral	# integrated optical flow measurement compensated for angular motion (rad)
+float32[3] gyro_rate_integral		# gyro measurement integrated to flow rate and synchronized with flow measurements (rad)
@@ -1,11 +1,8 @@
-uint64 timestamp  # time since system start (microseconds)
-
-float64 lat       # target position (deg * 1e7)
-float64 lon       # target position (deg * 1e7)
-float32 alt       # target position
-
-float32 vy        # target vel in y
-float32 vx        # target vel in x
-float32 vz        # target vel in z
-
-uint8 est_cap     # target reporting capabilities
+uint64 timestamp		# time since system start (microseconds)
+float64 lat			# target position (deg * 1e7)
+float64 lon			# target position (deg * 1e7) 
+float32 alt			# target position
+float32 vy			# target vel in y
+float32 vx			# target vel in x
+float32 vz			# target vel in z 
+uint8 est_cap # target reporting capabilities
@@ -1,16 +0,0 @@
-uint64 timestamp                     # time since system start (microseconds)
-uint64 last_filter_reset_timestamp   # time of last filter reset (microseconds)
-
-bool valid              # True if estimator states are okay to be used
-bool stale              # True if estimator stopped receiving follow_target messages for some time. The estimate can still be valid, though it might be inaccurate.
-
-float64 lat_est         # Estimated target latitude
-float64 lon_est         # Estimated target longitude
-float32 alt_est         # Estimated target altitude
-
-float32[3] pos_est      # Estimated target NED position (m)
-float32[3] vel_est      # Estimated target NED velocity (m/s)
-float32[3] acc_est      # Estimated target NED acceleration (m^2/s)
-
-uint64 prediction_count
-uint64 fusion_count
@@ -1,12 +0,0 @@
-uint64 timestamp                  # [microseconds] time since system start
-
-float32 tracked_target_course     # [rad] Tracked target course in NED local frame (North is course zero)
-float32 follow_angle              # [rad] Current follow angle setting
-
-float32 orbit_angle_setpoint      # [rad] Current orbit angle setpoint from the smooth trajectory generator
-float32 angular_rate_setpoint     # [rad/s] Angular rate commanded from Jerk-limited Orbit Angle trajectory for Orbit Angle
-
-float32[3] desired_position_raw   # [m] Raw 'idealistic' desired drone position if a drone could teleport from place to places
-
-bool in_emergency_ascent          # [bool] True when doing emergency ascent (when distance to ground is below safety altitude)
-float32 gimbal_pitch              # [rad] Gimbal pitch commanded to track target in the center of the frame
@@ -0,0 +1,29 @@
+# Optical flow in XYZ body frame in SI units.
+# http://en.wikipedia.org/wiki/International_System_of_Units
+
+uint64 timestamp		# time since system start (microseconds)
+
+uint8 sensor_id			# id of the sensor emitting the flow value
+float32 pixel_flow_x_integral	# accumulated optical flow in radians where a positive value is produced by a RH rotation about the X body axis
+float32 pixel_flow_y_integral	# accumulated optical flow in radians where a positive value is produced by a RH rotation about the Y body axis
+float32 gyro_x_rate_integral	# accumulated gyro value in radians where a positive value is produced by a RH rotation about the X body axis. Set to NaN if flow sensor does not have 3-axis gyro data.
+float32 gyro_y_rate_integral	# accumulated gyro value in radians where a positive value is produced by a RH rotation about the Y body axis. Set to NaN if flow sensor does not have 3-axis gyro data.
+float32 gyro_z_rate_integral	# accumulated gyro value in radians where a positive value is produced by a RH rotation about the Z body axis. Set to NaN if flow sensor does not have 3-axis gyro data.
+float32 ground_distance_m	# Altitude / distance to ground in meters
+uint32 integration_timespan	# accumulation timespan in microseconds
+uint32 time_since_last_sonar_update	# time since last sonar update in microseconds
+uint16 frame_count_since_last_readout	# number of accumulated frames in timespan
+int16 gyro_temperature	# Temperature * 100 in centi-degrees Celsius
+uint8 quality	# Average of quality of accumulated frames, 0: bad quality, 255: maximum quality
+
+float32 max_flow_rate # Magnitude of maximum angular which the optical flow sensor can measure reliably
+float32 min_ground_distance # Minimum distance from ground at which the optical flow sensor operates reliably
+float32 max_ground_distance # Maximum distance from ground at which the optical flow sensor operates reliably
+
+
+uint8 MODE_UNKNOWN        = 0
+uint8 MODE_BRIGHT         = 1
+uint8 MODE_LOWLIGHT       = 2
+uint8 MODE_SUPER_LOWLIGHT = 3
+
+uint8 mode
@@ -0,0 +1,33 @@
+# Message with information for a dish of pasta
+uint64 timestamp			# [us] time when the topic was published
+
+uint16 customer_table_id		# customer’s table ID to know where to serve
+
+uint8 menu_name				# menu, e.g. Carbonara, Amatriciana
+
+uint8 PASTA_MENU_UNDEFINED = 0		# Undefined: Default value if no value is set
+uint8 PASTA_MENU_CARBONARA = 1		# Carbonara: With Egg, Pecorino and Guanciale!
+uint8 PASTA_MENU_AMATRICIANA = 2	# Amatriciana: With Tomato, Pecorino and Guanciale!
+uint8 PASTA_MENU_AGLIO_E_OLIO = 3	# Aglio E Olio: With Olive oil and Garlic!
+uint8 PASTA_MENU_BOLOGNESE = 4		# Bolognese: With Beef and Tomato!
+
+uint8 cooked_texture			# how cooked the pasta should be, e.g. Al Dente
+
+uint8 PASTA_COOKED_UNDEFINED = 0	# Undefined: Default value if no value is set
+uint8 PASTA_COOKED_AL_DENTE = 1		# Al Dente: https://en.wikipedia.org/wiki/Al_dente
+uint8 PASTA_COOKED_RAW = 2		# Barely cooked
+
+uint8 pasta_type			# type of pasta, e.g. Spaghetti, Lasagne, Rigatoni
+
+uint8 PASTA_TYPE_UNDEFINED = 0		# Undefined: Default value if no value is set
+uint8 PASTA_TYPE_SPAGHETTI = 1		# Spaghetti: Long, stringy pasta
+uint8 PASTA_TYPE_RIGATONI = 2		# Rigatoni: Cylindrical pasta perfect for Carbonara!
+uint8 PASTA_TYPE_LASAGNE = 3		# Lasagne: Flat, big pasta that gets layered on each other
+
+float32 pasta_temperature		# [deg C] temperature of the pasta
+
+# TOPICS pasta_cook pasta_order
+
+# The topic pub/sub flow would be as follows:
+# Customer -> 'pasta_order' -> Waiter
+# Waiter -> 'pasta_cook' -> Chef
@@ -19,7 +19,7 @@ bool status_rc_ppm
 bool status_rc_sbus
 bool status_rc_st24
 bool status_rc_sumd
-bool status_safety_button_event # px4io safety button was pressed for longer than 1 second
+bool status_safety_off

 # PX4IO alarms (PX4IO_P_STATUS_ALARMS)
 bool alarm_pwm_error
@@ -1,30 +0,0 @@
-uint64 timestamp               # time since system start (microseconds)
-uint64 timestamp_sample
-
-uint32 device_id               # unique device ID for the sensor that does not change between power cycles
-
-float32[2] pixel_flow          # (radians) optical flow in radians where a positive value is produced by a RH rotation about the body axis
-
-float32[3] delta_angle         # (radians) accumulated gyro radians where a positive value is produced by a RH rotation about the body axis. Set to NaN if flow sensor does not have 3-axis gyro data.
-bool delta_angle_available
-
-float32 distance_m             # (meters) Distance to the center of the flow field
-bool distance_available
-
-uint32 integration_timespan_us # (microseconds) accumulation timespan in microseconds
-
-uint8 quality                  # quality, 0: bad quality, 255: maximum quality
-
-uint32 error_count
-
-float32 max_flow_rate          # (radians/s) Magnitude of maximum angular which the optical flow sensor can measure reliably
-
-float32 min_ground_distance    # (meters) Minimum distance from ground at which the optical flow sensor operates reliably
-float32 max_ground_distance    # (meters) Maximum distance from ground at which the optical flow sensor operates reliably
-
-uint8 MODE_UNKNOWN        = 0
-uint8 MODE_BRIGHT         = 1
-uint8 MODE_LOWLIGHT       = 2
-uint8 MODE_SUPER_LOWLIGHT = 3
-
-uint8 mode
@@ -1,6 +0,0 @@
-module_name: Sagtech MXS
-serial_config:
-    - command: mxs start -d ${SERIAL_DEV} -b p:${BAUD_PARAM}
-      port_config_param:
-        name: TRANS_MXS_CFG
-        group: Transponders
@@ -40,7 +40,7 @@ rtps:
    receive: true
  - msg:     offboard_control_mode
    receive: true
-  - msg:     sensor_optical_flow
+  - msg:     optical_flow
    receive: true
  - msg:     position_setpoint
    receive: true
@@ -37,8 +37,8 @@ uint16 VEHICLE_CMD_DO_SET_SERVO = 183			# Set a servo to a desired PWM value. |S
 uint16 VEHICLE_CMD_DO_REPEAT_SERVO = 184		# Cycle a between its nominal setting and a desired PWM for a desired number of cycles with a desired period. |Servo number| PWM (microseconds, 1000 to 2000 typical)| Cycle count| Cycle time (seconds)| Empty| Empty| Empty|
 uint16 VEHICLE_CMD_DO_FLIGHTTERMINATION = 185		# Terminate flight immediately |Flight termination activated if > 0.5| Empty| Empty| Empty| Empty| Empty| Empty|
 uint16 VEHICLE_CMD_DO_SET_ACTUATOR = 187		# Sets actuators (e.g. servos) to a desired value. |Actuator 1| Actuator 2| Actuator 3| Actuator 4| Actuator 5| Actuator 6| Index|
-uint16 VEHICLE_CMD_DO_LAND_START = 189			# Mission command to perform a landing. This is used as a marker in a mission to tell the autopilot where a sequence of mission items that represents a landing starts. It may also be sent via a COMMAND_LONG to trigger a landing, in which case the nearest (geographically) landing sequence in the mission will be used. The Latitude/Longitude is optional, and may be set to 0/0 if not needed. If specified then it will be used to help find the closest landing sequence. |Empty| Empty| Empty| Empty| Latitude| Longitude| Empty|
-uint16 VEHICLE_CMD_DO_GO_AROUND = 191			# Mission command to safely abort an autonmous landing. |Altitude (meters)| Empty| Empty| Empty| Empty| Empty| Empty|
+uint16 VEHICLE_CMD_DO_LAND_START = 189			# Mission command to perform a landing. This is used as a marker in a mission to tell the autopilot where a sequence of mission items that represents a landing starts. It may also be sent via a COMMAND_LONG to trigger a landing, in which case the nearest (geographically) landing sequence in the mission will be used. The Latitude/Longitude is optional, and may be set to 0/0 if not needed. If specified then it will be used to help find the closest landing sequence. |Empty| Empty| Empty| Empty| Latitude| Longitude| Empty|  */
+uint16 VEHICLE_CMD_DO_GO_AROUND = 191			# Mission command to safely abort an autonmous landing. |Altitude (meters)| Empty| Empty| Empty| Empty| Empty| Empty|  */
 uint16 VEHICLE_CMD_DO_REPOSITION = 192
 uint16 VEHICLE_CMD_DO_PAUSE_CONTINUE = 193
 uint16 VEHICLE_CMD_DO_SET_ROI_LOCATION = 195	# Sets the region of interest (ROI) to a location. This can then be used by the vehicles control system to control the vehicle attitude and the attitude of various sensors such as cameras. |Empty| Empty| Empty| Empty| Latitude| Longitude| Altitude|
@@ -1,21 +0,0 @@
-# Optical flow in XYZ body frame in SI units.
-
-uint64 timestamp               # time since system start (microseconds)
-uint64 timestamp_sample
-
-uint32 device_id               # unique device ID for the sensor that does not change between power cycles
-
-float32[2] pixel_flow          # (radians) accumulated optical flow in radians where a positive value is produced by a RH rotation about the body axis
-
-float32[3] delta_angle         # (radians) accumulated gyro radians where a positive value is produced by a RH rotation about the body axis. (NAN if unavailable)
-
-float32 distance_m             # (meters) Distance to the center of the flow field (NAN if unavailable)
-
-uint32 integration_timespan_us # (microseconds) accumulation timespan in microseconds
-
-uint8 quality                  # Average of quality of accumulated frames, 0: bad quality, 255: maximum quality
-
-float32 max_flow_rate          # (radians/s) Magnitude of maximum angular which the optical flow sensor can measure reliably
-
-float32 min_ground_distance    # (meters) Minimum distance from ground at which the optical flow sensor operates reliably
-float32 max_ground_distance    # (meters) Maximum distance from ground at which the optical flow sensor operates reliably
@@ -1,13 +0,0 @@
-uint64 timestamp                       # time since system start (microseconds)
-uint64 timestamp_sample                # the timestamp of the raw data (microseconds)
-
-float32[2] vel_body                    # velocity obtained from gyro-compensated and distance-scaled optical flow raw measurements in body frame(m/s)
-float32[2] vel_ne                      # same as vel_body but in local frame (m/s)
-
-float32[2] flow_uncompensated_integral # integrated optical flow measurement (rad)
-float32[2] flow_compensated_integral   # integrated optical flow measurement compensated for angular motion (rad)
-
-float32[3] gyro_rate                   # gyro measurement synchronized with flow measurements (rad/s)
-float32[3] gyro_rate_integral          # gyro measurement integrated to flow rate and synchronized with flow measurements (rad)
-
-# TOPICS estimator_optical_flow_vel vehicle_optical_flow_vel
@@ -117,5 +117,5 @@ uint64 armed_time       # Arming timestamp (microseconds)

 uint64 takeoff_time     # Takeoff timestamp (microseconds)

-bool safety_button_available # Set to true if a safety button is connected
-bool safety_off # Set to true if safety is off
+bool safety_button_available	# Set to true if a safety button is connected
+bool safety_off					# Set to true if safety is off
@@ -3,7 +3,8 @@
 uint64 timestamp				# time since system start (microseconds)

 bool calibration_enabled
-bool pre_flight_checks_pass		# true if all checks necessary to arm pass
+bool system_sensors_initialized
+bool system_hotplug_timeout		# true if the hotplug sensor search is over
 bool auto_mission_available
 bool angular_velocity_valid
 bool attitude_valid
@@ -3,7 +3,6 @@ uint64 timestamp_sample         # the timestamp of the raw data (microseconds)

 float32 yaw_composite	# composite yaw from GSF (rad)
 float32 yaw_variance	# composite yaw variance from GSF (rad^2)
-bool yaw_composite_valid

 float32[5] yaw		# yaw estimate for each model in the filter bank (rad)
 float32[5] innov_vn	# North velocity innovation for each model in the filter bank (m/s)
@@ -9,6 +9,7 @@ add_custom_command(OUTPUT ${PX4_BINARY_DIR}/logs
 )
 add_custom_target(logs_symlink DEPENDS ${PX4_BINARY_DIR}/logs)
 add_dependencies(px4 logs_symlink)
+
 add_custom_target(run_config
 	COMMAND Tools/sitl_run.sh $<TARGET_FILE:px4> ${config_sitl_debugger} ${config_sitl_viewer} ${config_sitl_model} ${PX4_SOURCE_DIR} ${PX4_BINARY_DIR}
 	WORKING_DIRECTORY ${SITL_WORKING_DIR}
@@ -158,7 +159,6 @@ set(models
 	iris_rplidar
 	iris_vision
 	nxp_cupcar
-	omnicopter
 	plane
 	plane_cam
 	plane_catapult
@@ -324,70 +324,6 @@ foreach(debugger ${debuggers})
 	endforeach()
 endforeach()

-# create targets for sihsim
-set(models_sih
-	none
-	airplane
-	quadx
-	xvert
-)
-
-set(worlds_sih
-	none
-)
-
-foreach(debugger ${debuggers})
-	foreach(model ${models_sih})
-		foreach(world ${worlds_sih})
-			if(world STREQUAL "none")
-				if(debugger STREQUAL "none")
-					if(model STREQUAL "none")
-						set(_targ_name "sihsim")
-					else()
-						set(_targ_name "sihsim_${model}")
-					endif()
-				else()
-					if(model STREQUAL "none")
-						set(_targ_name "sihsim__${debugger}_${world}")
-					else()
-						set(_targ_name "sihsim_${model}_${debugger}_${world}")
-					endif()
-				endif()
-
-				add_custom_target(${_targ_name}
-					COMMAND ${PX4_SOURCE_DIR}/Tools/sitl_run.sh $<TARGET_FILE:px4> ${debugger} sihsim ${model} ${world} ${PX4_SOURCE_DIR} ${PX4_BINARY_DIR}
-					WORKING_DIRECTORY ${SITL_WORKING_DIR}
-					USES_TERMINAL
-					DEPENDS logs_symlink
-				)
-				list(APPEND all_posix_vmd_make_targets ${_targ_name})
-			else()
-				if(debugger STREQUAL "none")
-					if(model STREQUAL "none")
-						set(_targ_name "sihsim___${world}")
-					else()
-						set(_targ_name "sihsim_${model}__${world}")
-					endif()
-				else()
-					if(model STREQUAL "none")
-						set(_targ_name "sihsim___${debugger}_${world}")
-					else()
-						set(_targ_name "sihsim_${model}_${debugger}_${world}")
-					endif()
-				endif()
-
-				add_custom_target(${_targ_name}
-					COMMAND ${PX4_SOURCE_DIR}/Tools/sitl_run.sh $<TARGET_FILE:px4> ${debugger} sihsim ${model} ${world} ${PX4_SOURCE_DIR} ${PX4_BINARY_DIR}
-					WORKING_DIRECTORY ${SITL_WORKING_DIR}
-					USES_TERMINAL
-					DEPENDS logs_symlink
-				)
-				list(APPEND all_posix_vmd_make_targets ${_targ_name})
-			endif()
-		endforeach()
-	endforeach()
-endforeach()
-
 # add flighgear targets
 if(ENABLE_LOCKSTEP_SCHEDULER STREQUAL "no")
 	set(models
@@ -84,7 +84,7 @@ void LockstepComponents::unregister_component(int component)

 	int components_used_bitset = _components_used_bitset;

-	if (_components_progress_bitset == components_used_bitset) {
+	if (_components_progress_bitset == components_used_bitset && components_used_bitset != 0) {
 		_components_progress_bitset = 0;
 		px4_sem_post(&_components_sem);
 	}
@@ -1,6 +1,6 @@
 /****************************************************************************
 *
- *   Copyright (C) 2015-2022 PX4 Development Team. All rights reserved.
+ *   Copyright (C) 2015-2016 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
@@ -43,32 +43,28 @@
 #include <string>
 #include <sstream>
 #include <vector>
-#include <algorithm>
 #include <stdio.h>
-#include <poll.h>
-#include <fcntl.h>
-#include <unistd.h>

 #include "pxh.h"

 namespace px4_daemon
 {

-apps_map_type Pxh::_apps = {};
 Pxh *Pxh::_instance = nullptr;

+apps_map_type Pxh::_apps = {};
+
+
 Pxh::Pxh()
 {
 	_history.try_to_add("commander takeoff"); // for convenience
 	_history.reset_to_end();
+	_instance = this;
 }

 Pxh::~Pxh()
 {
-	if (_local_terminal) {
-		tcsetattr(0, TCSANOW, &_orig_term);
-		_instance = nullptr;
-	}
+	_instance = nullptr;
 }

 int Pxh::process_line(const std::string &line, bool silently_fail)
@@ -137,167 +133,11 @@ int Pxh::process_line(const std::string &line, bool silently_fail)
 	}
 }

-void Pxh::_check_remote_uorb_command(std::string &line)
-{
-
-	if (line.empty()) {
-		return;
-	}
-
-	std::stringstream line_stream(line);
-	std::string word;
-
-	line_stream >> word;
-
-	if (word == "uorb") {
-		line += " -1";  // Run uorb command only once
-	}
-}
-
-void Pxh::run_remote_pxh(int remote_in_fd, int remote_out_fd)
-{
-	std::string mystr;
-	int p1[2], pipe_stdout;
-	int p2[2], pipe_stderr;
-	int backup_stdout_fd = dup(STDOUT_FILENO);
-	int backup_stderr_fd = dup(STDERR_FILENO);
-
-	if (pipe(p1) != 0) {
-		perror("Remote shell pipe creation failed");
-		return;
-	}
-
-	if (pipe(p2) != 0) {
-		perror("Remote shell pipe 2 creation failed");
-		close(p1[0]);
-		close(p1[1]);
-		return;
-	}
-
-	// Create pipe to receive stdout and stderr
-	dup2(p1[1], STDOUT_FILENO);
-	close(p1[1]);
-
-	dup2(p2[1], STDERR_FILENO);
-	close(p2[1]);
-
-	pipe_stdout = p1[0];
-	pipe_stderr = p2[0];
-
-	// Set fds for non-blocking operation
-	fcntl(pipe_stdout, F_SETFL, fcntl(pipe_stdout, F_GETFL) | O_NONBLOCK);
-	fcntl(pipe_stderr, F_SETFL, fcntl(pipe_stderr, F_GETFL) | O_NONBLOCK);
-	fcntl(remote_in_fd, F_SETFL, fcntl(remote_in_fd, F_GETFL) | O_NONBLOCK);
-
-	// Check for input on any pipe (i.e. stdout, stderr, or remote_in_fd
-	// stdout and stderr will be sent to the local terminal and a copy of the data
-	// will be sent over to the mavlink shell through the remote_out_fd.
-	//
-	// Any data from remote_in_fd will be process as shell commands when an '\n' is received
-	while (!_should_exit) {
-
-		struct pollfd fds[3] { {pipe_stderr, POLLIN}, {pipe_stdout, POLLIN}, {remote_in_fd, POLLIN}};
-
-		if (poll(fds, 3, -1) == -1) {
-			perror("Mavlink Shell Poll Error");
-			break;
-		}
-
-		if (fds[0].revents & POLLIN) {
-
-			uint8_t buffer[512];
-			size_t len;
-
-			if ((len = read(pipe_stderr, buffer, sizeof(buffer))) <= 0) {
-				break; //EOF or ERROR
-			}
-
-			// Send all the stderr data to the local terminal as well as the remote shell
-			if (write(backup_stderr_fd, buffer, len) <= 0) {
-				perror("Remote shell write stdout");
-				break;
-			}
-
-			if (write(remote_out_fd, buffer, len) <= 0) {
-				perror("Remote shell write");
-				break;
-			}
-
-			// Process all the stderr data first
-			continue;
-		}
-
-		if (fds[1].revents & POLLIN) {
-
-			uint8_t buffer[512];
-			size_t len;
-
-			if ((len = read(pipe_stdout, buffer, sizeof(buffer))) <= 0) {
-				break; //EOF or ERROR
-			}
-
-			// Send all the stdout data to the local terminal as well as the remote shell
-			if (write(backup_stdout_fd, buffer, len) <= 0) {
-				perror("Remote shell write stdout");
-				break;
-			}
-
-			if (write(remote_out_fd, buffer, len) <= 0) {
-				perror("Remote shell write");
-				break;
-			}
-		}
-
-		if (fds[2].revents & POLLIN) {
-
-			char c;
-
-			if (read(remote_in_fd, &c, 1) <= 0) {
-				break; // EOF or ERROR
-			}
-
-			switch (c) {
-
-			case '\n':	// user hit enter
-				printf("\n");
-				_check_remote_uorb_command(mystr);
-				process_line(mystr, false);
-				// reset string
-				mystr = "";
-
-				_print_prompt();
-
-				break;
-
-			default:	// any other input
-				if (c > 3) {
-					fprintf(stdout, "%c", c);
-					fflush(stdout);
-					mystr += (char)c;
-				}
-
-				break;
-			}
-		}
-	}
-
-	// Restore stdout and stderr
-	dup2(backup_stdout_fd, STDOUT_FILENO);
-	dup2(backup_stderr_fd, STDERR_FILENO);
-	close(backup_stdout_fd);
-	close(backup_stderr_fd);
-
-	close(pipe_stdout);
-	close(pipe_stderr);
-	close(remote_in_fd);
-	close(remote_out_fd);
-}

 void Pxh::run_pxh()
 {
-	// Only the local_terminal needed for static calls
-	_instance = this;
-	_local_terminal = true;
+	_should_exit = false;
+
 	_setup_term();

 	std::string mystr;
@@ -314,10 +154,7 @@ void Pxh::run_pxh()

 		switch (c) {
 		case EOF:
-			break;
-
-		case '\t':
-			_tab_completion(mystr);
+			_should_exit = true;
 			break;

 		case 127:	// backslash
@@ -393,6 +230,7 @@ void Pxh::run_pxh()
 			break;
 		}

+
 		if (update_prompt) {
 			// reprint prompt with mystr
 			mystr.insert(mystr.length() - cursor_position, add_string);
@@ -405,11 +243,14 @@ void Pxh::run_pxh()
 				_move_cursor(cursor_position);
 			}
 		}
+
 	}
 }

 void Pxh::stop()
 {
+	_restore_term();
+
 	if (_instance) {
 		_instance->_should_exit = true;
 	}
@@ -453,101 +294,4 @@ void Pxh::_move_cursor(int position)
 	printf("\033[%dD", position);
 }

-void Pxh::_tab_completion(std::string &mystr)
-{
-	// parse line and get command
-	std::stringstream line(mystr);
-	std::string cmd;
-	line >> cmd;
-
-	// cmd is empty or white space send a list of available commands
-	if (cmd.size() == 0) {
-
-		printf("\n");
-
-		for (auto it = _apps.begin(); it != _apps.end();  ++it) {
-			printf("%s ", it->first.c_str());
-		}
-
-		printf("\n");
-		mystr = "";
-
-	} else {
-
-		// find tab completion matches
-		std::vector<std::string> matches;
-
-		for (auto it = _apps.begin(); it != _apps.end();  ++it) {
-			if (it->first.compare(0, cmd.size(), cmd) == 0) {
-				matches.push_back(it->first);
-			}
-		}
-
-		if (matches.size() >= 1) {
-			// if more than one match print all matches
-			if (matches.size() != 1) {
-				printf("\n");
-
-				for (const auto &item : matches) {
-					printf("%s    ", item.c_str());
-				}
-
-				printf("\n");
-			}
-
-			// find minimum size match
-			size_t min_size = 0;
-
-			for (const auto &item : matches) {
-				if (min_size == 0) {
-					min_size = item.size();
-
-				} else if (item.size() < min_size) {
-					min_size = item.size();
-				}
-			}
-
-			// parse through elements to find longest match
-			std::string longest_match;
-			bool done = false;
-
-			for (int i = 0; i < (int)min_size ; ++i) {
-				bool first_time = true;
-
-				for (const auto &item : matches) {
-					if (first_time) {
-						longest_match += item[i];
-						first_time = false;
-
-					} else if (longest_match[i] != item[i]) {
-						done = true;
-						longest_match.pop_back();
-						break;
-					}
-				}
-
-				if (done) { break; }
-
-				mystr = longest_match;
-			}
-		}
-
-		std::string flags;
-
-		while (line >> cmd) {
-			flags += " " + cmd;
-		}
-
-		// add flags back in when there is a command match
-		if (matches.size() == 1) {
-			if (flags.empty()) {
-				mystr += " ";
-
-			} else {
-				mystr += flags;
-			}
-		}
-	}
-}
-
 } // namespace px4_daemon
@@ -1,6 +1,6 @@
 /****************************************************************************
 *
- *   Copyright (C) 2016-2022 PX4 Development Team. All rights reserved.
+ *   Copyright (C) 2016 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
@@ -72,12 +72,7 @@ public:
 	void run_pxh();

 	/**
-	 * Run the remote mavlink pxh shell.
-	 */
-	void run_remote_pxh(int remote_in_fd, int remote_out_fd);
-
-	/**
-	 * Can be called to stop all pxh shells.
+	 * Can be called to stop pxh.
 	 */
 	static void stop();

@@ -85,14 +80,11 @@ private:
 	void _print_prompt();
 	void _move_cursor(int position);
 	void _clear_line();
-	void _tab_completion(std::string &prefix);
-	void _check_remote_uorb_command(std::string &line);

 	void _setup_term();
 	static void _restore_term();

 	bool _should_exit{false};
-	bool _local_terminal{false};
 	History _history;
 	struct termios _orig_term {};

@@ -150,12 +150,8 @@ Server::_server_main()
 		int n_ready = poll(poll_fds.data(), poll_fds.size(), -1);

 		if (n_ready < 0) {
-			// Reboot command causes System Interrupt to stop poll(). This is not an error
-			if (errno != EINTR) {
-				PX4_ERR("poll() failed: %s", strerror(errno));
-			}
-
-			break;
+			PX4_ERR("poll() failed: %s", strerror(errno));
+			return;
 		}

 		_lock();
@@ -2,6 +2,8 @@ menu "Differential pressure"
    menuconfig COMMON_DIFFERENTIAL_PRESSURE
        bool "Common differential pressure module's"
        default n
+        select DRIVERS_DIFFERENTIAL_PRESSURE_ETS
+        select DRIVERS_DIFFERENTIAL_PRESSURE_MS4515
        select DRIVERS_DIFFERENTIAL_PRESSURE_MS4525DO
        select DRIVERS_DIFFERENTIAL_PRESSURE_MS5525DSO
        select DRIVERS_DIFFERENTIAL_PRESSURE_SDP3X
@@ -86,11 +86,6 @@ struct pwm_output_values {
 */
 #define PWM_MOTOR_OFF	900

-/**
- * Default value for a servo stop
- */
-#define PWM_SERVO_STOP	1500
-
 /**
 * Default minimum PWM in us
 */
@@ -193,6 +188,12 @@ typedef uint16_t	servo_position_t;
 /** get the maximum PWM value the output will send */
 #define PWM_SERVO_GET_MAX_PWM	_PX4_IOC(_PWM_SERVO_BASE, 19)

+/** force safety switch off (to disable use of safety switch) */
+#define PWM_SERVO_SET_FORCE_SAFETY_OFF		_PX4_IOC(_PWM_SERVO_BASE, 25)
+
+/** force safety switch on (to enable use of safety switch) */
+#define PWM_SERVO_SET_FORCE_SAFETY_ON		_PX4_IOC(_PWM_SERVO_BASE, 28)
+
 /*
 *
 *
@@ -68,7 +68,6 @@

 #define DRV_IMU_DEVTYPE_SIM 0x14
 #define DRV_DIFF_PRESS_DEVTYPE_SIM 0x15
-#define DRV_FLOW_DEVTYPE_SIM 0x16

 #define DRV_IMU_DEVTYPE_MPU6000  0x21
 #define DRV_GYR_DEVTYPE_L3GD20   0x22
@@ -137,6 +136,10 @@
 #define DRV_PWM_DEVTYPE_PCA9685     0x69
 #define DRV_ACC_DEVTYPE_BMI088		0x6a
 #define DRV_OSD_DEVTYPE_ATXXXX		0x6b
+#define DRV_FLOW_DEVTYPE_PMW3901	0x6c
+#define DRV_FLOW_DEVTYPE_PAW3902	0x6d
+#define DRV_FLOW_DEVTYPE_PX4FLOW	0x6e
+#define DRV_FLOW_DEVTYPE_PAA3905	0x6f


 #define DRV_DIST_DEVTYPE_LL40LS       0x70
@@ -203,16 +206,8 @@

 #define DRV_GPS_DEVTYPE_SIM 0xAF

-#define DRV_TRNS_DEVTYPE_MXS 0xB0
-
 #define DRV_HYGRO_DEVTYPE_SHT3X 0xB1

-#define DRV_FLOW_DEVTYPE_MAVLINK 0xB2
-#define DRV_FLOW_DEVTYPE_PMW3901 0xB3
-#define DRV_FLOW_DEVTYPE_PAW3902 0xB4
-#define DRV_FLOW_DEVTYPE_PX4FLOW 0xB5
-#define DRV_FLOW_DEVTYPE_PAA3905 0xB6
-
 #define DRV_DEVTYPE_UNUSED		0xff

 #endif /* _DRV_SENSOR_H */
@@ -752,8 +752,8 @@ int DShot::custom_command(int argc, char *argv[])
 	};

 	constexpr VerbCommand commands[] = {
-		{"reverse", DShot_cmd_spin_direction_2, 10},
-		{"normal", DShot_cmd_spin_direction_1, 10},
+		{"reverse", DShot_cmd_spin_direction_reversed, 10},
+		{"normal", DShot_cmd_spin_direction_normal, 10},
 		{"save", DShot_cmd_save_settings, 10},
 		{"3d_on", DShot_cmd_3d_mode_on, 10},
 		{"3d_off", DShot_cmd_3d_mode_off, 10},
@@ -1,6 +1,6 @@
 ############################################################################
 #
-#   Copyright (c) 2020 PX4 Development Team. All rights reserved.
+#   Copyright (c) 2019 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
@@ -31,8 +31,7 @@
 #
 ############################################################################

-px4_add_library(vehicle_optical_flow
-	VehicleOpticalFlow.cpp
-	VehicleOpticalFlow.hpp
-)
-target_link_libraries(vehicle_optical_flow PRIVATE px4_work_queue)
+add_subdirectory(paw3902)
+add_subdirectory(pmw3901)
+add_subdirectory(px4flow)
+add_subdirectory(thoneflow)
@@ -43,10 +43,6 @@ PAA3905::PAA3905(const I2CSPIDriverConfig &config) :
 	I2CSPIDriver(config),
 	_drdy_gpio(config.drdy_gpio)
 {
-	if (_drdy_gpio != 0) {
-		_no_motion_interrupt_perf = perf_alloc(PC_COUNT, MODULE_NAME": no motion interrupt");
-	}
-
 	float yaw_rotation_degrees = (float)config.custom1;

 	if (yaw_rotation_degrees >= 0.f) {
@@ -56,21 +52,27 @@ PAA3905::PAA3905(const I2CSPIDriverConfig &config) :
 		_rotation = matrix::Dcmf{matrix::Eulerf{0.f, 0.f, math::radians(yaw_rotation_degrees)}};

 	} else {
-		_rotation.identity();
+		// otherwise use the parameter SENS_FLOW_ROT
+		param_t rot = param_find("SENS_FLOW_ROT");
+		int32_t val = 0;
+
+		if (param_get(rot, &val) == PX4_OK) {
+			_rotation = get_rot_matrix((enum Rotation)val);
+
+		} else {
+			_rotation.identity();
+		}
 	}
 }

 PAA3905::~PAA3905()
 {
 	// free perf counters
-	perf_free(_cycle_perf);
+	perf_free(_sample_perf);
 	perf_free(_interval_perf);
-	perf_free(_reset_perf);
+	perf_free(_comms_errors);
 	perf_free(_false_motion_perf);
-	perf_free(_mode_change_bright_perf);
-	perf_free(_mode_change_low_light_perf);
-	perf_free(_mode_change_super_low_light_perf);
-	perf_free(_no_motion_interrupt_perf);
+	perf_free(_register_write_fail_perf);
 }

 int PAA3905::init()
@@ -82,35 +84,35 @@ int PAA3905::init()

 	Configure();

+	_previous_collect_timestamp = hrt_absolute_time();
+
 	return PX4_OK;
 }

 int PAA3905::probe()
 {
-	for (int retry = 0; retry < 3; retry++) {
-		const uint8_t Product_ID = RegisterRead(Register::Product_ID);
-		const uint8_t Revision_ID = RegisterRead(Register::Revision_ID);
-		const uint8_t Inverse_Product_ID = RegisterRead(Register::Inverse_Product_ID);
+	const uint8_t Product_ID = RegisterRead(Register::Product_ID);

-		if (Product_ID != PRODUCT_ID) {
-			PX4_ERR("Product_ID: %X", Product_ID);
-			break;
-		}
-
-		if (Revision_ID != REVISION_ID) {
-			PX4_ERR("Revision_ID: %X", Revision_ID);
-			break;
-		}
-
-		if (Inverse_Product_ID != PRODUCT_ID_INVERSE) {
-			PX4_ERR("Inverse_Product_ID: %X", Inverse_Product_ID);
-			break;
-		}
-
-		return PX4_OK;
+	if (Product_ID != PRODUCT_ID) {
+		PX4_ERR("Product_ID: %X", Product_ID);
+		return PX4_ERROR;
 	}

-	return PX4_ERROR;
+	const uint8_t Revision_ID = RegisterRead(Register::Revision_ID);
+
+	if (Revision_ID != REVISION_ID) {
+		PX4_ERR("Revision_ID: %X", Revision_ID);
+		return PX4_ERROR;
+	}
+
+	const uint8_t Inverse_Product_ID = RegisterRead(Register::Inverse_Product_ID);
+
+	if (Inverse_Product_ID != PRODUCT_ID_INVERSE) {
+		PX4_ERR("Inverse_Product_ID: %X", Inverse_Product_ID);
+		return PX4_ERROR;
+	}
+
+	return PX4_OK;
 }

 int PAA3905::DataReadyInterruptCallback(int irq, void *context, void *arg)
@@ -121,14 +123,12 @@ int PAA3905::DataReadyInterruptCallback(int irq, void *context, void *arg)

 void PAA3905::DataReady()
 {
-	_drdy_timestamp_sample.store(hrt_absolute_time());
 	ScheduleNow();
 }

 bool PAA3905::DataReadyInterruptConfigure()
 {
 	if (_drdy_gpio == 0) {
-		_data_ready_interrupt_enabled = false;
 		return false;
 	}

@@ -144,12 +144,12 @@ bool PAA3905::DataReadyInterruptConfigure()

 bool PAA3905::DataReadyInterruptDisable()
 {
-	_data_ready_interrupt_enabled = false;
-
 	if (_drdy_gpio == 0) {
 		return false;
 	}

+	_data_ready_interrupt_enabled = false;
+
 	return px4_arch_gpiosetevent(_drdy_gpio, false, false, false, nullptr, nullptr) == 0;
 }

@@ -159,10 +159,8 @@ void PAA3905::exit_and_cleanup()
 	I2CSPIDriverBase::exit_and_cleanup();
 }

-void PAA3905::Reset()
+void PAA3905::Configure()
 {
-	perf_count(_reset_perf);
-
 	DataReadyInterruptDisable();
 	ScheduleClear();

@@ -171,312 +169,274 @@ void PAA3905::Reset()
 	px4_usleep(1000);
 	_last_reset = hrt_absolute_time();

-	_discard_reading = 3;
+	StandardDetectionSetting();
+	ModeAuto012();

-	// Read from registers 0x02, 0x03, 0x04, 0x05 and 0x06 one time regardless of the motion pin state.
-	RegisterRead(0x02);
-	RegisterRead(0x03);
-	RegisterRead(0x04);
-	RegisterRead(0x05);
-	RegisterRead(0x06);
-}
+	CheckMode();

-void PAA3905::Configure()
-{
-	Reset();
+	switch (_mode) {
+	case Mode::Bright:
+		_scheduled_interval_us = SAMPLE_INTERVAL_MODE_0;
+		break;

-	ConfigureStandardDetectionSetting();
+	case Mode::LowLight:
+		_scheduled_interval_us = SAMPLE_INTERVAL_MODE_1;
+		break;

-	ConfigureAutomaticModeSwitching();
+	case Mode::SuperLowLight:
+		_scheduled_interval_us = SAMPLE_INTERVAL_MODE_2;
+		break;
+	}

 	EnableLed();

-	// Read Register 0x15. Check Bit [7:6] for AMS mode
-	const uint8_t Observation = RegisterRead(Register::Observation);
-	UpdateMode(Observation);
+	_discard_reading = 3;
+	_valid_count = 0;

 	if (DataReadyInterruptConfigure()) {
-		// backup schedule
-		ScheduleDelayed(500_ms);
+		// backup schedule as a watchdog timeout
+		ScheduleDelayed(_scheduled_interval_us * 2);

 	} else {
-		ScheduleOnInterval(_scheduled_interval_us, _scheduled_interval_us);
+		ScheduleOnInterval(_scheduled_interval_us);
 	}
 }

-void PAA3905::ConfigureStandardDetectionSetting()
+void PAA3905::CheckMode()
 {
-	// Standard Detection Setting is recommended for general tracking operations. In this mode, the chip can detect
-	// when it is operating over striped, checkerboard, and glossy tile surfaces where tracking performance is
-	// compromised.
-
-	RegisterWrite(0x7F, 0x00);
-	RegisterWrite(0x51, 0xFF);
-	RegisterWrite(0x4E, 0x2A);
-	RegisterWrite(0x66, 0x3E);
-	RegisterWrite(0x7F, 0x14);
-	RegisterWrite(0x7E, 0x71);
-	RegisterWrite(0x55, 0x00);
-	RegisterWrite(0x59, 0x00);
-	RegisterWrite(0x6F, 0x2C);
-	RegisterWrite(0x7F, 0x05);
-	RegisterWrite(0x4D, 0xAC);
-	RegisterWrite(0x4E, 0x32);
-	RegisterWrite(0x7F, 0x09);
-	RegisterWrite(0x5C, 0xAF);
-	RegisterWrite(0x5F, 0xAF);
-	RegisterWrite(0x70, 0x08);
-	RegisterWrite(0x71, 0x04);
-	RegisterWrite(0x72, 0x06);
-	RegisterWrite(0x74, 0x3C);
-	RegisterWrite(0x75, 0x28);
-	RegisterWrite(0x76, 0x20);
-	RegisterWrite(0x4E, 0xBF);
-	RegisterWrite(0x7F, 0x03);
-	RegisterWrite(0x64, 0x14);
-	RegisterWrite(0x65, 0x0A);
-	RegisterWrite(0x66, 0x10);
-	RegisterWrite(0x55, 0x3C);
-	RegisterWrite(0x56, 0x28);
-	RegisterWrite(0x57, 0x20);
-	RegisterWrite(0x4A, 0x2D);
-
-	RegisterWrite(0x4B, 0x2D);
-	RegisterWrite(0x4E, 0x4B);
-	RegisterWrite(0x69, 0xFA);
-	RegisterWrite(0x7F, 0x05);
-	RegisterWrite(0x69, 0x1F);
-	RegisterWrite(0x47, 0x1F);
-	RegisterWrite(0x48, 0x0C);
-	RegisterWrite(0x5A, 0x20);
-	RegisterWrite(0x75, 0x0F);
-	RegisterWrite(0x4A, 0x0F);
-	RegisterWrite(0x42, 0x02);
-	RegisterWrite(0x45, 0x03);
-	RegisterWrite(0x65, 0x00);
-	RegisterWrite(0x67, 0x76);
-	RegisterWrite(0x68, 0x76);
-	RegisterWrite(0x6A, 0xC5);
-	RegisterWrite(0x43, 0x00);
-	RegisterWrite(0x7F, 0x06);
-	RegisterWrite(0x4A, 0x18);
-	RegisterWrite(0x4B, 0x0C);
-	RegisterWrite(0x4C, 0x0C);
-	RegisterWrite(0x4D, 0x0C);
-	RegisterWrite(0x46, 0x0A);
-	RegisterWrite(0x59, 0xCD);
-	RegisterWrite(0x7F, 0x0A);
-	RegisterWrite(0x4A, 0x2A);
-	RegisterWrite(0x48, 0x96);
-	RegisterWrite(0x52, 0xB4);
-	RegisterWrite(0x7F, 0x00);
-	RegisterWrite(0x5B, 0xA0);
-}
-
-void PAA3905::ConfigureEnhancedDetectionMode()
-{
-	// Enhance Detection Setting relatively has better detection sensitivity, it is recommended where yaw motion
-	// detection is required, and also where more sensitive challenging surface detection is required. The recommended
-	// operating height must be greater than 15 cm to avoid false detection on challenging surfaces due to increasing of
-	// sensitivity.
-
-	RegisterWrite(0x7F, 0x00);
-	RegisterWrite(0x51, 0xFF);
-	RegisterWrite(0x4E, 0x2A);
-	RegisterWrite(0x66, 0x26);
-	RegisterWrite(0x7F, 0x14);
-	RegisterWrite(0x7E, 0x71);
-	RegisterWrite(0x55, 0x00);
-	RegisterWrite(0x59, 0x00);
-	RegisterWrite(0x6F, 0x2C);
-	RegisterWrite(0x7F, 0x05);
-	RegisterWrite(0x4D, 0xAC);
-	RegisterWrite(0x4E, 0x65);
-	RegisterWrite(0x7F, 0x09);
-	RegisterWrite(0x5C, 0xAF);
-	RegisterWrite(0x5F, 0xAF);
-	RegisterWrite(0x70, 0x00);
-	RegisterWrite(0x71, 0x00);
-	RegisterWrite(0x72, 0x00);
-	RegisterWrite(0x74, 0x14);
-	RegisterWrite(0x75, 0x14);
-	RegisterWrite(0x76, 0x06);
-	RegisterWrite(0x4E, 0x8F);
-	RegisterWrite(0x7F, 0x03);
-	RegisterWrite(0x64, 0x00);
-	RegisterWrite(0x65, 0x00);
-	RegisterWrite(0x66, 0x00);
-	RegisterWrite(0x55, 0x14);
-	RegisterWrite(0x56, 0x14);
-	RegisterWrite(0x57, 0x06);
-	RegisterWrite(0x4A, 0x20);
-
-	RegisterWrite(0x4B, 0x20);
-	RegisterWrite(0x4E, 0x32);
-	RegisterWrite(0x69, 0xFE);
-	RegisterWrite(0x7F, 0x05);
-	RegisterWrite(0x69, 0x14);
-	RegisterWrite(0x47, 0x14);
-	RegisterWrite(0x48, 0x1C);
-	RegisterWrite(0x5A, 0x20);
-	RegisterWrite(0x75, 0xE5);
-	RegisterWrite(0x4A, 0x05);
-	RegisterWrite(0x42, 0x04);
-	RegisterWrite(0x45, 0x03);
-	RegisterWrite(0x65, 0x00);
-	RegisterWrite(0x67, 0x50);
-	RegisterWrite(0x68, 0x50);
-	RegisterWrite(0x6A, 0xC5);
-	RegisterWrite(0x43, 0x00);
-	RegisterWrite(0x7F, 0x06);
-	RegisterWrite(0x4A, 0x1E);
-	RegisterWrite(0x4B, 0x1E);
-	RegisterWrite(0x4C, 0x34);
-	RegisterWrite(0x4D, 0x34);
-	RegisterWrite(0x46, 0x32);
-	RegisterWrite(0x59, 0x0D);
-	RegisterWrite(0x7F, 0x0A);
-	RegisterWrite(0x4A, 0x2A);
-	RegisterWrite(0x48, 0x96);
-	RegisterWrite(0x52, 0xB4);
-	RegisterWrite(0x7F, 0x00);
-	RegisterWrite(0x5B, 0xA0);
-}
-
-void PAA3905::ConfigureAutomaticModeSwitching()
-{
-	// Automatic switching between Mode 0, 1 and 2:
-	RegisterWrite(0x7F, 0x08);
-	RegisterWrite(0x68, 0x02);
-	RegisterWrite(0x7F, 0x00);
-
-	// TODO: for mode 0 and 1 only
-	// Automatic switching between Mode 0 and 1 only:
-	// RegisterWrite(0x7F, 0x08);
-	// RegisterWrite(0x68, 0x01); // different than mode 0,1,2
-	// RegisterWrite(0x7F, 0x00);
-}
-
-void PAA3905::EnableLed()
-{
-	// Enable LED_N controls
-	RegisterWrite(0x7F, 0x14);
-	RegisterWrite(0x6F, 0x0C);
-	RegisterWrite(0x7F, 0x00);
-}
-
-bool PAA3905::UpdateMode(const uint8_t observation)
-{
-	bool mode_changed = false;
+	// Read Register 0x15. Check Bit [7:6] for AMS mode and store it into a variable.
+	const uint8_t Observation = RegisterRead(Register::Observation);

 	// Bit [7:6] AMS mode
 	const uint8_t ams_mode = (Observation & (Bit7 | Bit6)) >> 5;

 	if (ams_mode == 0x0) {
-		// Mode 0 (Bright)
-		if (_mode != Mode::Bright) {
-			mode_changed = true;
-			perf_count(_mode_change_bright_perf);
-		}
-
-		_mode = Mode::Bright;
-		_scheduled_interval_us = SAMPLE_INTERVAL_MODE_0;
+		// Mode 0
+		_mode = Mode::SuperLowLight;

 	} else if (ams_mode == 0x1) {
-		// Mode 1 (LowLight)
-		if (_mode != Mode::LowLight) {
-			mode_changed = true;
-			perf_count(_mode_change_low_light_perf);
-		}
-
+		// Mode 1
 		_mode = Mode::LowLight;
-		_scheduled_interval_us = SAMPLE_INTERVAL_MODE_1;

 	} else if (ams_mode == 0x2) {
-		// Mode 2 (SuperLowLight)
-		if (_mode != Mode::SuperLowLight) {
-			mode_changed = true;
-			perf_count(_mode_change_super_low_light_perf);
-		}
-
+		// Mode 2
 		_mode = Mode::SuperLowLight;
-		_scheduled_interval_us = SAMPLE_INTERVAL_MODE_2;
 	}
-
-	return mode_changed;
 }

-uint8_t PAA3905::RegisterRead(uint8_t reg)
+void PAA3905::StandardDetectionSetting()
 {
-	// tSWR SPI Time Between Write And Read Commands
-	const hrt_abstime elapsed_last_write = hrt_elapsed_time(&_last_write_time);
+	RegisterWriteVerified(0x7F, 0x00);
+	RegisterWriteVerified(0x51, 0xFF);
+	RegisterWriteVerified(0x4E, 0x2A);
+	RegisterWriteVerified(0x66, 0x3E);
+	RegisterWriteVerified(0x7F, 0x14);
+	RegisterWriteVerified(0x7E, 0x71);
+	RegisterWriteVerified(0x55, 0x00);
+	RegisterWriteVerified(0x59, 0x00);
+	RegisterWriteVerified(0x6F, 0x2C);
+	RegisterWriteVerified(0x7F, 0x05);
+	RegisterWriteVerified(0x4D, 0xAC);
+	RegisterWriteVerified(0x4E, 0x32);
+	RegisterWriteVerified(0x7F, 0x09);
+	RegisterWriteVerified(0x5C, 0xAF);
+	RegisterWriteVerified(0x5F, 0xAF);
+	RegisterWriteVerified(0x70, 0x08);
+	RegisterWriteVerified(0x71, 0x04);
+	RegisterWriteVerified(0x72, 0x06);
+	RegisterWriteVerified(0x74, 0x3C);
+	RegisterWriteVerified(0x75, 0x28);
+	RegisterWriteVerified(0x76, 0x20);
+	RegisterWriteVerified(0x4E, 0xBF);
+	RegisterWriteVerified(0x7F, 0x03);
+	RegisterWriteVerified(0x64, 0x14);
+	RegisterWriteVerified(0x65, 0x0A);
+	RegisterWriteVerified(0x66, 0x10);
+	RegisterWriteVerified(0x55, 0x3C);
+	RegisterWriteVerified(0x56, 0x28);
+	RegisterWriteVerified(0x57, 0x20);
+	RegisterWriteVerified(0x4A, 0x2D);

-	if (elapsed_last_write < TIME_TSWR_us) {
-		px4_udelay(TIME_TSWR_us - elapsed_last_write);
+	RegisterWriteVerified(0x4B, 0x2D);
+	RegisterWriteVerified(0x4E, 0x4B);
+	RegisterWriteVerified(0x69, 0xFA);
+	RegisterWriteVerified(0x7F, 0x05);
+	RegisterWriteVerified(0x69, 0x1F);
+	RegisterWriteVerified(0x47, 0x1F);
+	RegisterWriteVerified(0x48, 0x0C);
+	RegisterWriteVerified(0x5A, 0x20);
+	RegisterWriteVerified(0x75, 0x0F);
+	RegisterWriteVerified(0x4A, 0x0F);
+	RegisterWriteVerified(0x42, 0x02);
+	RegisterWriteVerified(0x45, 0x03);
+	RegisterWriteVerified(0x65, 0x00);
+	RegisterWriteVerified(0x67, 0x76);
+	RegisterWriteVerified(0x68, 0x76);
+	RegisterWriteVerified(0x6A, 0xC5);
+	RegisterWriteVerified(0x43, 0x00);
+	RegisterWriteVerified(0x7F, 0x06);
+	RegisterWriteVerified(0x4A, 0x18);
+	RegisterWriteVerified(0x4B, 0x0C);
+	RegisterWriteVerified(0x4C, 0x0C);
+	RegisterWriteVerified(0x4D, 0x0C);
+	RegisterWriteVerified(0x46, 0x0A);
+	RegisterWriteVerified(0x59, 0xCD);
+	RegisterWriteVerified(0x7F, 0x0A);
+	RegisterWriteVerified(0x4A, 0x2A);
+	RegisterWriteVerified(0x48, 0x96);
+	RegisterWriteVerified(0x52, 0xB4);
+	RegisterWriteVerified(0x7F, 0x00);
+	RegisterWriteVerified(0x5B, 0xA0);
+}
+
+void PAA3905::EnhancedDetectionMode()
+{
+	RegisterWriteVerified(0x7F, 0x00);
+	RegisterWriteVerified(0x51, 0xFF);
+	RegisterWriteVerified(0x4E, 0x2A);
+	RegisterWriteVerified(0x66, 0x26);
+	RegisterWriteVerified(0x7F, 0x14);
+	RegisterWriteVerified(0x7E, 0x71);
+	RegisterWriteVerified(0x55, 0x00);
+	RegisterWriteVerified(0x59, 0x00);
+	RegisterWriteVerified(0x6F, 0x2C);
+	RegisterWriteVerified(0x7F, 0x05);
+	RegisterWriteVerified(0x4D, 0xAC);
+	RegisterWriteVerified(0x4E, 0x65);
+	RegisterWriteVerified(0x7F, 0x09);
+	RegisterWriteVerified(0x5C, 0xAF);
+	RegisterWriteVerified(0x5F, 0xAF);
+	RegisterWriteVerified(0x70, 0x00);
+	RegisterWriteVerified(0x71, 0x00);
+	RegisterWriteVerified(0x72, 0x00);
+	RegisterWriteVerified(0x74, 0x14);
+	RegisterWriteVerified(0x75, 0x14);
+	RegisterWriteVerified(0x76, 0x06);
+	RegisterWriteVerified(0x4E, 0x8F);
+	RegisterWriteVerified(0x7F, 0x03);
+	RegisterWriteVerified(0x64, 0x00);
+	RegisterWriteVerified(0x65, 0x00);
+	RegisterWriteVerified(0x66, 0x00);
+	RegisterWriteVerified(0x55, 0x14);
+	RegisterWriteVerified(0x56, 0x14);
+	RegisterWriteVerified(0x57, 0x06);
+	RegisterWriteVerified(0x4A, 0x20);
+
+	RegisterWriteVerified(0x4B, 0x20);
+	RegisterWriteVerified(0x4E, 0x32);
+	RegisterWriteVerified(0x69, 0xFE);
+	RegisterWriteVerified(0x7F, 0x05);
+	RegisterWriteVerified(0x69, 0x14);
+	RegisterWriteVerified(0x47, 0x14);
+	RegisterWriteVerified(0x48, 0x1C);
+	RegisterWriteVerified(0x5A, 0x20);
+	RegisterWriteVerified(0x75, 0xE5);
+	RegisterWriteVerified(0x4A, 0x05);
+	RegisterWriteVerified(0x42, 0x04);
+	RegisterWriteVerified(0x45, 0x03);
+	RegisterWriteVerified(0x65, 0x00);
+	RegisterWriteVerified(0x67, 0x50);
+	RegisterWriteVerified(0x68, 0x50);
+	RegisterWriteVerified(0x6A, 0xC5);
+	RegisterWriteVerified(0x43, 0x00);
+	RegisterWriteVerified(0x7F, 0x06);
+	RegisterWriteVerified(0x4A, 0x1E);
+	RegisterWriteVerified(0x4B, 0x1E);
+	RegisterWriteVerified(0x4C, 0x34);
+	RegisterWriteVerified(0x4D, 0x34);
+	RegisterWriteVerified(0x46, 0x32);
+	RegisterWriteVerified(0x59, 0x0D);
+	RegisterWriteVerified(0x7F, 0x0A);
+	RegisterWriteVerified(0x4A, 0x2A);
+	RegisterWriteVerified(0x48, 0x96);
+	RegisterWriteVerified(0x52, 0xB4);
+	RegisterWriteVerified(0x7F, 0x00);
+	RegisterWriteVerified(0x5B, 0xA0);
+}
+
+void PAA3905::ModeAuto012()
+{
+	// Automatic switching between Mode 0, 1 and 2:
+	RegisterWriteVerified(0x7F, 0x08);
+	RegisterWriteVerified(0x68, 0x02);
+	RegisterWriteVerified(0x7F, 0x00);
+}
+
+void PAA3905::EnableLed()
+{
+	// Enable LED_N controls
+	RegisterWriteVerified(0x7F, 0x14);
+	RegisterWriteVerified(0x6F, 0x0C);
+	RegisterWriteVerified(0x7F, 0x00);
+}
+
+uint8_t PAA3905::RegisterRead(uint8_t reg, int retries)
+{
+	for (int i = 0; i < retries; i++) {
+		px4_udelay(TIME_us_TSRAD);
+		uint8_t cmd[2] {reg, 0};
+
+		if (transfer(&cmd[0], &cmd[0], sizeof(cmd)) == 0) {
+			return cmd[1];
+		}
 	}

-	// tSRW/tSRR SPI Time Between Read And Subsequent Commands
-	const hrt_abstime elapsed_last_read = hrt_elapsed_time(&_last_read_time);
-
-	if (elapsed_last_write < TIME_TSRW_TSRR_us) {
-		px4_udelay(TIME_TSRW_TSRR_us - elapsed_last_read);
-	}
-
-	uint8_t cmd[2];
-	cmd[0] = DIR_READ(reg);
-	cmd[1] = 0;
-	transfer(&cmd[0], &cmd[0], sizeof(cmd));
-	hrt_store_absolute_time(&_last_read_time);
-
-	return cmd[1];
+	perf_count(_comms_errors);
+	return 0;
 }

 void PAA3905::RegisterWrite(uint8_t reg, uint8_t data)
 {
-	// tSWW SPI Time Between Write Commands
-	const hrt_abstime elapsed_last_write = hrt_elapsed_time(&_last_write_time);
-
-	if (elapsed_last_write < TIME_TSWW_us) {
-		px4_udelay(TIME_TSWW_us - elapsed_last_write);
-	}
-
 	uint8_t cmd[2];
 	cmd[0] = DIR_WRITE(reg);
 	cmd[1] = data;
-	transfer(&cmd[0], nullptr, sizeof(cmd));
-	hrt_store_absolute_time(&_last_write_time);
+
+	if (transfer(&cmd[0], nullptr, sizeof(cmd)) != 0) {
+		perf_count(_comms_errors);
+	}
+}
+
+bool PAA3905::RegisterWriteVerified(uint8_t reg, uint8_t data, int retries)
+{
+	for (int i = 0; i < retries; i++) {
+		uint8_t cmd[2];
+		cmd[0] = DIR_WRITE(reg);
+		cmd[1] = data;
+		transfer(&cmd[0], nullptr, sizeof(cmd));
+		px4_udelay(TIME_us_TSWW);
+
+		// read back to verify
+		uint8_t data_read = RegisterRead(reg);
+
+		if (data_read == data) {
+			return true;
+		}
+
+		PX4_DEBUG("Register write failed 0x%02hhX: 0x%02hhX (actual value 0x%02hhX)", reg, data, data_read);
+	}
+
+	perf_count(_register_write_fail_perf);
+
+	return false;
 }

 void PAA3905::RunImpl()
 {
-	perf_begin(_cycle_perf);
-	perf_count(_interval_perf);
+	// backup schedule
+	if (_data_ready_interrupt_enabled) {
+		ScheduleDelayed(_scheduled_interval_us * 2);
+	}

-	const hrt_abstime now = hrt_absolute_time();
+	// force reset if there hasn't been valid data for an extended period (sensor could be in a bad state)
+	static constexpr hrt_abstime RESET_TIMEOUT_US = 5_s;

-	// force reconfigure if we haven't received valid data for quite some time
-	if ((now > _last_good_data + RESET_TIMEOUT_US) && (now > _last_reset + RESET_TIMEOUT_US)) {
+	if ((hrt_elapsed_time(&_last_good_publish) > RESET_TIMEOUT_US) && (hrt_elapsed_time(&_last_reset) > RESET_TIMEOUT_US)) {
 		Configure();
-		perf_end(_cycle_perf);
 		return;
 	}

-	hrt_abstime timestamp_sample = now;
-
-	if (_data_ready_interrupt_enabled) {
-		// scheduled from interrupt if _drdy_timestamp_sample was set as expected
-		const hrt_abstime drdy_timestamp_sample = _drdy_timestamp_sample.fetch_and(0);
-
-		if (now < drdy_timestamp_sample + _scheduled_interval_us) {
-			timestamp_sample = drdy_timestamp_sample;
-
-		} else {
-			perf_count(_no_motion_interrupt_perf);
-		}
-
-		// push backup schedule back
-		ScheduleDelayed(500_ms);
-	}
+	perf_begin(_sample_perf);
+	perf_count(_interval_perf);

 	struct TransferBuffer {
 		uint8_t cmd = Register::Motion_Burst;
@@ -484,52 +444,37 @@ void PAA3905::RunImpl()
 	} buf{};
 	static_assert(sizeof(buf) == (14 + 1));

-	if (transfer((uint8_t *)&buf, (uint8_t *)&buf, sizeof(buf)) != 0) {
-		perf_end(_cycle_perf);
+	const hrt_abstime timestamp_sample = hrt_absolute_time();
+
+	if (transfer((uint8_t *)&buf, (uint8_t *)&buf, sizeof(buf)) != PX4_OK) {
+		perf_count(_comms_errors);
+		perf_end(_sample_perf);
 		return;
 	}

-	hrt_store_absolute_time(&_last_read_time);
+	perf_end(_sample_perf);
+
+	const uint64_t dt_flow = timestamp_sample - _previous_collect_timestamp;
+
+	// update for next iteration
+	_previous_collect_timestamp = timestamp_sample;

 	if (_discard_reading > 0) {
 		_discard_reading--;
-		perf_end(_cycle_perf);
+		ResetAccumulatedData();
+		_valid_count = 0;
 		return;
 	}

-	// Bit [5:0] check if chip is working correctly
-	//  0x3F: chip is working correctly
-	if ((buf.data.Observation & (Bit5 | Bit4 | Bit3 | Bit2 | Bit1 | Bit0)) != 0x3F) {
-		// Other value: recommend to issue a software reset
-		Configure();
-		perf_end(_cycle_perf);
-		return;
-	}
-
-	if (UpdateMode(buf.data.Observation)) {
-		// update scheduling if mode changed
-		if (!_data_ready_interrupt_enabled) {
-			ScheduleOnInterval(_scheduled_interval_us, _scheduled_interval_us);
-		}
-	}
+	CheckMode(); // update _mode variable

 	// check SQUAL & Shutter values
 	// To suppress false motion reports, discard Delta X and Delta Y values if the SQUAL and Shutter values meet the condition
 	// Bright Mode,          SQUAL < 0x19, Shutter ≥ 0x00FF80
 	// Low Light Mode,       SQUAL < 0x46, Shutter ≥ 0x00FF80
 	// Super Low Light Mode, SQUAL < 0x55, Shutter ≥ 0x025998
+	const uint32_t shutter = (buf.data.Shutter_Upper << 16) | (buf.data.Shutter_Middle << 8) | buf.data.Shutter_Lower;

-	// 23-bit Shutter register
-	const uint8_t Shutter_Lower = buf.data.Shutter_Lower;
-	const uint8_t Shutter_Middle = buf.data.Shutter_Middle;
-	const uint8_t Shutter_Upper = buf.data.Shutter_Upper & (Bit6 | Bit5 | Bit4 | Bit3 | Bit2 | Bit1 | Bit0);
-
-	const uint32_t shutter = (Shutter_Upper << 16) | (Shutter_Middle << 8) | Shutter_Lower;
-
-	// Motion since last report and Surface quality non-zero
-	const bool motion_detected = buf.data.Motion & Motion_Bit::MotionOccurred;
-
-	// Number of Features = SQUAL * 4
 	bool data_valid = (buf.data.SQUAL > 0);

 	switch (_mode) {
@@ -568,73 +513,97 @@ void PAA3905::RunImpl()
 	}

 	if (data_valid) {
-		// publish sensor_optical_flow
-		sensor_optical_flow_s report{};
-		report.timestamp_sample = timestamp_sample;
-		report.device_id = get_device_id();
+		const int16_t delta_x_raw = combine(buf.data.Delta_X_H, buf.data.Delta_X_L);
+		const int16_t delta_y_raw = combine(buf.data.Delta_Y_H, buf.data.Delta_Y_L);

-		report.integration_timespan_us = _scheduled_interval_us;
-		report.quality = buf.data.SQUAL;
+		_flow_dt_sum_usec += dt_flow;
+		_flow_sum_x += delta_x_raw;
+		_flow_sum_y += delta_y_raw;
+		_flow_sample_counter++;
+		_flow_quality_sum += buf.data.SQUAL;

-		// set specs according to datasheet
-		report.max_flow_rate = 7.4f;           // Datasheet: 7.4 rad/s
-		report.min_ground_distance = 0.08f;    // Datasheet: 80mm
-		report.max_ground_distance = INFINITY; // Datasheet: infinity
+		_valid_count++;

-		switch (_mode) {
-		case Mode::Bright:
-			report.mode = sensor_optical_flow_s::MODE_BRIGHT;
-			break;
-
-		case Mode::LowLight:
-			report.mode = sensor_optical_flow_s::MODE_LOWLIGHT;
-			break;
-
-		case Mode::SuperLowLight:
-			report.mode = sensor_optical_flow_s::MODE_SUPER_LOWLIGHT;
-			break;
-		}
-
-		if (motion_detected) {
-			// only populate flow if data valid (motion and quality > 0)
-			const int16_t delta_x_raw = combine(buf.data.Delta_X_H, buf.data.Delta_X_L);
-			const int16_t delta_y_raw = combine(buf.data.Delta_Y_H, buf.data.Delta_Y_L);
-
-			// rotate measurements in yaw from sensor frame to body frame
-			const matrix::Vector3f pixel_flow_rotated = _rotation * matrix::Vector3f{(float)delta_x_raw, (float)delta_y_raw, 0.f};
-
-			// datasheet provides 11.914 CPI (count per inch) scaling per meter of height
-			static constexpr float PIXART_RESOLUTION = 11.914f; // counts per inch (CPI) per meter (from surface)
-			static constexpr float INCHES_PER_METER = 39.3701f;
-
-			// CPI/m -> radians
-			static constexpr float SCALE = 1.f / (PIXART_RESOLUTION * INCHES_PER_METER);
-
-			report.pixel_flow[0] = pixel_flow_rotated(0) * SCALE;
-			report.pixel_flow[1] = pixel_flow_rotated(1) * SCALE;
-		}
-
-		report.timestamp = hrt_absolute_time();
-		_sensor_optical_flow_pub.publish(report);
-
-		if (report.quality >= 1) {
-			_last_good_data = report.timestamp_sample;
-		}
+	} else {
+		_valid_count = 0;
+		ResetAccumulatedData();
+		return;
 	}

-	perf_end(_cycle_perf);
+	// returns if the collect time has not been reached
+	if (_flow_dt_sum_usec < COLLECT_TIME) {
+		return;
+	}
+
+	optical_flow_s report{};
+	report.timestamp = timestamp_sample;
+	//report.device_id = get_device_id();
+
+	float pixel_flow_x_integral = (float)_flow_sum_x / 500.0f;	// proportional factor + convert from pixels to radians
+	float pixel_flow_y_integral = (float)_flow_sum_y / 500.0f;	// proportional factor + convert from pixels to radians
+
+	// rotate measurements in yaw from sensor frame to body frame
+	const matrix::Vector3f pixel_flow_rotated = _rotation * matrix::Vector3f{pixel_flow_x_integral, pixel_flow_y_integral, 0.f};
+	report.pixel_flow_x_integral = pixel_flow_rotated(0);
+	report.pixel_flow_y_integral = pixel_flow_rotated(1);
+
+	report.frame_count_since_last_readout = _flow_sample_counter; // number of frames
+	report.integration_timespan = _flow_dt_sum_usec;              // microseconds
+
+	report.quality = _flow_quality_sum / _flow_sample_counter;
+
+	// No gyro on this board
+	report.gyro_x_rate_integral = NAN;
+	report.gyro_y_rate_integral = NAN;
+	report.gyro_z_rate_integral = NAN;
+
+	// set (conservative) specs according to datasheet
+	report.max_flow_rate = 7.4f;        // Datasheet: 7.4 rad/s
+	report.min_ground_distance = 0.08f; // Datasheet: 80mm
+	report.max_ground_distance = 30.0f; // Datasheet: infinity
+
+
+	switch (_mode) {
+	case Mode::Bright:
+		report.mode = optical_flow_s::MODE_BRIGHT;
+		break;
+
+	case Mode::LowLight:
+		report.mode = optical_flow_s::MODE_LOWLIGHT;
+		break;
+
+	case Mode::SuperLowLight:
+		report.mode = optical_flow_s::MODE_SUPER_LOWLIGHT;
+		break;
+	}
+
+	report.timestamp = hrt_absolute_time();
+	_optical_flow_pub.publish(report);
+
+	if (report.quality > 10) {
+		_last_good_publish = report.timestamp;
+	}
+
+	ResetAccumulatedData();
+}
+
+void PAA3905::ResetAccumulatedData()
+{
+	// reset
+	_flow_dt_sum_usec = 0;
+	_flow_sum_x = 0;
+	_flow_sum_y = 0;
+	_flow_sample_counter = 0;
+	_flow_quality_sum = 0;
 }

 void PAA3905::print_status()
 {
 	I2CSPIDriverBase::print_status();

-	perf_print_counter(_cycle_perf);
+	perf_print_counter(_sample_perf);
 	perf_print_counter(_interval_perf);
-	perf_print_counter(_reset_perf);
+	perf_print_counter(_comms_errors);
 	perf_print_counter(_false_motion_perf);
-	perf_print_counter(_mode_change_bright_perf);
-	perf_print_counter(_mode_change_low_light_perf);
-	perf_print_counter(_mode_change_super_low_light_perf);
-	perf_print_counter(_no_motion_interrupt_perf);
+	perf_print_counter(_register_write_fail_perf);
 }
@@ -32,9 +32,9 @@
 ****************************************************************************/

 /**
- * @file PAA3905.hpp
+ * @file paa3905.hpp
 *
- * Driver for the PAA3905E1-Q: Optical Motion Tracking Chip
+ * Driver for the Pixart paa3905 optical flow sensors connected via SPI.
 */

 #pragma once
@@ -48,15 +48,16 @@
 #include <drivers/device/spi.h>
 #include <conversion/rotation.h>
 #include <lib/perf/perf_counter.h>
+#include <lib/parameters/param.h>
 #include <drivers/drv_hrt.h>
 #include <uORB/PublicationMulti.hpp>
-#include <uORB/topics/sensor_optical_flow.h>
+#include <uORB/topics/optical_flow.h>

 using namespace time_literals;
 using namespace PixArt_PAA3905;

-#define DIR_WRITE(a) ((a) | Bit7)
-#define DIR_READ(a) ((a) & 0x7F)
+#define DIR_WRITE(a) ((a) | (1 << 7))
+#define DIR_READ(a) ((a) & 0x7f)

 class PAA3905 : public device::SPI, public I2CSPIDriver<PAA3905>
 {
@@ -77,61 +78,59 @@ private:

 	int probe() override;

-	void Reset();
-
 	static int DataReadyInterruptCallback(int irq, void *context, void *arg);
 	void DataReady();
 	bool DataReadyInterruptConfigure();
 	bool DataReadyInterruptDisable();

-	uint8_t RegisterRead(uint8_t reg);
+	uint8_t	RegisterRead(uint8_t reg, int retries = 2);
 	void RegisterWrite(uint8_t reg, uint8_t data);
-
-	void Configure();
-
-	void ConfigureAutomaticModeSwitching();
-
-	void ConfigureModeBright();
-	void ConfigureModeLowLight();
-	void ConfigureModeSuperLowLight();
-
-	void ConfigureStandardDetectionSetting();
-	void ConfigureEnhancedDetectionMode();
+	bool RegisterWriteVerified(uint8_t reg, uint8_t data, int retries = 1);

 	void EnableLed();

-	bool UpdateMode(const uint8_t observation);
+	void StandardDetectionSetting();
+	void EnhancedDetectionMode();
+	void ModeAuto012();

-	uORB::PublicationMulti<sensor_optical_flow_s> _sensor_optical_flow_pub{ORB_ID(sensor_optical_flow)};
+	void CheckMode();

-	perf_counter_t _cycle_perf{perf_alloc(PC_ELAPSED, MODULE_NAME": cycle")};
-	perf_counter_t _interval_perf{perf_alloc(PC_INTERVAL, MODULE_NAME": interval")};
-	perf_counter_t _reset_perf{perf_alloc(PC_COUNT, MODULE_NAME": reset")};
-	perf_counter_t _false_motion_perf{perf_alloc(PC_COUNT, MODULE_NAME": false motion report")};
-	perf_counter_t _mode_change_bright_perf{perf_alloc(PC_COUNT, MODULE_NAME": mode change bright (0)")};
-	perf_counter_t _mode_change_low_light_perf{perf_alloc(PC_COUNT, MODULE_NAME": mode change low light (1)")};
-	perf_counter_t _mode_change_super_low_light_perf{perf_alloc(PC_COUNT, MODULE_NAME": mode change super low light (2)")};
-	perf_counter_t _no_motion_interrupt_perf{nullptr};
+	void Configure();
+
+	void ResetAccumulatedData();
+
+	uORB::PublicationMulti<optical_flow_s> _optical_flow_pub{ORB_ID(optical_flow)};
+
+	perf_counter_t	_sample_perf{perf_alloc(PC_ELAPSED, MODULE_NAME": read")};
+	perf_counter_t	_interval_perf{perf_alloc(PC_INTERVAL, MODULE_NAME": interval")};
+	perf_counter_t	_comms_errors{perf_alloc(PC_COUNT, MODULE_NAME": com err")};
+	perf_counter_t	_false_motion_perf{perf_alloc(PC_COUNT, MODULE_NAME": false motion report")};
+	perf_counter_t	_register_write_fail_perf{perf_alloc(PC_COUNT, MODULE_NAME": verified register write failed")};
+
+	static constexpr uint64_t COLLECT_TIME{15000}; // 15 milliseconds, optical flow data publish rate

 	const spi_drdy_gpio_t _drdy_gpio;

-	matrix::Dcmf _rotation;
+	uint64_t _previous_collect_timestamp{0};
+	uint64_t _flow_dt_sum_usec{0};
+	uint8_t _flow_sample_counter{0};
+	uint16_t _flow_quality_sum{0};

-	int _discard_reading{3};
+	matrix::Dcmf	_rotation;

-	Mode _mode{Mode::LowLight};
+	int             _discard_reading{3};

-	uint32_t _scheduled_interval_us{SAMPLE_INTERVAL_MODE_0};
+	int		_flow_sum_x{0};
+	int		_flow_sum_y{0};
+
+	Mode		_mode{Mode::LowLight};
+
+	uint32_t _scheduled_interval_us{SAMPLE_INTERVAL_MODE_1};
+
+	int _valid_count{0};

-	px4::atomic<hrt_abstime> _drdy_timestamp_sample{0};
 	bool _data_ready_interrupt_enabled{false};

-	hrt_abstime _last_write_time{0};
-	hrt_abstime _last_read_time{0};
-
-	// force reset if there hasn't been valid data for an extended period (sensor could be in a bad state)
-	static constexpr hrt_abstime RESET_TIMEOUT_US = 3_s;
-
-	hrt_abstime _last_good_data{0};
+	hrt_abstime _last_good_publish{0};
 	hrt_abstime _last_reset{0};
 };
@@ -33,8 +33,8 @@

 #pragma once

-#include <cstdint>
-
+namespace PixArt_PAA3905
+{
 // TODO: move to a central header
 static constexpr uint8_t Bit0 = (1 << 0);
 static constexpr uint8_t Bit1 = (1 << 1);
@@ -45,24 +45,19 @@ static constexpr uint8_t Bit5 = (1 << 5);
 static constexpr uint8_t Bit6 = (1 << 6);
 static constexpr uint8_t Bit7 = (1 << 7);

-namespace PixArt_PAA3905
-{
-
-static constexpr uint8_t PRODUCT_ID         = 0xA2;
-static constexpr uint8_t REVISION_ID        = 0x00;
+static constexpr uint8_t PRODUCT_ID = 0xA2;
+static constexpr uint8_t REVISION_ID = 0x00;
 static constexpr uint8_t PRODUCT_ID_INVERSE = 0x5D;

-static constexpr uint32_t SAMPLE_INTERVAL_MODE_0{1000000 / 126}; // 126 fps
-static constexpr uint32_t SAMPLE_INTERVAL_MODE_1{1000000 / 126}; // 126 fps
-static constexpr uint32_t SAMPLE_INTERVAL_MODE_2{1000000 / 50};  // 50 fps
+static constexpr uint32_t SAMPLE_INTERVAL_MODE_0{1000000 / 126};	// 126 fps
+static constexpr uint32_t SAMPLE_INTERVAL_MODE_1{1000000 / 126};	// 126 fps
+static constexpr uint32_t SAMPLE_INTERVAL_MODE_2{1000000 / 50};		// 50 fps

 static constexpr uint32_t SPI_SPEED = 2 * 1000 * 1000; // 2MHz SPI serial interface

-// Various time delays
-static constexpr uint32_t TIME_TSWW_us      = 11; // SPI Time Between Write Commands (actually 10.5us)
-static constexpr uint32_t TIME_TSWR_us      = 6;  // SPI Time Between Write and Read Commands
-static constexpr uint32_t TIME_TSRW_TSRR_us = 2;  // SPI Time Between Read And Subsequent Commands (actually 1.5us)
-static constexpr uint32_t TIME_TSRAD_us     = 2;  // SPI Read Address-Data Delay
+// Various time delay needed for paa3905
+static constexpr uint32_t TIME_us_TSWW  = 11; // actually 10.5us
+static constexpr uint32_t TIME_us_TSRAD = 2;

 enum Register : uint8_t {
 	Product_ID         = 0x00,
@@ -91,20 +86,9 @@ enum Register : uint8_t {
 	Inverse_Product_ID = 0x5F,
 };

-enum Motion_Bit : uint8_t {
-	MotionOccurred     = Bit7, // Motion since last report
-
-	ChallengingSurface = Bit0, // Challenging surface is detected
-};
-
+// Observation
 enum Observation_Bit : uint8_t {
-	// Bit [7:6]
-	AMS_mode_0 = 0,
-	AMS_mode_1 = Bit6,
-	AMS_mode_2 = Bit7,
-
-	// Bit [5:0]
-	WorkingCorrectly = 0x3F,
+	Reset = 0x5A,
 };

 enum class Mode {
@@ -49,7 +49,7 @@ extern "C" __EXPORT int paa3905_main(int argc, char *argv[])
 	using ThisDriver = PAA3905;
 	BusCLIArguments cli{false, true};
 	cli.custom1 = -1;
-	cli.spi_mode = SPIDEV_MODE3;
+	cli.spi_mode = SPIDEV_MODE0;
 	cli.default_spi_frequency = SPI_SPEED;

 	while ((ch = cli.getOpt(argc, argv, "Y:")) != EOF) {
@@ -32,7 +32,7 @@
 ****************************************************************************/

 /**
- * PAA3905 Optical Flow
+ * paa3905 Optical Flow
 *
 * @reboot_required true
 *
@@ -1,6 +1,6 @@
 ############################################################################
 #
-#   Copyright (c) 2019-2022 PX4 Development Team. All rights reserved.
+#   Copyright (c) 2019-2021 PX4 Development Team. All rights reserved.
 #
 # Redistribution and use in source and binary forms, with or without
 # modification, are permitted provided that the following conditions
@@ -38,7 +38,7 @@ px4_add_module(
 		paw3902_main.cpp
 		PAW3902.cpp
 		PAW3902.hpp
-		PixArt_PAW3902_Registers.hpp
+		PixArt_PAW3902JF_Registers.hpp
 	DEPENDS
 		conversion
 		drivers__device
--- a/Show More
+++ b/Show More