Added demo for LP55231

removed unused code - _constrainOneSide and _constrainAbs
WeatherVane: Allow weathervane on multirotors not just VTOLs
2026-06-02 03:20:04 +08:00 · 2022-06-24 07:15:59 -05:00 · 2022-06-22 23:21:16 +02:00 · 2022-06-22 14:19:28 +02:00 · 2022-06-22 14:19:28 +02:00 · 2022-06-21 16:59:14 -04:00
309 changed files with 18413 additions and 9429 deletions
@@ -86,10 +86,28 @@ unset BOARD_RC_SENSORS
 . ${R}etc/init.d/rc.serial

 # Check for flow sensor
-if param compare SENS_EN_PX4FLOW 1
+if param compare -s 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
@@ -0,0 +1,101 @@
+#!/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
@@ -0,0 +1,23 @@
+#!/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
@@ -0,0 +1,33 @@
+#!/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)
@@ -0,0 +1,45 @@
+#!/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,3 +18,7 @@ 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,7 +9,6 @@

 # 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,8 +35,12 @@ 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,3 +30,12 @@ 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,6 +154,8 @@ 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)
@@ -202,8 +204,11 @@ 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
-if ! replay tryapplyparams
+elif ! replay tryapplyparams
 then
  . px4-rc.simulator
 fi
@@ -104,7 +104,6 @@ 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,6 +47,7 @@ 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

@@ -173,7 +174,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"
@@ -5,12 +5,13 @@ set -e
 SCRIPT_DIR="$( cd "$( dirname "${BASH_SOURCE[0]}" )" && pwd )"
 cd "$SCRIPT_DIR/jMAVSim"

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

 if [ "$device" == "" ]; then
-	device="-tcp $ip:$tcp_port"
+	if [ "$protocol" == "tcp" ]; then
+		device="-tcp $ip:$port"
+	else
+		device="-udp $port"
+	fi
 else
 	device="-serial $device $baudrate"
 fi
@@ -1,72 +0,0 @@
-#!/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()
@@ -1,204 +0,0 @@
-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']
+                           'magnetometer', 'baro', 'optical_flow', 'rpm_sensor', 'transponder']

    max_line_length = 80 # wrap lines that are longer than this

@@ -74,6 +74,9 @@ 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"
@@ -214,6 +217,12 @@ 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,6 +2,7 @@ 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
@@ -10,7 +11,16 @@ 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,6 +3,8 @@
 # board sensors init
 #------------------------------------------------------------------------------

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

@@ -4,7 +4,6 @@ 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
@@ -14,5 +13,18 @@ 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,6 +4,7 @@
 #------------------------------------------------------------------------------

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

 safety_button start
 tone_alarm start
@@ -4,7 +4,6 @@ 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
@@ -14,5 +13,18 @@ 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,6 +5,7 @@

 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,6 +30,7 @@ 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,6 +141,7 @@
 #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,6 +30,7 @@ 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,6 +142,7 @@
 #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,6 +1,8 @@
 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
@@ -12,5 +14,16 @@ 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
@@ -106,3 +106,4 @@ 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_COMMON_BAROMETERS=y
+CONFIG_DRIVERS_BAROMETER_MS5611=y
 CONFIG_DRIVERS_BATT_SMBUS=y
 CONFIG_DRIVERS_CAMERA_CAPTURE=y
 CONFIG_DRIVERS_CAMERA_TRIGGER=y
@@ -24,14 +24,13 @@ 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
@@ -42,6 +41,7 @@ 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,6 +50,7 @@ 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
@@ -65,17 +66,15 @@ 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_BL_UPDATE=y
 CONFIG_SYSTEMCMDS_ACTUATOR_TEST=y
+CONFIG_SYSTEMCMDS_BL_UPDATE=y
 CONFIG_SYSTEMCMDS_DMESG=y
 CONFIG_SYSTEMCMDS_DUMPFILE=y
 CONFIG_SYSTEMCMDS_GPIO=y
@@ -93,7 +92,6 @@ 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
@@ -102,4 +100,4 @@ CONFIG_SYSTEMCMDS_UORB=y
 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
@@ -5,7 +5,6 @@ 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
@@ -13,3 +12,4 @@ 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=300
-CONFIG_UART4_TXBUFSIZE=900
+CONFIG_UART4_RXBUFSIZE=600
+CONFIG_UART4_TXBUFSIZE=600
 CONFIG_USART1_RXBUFSIZE=300
 CONFIG_USART1_RXDMA=y
 CONFIG_USART1_TXBUFSIZE=300
@@ -1,16 +1,22 @@
 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_LOAD_MON=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_LED_CONTROL=y
 CONFIG_SYSTEMCMDS_MFT=y
 CONFIG_SYSTEMCMDS_MTD=y
@@ -19,5 +25,7 @@ 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,6 +2,7 @@ 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
@@ -12,6 +13,18 @@ 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,5 +1,6 @@
 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"
@@ -31,6 +32,7 @@ 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_ATTITUDE_ESTIMATOR_Q=y
 CONFIG_MODULES_AIRSPEED_SELECTOR=y
+CONFIG_MODULES_ATTITUDE_ESTIMATOR_Q=y
 CONFIG_MODULES_BATTERY_STATUS=y
 CONFIG_MODULES_CAMERA_FEEDBACK=y
 CONFIG_MODULES_COMMANDER=y
@@ -42,6 +42,7 @@ 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
@@ -60,12 +61,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_BL_UPDATE=y
 CONFIG_SYSTEMCMDS_ACTUATOR_TEST=y
+CONFIG_SYSTEMCMDS_BL_UPDATE=y
 CONFIG_SYSTEMCMDS_DMESG=y
 CONFIG_SYSTEMCMDS_DUMPFILE=y
 CONFIG_SYSTEMCMDS_GPIO=y
@@ -1,6 +1,8 @@
 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
@@ -10,13 +12,20 @@ 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_nPOWER_IN_A                /* PB5 */ (GPIO_INPUT|GPIO_PULLUP|GPIO_PORTB|GPIO_PIN5)
+#define GPIO_POWER_IN_A                 /* PB5 */ (GPIO_INPUT|GPIO_PULLUP|GPIO_PORTB|GPIO_PIN5)

-#define GPIO_nVDD_BRICK1_VALID          GPIO_nPOWER_IN_A /* Brick 1 Is Chosen */
+#define GPIO_VDD_BRICK1_VALID           GPIO_POWER_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_nVDD_BRICK1_VALID))
+#define BOARD_ADC_BRICK_VALID   (px4_arch_gpioread(GPIO_VDD_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_nPOWER_IN_A,                 \
+		GPIO_POWER_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=600
-CONFIG_UART4_RXDMA=y
-CONFIG_UART4_TXBUFSIZE=1500
+CONFIG_UART4_RXBUFSIZE=1200
+CONFIG_UART4_RXDMA=n
+CONFIG_UART4_TXBUFSIZE=1200
 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=600
-CONFIG_USART6_RXDMA=y
-CONFIG_USART6_TXBUFSIZE=1500
-CONFIG_USART6_TXDMA=y
+CONFIG_USART6_RXBUFSIZE=1200
+CONFIG_USART6_RXDMA=n
+CONFIG_USART6_TXBUFSIZE=1200
+CONFIG_USART6_TXDMA=n
 CONFIG_USBDEV=y
 CONFIG_USBDEV_BUSPOWERED=y
 CONFIG_USBDEV_MAXPOWER=500
@@ -32,6 +32,7 @@ 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
@@ -43,6 +43,7 @@ CONFIG_DRIVERS_SMART_BATTERY_BATMON=y
 CONFIG_COMMON_TELEMETRY=y
 CONFIG_DRIVERS_TONE_ALARM=y
 CONFIG_DRIVERS_UAVCAN=y
+CONFIG_DRIVERS_LIGHTS_RGBLED_LP55231=y
 CONFIG_BOARD_UAVCAN_TIMER_OVERRIDE=6
 CONFIG_MODULES_AIRSPEED_SELECTOR=y
 CONFIG_MODULES_ATTITUDE_ESTIMATOR_Q=y
@@ -24,4 +24,5 @@ else
 fi

 rgbled_pwm start
+rgbled_lp55231 -X start
 safety_button start
@@ -1,11 +1,9 @@
-CONFIG_BOARD_PROTECTED=y
+# CONFIG_BOARD_UAVCAN_TIMER_OVERRIDE is not set
 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
@@ -17,8 +15,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
@@ -29,11 +27,9 @@ 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
@@ -43,3 +39,6 @@ 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,7 +4,9 @@ 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,4 +1,5 @@
 # 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
@@ -21,6 +22,7 @@ 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
@@ -30,5 +32,7 @@ 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,3 +1,4 @@
+CONFIG_COMMON_BAROMETERS=n
 CONFIG_COMMON_TELEMETRY=n
 CONFIG_DRIVERS_OSD=n
 CONFIG_EXAMPLES_FAKE_GPS=n
@@ -8,5 +9,7 @@ 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,7 +34,6 @@ 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
+if ver hwtypecmp V6X04 V6X14 V6X54
 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
+if ver hwtypecmp V6X03 V6X13 V6X04 V6X14 V6X53 V6X54
 then
 	# Internal SPI bus ICM-42670-P (hard-mounted)
 	icm42670p -R 10 -s start
@@ -39,6 +39,7 @@ 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,6 +41,7 @@ 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,24 +65,25 @@ 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
-	follow_target.msg
 	failure_detector_status.msg
+	follow_target.msg
+	follow_target_estimator.msg
+	follow_target_status.msg
 	generator_status.msg
 	geofence_result.msg
 	gimbal_device_attitude_status.msg
@@ -97,8 +98,8 @@ set(msg_files
 	heater_status.msg
 	home_position.msg
 	hover_thrust_estimate.msg
-	internal_combustion_engine_status.msg
 	input_rc.msg
+	internal_combustion_engine_status.msg
 	iridiumsbd_status.msg
 	irlock_report.msg
 	landing_gear.msg
@@ -121,17 +122,16 @@ set(msg_files
 	obstacle_distance.msg
 	offboard_control_mode.msg
 	onboard_computer_status.msg
-	optical_flow.msg
 	orbit_status.msg
 	parameter_update.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,17 +146,18 @@ set(msg_files
 	sensor_baro.msg
 	sensor_combined.msg
 	sensor_correction.msg
-	sensor_gps.msg
 	sensor_gnss_relative.msg
+	sensor_gps.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_imu.msg
 	sensors_status.msg
+	sensors_status_imu.msg
 	system_power.msg
 	takeoff_status.msg
 	task_stack_info.msg
@@ -174,8 +175,8 @@ set(msg_files
 	uavcan_parameter_value.msg
 	ulog_stream.msg
 	ulog_stream_ack.msg
-	uwb_grid.msg
 	uwb_distance.msg
+	uwb_grid.msg
 	vehicle_acceleration.msg
 	vehicle_air_data.msg
 	vehicle_angular_acceleration.msg
@@ -196,6 +197,8 @@ 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
@@ -1,8 +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_integral		# gyro measurement integrated to flow rate and synchronized with flow measurements (rad)
@@ -1,8 +1,11 @@
-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
@@ -0,0 +1,16 @@
+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
@@ -0,0 +1,12 @@
+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
@@ -1,29 +0,0 @@
-# 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
@@ -19,7 +19,7 @@ bool status_rc_ppm
 bool status_rc_sbus
 bool status_rc_st24
 bool status_rc_sumd
-bool status_safety_off
+bool status_safety_button_event # px4io safety button was pressed for longer than 1 second

 # PX4IO alarms (PX4IO_P_STATUS_ALARMS)
 bool alarm_pwm_error
@@ -0,0 +1,30 @@
+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
@@ -0,0 +1,6 @@
+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:     optical_flow
+  - msg:     sensor_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|
@@ -0,0 +1,21 @@
+# 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
@@ -0,0 +1,13 @@
+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,8 +3,7 @@
 uint64 timestamp				# time since system start (microseconds)

 bool calibration_enabled
-bool system_sensors_initialized
-bool system_hotplug_timeout		# true if the hotplug sensor search is over
+bool pre_flight_checks_pass		# true if all checks necessary to arm pass
 bool auto_mission_available
 bool angular_velocity_valid
 bool attitude_valid
@@ -3,6 +3,7 @@ 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,7 +9,6 @@ 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}
@@ -159,6 +158,7 @@ set(models
 	iris_rplidar
 	iris_vision
 	nxp_cupcar
+	omnicopter
 	plane
 	plane_cam
 	plane_catapult
@@ -324,6 +324,70 @@ 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 && components_used_bitset != 0) {
+	if (_components_progress_bitset == components_used_bitset) {
 		_components_progress_bitset = 0;
 		px4_sem_post(&_components_sem);
 	}
@@ -1,6 +1,6 @@
 /****************************************************************************
 *
- *   Copyright (C) 2015-2016 PX4 Development Team. All rights reserved.
+ *   Copyright (C) 2015-2022 PX4 Development Team. All rights reserved.
 *
 * Redistribution and use in source and binary forms, with or without
 * modification, are permitted provided that the following conditions
@@ -43,28 +43,32 @@
 #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
 {

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

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

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

 int Pxh::process_line(const std::string &line, bool silently_fail)
@@ -133,11 +137,167 @@ 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()
 {
-	_should_exit = false;
-
+	// Only the local_terminal needed for static calls
+	_instance = this;
+	_local_terminal = true;
 	_setup_term();

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

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

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

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

 void Pxh::stop()
 {
-	_restore_term();
-
 	if (_instance) {
 		_instance->_should_exit = true;
 	}
@@ -294,4 +453,101 @@ 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 PX4 Development Team. All rights reserved.
+ *   Copyright (C) 2016-2022 PX4 Development Team. All rights reserved.
 *
 * Redistribution and use in source and binary forms, with or without
 * modification, are permitted provided that the following conditions
@@ -72,7 +72,12 @@ public:
 	void run_pxh();

 	/**
-	 * Can be called to stop 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.
 	 */
 	static void stop();

@@ -80,11 +85,14 @@ 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,8 +150,12 @@ Server::_server_main()
 		int n_ready = poll(poll_fds.data(), poll_fds.size(), -1);

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

 		_lock();
@@ -2,8 +2,6 @@ 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,6 +86,11 @@ 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
 */
@@ -188,12 +193,6 @@ 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,6 +68,7 @@

 #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
@@ -136,10 +137,6 @@
 #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
@@ -206,7 +203,16 @@

 #define DRV_GPS_DEVTYPE_SIM 0xAF

+#define DRV_TRNS_DEVTYPE_MXS 0xB0
+
 #define DRV_HYGRO_DEVTYPE_SHT3X 0xB1
+#define DRV_LED_DEVTYPE_RGBLED_LP55231 0xB2
+
+#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

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

 	constexpr VerbCommand commands[] = {
-		{"reverse", DShot_cmd_spin_direction_reversed, 10},
-		{"normal", DShot_cmd_spin_direction_normal, 10},
+		{"reverse", DShot_cmd_spin_direction_2, 10},
+		{"normal", DShot_cmd_spin_direction_1, 10},
 		{"save", DShot_cmd_save_settings, 10},
 		{"3d_on", DShot_cmd_3d_mode_on, 10},
 		{"3d_off", DShot_cmd_3d_mode_off, 10},
@@ -34,4 +34,6 @@
 #add_subdirectory(neopixel) # requires board support (BOARD_HAS_N_S_RGB_LED)
 add_subdirectory(rgbled)
 add_subdirectory(rgbled_ncp5623c)
+add_subdirectory(rgbled_lp55231)
+
 #add_subdirectory(rgbled_pwm) # requires board support (BOARD_HAS_LED_PWM/BOARD_HAS_UI_LED_PWM)
@@ -0,0 +1,42 @@
+############################################################################
+#
+#   Copyright (c) 2015 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
+# are met:
+#
+# 1. Redistributions of source code must retain the above copyright
+#    notice, this list of conditions and the following disclaimer.
+# 2. Redistributions in binary form must reproduce the above copyright
+#    notice, this list of conditions and the following disclaimer in
+#    the documentation and/or other materials provided with the
+#    distribution.
+# 3. Neither the name PX4 nor the names of its contributors may be
+#    used to endorse or promote products derived from this software
+#    without specific prior written permission.
+#
+# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+# "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+# LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+# FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+# COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+# INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+# BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
+# OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
+# AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+# LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+# ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+# POSSIBILITY OF SUCH DAMAGE.
+#
+############################################################################
+
+px4_add_module(
+	MODULE drivers__rgbled_lp55231
+	MAIN rgbled_lp55231
+	SRCS
+		rgbled_lp55231.cpp
+	DEPENDS
+		drivers__device
+		led
+	)
@@ -0,0 +1,5 @@
+menuconfig DRIVERS_LIGHTS_RGBLED_LP55231
+	bool "rgbled_lp55231"
+	default n
+	---help---
+		Enable support for rgbled_lp55231
@@ -0,0 +1,385 @@
+/****************************************************************************
+ *
+ *   Copyright (c) 2022 PX4 Development Team. All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright
+ *    notice, this list of conditions and the following disclaimer.
+ * 2. Redistributions in binary form must reproduce the above copyright
+ *    notice, this list of conditions and the following disclaimer in
+ *    the documentation and/or other materials provided with the
+ *    distribution.
+ * 3. Neither the name PX4 nor the names of its contributors may be
+ *    used to endorse or promote products derived from this software
+ *    without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
+ * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
+ * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *
+ ****************************************************************************/
+
+/**
+ *
+ * Driver for RGB LP55231 LED controller connected via I2C.
+ */
+
+#include <drivers/device/i2c.h>
+#include <lib/led/led.h>
+#include <lib/parameters/param.h>
+#include <px4_platform_common/getopt.h>
+#include <px4_platform_common/i2c_spi_buses.h>
+#include <px4_platform_common/module.h>
+
+using namespace time_literals;
+
+static constexpr uint8_t ADDRESS = 0x32;
+
+// register stuff
+static constexpr uint8_t REG_CNTRL1 = 0x00;
+static constexpr uint8_t REG_CNTRL2 = 0x01;
+static constexpr uint8_t REG_RATIO_MSB = 0x02;
+static constexpr uint8_t REG_RATIO_LSB = 0x03;
+static constexpr uint8_t REG_OUTPUT_ONOFF_MSB = 0x04;
+static constexpr uint8_t REG_OUTPUT_ONOFF_LSB = 0x05;
+
+// Per LED control channels - fader channel assig, log dimming enable, temperature compensation
+static constexpr uint8_t REG_D1_CTRL = 0x06;
+static constexpr uint8_t REG_D2_CTRL = 0x07;
+static constexpr uint8_t REG_D3_CTRL = 0x08;
+static constexpr uint8_t REG_D4_CTRL = 0x09;
+static constexpr uint8_t REG_D5_CTRL = 0x0a;
+static constexpr uint8_t REG_D6_CTRL = 0x0b;
+static constexpr uint8_t REG_D7_CTRL = 0x0c;
+static constexpr uint8_t REG_D8_CTRL = 0x0d;
+static constexpr uint8_t REG_D9_CTRL = 0x0e;
+
+// 0x0f to 0x15 reserved
+
+// Direct PWM control registers
+static constexpr uint8_t REG_D1_PWM  = 0x16;
+static constexpr uint8_t REG_D2_PWM  = 0x17;
+static constexpr uint8_t REG_D3_PWM  = 0x18;
+static constexpr uint8_t REG_D4_PWM  = 0x19;
+static constexpr uint8_t REG_D5_PWM  = 0x1a;
+static constexpr uint8_t REG_D6_PWM  = 0x1b;
+static constexpr uint8_t REG_D7_PWM  = 0x1c;
+static constexpr uint8_t REG_D8_PWM  = 0x1d;
+static constexpr uint8_t REG_D9_PWM  = 0x1e;
+
+// 0x1f to 0x25 reserved
+
+// Drive current registers
+static constexpr uint8_t REG_D1_I_CTL = 0x26;
+static constexpr uint8_t REG_D2_I_CTL  = 0x27;
+static constexpr uint8_t REG_D3_I_CTL  = 0x28;
+static constexpr uint8_t REG_D4_I_CTL  = 0x29;
+static constexpr uint8_t REG_D5_I_CTL  = 0x2a;
+static constexpr uint8_t REG_D6_I_CTL  = 0x2b;
+static constexpr uint8_t REG_D7_I_CTL  = 0x2c;
+static constexpr uint8_t REG_D8_I_CTL  = 0x2d;
+static constexpr uint8_t REG_D9_I_CTL  = 0x2e;
+
+// 0x2f to 0x35 reserved
+
+static constexpr uint8_t REG_MISC     = 0x36;
+static constexpr uint8_t REG_PC1      = 0x37;
+static constexpr uint8_t REG_PC2      = 0x38;
+static constexpr uint8_t REG_PC3      = 0x39;
+static constexpr uint8_t REG_STATUS_IRQ = 0x3A;
+static constexpr uint8_t REG_INT_GPIO   = 0x3B;
+static constexpr uint8_t REG_GLOBAL_VAR = 0x3C;
+static constexpr uint8_t REG_RESET      = 0x3D;
+static constexpr uint8_t REG_TEMP_CTL   = 0x3E;
+static constexpr uint8_t REG_TEMP_READ  = 0x3F;
+static constexpr uint8_t REG_TEMP_WRITE = 0x40;
+static constexpr uint8_t REG_TEST_CTL   = 0x41;
+static constexpr uint8_t REG_TEST_ADC   = 0x42;
+
+// 0x43 to 0x44 reserved
+
+static constexpr uint8_t REG_ENGINE_A_VAR = 0x45;
+static constexpr uint8_t REG_ENGINE_B_VAR = 0x46;
+static constexpr uint8_t REG_ENGINE_C_VAR = 0x47;
+
+static constexpr uint8_t REG_MASTER_FADE_1 = 0x48;
+static constexpr uint8_t REG_MASTER_FADE_2 = 0x49;
+static constexpr uint8_t REG_MASTER_FADE_3 = 0x4A;
+
+// 0x4b Reserved
+
+static constexpr uint8_t REG_PROG1_START = 0x4C;
+static constexpr uint8_t REG_PROG2_START = 0x4D;
+static constexpr uint8_t REG_PROG3_START = 0x4E;
+static constexpr uint8_t REG_PROG_PAGE_SEL = 0x4f;
+
+// Memory is more confusing - there are 6 pages, sel by addr 4f
+static constexpr uint8_t REG_PROG_MEM_BASE = 0x50;
+static constexpr uint8_t REG_PROG_MEM_END  = 0x6f;
+
+static constexpr uint8_t REG_ENG1_MAP_MSB = 0x70;
+static constexpr uint8_t REG_ENG1_MAP_LSB = 0x71;
+static constexpr uint8_t REG_ENG2_MAP_MSB = 0x72;
+static constexpr uint8_t REG_ENG2_MAP_LSB = 0x73;
+static constexpr uint8_t REG_ENG3_MAP_MSB = 0x74;
+static constexpr uint8_t REG_ENG3_MAP_LSB = 0x75;
+
+static constexpr uint8_t REG_GAIN_CHANGE = 0x76;
+
+// Colors on eval board
+static constexpr uint8_t CHANNEL_L0_RED = 0;
+static constexpr uint8_t CHANNEL_L0_GREEN = 3;
+static constexpr uint8_t CHANNEL_L0_BLUE = 4;
+static constexpr uint8_t CHANNEL_L1_RED = 1;
+static constexpr uint8_t CHANNEL_L1_GREEN = 5;
+static constexpr uint8_t CHANNEL_L1_BLUE = 6;
+static constexpr uint8_t CHANNEL_L2_RED = 2;
+static constexpr uint8_t CHANNEL_L2_GREEN = 7;
+static constexpr uint8_t CHANNEL_L2_BLUE = 8;
+
+class RGBLED_LP55231 : public device::I2C, public I2CSPIDriver<RGBLED_LP55231>
+{
+public:
+	RGBLED_LP55231(const I2CSPIDriverConfig &config);
+	virtual ~RGBLED_LP55231() override;
+
+	static void print_usage();
+
+	int init() override;
+	int probe() override;
+
+	void RunImpl();
+
+private:
+	void print_status() override;
+
+	int write(uint8_t address, uint8_t value);
+	int read(uint8_t address, uint8_t &value);
+
+	int setChannelPWM(uint8_t channel, uint8_t value);
+
+	int setLed(uint8_t channel_red, uint8_t channel_green, uint8_t channel_blue,
+		   const LedControlDataSingle &led);
+
+	int reset();
+	int enable();
+
+	LedController		_led_controller;
+
+	// uint8_t _channel = 0;
+};
+
+RGBLED_LP55231::RGBLED_LP55231(const I2CSPIDriverConfig &config) :
+	I2C(config),
+	I2CSPIDriver(config)
+{
+}
+
+RGBLED_LP55231::~RGBLED_LP55231()
+{
+	reset();
+}
+
+
+int RGBLED_LP55231::write(uint8_t address, uint8_t value)
+{
+	uint8_t data[2] = {address, value};
+	return transfer(data, sizeof(data), nullptr, 0);
+}
+
+int RGBLED_LP55231::read(uint8_t address, uint8_t &value)
+{
+	return transfer(&address, 1, (uint8_t *)&value, 1);
+}
+
+
+int RGBLED_LP55231::enable()
+{
+	int ret = write(REG_CNTRL1, 0x40);
+	// enable charge pump, internal oscillator, auto increment
+	ret = write(REG_MISC, 0x53);
+	return ret;
+}
+
+int RGBLED_LP55231::reset()
+{
+	return write(REG_RESET, 0xFF);
+}
+
+
+int RGBLED_LP55231::init()
+{
+	int ret = I2C::init();
+
+	if (ret != OK) {
+		return ret;
+	}
+
+	ret = reset();
+	ret = enable();
+
+	// kick off work queue
+	ScheduleNow();
+
+	return OK;
+}
+
+int RGBLED_LP55231::setChannelPWM(uint8_t channel, uint8_t value)
+{
+	assert(channel < 9);
+	return write(REG_D1_PWM + channel, value);
+}
+
+int RGBLED_LP55231::setLed(uint8_t channel_red, uint8_t channel_green, uint8_t channel_blue,
+			   const LedControlDataSingle &led)
+{
+	uint8_t r{0}, g{0}, b{0};
+	uint8_t brightness = led.brightness;
+
+	switch (led.color) {
+	case led_control_s::COLOR_RED:
+		r = brightness; g = 0; b = 0;
+		break;
+
+	case led_control_s::COLOR_GREEN:
+		r = 0; g = brightness; b = 0;
+		break;
+
+	case led_control_s::COLOR_BLUE:
+		r = 0; g = 0; b = brightness;
+		break;
+
+	case led_control_s::COLOR_AMBER: //make it the same as yellow
+	case led_control_s::COLOR_YELLOW:
+		r = brightness / 2; g = brightness / 2; b = 0;
+		break;
+
+	case led_control_s::COLOR_PURPLE:
+		r = brightness / 2; g = 0; b = brightness / 2;
+		break;
+
+	case led_control_s::COLOR_CYAN:
+		r = 0; g = brightness / 2; b = brightness / 2;
+		break;
+
+	case led_control_s::COLOR_WHITE:
+		r = brightness / 3; g = brightness / 3; b = brightness / 3;
+		break;
+
+	default: // led_control_s::COLOR_OFF
+		r = 0; g = 0; b = 0;
+		break;
+	}
+
+	int ret = 0;
+	ret = setChannelPWM(channel_red, r);
+	ret = setChannelPWM(channel_green, g);
+	ret = setChannelPWM(channel_blue, b);
+	return ret;
+}
+
+int RGBLED_LP55231::probe()
+{
+	// try read output on / off (should be 0xff)
+	uint8_t on_off;
+	int ret = read(REG_OUTPUT_ONOFF_LSB, on_off);
+
+	if (on_off != 0xFF) {
+		return -1;
+	}
+
+	// try read current control (should be 0xAF)
+	uint8_t current_control;
+	ret = read(REG_D1_I_CTL, current_control);
+
+	if (current_control != 0xAF) {
+		return -1;
+	}
+
+	_retries = 1;
+
+	return ret;
+}
+
+void
+RGBLED_LP55231::print_status()
+{
+	PX4_INFO("No status implemented");
+}
+
+
+
+void RGBLED_LP55231::RunImpl()
+{
+	LedControlData led_control_data;
+
+	if (_led_controller.update(led_control_data) == 1) {
+		setLed(CHANNEL_L0_RED, CHANNEL_L0_GREEN, CHANNEL_L0_BLUE, led_control_data.leds[0]);
+		setLed(CHANNEL_L1_RED, CHANNEL_L1_GREEN, CHANNEL_L1_BLUE, led_control_data.leds[1]);
+		setLed(CHANNEL_L2_RED, CHANNEL_L2_GREEN, CHANNEL_L2_BLUE, led_control_data.leds[2]);
+	}
+
+	/* re-queue ourselves to run again later */
+	ScheduleDelayed(_led_controller.maximum_update_interval());
+
+	// setChannelPWM(_channel, 0);
+	// _channel++;
+	// _channel = _channel % 9;
+	// setChannelPWM(_channel, 255);
+	// ScheduleDelayed(500_ms);
+}
+
+
+void
+RGBLED_LP55231::print_usage()
+{
+	PRINT_MODULE_USAGE_NAME("rgbled_lp55231", "driver");
+	PRINT_MODULE_USAGE_COMMAND("start");
+	PRINT_MODULE_USAGE_PARAMS_I2C_SPI_DRIVER(true, false);
+	PRINT_MODULE_USAGE_PARAMS_I2C_ADDRESS(0x32);
+	PRINT_MODULE_USAGE_DEFAULT_COMMANDS();
+}
+
+extern "C" __EXPORT int rgbled_lp55231_main(int argc, char *argv[])
+{
+	using ThisDriver = RGBLED_LP55231;
+	BusCLIArguments cli{true, false};
+	cli.default_i2c_frequency = 100000;
+	cli.i2c_address = ADDRESS;
+
+	const char *verb = cli.parseDefaultArguments(argc, argv);
+
+	if (!verb) {
+		ThisDriver::print_usage();
+		return -1;
+	}
+
+	BusInstanceIterator iterator(MODULE_NAME, cli, DRV_LED_DEVTYPE_RGBLED_LP55231);
+
+	if (!strcmp(verb, "start")) {
+		return ThisDriver::module_start(cli, iterator);
+	}
+
+	if (!strcmp(verb, "stop")) {
+		return ThisDriver::module_stop(iterator);
+	}
+
+	if (!strcmp(verb, "status")) {
+		return ThisDriver::module_status(iterator);
+	}
+
+	ThisDriver::print_usage();
+	return -1;
+}
@@ -43,6 +43,10 @@ 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) {
@@ -52,27 +56,21 @@ PAA3905::PAA3905(const I2CSPIDriverConfig &config) :
 		_rotation = matrix::Dcmf{matrix::Eulerf{0.f, 0.f, math::radians(yaw_rotation_degrees)}};

 	} else {
-		// 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();
-		}
+		_rotation.identity();
 	}
 }

 PAA3905::~PAA3905()
 {
 	// free perf counters
-	perf_free(_sample_perf);
+	perf_free(_cycle_perf);
 	perf_free(_interval_perf);
-	perf_free(_comms_errors);
+	perf_free(_reset_perf);
 	perf_free(_false_motion_perf);
-	perf_free(_register_write_fail_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);
 }

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

 	Configure();

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

 int PAA3905::probe()
 {
-	const uint8_t Product_ID = RegisterRead(Register::Product_ID);
+	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);

-	if (Product_ID != PRODUCT_ID) {
-		PX4_ERR("Product_ID: %X", Product_ID);
-		return PX4_ERROR;
+		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;
 	}

-	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;
+	return PX4_ERROR;
 }

 int PAA3905::DataReadyInterruptCallback(int irq, void *context, void *arg)
@@ -123,12 +121,14 @@ 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,8 +159,10 @@ void PAA3905::exit_and_cleanup()
 	I2CSPIDriverBase::exit_and_cleanup();
 }

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

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

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

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

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

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

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

 	EnableLed();

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

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

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

-void PAA3905::CheckMode()
+void PAA3905::ConfigureStandardDetectionSetting()
 {
-	// Read Register 0x15. Check Bit [7:6] for AMS mode and store it into a variable.
-	const uint8_t Observation = RegisterRead(Register::Observation);
+	// 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.

-	// Bit [7:6] AMS mode
-	const uint8_t ams_mode = (Observation & (Bit7 | Bit6)) >> 5;
+	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);

-	if (ams_mode == 0x0) {
-		// Mode 0
-		_mode = Mode::SuperLowLight;
-
-	} else if (ams_mode == 0x1) {
-		// Mode 1
-		_mode = Mode::LowLight;
-
-	} else if (ams_mode == 0x2) {
-		// Mode 2
-		_mode = Mode::SuperLowLight;
-	}
+	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::StandardDetectionSetting()
+void PAA3905::ConfigureEnhancedDetectionMode()
 {
-	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);
+	// 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.

-	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);
+	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::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()
+void PAA3905::ConfigureAutomaticModeSwitching()
 {
 	// Automatic switching between Mode 0, 1 and 2:
-	RegisterWriteVerified(0x7F, 0x08);
-	RegisterWriteVerified(0x68, 0x02);
-	RegisterWriteVerified(0x7F, 0x00);
+	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
-	RegisterWriteVerified(0x7F, 0x14);
-	RegisterWriteVerified(0x6F, 0x0C);
-	RegisterWriteVerified(0x7F, 0x00);
+	RegisterWrite(0x7F, 0x14);
+	RegisterWrite(0x6F, 0x0C);
+	RegisterWrite(0x7F, 0x00);
 }

-uint8_t PAA3905::RegisterRead(uint8_t reg, int retries)
+bool PAA3905::UpdateMode(const uint8_t observation)
 {
-	for (int i = 0; i < retries; i++) {
-		px4_udelay(TIME_us_TSRAD);
-		uint8_t cmd[2] {reg, 0};
+	bool mode_changed = false;

-		if (transfer(&cmd[0], &cmd[0], sizeof(cmd)) == 0) {
-			return cmd[1];
+	// 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;
+
+	} else if (ams_mode == 0x1) {
+		// Mode 1 (LowLight)
+		if (_mode != Mode::LowLight) {
+			mode_changed = true;
+			perf_count(_mode_change_low_light_perf);
+		}
+
+		_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 = Mode::SuperLowLight;
+		_scheduled_interval_us = SAMPLE_INTERVAL_MODE_2;
 	}

-	perf_count(_comms_errors);
-	return 0;
+	return mode_changed;
+}
+
+uint8_t PAA3905::RegisterRead(uint8_t reg)
+{
+	// tSWR SPI Time Between Write And Read Commands
+	const hrt_abstime elapsed_last_write = hrt_elapsed_time(&_last_write_time);
+
+	if (elapsed_last_write < TIME_TSWR_us) {
+		px4_udelay(TIME_TSWR_us - elapsed_last_write);
+	}
+
+	// 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];
 }

 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;
-
-	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;
+	transfer(&cmd[0], nullptr, sizeof(cmd));
+	hrt_store_absolute_time(&_last_write_time);
 }

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

-	// 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;
+	const hrt_abstime now = hrt_absolute_time();

-	if ((hrt_elapsed_time(&_last_good_publish) > RESET_TIMEOUT_US) && (hrt_elapsed_time(&_last_reset) > RESET_TIMEOUT_US)) {
+	// 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)) {
 		Configure();
+		perf_end(_cycle_perf);
 		return;
 	}

-	perf_begin(_sample_perf);
-	perf_count(_interval_perf);
+	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);
+	}

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

-	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);
+	if (transfer((uint8_t *)&buf, (uint8_t *)&buf, sizeof(buf)) != 0) {
+		perf_end(_cycle_perf);
 		return;
 	}

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

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

-	CheckMode(); // update _mode variable
+	// 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);
+		}
+	}

 	// 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) {
@@ -513,97 +568,73 @@ void PAA3905::RunImpl()
 	}

 	if (data_valid) {
-		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);
+		// publish sensor_optical_flow
+		sensor_optical_flow_s report{};
+		report.timestamp_sample = timestamp_sample;
+		report.device_id = get_device_id();

-		_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;
+		report.integration_timespan_us = _scheduled_interval_us;
+		report.quality = buf.data.SQUAL;

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

-	} else {
-		_valid_count = 0;
-		ResetAccumulatedData();
-		return;
+		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;
+		}
 	}

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

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

-	perf_print_counter(_sample_perf);
+	perf_print_counter(_cycle_perf);
 	perf_print_counter(_interval_perf);
-	perf_print_counter(_comms_errors);
+	perf_print_counter(_reset_perf);
 	perf_print_counter(_false_motion_perf);
-	perf_print_counter(_register_write_fail_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);
 }
@@ -32,9 +32,9 @@
 ****************************************************************************/

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

 #pragma once
@@ -48,16 +48,15 @@
 #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/optical_flow.h>
+#include <uORB/topics/sensor_optical_flow.h>

 using namespace time_literals;
 using namespace PixArt_PAA3905;

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

 class PAA3905 : public device::SPI, public I2CSPIDriver<PAA3905>
 {
@@ -78,59 +77,61 @@ 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, int retries = 2);
+	uint8_t RegisterRead(uint8_t reg);
 	void RegisterWrite(uint8_t reg, uint8_t data);
-	bool RegisterWriteVerified(uint8_t reg, uint8_t data, int retries = 1);
-
-	void EnableLed();
-
-	void StandardDetectionSetting();
-	void EnhancedDetectionMode();
-	void ModeAuto012();
-
-	void CheckMode();

 	void Configure();

-	void ResetAccumulatedData();
+	void ConfigureAutomaticModeSwitching();

-	uORB::PublicationMulti<optical_flow_s> _optical_flow_pub{ORB_ID(optical_flow)};
+	void ConfigureModeBright();
+	void ConfigureModeLowLight();
+	void ConfigureModeSuperLowLight();

-	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")};
+	void ConfigureStandardDetectionSetting();
+	void ConfigureEnhancedDetectionMode();

-	static constexpr uint64_t COLLECT_TIME{15000}; // 15 milliseconds, optical flow data publish rate
+	void EnableLed();
+
+	bool UpdateMode(const uint8_t observation);
+
+	uORB::PublicationMulti<sensor_optical_flow_s> _sensor_optical_flow_pub{ORB_ID(sensor_optical_flow)};
+
+	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};

 	const spi_drdy_gpio_t _drdy_gpio;

-	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};
+	matrix::Dcmf _rotation;

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

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

-	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};
+	uint32_t _scheduled_interval_us{SAMPLE_INTERVAL_MODE_0};

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

-	hrt_abstime _last_good_publish{0};
+	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_reset{0};
 };
@@ -33,8 +33,8 @@

 #pragma once

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

-static constexpr uint8_t PRODUCT_ID = 0xA2;
-static constexpr uint8_t REVISION_ID = 0x00;
+namespace PixArt_PAA3905
+{
+
+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 delay needed for paa3905
-static constexpr uint32_t TIME_us_TSWW  = 11; // actually 10.5us
-static constexpr uint32_t TIME_us_TSRAD = 2;
+// 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

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

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

 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_MODE0;
+	cli.spi_mode = SPIDEV_MODE3;
 	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-2021 PX4 Development Team. All rights reserved.
+#   Copyright (c) 2019-2022 PX4 Development Team. All rights reserved.
 #
 # Redistribution and use in source and binary forms, with or without
 # modification, are permitted provided that the following conditions
@@ -38,7 +38,7 @@ px4_add_module(
 		paw3902_main.cpp
 		PAW3902.cpp
 		PAW3902.hpp
-		PixArt_PAW3902JF_Registers.hpp
+		PixArt_PAW3902_Registers.hpp
 	DEPENDS
 		conversion
 		drivers__device
--- a/Show More
+++ b/Show More