parameters: new parameter server backend

- parameter backend is now running in a wq and accessible over pub/sub - parameters library can call into backend directly (with locking), but operations will gradually start moving to pub/sub - majority of parameters migrated as is, only small changes to parameter autosave and parameter notification that are now performed from the WQ thread
systemcmds/bsondump: new command line utility (extracted from parameters)
2026-05-24 04:47:34 +08:00 · 2022-12-20 09:51:48 -05:00 · 2022-12-19 21:46:37 -05:00 · 2022-12-19 16:50:09 -05:00 · 2022-12-19 13:24:55 -05:00 · 2022-12-19 11:36:48 +01:00
231 changed files with 7257 additions and 4391 deletions
@@ -684,7 +684,7 @@ void checkStatus() {
  sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-*` --cmd "param save"'
  sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-*` --cmd "param status"'
  sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-*` --cmd "param show SYS*"'
-  sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-*` --cmd "param dump"'
+  sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-*` --cmd "bsondump /fs/mtd_params"'

  // status commands
  sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-*` --cmd "cat /proc/fs/blocks"'
@@ -715,7 +715,7 @@ void checkStatus() {
  sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-*` --cmd "mavlink status" || true'
  sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-*` --cmd "mount"'
  sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-*` --cmd "mtd status"'
-  sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-*` --cmd "param dump"'
+  sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-*` --cmd "bsondump /fs/mtd_params"'
  sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-*` --cmd "param show" || true'
  sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-*` --cmd "param status"'
  sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-*` --cmd "perf latency"'
@@ -767,13 +767,13 @@ void runTests() {

  sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-*` --cmd "tests file" || true'

-  sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-*` --cmd "param dump /fs/mtd_params"'
+  sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-*` --cmd "bsondump /fs/mtd_params"'
  sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-*` --cmd "mtd readtest"'
-  sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-*` --cmd "param dump /fs/mtd_params"'
+  sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-*` --cmd "bsondump /fs/mtd_params"'
  sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-*` --cmd "mtd rwtest"'
-  sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-*` --cmd "param dump /fs/mtd_params"'
+  sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-*` --cmd "bsondump /fs/mtd_params"'
  sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-*` --cmd "mtd erase"'
-  sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-*` --cmd "param dump /fs/mtd_params"'
+  sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-*` --cmd "bsondump /fs/mtd_params"'

  sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-*` --cmd "sd_bench"'
  sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-*` --cmd "sd_bench -v"'
@@ -16,7 +16,7 @@ jobs:
      matrix:
        config:
          - {model: "iris",          latitude:  "59.617693", longitude: "-151.145316", altitude:  "48", build_type: "RelWithDebInfo" } # Alaska
-          - {model: "standard_vtol", latitude: "-38.071235", longitude:  "145.281220", altitude:  "31", build_type: "AddressSanitizer" } # Australia
+          # - {model: "standard_vtol", latitude: "-38.071235", longitude:  "145.281220", altitude:  "31", build_type: "AddressSanitizer" } # Australia
          - {model: "tailsitter" ,   latitude:  "29.660316", longitude:  "-82.316658", altitude:  "30", build_type: "RelWithDebInfo" } # Florida
          - {model: "standard_vtol", latitude:  "47.397742", longitude:    "8.545594", altitude: "488", build_type: "Coverage" } # Zurich

@@ -40,8 +40,8 @@ menu "Toolchain"
        help
            forces nolockstep behaviour, despite REPLAY env variable

-    config BOARD_LINUX
-        bool "Linux OS"
+    config BOARD_LINUX_TARGET
+        bool "Linux OS Target"
        depends on PLATFORM_POSIX
        help
            Board Platform is running the Linux operating system
@@ -74,7 +74,7 @@ if [ -f $PARAM_FILE ]; then
 	then
 		echo "ERROR [init] param import failed"

-		param dump $PARAM_FILE
+		bsondump $PARAM_FILE

 		# try to make a backup copy
 		cp $PARAM_FILE param_import_fail.bson
@@ -85,7 +85,7 @@ if [ -f $PARAM_FILE ]; then
 			echo "[init] importing from parameter backup"

 			# dump current backup file contents for comparison
-			param dump $PARAM_BACKUP_FILE
+			bsondump $PARAM_BACKUP_FILE

 			param import $PARAM_BACKUP_FILE

@@ -142,7 +142,9 @@ then
 	param set CAL_GYRO2_ID 1311004 # 1311004: DRV_IMU_DEVTYPE_SIM, BUS: 3, ADDR: 1, TYPE: SIMULATION

 	param set CAL_MAG0_ID 197388
+	param set CAL_MAG0_PRIO 50
 	param set CAL_MAG1_ID 197644
+	param set CAL_MAG1_PRIO 50

 	param set SENS_BOARD_X_OFF 0.000001
 	param set SENS_DPRES_OFF 0.001
@@ -225,7 +227,7 @@ then
 elif [ ! -e "$autostart_file" ] && [ "$SYS_AUTOSTART" -ne "0" ]
 then
 	echo "Error: no autostart file found ($autostart_file)"
-	exit 1
+	#exit 1
 fi

 #user defined params for instances can be in PATH
@@ -130,7 +130,7 @@ else
 		echo "ERROR [init] param import failed"
 		set STARTUP_TUNE 2 # tune 2 = ERROR_TUNE

-		param dump $PARAM_FILE
+		bsondump $PARAM_FILE

 		if [ -d "/fs/microsd" ]
 		then
@@ -143,7 +143,7 @@ else
 				echo "[init] importing from parameter backup"

 				# dump current backup file contents for comparison
-				param dump $PARAM_BACKUP_FILE
+				bsondump $PARAM_BACKUP_FILE

 				param import $PARAM_BACKUP_FILE

@@ -43,13 +43,13 @@ do
 	${DIR}/run_nsh_cmd.py --device ${SERIAL_DEVICE} --cmd 'param reset SYS_HITL'
 	${DIR}/run_nsh_cmd.py --device ${SERIAL_DEVICE} --cmd 'param status'
 	${DIR}/run_nsh_cmd.py --device ${SERIAL_DEVICE} --cmd 'param save'
-	${DIR}/run_nsh_cmd.py --device ${SERIAL_DEVICE} --cmd 'param dump'
+	${DIR}/run_nsh_cmd.py --device ${SERIAL_DEVICE} --cmd 'bsondump /fs/mtd_params'

 	${DIR}/reboot.py --device ${SERIAL_DEVICE}

 	${DIR}/run_nsh_cmd.py --device ${SERIAL_DEVICE} --cmd 'param status'
-	${DIR}/run_nsh_cmd.py --device ${SERIAL_DEVICE} --cmd 'param dump /fs/mtd_params' || true
-	${DIR}/run_nsh_cmd.py --device ${SERIAL_DEVICE} --cmd 'param dump /fs/microsd/parameters_backup.bson' || true
+	${DIR}/run_nsh_cmd.py --device ${SERIAL_DEVICE} --cmd 'bsondump /fs/mtd_params' || true
+	${DIR}/run_nsh_cmd.py --device ${SERIAL_DEVICE} --cmd 'bsondump /fs/microsd/parameters_backup.bson' || true

 	${DIR}/run_nsh_cmd.py --device ${SERIAL_DEVICE} --cmd 'ps'
 	${DIR}/run_nsh_cmd.py --device ${SERIAL_DEVICE} --cmd 'work_queue status'
@@ -28,7 +28,7 @@ args = parser.parse_args()
 verbose = args.verbose

 build_configs = []
-excluded_boards = ['modalai_voxl2']  # TODO: fix and enable
+excluded_boards = ['modalai_voxl2', 'px4_ros2']  # TODO: fix and enable
 excluded_manufacturers = ['atlflight']
 excluded_platforms = ['qurt']
 excluded_labels = [
@@ -122,21 +122,13 @@ def generate_output_from_file(format_idx, filename, outputdir, package, template
    for field in spec.parsed_fields():
        field_name_and_type.update({field.name: field.type})

-    # assert if the timestamp field exists
-    try:
-        assert 'timestamp' in field_name_and_type
-    except AssertionError:
-        print("[ERROR] uORB topic files generator:\n\tgenerate_output_from_file:\tNo 'timestamp' field found in " +
-              spec.short_name + " msg definition!")
-        exit(1)
-
    # assert if the timestamp field is of type uint64
-    try:
-        assert field_name_and_type.get('timestamp') == 'uint64'
-    except AssertionError:
-        print("[ERROR] uORB topic files generator:\n\tgenerate_output_from_file:\t'timestamp' field in " + spec.short_name +
-              " msg definition is not of type uint64 but rather of type " + field_name_and_type.get('timestamp') + "!")
-        exit(1)
+    # try:
+    #     assert field_name_and_type.get('timestamp') == 'uint64'
+    # except AssertionError:
+    #     print("[ERROR] uORB topic files generator:\n\tgenerate_output_from_file:\t'timestamp' field in " + spec.short_name +
+    #           " msg definition is not of type uint64 but rather of type " + field_name_and_type.get('timestamp') + "!")
+    #     exit(1)

    topics = get_topics(filename)

@@ -136,9 +136,9 @@ serial_ports = {
        "default_baudrate": 1, # set default to an unusable value to detect that this serial port has not been configured
        },

-    # Pixhawk Payload Bus
-    "PPB": {
-        "label": "Pixhawk Payload Bus",
+    # EXT2
+    "EXT2": {
+        "label": "EXT2",
        "index": 401,
        "default_baudrate": 57600,
        },
@@ -79,8 +79,9 @@ CONFIG_MODULES_UUV_ATT_CONTROL=y
 CONFIG_MODULES_UUV_POS_CONTROL=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_BSONDUMP=y
 CONFIG_SYSTEMCMDS_DUMPFILE=y
 CONFIG_SYSTEMCMDS_GPIO=y
 CONFIG_SYSTEMCMDS_HARDFAULT_LOG=y
@@ -70,6 +70,7 @@ CONFIG_MODULES_TEMPERATURE_COMPENSATION=y
 CONFIG_MODULES_VTOL_ATT_CONTROL=y
 CONFIG_SYSTEMCMDS_ACTUATOR_TEST=y
 CONFIG_SYSTEMCMDS_BL_UPDATE=y
+CONFIG_SYSTEMCMDS_BSONDUMP=y
 CONFIG_SYSTEMCMDS_DMESG=y
 CONFIG_SYSTEMCMDS_HARDFAULT_LOG=y
 CONFIG_SYSTEMCMDS_I2CDETECT=y
@@ -159,6 +159,7 @@ CONFIG_NET_BROADCAST=y
 CONFIG_NET_ETH_PKTSIZE=1518
 CONFIG_NET_ICMP=y
 CONFIG_NET_ICMP_SOCKET=y
+CONFIG_NET_NACTIVESOCKETS=16
 CONFIG_NET_SOLINGER=y
 CONFIG_NET_TCP=y
 CONFIG_NET_TCPBACKLOG=y
@@ -76,8 +76,9 @@ CONFIG_MODULES_UUV_ATT_CONTROL=y
 CONFIG_MODULES_UUV_POS_CONTROL=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_BSONDUMP=y
 CONFIG_SYSTEMCMDS_DMESG=y
 CONFIG_SYSTEMCMDS_DUMPFILE=y
 CONFIG_SYSTEMCMDS_GPIO=y
@@ -138,6 +138,7 @@ CONFIG_NET_ARP_SEND=y
 CONFIG_NET_BROADCAST=y
 CONFIG_NET_ICMP=y
 CONFIG_NET_ICMP_SOCKET=y
+CONFIG_NET_NACTIVESOCKETS=16
 CONFIG_NET_SOLINGER=y
 CONFIG_NET_TCP=y
 CONFIG_NET_TCPBACKLOG=y
@@ -35,8 +35,8 @@ add_compile_options(-Wno-cast-align)

 include(ExternalProject)
 ExternalProject_Add(librobotcontrol
-	GIT_REPOSITORY https://github.com/dagar/librobotcontrol.git
-	GIT_TAG 1abcb0a # latest as of 2019-12-29
+        GIT_REPOSITORY https://github.com/beagleboard/librobotcontrol.git
+        GIT_TAG 290e14f
 	CMAKE_CACHE_ARGS -DCMAKE_TOOLCHAIN_FILE:STRING=${CMAKE_TOOLCHAIN_FILE}
 	INSTALL_COMMAND ""
 	TEST_COMMAND ""
@@ -1,5 +1,5 @@
 CONFIG_PLATFORM_POSIX=y
-CONFIG_BOARD_LINUX=y
+CONFIG_BOARD_LINUX_TARGET=y
 CONFIG_BOARD_TOOLCHAIN="arm-linux-gnueabihf"
 CONFIG_BOARD_ARCHITECTURE="cortex-a8"
 CONFIG_BOARD_TESTING=y
@@ -63,6 +63,7 @@ CONFIG_MODULES_TEMPERATURE_COMPENSATION=y
 CONFIG_MODULES_UUV_ATT_CONTROL=y
 CONFIG_MODULES_UUV_POS_CONTROL=y
 CONFIG_MODULES_VTOL_ATT_CONTROL=y
+CONFIG_SYSTEMCMDS_BSONDUMP=y
 CONFIG_SYSTEMCMDS_DYN=y
 CONFIG_SYSTEMCMDS_LED_CONTROL=y
 CONFIG_SYSTEMCMDS_PARAM=y
@@ -81,8 +81,9 @@ CONFIG_MODULES_SIMULATION_SIMULATOR_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_BSONDUMP=y
 CONFIG_SYSTEMCMDS_DMESG=y
 CONFIG_SYSTEMCMDS_DUMPFILE=y
 CONFIG_SYSTEMCMDS_GPIO=y
@@ -27,14 +27,12 @@ CONFIG_COMMON_LIGHT=y
 CONFIG_DRIVERS_LIGHTS_RGBLED_PWM=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_OUT=y
 CONFIG_DRIVERS_RC_INPUT=y
 CONFIG_DRIVERS_ROBOCLAW=y
-CONFIG_DRIVERS_RPM=y
 CONFIG_DRIVERS_SAFETY_BUTTON=y
 CONFIG_DRIVERS_SMART_BATTERY_BATMON=y
 CONFIG_COMMON_TELEMETRY=y
@@ -46,6 +44,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
@@ -54,6 +53,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
@@ -69,7 +69,6 @@ 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
@@ -80,10 +79,10 @@ CONFIG_MODULES_SIMULATION_SENSOR_GPS_SIM=y
 CONFIG_MODULES_SIMULATION_SENSOR_MAG_SIM=y
 CONFIG_MODULES_SIMULATION_SIMULATOR_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_BSONDUMP=y
 CONFIG_SYSTEMCMDS_DMESG=y
 CONFIG_SYSTEMCMDS_DUMPFILE=y
 CONFIG_SYSTEMCMDS_GPIO=y
@@ -106,4 +105,3 @@ CONFIG_SYSTEMCMDS_USB_CONNECTED=y
 CONFIG_SYSTEMCMDS_VER=y
 CONFIG_SYSTEMCMDS_WORK_QUEUE=y
 CONFIG_EXAMPLES_FAKE_GPS=y
-#CONFIG_DRIVERS_TRANSPONDER_SAGETECH_MXS=y
@@ -75,6 +75,7 @@ CONFIG_MODULES_TEMPERATURE_COMPENSATION=y
 CONFIG_MODULES_VTOL_ATT_CONTROL=y
 CONFIG_SYSTEMCMDS_ACTUATOR_TEST=y
 CONFIG_SYSTEMCMDS_BL_UPDATE=y
+CONFIG_SYSTEMCMDS_BSONDUMP=y
 CONFIG_SYSTEMCMDS_DMESG=y
 CONFIG_SYSTEMCMDS_DUMPFILE=y
 CONFIG_SYSTEMCMDS_GPIO=y
@@ -1,12 +1,8 @@
 CONFIG_DRIVERS_ADC_ADS1115=n
 CONFIG_DRIVERS_IMU_ANALOG_DEVICES_ADIS16448=n
 CONFIG_DRIVERS_IRLOCK=n
-CONFIG_DRIVERS_PCA9685=n
 CONFIG_DRIVERS_PCA9685_PWM_OUT=n
-CONFIG_DRIVERS_RPM=n
 CONFIG_DRIVERS_UAVCAN=n
-CONFIG_MODULES_ATTITUDE_ESTIMATOR_Q=n
-CONFIG_MODULES_LOCAL_POSITION_ESTIMATOR=n
 CONFIG_MODULES_ROVER_POS_CONTROL=n
 CONFIG_MODULES_TEMPERATURE_COMPENSATION=n
 CONFIG_BOARD_TESTING=y
@@ -80,8 +80,9 @@ CONFIG_MODULES_UUV_ATT_CONTROL=y
 CONFIG_MODULES_UUV_POS_CONTROL=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_BSONDUMP=y
 CONFIG_SYSTEMCMDS_DMESG=y
 CONFIG_SYSTEMCMDS_DUMPFILE=y
 CONFIG_SYSTEMCMDS_GPIO=y
@@ -1,5 +1,5 @@
 CONFIG_PLATFORM_POSIX=y
-CONFIG_BOARD_LINUX=y
+CONFIG_BOARD_LINUX_TARGET=y
 CONFIG_BOARD_TOOLCHAIN="arm-linux-gnueabihf"
 CONFIG_BOARD_ARCHITECTURE="cortex-a53"
 CONFIG_BOARD_TESTING=y
@@ -66,6 +66,7 @@ CONFIG_MODULES_TEMPERATURE_COMPENSATION=y
 CONFIG_MODULES_UUV_ATT_CONTROL=y
 CONFIG_MODULES_UUV_POS_CONTROL=y
 CONFIG_MODULES_VTOL_ATT_CONTROL=y
+CONFIG_SYSTEMCMDS_BSONDUMP=y
 CONFIG_SYSTEMCMDS_DYN=y
 CONFIG_SYSTEMCMDS_LED_CONTROL=y
 CONFIG_SYSTEMCMDS_PARAM=y
@@ -28,10 +28,8 @@ CONFIG_DRIVERS_PCA9685=y
 CONFIG_DRIVERS_PCA9685_PWM_OUT=y
 CONFIG_DRIVERS_POWER_MONITOR_INA226=y
 CONFIG_DRIVERS_PWM_OUT=y
-CONFIG_MODULES_SIMULATION_PWM_OUT_SIM=y
 CONFIG_DRIVERS_PX4IO=y
 CONFIG_DRIVERS_ROBOCLAW=y
-CONFIG_DRIVERS_RPM=y
 CONFIG_DRIVERS_SMART_BATTERY_BATMON=y
 CONFIG_COMMON_TELEMETRY=y
 CONFIG_DRIVERS_TONE_ALARM=y
@@ -40,6 +38,7 @@ CONFIG_MODULES_AIRSPEED_SELECTOR=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
@@ -48,6 +47,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
@@ -62,7 +62,6 @@ 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
@@ -73,10 +72,10 @@ CONFIG_MODULES_SIMULATION_SENSOR_GPS_SIM=y
 CONFIG_MODULES_SIMULATION_SENSOR_MAG_SIM=y
 CONFIG_MODULES_SIMULATION_SIMULATOR_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_BSONDUMP=y
 CONFIG_SYSTEMCMDS_DMESG=y
 CONFIG_SYSTEMCMDS_DUMPFILE=y
 CONFIG_SYSTEMCMDS_GPIO=y
@@ -71,6 +71,7 @@ CONFIG_MODULES_TEMPERATURE_COMPENSATION=y
 CONFIG_MODULES_VTOL_ATT_CONTROL=y
 CONFIG_SYSTEMCMDS_ACTUATOR_TEST=y
 CONFIG_SYSTEMCMDS_BL_UPDATE=y
+CONFIG_SYSTEMCMDS_BSONDUMP=y
 CONFIG_SYSTEMCMDS_DMESG=y
 CONFIG_SYSTEMCMDS_DUMPFILE=y
 CONFIG_SYSTEMCMDS_GPIO=y
@@ -85,8 +85,9 @@ CONFIG_MODULES_UUV_ATT_CONTROL=y
 CONFIG_MODULES_UUV_POS_CONTROL=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_BSONDUMP=y
 CONFIG_SYSTEMCMDS_DMESG=y
 CONFIG_SYSTEMCMDS_DUMPFILE=y
 CONFIG_SYSTEMCMDS_GPIO=y
@@ -82,6 +82,7 @@ CONFIG_MODULES_UUV_POS_CONTROL=y
 CONFIG_MODULES_VTOL_ATT_CONTROL=y
 CONFIG_SYSTEMCMDS_ACTUATOR_TEST=y
 CONFIG_SYSTEMCMDS_BL_UPDATE=y
+CONFIG_SYSTEMCMDS_BSONDUMP=y
 CONFIG_SYSTEMCMDS_DMESG=y
 CONFIG_SYSTEMCMDS_DUMPFILE=y
 CONFIG_SYSTEMCMDS_GPIO=y
@@ -76,6 +76,7 @@ CONFIG_MODULES_TEMPERATURE_COMPENSATION=y
 CONFIG_MODULES_VTOL_ATT_CONTROL=y
 CONFIG_SYSTEMCMDS_ACTUATOR_TEST=y
 CONFIG_SYSTEMCMDS_BL_UPDATE=y
+CONFIG_SYSTEMCMDS_BSONDUMP=y
 CONFIG_SYSTEMCMDS_DMESG=y
 CONFIG_SYSTEMCMDS_GPIO=y
 CONFIG_SYSTEMCMDS_HARDFAULT_LOG=y
@@ -34,4 +34,8 @@
 add_library(drivers_board
 	board_config.h
 	init.c
+	spi.cpp
    )
+
+# Add custom drivers for SLPI
+add_subdirectory(${PX4_BOARD_DIR}/src/drivers/icm42688p)
@@ -46,5 +46,14 @@
 */
 #define PX4_NUMBER_I2C_BUSES    3

+/*
+ * SPI buses
+ */
+#define CONFIG_SPI 1
+#define BOARD_SPI_BUS_MAX_BUS_ITEMS 1
+
+/*
+ * Include these last to make use of the definitions above
+ */
 #include <system_config.h>
 #include <px4_platform_common/board_common.h>
@@ -1,6 +1,6 @@
 ############################################################################
 #
-#   Copyright (c) 2019 PX4 Development Team. All rights reserved.
+#   Copyright (c) 2020 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
@@ -30,20 +30,19 @@
 # POSSIBILITY OF SUCH DAMAGE.
 #
 ############################################################################
-
-add_subdirectory(AngularVelocityControl)
-
 px4_add_module(
-	MODULE modules__angular_velocity_control
-	MAIN angular_velocity_controller
+	MODULE drivers__imu__invensense__icm42688p
+	MAIN icm42688p
 	COMPILE_FLAGS
 		${MAX_CUSTOM_OPT_LEVEL}
 	SRCS
-		AngularVelocityController.cpp
-		AngularVelocityController.hpp
+		icm42688p_main.cpp
+		ICM42688P.cpp
+		ICM42688P.hpp
+		InvenSense_ICM42688P_registers.hpp
 	DEPENDS
-		circuit_breaker
-		mathlib
-		AngularVelocityControl
 		px4_work_queue
+		drivers_accelerometer
+		drivers_gyroscope
+		drivers__device
 	)
@@ -0,0 +1,985 @@
+/****************************************************************************
+ *
+ *   Copyright (c) 2020 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.
+ *
+ ****************************************************************************/
+
+#include "ICM42688P.hpp"
+
+bool hitl_mode = false;
+
+using namespace time_literals;
+
+static constexpr int16_t combine(uint8_t msb, uint8_t lsb)
+{
+	return (msb << 8u) | lsb;
+}
+
+ICM42688P::ICM42688P(const I2CSPIDriverConfig &config) :
+	// SPI(DRV_IMU_DEVTYPE_ICM42688P, MODULE_NAME, bus, device, spi_mode, bus_frequency),
+	// I2CSPIDriver(MODULE_NAME, px4::device_bus_to_wq(get_device_id()), bus_option, bus),
+	// _drdy_gpio(drdy_gpio)
+	SPI(config),
+	I2CSPIDriver(config),
+	_drdy_gpio(config.drdy_gpio),
+	_px4_accel(get_device_id(), config.rotation),
+	_px4_gyro(get_device_id(), config.rotation)
+{
+	if (config.drdy_gpio != 0) {
+		_drdy_missed_perf = perf_alloc(PC_COUNT, MODULE_NAME": DRDY missed");
+	}
+
+	if (!hitl_mode) {
+		// _px4_accel = std::make_shared<PX4Accelerometer>(get_device_id(), rotation);
+		// _px4_gyro = std::make_shared<PX4Gyroscope>(get_device_id(), rotation);
+		ConfigureSampleRate(_px4_gyro.get_max_rate_hz());
+		// _imu_server_pub.advertise();
+
+	} else {
+		ConfigureSampleRate(0);
+	}
+}
+
+ICM42688P::~ICM42688P()
+{
+	perf_free(_bad_register_perf);
+	perf_free(_bad_transfer_perf);
+	perf_free(_fifo_empty_perf);
+	perf_free(_fifo_overflow_perf);
+	perf_free(_fifo_reset_perf);
+	perf_free(_drdy_missed_perf);
+
+	// if (!hitl_mode){
+	//  		_imu_server_pub.unadvertise();
+	// }
+}
+
+int ICM42688P::init()
+{
+	int ret = SPI::init();
+
+	if (ret != PX4_OK) {
+		DEVICE_DEBUG("SPI::init failed (%i)", ret);
+		return ret;
+	}
+
+	return Reset() ? 0 : -1;
+}
+
+bool ICM42688P::Reset()
+{
+	_state = STATE::RESET;
+	DataReadyInterruptDisable();
+	ScheduleClear();
+	ScheduleNow();
+	return true;
+}
+
+void ICM42688P::exit_and_cleanup()
+{
+	DataReadyInterruptDisable();
+	I2CSPIDriverBase::exit_and_cleanup();
+}
+
+void ICM42688P::print_status()
+{
+	I2CSPIDriverBase::print_status();
+
+	PX4_INFO("FIFO empty interval: %d us (%.1f Hz)", _fifo_empty_interval_us, 1e6 / _fifo_empty_interval_us);
+
+	perf_print_counter(_bad_register_perf);
+	perf_print_counter(_bad_transfer_perf);
+	perf_print_counter(_fifo_empty_perf);
+	perf_print_counter(_fifo_overflow_perf);
+	perf_print_counter(_fifo_reset_perf);
+	perf_print_counter(_drdy_missed_perf);
+}
+
+int ICM42688P::probe()
+{
+	for (int i = 0; i < 3; i++) {
+		uint8_t whoami = RegisterRead(Register::BANK_0::WHO_AM_I);
+
+		if (whoami == WHOAMI) {
+			PX4_INFO("ICM42688P::probe successful!");
+			return PX4_OK;
+
+		} else {
+			DEVICE_DEBUG("unexpected WHO_AM_I 0x%02x", whoami);
+
+			uint8_t reg_bank_sel = RegisterRead(Register::BANK_0::REG_BANK_SEL);
+			int bank = reg_bank_sel >> 4;
+
+			if (bank >= 1 && bank <= 3) {
+				DEVICE_DEBUG("incorrect register bank for WHO_AM_I REG_BANK_SEL:0x%02x, bank:%d", reg_bank_sel, bank);
+				// force bank selection and retry
+				SelectRegisterBank(REG_BANK_SEL_BIT::USER_BANK_0, true);
+			}
+		}
+	}
+
+	return PX4_ERROR;
+}
+
+void ICM42688P::RunImpl()
+{
+	PX4_INFO(">>> ICM42688P this: %p", this);
+
+	const hrt_abstime now = hrt_absolute_time();
+
+	switch (_state) {
+	case STATE::RESET:
+		// DEVICE_CONFIG: Software reset configuration
+		RegisterWrite(Register::BANK_0::DEVICE_CONFIG, DEVICE_CONFIG_BIT::SOFT_RESET_CONFIG);
+		_reset_timestamp = now;
+		_failure_count = 0;
+		_state = STATE::WAIT_FOR_RESET;
+		ScheduleDelayed(2_ms); // to be safe wait 2 ms for soft reset to be effective
+		break;
+
+	case STATE::WAIT_FOR_RESET:
+		if ((RegisterRead(Register::BANK_0::WHO_AM_I) == WHOAMI)
+		    && (RegisterRead(Register::BANK_0::DEVICE_CONFIG) == 0x00)
+		    && (RegisterRead(Register::BANK_0::INT_STATUS) & INT_STATUS_BIT::RESET_DONE_INT)) {
+
+			_state = STATE::CONFIGURE;
+			ScheduleDelayed(10_ms); // 30 ms gyro startup time, 10 ms accel from sleep to valid data
+
+		} else {
+			// RESET not complete
+			if (hrt_elapsed_time(&_reset_timestamp) > 1000_ms) {
+				PX4_DEBUG("Reset failed, retrying");
+				_state = STATE::RESET;
+				ScheduleDelayed(100_ms);
+
+			} else {
+				PX4_DEBUG("Reset not complete, check again in 10 ms");
+				ScheduleDelayed(10_ms);
+			}
+		}
+
+		break;
+
+	case STATE::CONFIGURE:
+
+		if (Configure()) {
+
+			// Wakeup accel and gyro after configuring registers
+			ScheduleDelayed(1_ms); // add a delay here to be safe
+			RegisterWrite(Register::BANK_0::PWR_MGMT0, PWR_MGMT0_BIT::GYRO_MODE_LOW_NOISE | PWR_MGMT0_BIT::ACCEL_MODE_LOW_NOISE);
+			ScheduleDelayed(30_ms); // 30 ms gyro startup time, 10 ms accel from sleep to valid data
+
+			// if configure succeeded then start reading from FIFO
+			_state = STATE::FIFO_READ;
+
+			if (DataReadyInterruptConfigure()) {
+				_data_ready_interrupt_enabled = true;
+
+				// backup schedule as a watchdog timeout
+				ScheduleDelayed(100_ms);
+
+			} else {
+				PX4_ERR("ICM42688P::RunImpl interrupt configuration failed");
+
+				_data_ready_interrupt_enabled = false;
+				ScheduleOnInterval(_fifo_empty_interval_us, _fifo_empty_interval_us);
+			}
+
+			FIFOReset();
+
+		} else {
+
+			PX4_ERR("ICM42688P::RunImpl configuration failed");
+
+			// CONFIGURE not complete
+			if (hrt_elapsed_time(&_reset_timestamp) > 1000_ms) {
+				PX4_DEBUG("Configure failed, resetting");
+				_state = STATE::RESET;
+
+			} else {
+				PX4_DEBUG("Configure failed, retrying");
+			}
+
+			ScheduleDelayed(100_ms);
+		}
+
+		break;
+
+	case STATE::FIFO_READ: {
+#ifndef __PX4_QURT
+			uint32_t samples = 0;
+
+			if (_data_ready_interrupt_enabled) {
+				// scheduled from interrupt if _drdy_fifo_read_samples was set as expected
+				if (_drdy_fifo_read_samples.fetch_and(0) != _fifo_gyro_samples) {
+					perf_count(_drdy_missed_perf);
+
+				} else {
+					samples = _fifo_gyro_samples;
+				}
+
+				// push backup schedule back
+				ScheduleDelayed(_fifo_empty_interval_us * 2);
+			}
+
+			if (samples == 0) {
+				// check current FIFO count
+				const uint16_t fifo_count = FIFOReadCount();
+
+				if (fifo_count >= FIFO::SIZE) {
+					FIFOReset();
+					perf_count(_fifo_overflow_perf);
+
+				} else if (fifo_count == 0) {
+					perf_count(_fifo_empty_perf);
+
+				} else {
+					// FIFO count (size in bytes)
+					samples = (fifo_count / sizeof(FIFO::DATA));
+
+					if (samples > FIFO_MAX_SAMPLES) {
+						// not technically an overflow, but more samples than we expected or can publish
+						FIFOReset();
+						perf_count(_fifo_overflow_perf);
+						samples = 0;
+					}
+				}
+			}
+
+			bool success = false;
+
+			if (samples >= 1) {
+				if (FIFORead(now, samples)) {
+					success = true;
+
+					if (_failure_count > 0) {
+						_failure_count--;
+					}
+				}
+			}
+
+			if (!success) {
+				_failure_count++;
+
+				// full reset if things are failing consistently
+				if (_failure_count > 10) {
+					Reset();
+					return;
+				}
+			}
+
+			// check configuration registers periodically or immediately following any failure
+			if (RegisterCheck(_register_bank0_cfg[_checked_register_bank0])
+			    && RegisterCheck(_register_bank1_cfg[_checked_register_bank1])
+			    && RegisterCheck(_register_bank2_cfg[_checked_register_bank2])
+			   ) {
+				_last_config_check_timestamp = now;
+				_checked_register_bank0 = (_checked_register_bank0 + 1) % size_register_bank0_cfg;
+				_checked_register_bank1 = (_checked_register_bank1 + 1) % size_register_bank1_cfg;
+				_checked_register_bank2 = (_checked_register_bank2 + 1) % size_register_bank2_cfg;
+
+			} else {
+				// register check failed, force reset
+				perf_count(_bad_register_perf);
+				Reset();
+			}
+
+#endif
+		}
+
+		break;
+	}
+}
+
+void ICM42688P::ConfigureSampleRate(int sample_rate)
+{
+	if (sample_rate == 0) {
+		sample_rate = 800; // default to 800 Hz
+	}
+
+	// round down to nearest FIFO sample dt
+	const float min_interval = FIFO_SAMPLE_DT;
+	_fifo_empty_interval_us = math::max(roundf((1e6f / (float)sample_rate) / min_interval) * min_interval, min_interval);
+
+	_fifo_gyro_samples = roundf(math::min((float)_fifo_empty_interval_us / (1e6f / GYRO_RATE), (float)FIFO_MAX_SAMPLES));
+
+	// recompute FIFO empty interval (us) with actual gyro sample limit
+	_fifo_empty_interval_us = _fifo_gyro_samples * (1e6f / GYRO_RATE);
+
+	ConfigureFIFOWatermark(_fifo_gyro_samples);
+}
+
+void ICM42688P::ConfigureFIFOWatermark(uint8_t samples)
+{
+	// FIFO watermark threshold in number of bytes
+	const uint16_t fifo_watermark_threshold = samples * sizeof(FIFO::DATA);
+
+	for (auto &r : _register_bank0_cfg) {
+		if (r.reg == Register::BANK_0::FIFO_CONFIG2) {
+			// FIFO_WM[7:0]  FIFO_CONFIG2
+			r.set_bits = fifo_watermark_threshold & 0xFF;
+
+		} else if (r.reg == Register::BANK_0::FIFO_CONFIG3) {
+			// FIFO_WM[11:8] FIFO_CONFIG3
+			r.set_bits = (fifo_watermark_threshold >> 8) & 0x0F;
+		}
+	}
+}
+
+void ICM42688P::SelectRegisterBank(enum REG_BANK_SEL_BIT bank, bool force)
+{
+	if (bank != _last_register_bank || force) {
+		// select BANK_0
+		uint8_t cmd_bank_sel[2] {};
+		cmd_bank_sel[0] = static_cast<uint8_t>(Register::BANK_0::REG_BANK_SEL);
+		cmd_bank_sel[1] = bank;
+		transfer(cmd_bank_sel, cmd_bank_sel, sizeof(cmd_bank_sel));
+
+		_last_register_bank = bank;
+	}
+}
+
+bool ICM42688P::Configure()
+{
+	// first set and clear all configured register bits
+	for (const auto &reg_cfg : _register_bank0_cfg) {
+		RegisterSetAndClearBits(reg_cfg.reg, reg_cfg.set_bits, reg_cfg.clear_bits);
+	}
+
+	for (const auto &reg_cfg : _register_bank1_cfg) {
+		RegisterSetAndClearBits(reg_cfg.reg, reg_cfg.set_bits, reg_cfg.clear_bits);
+	}
+
+	for (const auto &reg_cfg : _register_bank2_cfg) {
+		RegisterSetAndClearBits(reg_cfg.reg, reg_cfg.set_bits, reg_cfg.clear_bits);
+	}
+
+	// now check that all are configured
+	bool success = true;
+
+	for (const auto &reg_cfg : _register_bank0_cfg) {
+		if (!RegisterCheck(reg_cfg)) {
+			success = false;
+		}
+	}
+
+	for (const auto &reg_cfg : _register_bank1_cfg) {
+		if (!RegisterCheck(reg_cfg)) {
+			success = false;
+		}
+	}
+
+	for (const auto &reg_cfg : _register_bank2_cfg) {
+		if (!RegisterCheck(reg_cfg)) {
+			success = false;
+		}
+	}
+
+	// // 20-bits data format used
+	// //  the only FSR settings that are operational are ±2000dps for gyroscope and ±16g for accelerometer
+	if (!hitl_mode) {
+		_px4_accel.set_range(16.f * CONSTANTS_ONE_G);
+		_px4_accel.set_scale(CONSTANTS_ONE_G / 8192.f);
+		_px4_gyro.set_range(math::radians(2000.f));
+		_px4_gyro.set_scale(math::radians(1.f / 131.f));
+	}
+
+	return success;
+}
+
+static bool interrupt_debug = true;
+static uint32_t interrupt_debug_count = 0;
+static const uint32_t interrupt_debug_trigger = 800;
+static hrt_abstime last_interrupt_time = 0;
+static hrt_abstime avg_interrupt_delta = 0;
+static hrt_abstime max_interrupt_delta = 0;
+static hrt_abstime min_interrupt_delta = 60 * 1000 * 1000;
+static hrt_abstime cumulative_interrupt_delta = 0;
+
+int ICM42688P::DataReadyInterruptCallback(int irq, void *context, void *arg)
+{
+	hrt_abstime current_interrupt_time = hrt_absolute_time();
+
+	if (interrupt_debug) {
+		if (last_interrupt_time) {
+			hrt_abstime interrupt_delta_time = current_interrupt_time - last_interrupt_time;
+
+			if (interrupt_delta_time > max_interrupt_delta) { max_interrupt_delta = interrupt_delta_time; }
+
+			if (interrupt_delta_time < min_interrupt_delta) { min_interrupt_delta = interrupt_delta_time; }
+
+			cumulative_interrupt_delta += interrupt_delta_time;
+		}
+
+		last_interrupt_time = current_interrupt_time;
+
+		interrupt_debug_count++;
+
+		if (interrupt_debug_count == interrupt_debug_trigger) {
+			avg_interrupt_delta = cumulative_interrupt_delta / interrupt_debug_trigger;
+			PX4_INFO(">>> Max: %llu, Min: %llu, Avg: %llu", max_interrupt_delta,
+				 min_interrupt_delta, avg_interrupt_delta);
+			interrupt_debug_count = 0;
+			cumulative_interrupt_delta = 0;
+		}
+	}
+
+	static_cast<ICM42688P *>(arg)->DataReady();
+
+	return 0;
+}
+
+void ICM42688P::DataReady()
+{
+#ifndef __PX4_QURT
+	uint32_t expected = 0;
+
+	if (_drdy_fifo_read_samples.compare_exchange(&expected, _fifo_gyro_samples)) {
+		ScheduleNow();
+	}
+
+#else
+	uint16_t fifo_byte_count = FIFOReadCount();
+
+	FIFORead(hrt_absolute_time(), fifo_byte_count / sizeof(FIFO::DATA));
+#endif
+}
+
+bool ICM42688P::DataReadyInterruptConfigure()
+{
+	if (_drdy_gpio == 0) {
+		return false;
+	}
+
+	// Setup data ready on falling edge
+	return px4_arch_gpiosetevent(_drdy_gpio, false, true, true, &DataReadyInterruptCallback, this) == 0;
+}
+
+bool ICM42688P::DataReadyInterruptDisable()
+{
+	if (_drdy_gpio == 0) {
+		return false;
+	}
+
+	return px4_arch_gpiosetevent(_drdy_gpio, false, false, false, nullptr, nullptr) == 0;
+}
+
+template <typename T>
+bool ICM42688P::RegisterCheck(const T &reg_cfg)
+{
+	bool success = true;
+
+	const uint8_t reg_value = RegisterRead(reg_cfg.reg);
+
+	if (reg_cfg.set_bits && ((reg_value & reg_cfg.set_bits) != reg_cfg.set_bits)) {
+		PX4_DEBUG("0x%02hhX: 0x%02hhX (0x%02hhX not set)", (uint8_t)reg_cfg.reg, reg_value, reg_cfg.set_bits);
+		success = false;
+	}
+
+	if (reg_cfg.clear_bits && ((reg_value & reg_cfg.clear_bits) != 0)) {
+		PX4_DEBUG("0x%02hhX: 0x%02hhX (0x%02hhX not cleared)", (uint8_t)reg_cfg.reg, reg_value, reg_cfg.clear_bits);
+		success = false;
+	}
+
+	return success;
+}
+
+template <typename T>
+uint8_t ICM42688P::RegisterRead(T reg)
+{
+	uint8_t cmd[2] {};
+	cmd[0] = static_cast<uint8_t>(reg) | DIR_READ;
+	SelectRegisterBank(reg);
+	transfer(cmd, cmd, sizeof(cmd));
+	return cmd[1];
+}
+
+template <typename T>
+void ICM42688P::RegisterWrite(T reg, uint8_t value)
+{
+	uint8_t cmd[2] { (uint8_t)reg, value };
+	SelectRegisterBank(reg);
+	transfer(cmd, cmd, sizeof(cmd));
+}
+
+template <typename T>
+void ICM42688P::RegisterSetAndClearBits(T reg, uint8_t setbits, uint8_t clearbits)
+{
+	const uint8_t orig_val = RegisterRead(reg);
+
+	uint8_t val = (orig_val & ~clearbits) | setbits;
+
+	if (orig_val != val) {
+		RegisterWrite(reg, val);
+	}
+}
+
+uint16_t ICM42688P::FIFOReadCount()
+{
+	// read FIFO count
+	uint8_t fifo_count_buf[3] {};
+	fifo_count_buf[0] = static_cast<uint8_t>(Register::BANK_0::FIFO_COUNTH) | DIR_READ;
+	SelectRegisterBank(REG_BANK_SEL_BIT::USER_BANK_0);
+
+	if (transfer(fifo_count_buf, fifo_count_buf, sizeof(fifo_count_buf)) != PX4_OK) {
+		perf_count(_bad_transfer_perf);
+		return 0;
+	}
+
+	return combine(fifo_count_buf[1], fifo_count_buf[2]);
+}
+
+// static uint32_t debug_decimator = 0;
+// static hrt_abstime last_sample_time = 0;
+// static bool imu_debug = true;
+
+bool ICM42688P::FIFORead(const hrt_abstime &timestamp_sample, uint16_t samples)
+{
+	FIFOTransferBuffer buffer{};
+	const size_t max_transfer_size = 10 * sizeof(FIFO::DATA) + 4;
+	const size_t transfer_size = math::min(samples * sizeof(FIFO::DATA) + 4, max_transfer_size);
+	SelectRegisterBank(REG_BANK_SEL_BIT::USER_BANK_0);
+
+	if (transfer((uint8_t *)&buffer, (uint8_t *)&buffer, transfer_size) != PX4_OK) {
+		perf_count(_bad_transfer_perf);
+		return false;
+	}
+
+	if (buffer.INT_STATUS & INT_STATUS_BIT::FIFO_FULL_INT) {
+		perf_count(_fifo_overflow_perf);
+		FIFOReset();
+		return false;
+	}
+
+	const uint16_t fifo_count_bytes = combine(buffer.FIFO_COUNTH, buffer.FIFO_COUNTL);
+
+	if (fifo_count_bytes >= FIFO::SIZE) {
+		perf_count(_fifo_overflow_perf);
+		FIFOReset();
+		return false;
+	}
+
+	const uint16_t fifo_count_samples = fifo_count_bytes / sizeof(FIFO::DATA);
+
+	if (fifo_count_samples == 0) {
+		perf_count(_fifo_empty_perf);
+		return false;
+	}
+
+	// check FIFO header in every sample
+	uint16_t valid_samples = 0;
+
+	// for (int i = 0; i < math::min(samples, fifo_count_samples); i++) {
+	for (int i = 0; i < math::min(samples, (uint16_t) 10); i++) {
+		bool valid = true;
+
+		// With FIFO_ACCEL_EN and FIFO_GYRO_EN header should be 8’b_0110_10xx
+		const uint8_t FIFO_HEADER = buffer.f[i].FIFO_Header;
+
+		if (FIFO_HEADER & FIFO::FIFO_HEADER_BIT::HEADER_MSG) {
+			// FIFO sample empty if HEADER_MSG set
+			valid = false;
+
+		} else if (!(FIFO_HEADER & FIFO::FIFO_HEADER_BIT::HEADER_ACCEL)) {
+			// accel bit not set
+			valid = false;
+
+		} else if (!(FIFO_HEADER & FIFO::FIFO_HEADER_BIT::HEADER_GYRO)) {
+			// gyro bit not set
+			valid = false;
+
+		} else if (!(FIFO_HEADER & FIFO::FIFO_HEADER_BIT::HEADER_20)) {
+			// Packet does not contain a new and valid extended 20-bit data
+			valid = false;
+
+		} else if (FIFO_HEADER & FIFO::FIFO_HEADER_BIT::HEADER_ODR_ACCEL) {
+			// accel ODR changed
+			valid = false;
+
+		} else if (FIFO_HEADER & FIFO::FIFO_HEADER_BIT::HEADER_ODR_GYRO) {
+			// gyro ODR changed
+			valid = false;
+		}
+
+		if (valid) {
+			valid_samples++;
+
+		} else {
+			perf_count(_bad_transfer_perf);
+			break;
+		}
+	}
+
+	// if (imu_debug) {
+	// 	debug_decimator++;
+	// 	if (debug_decimator == 801) {
+	// 		debug_decimator = 0;
+	// 		PX4_INFO("Initial: %u Next: %u Valid: %u Delta: %llu", samples, fifo_count_samples, valid_samples, timestamp_sample - last_sample_time);
+	// 	}
+	// 	last_sample_time = timestamp_sample;
+	// }
+
+	if (valid_samples > 0) {
+		if (ProcessTemperature(buffer.f, valid_samples)) {
+			ProcessGyro(timestamp_sample, buffer.f, valid_samples);
+			ProcessAccel(timestamp_sample, buffer.f, valid_samples);
+			// Pass only most recent valid sample to IMU server
+			// ProcessIMU(timestamp_sample, buffer.f[valid_samples - 1]);
+			return true;
+		}
+	}
+
+	return false;
+}
+
+void ICM42688P::FIFOReset()
+{
+	perf_count(_fifo_reset_perf);
+
+	// SIGNAL_PATH_RESET: FIFO flush
+	RegisterSetBits(Register::BANK_0::SIGNAL_PATH_RESET, SIGNAL_PATH_RESET_BIT::FIFO_FLUSH);
+
+	// reset while FIFO is disabled
+	_drdy_fifo_read_samples.store(0);
+}
+
+static constexpr int32_t reassemble_20bit(const uint32_t a, const uint32_t b, const uint32_t c)
+{
+	// 0xXXXAABBC
+	uint32_t high   = ((a << 12) & 0x000FF000);
+	uint32_t low    = ((b << 4)  & 0x00000FF0);
+	uint32_t lowest = (c         & 0x0000000F);
+
+	uint32_t x = high | low | lowest;
+
+	if (a & Bit7) {
+		// sign extend
+		x |= 0xFFF00000u;
+	}
+
+	return static_cast<int32_t>(x);
+}
+
+void ICM42688P::ProcessIMU(const hrt_abstime &timestamp_sample, const FIFO::DATA &fifo)
+{
+	// float accel_x = 0.0, accel_y = 0.0, accel_z = 0.0;
+	// float gyro_x = 0.0,  gyro_y = 0.0,  gyro_z = 0.0;
+	//
+	// // 20 bit hires mode
+	//
+	// // Sign extension + Accel [19:12] + Accel [11:4] + Accel [3:2] (20 bit extension byte)
+	// // Accel data is 18 bit
+	// int32_t temp_accel_x = reassemble_20bit(fifo.ACCEL_DATA_X1, fifo.ACCEL_DATA_X0,
+	// 				   fifo.Ext_Accel_X_Gyro_X & 0xF0 >> 4);
+	// int32_t temp_accel_y = reassemble_20bit(fifo.ACCEL_DATA_Y1, fifo.ACCEL_DATA_Y0,
+	// 				   fifo.Ext_Accel_Y_Gyro_Y & 0xF0 >> 4);
+	// int32_t temp_accel_z = reassemble_20bit(fifo.ACCEL_DATA_Z1, fifo.ACCEL_DATA_Z0,
+	// 				   fifo.Ext_Accel_Z_Gyro_Z & 0xF0 >> 4);
+	//
+	// // Gyro [19:12] + Gyro [11:4] + Gyro [3:0] (bottom 4 bits of 20 bit extension byte)
+	// int32_t temp_gyro_x = reassemble_20bit(fifo.GYRO_DATA_X1, fifo.GYRO_DATA_X0,
+	//                    fifo.Ext_Accel_X_Gyro_X & 0x0F);
+	// int32_t temp_gyro_y = reassemble_20bit(fifo.GYRO_DATA_Y1, fifo.GYRO_DATA_Y0,
+	//                    fifo.Ext_Accel_Y_Gyro_Y & 0x0F);
+	// int32_t temp_gyro_z = reassemble_20bit(fifo.GYRO_DATA_Z1, fifo.GYRO_DATA_Z0,
+	//                    fifo.Ext_Accel_Z_Gyro_Z & 0x0F);
+
+	// // accel samples invalid if -524288
+	// if (temp_accel_x != -524288 && temp_accel_y != -524288 && temp_accel_z != -524288) {
+	// 	// shift accel by 2 (2 least significant bits are always 0)
+	// 	accel_x = (float) temp_accel_x / 4.f;
+	// 	accel_y = (float) temp_accel_y / 4.f;
+	// 	accel_z = (float) temp_accel_z / 4.f;
+	//
+	// 	// shift gyro by 1 (least significant bit is always 0)
+	// 	gyro_x = (float) temp_gyro_x / 2.f;
+	// 	gyro_y = (float) temp_gyro_y / 2.f;
+	// 	gyro_z = (float) temp_gyro_z / 2.f;
+	//
+	// 	// correct frame for publication
+	// 	// sensor's frame is +x forward, +y left, +z up
+	// 	// flip y & z to publish right handed with z down (x forward, y right, z down)
+	// 	accel_y = -accel_y;
+	// 	accel_z = -accel_z;
+	// 	gyro_y  = -gyro_y;
+	// 	gyro_z  = -gyro_z;
+	//
+	// 	// Scale everything appropriately
+	// 	float accel_scale_factor = (CONSTANTS_ONE_G / 8192.f);
+	// 	accel_x *= accel_scale_factor;
+	// 	accel_y *= accel_scale_factor;
+	// 	accel_z *= accel_scale_factor;
+	//
+	// 	float gyro_scale_factor = math::radians(1.f / 131.f);
+	// 	gyro_x *= gyro_scale_factor;
+	// 	gyro_y *= gyro_scale_factor;
+	// 	gyro_z *= gyro_scale_factor;
+	//
+	// 	// Store the data in our array
+	// 	_imu_server_data.accel_x[_imu_server_index] = accel_x;
+	// 	_imu_server_data.accel_y[_imu_server_index] = accel_y;
+	// 	_imu_server_data.accel_z[_imu_server_index] = accel_z;
+	// 	_imu_server_data.gyro_x[_imu_server_index]  = gyro_x;
+	// 	_imu_server_data.gyro_y[_imu_server_index]  = gyro_y;
+	// 	_imu_server_data.gyro_z[_imu_server_index]  = gyro_z;
+	// 	_imu_server_data.ts[_imu_server_index]      = timestamp_sample;
+	// 	_imu_server_index++;
+	//
+	// 	// If array is full, publish the data
+	// 	if (_imu_server_index == 10) {
+	// 		_imu_server_index = 0;
+	// 		_imu_server_data.timestamp = hrt_absolute_time();
+	// 		_imu_server_data.temperature = 0; // Not used right now
+	// 		_imu_server_pub.publish(_imu_server_data);
+	// 	}
+	// }
+}
+
+void ICM42688P::ProcessAccel(const hrt_abstime &timestamp_sample, const FIFO::DATA fifo[], const uint8_t samples)
+{
+	sensor_accel_fifo_s accel{};
+	accel.timestamp_sample = timestamp_sample;
+	accel.samples = 0;
+	accel.dt = FIFO_SAMPLE_DT;
+
+	// 18-bits of accelerometer data
+	bool scale_20bit = false;
+
+	// first pass
+	for (int i = 0; i < samples; i++) {
+		// 20 bit hires mode
+		// Sign extension + Accel [19:12] + Accel [11:4] + Accel [3:2] (20 bit extension byte)
+		// Accel data is 18 bit ()
+		int32_t accel_x = reassemble_20bit(fifo[i].ACCEL_DATA_X1, fifo[i].ACCEL_DATA_X0,
+						   fifo[i].Ext_Accel_X_Gyro_X & 0xF0 >> 4);
+		int32_t accel_y = reassemble_20bit(fifo[i].ACCEL_DATA_Y1, fifo[i].ACCEL_DATA_Y0,
+						   fifo[i].Ext_Accel_Y_Gyro_Y & 0xF0 >> 4);
+		int32_t accel_z = reassemble_20bit(fifo[i].ACCEL_DATA_Z1, fifo[i].ACCEL_DATA_Z0,
+						   fifo[i].Ext_Accel_Z_Gyro_Z & 0xF0 >> 4);
+
+		// sample invalid if -524288
+		if (accel_x != -524288 && accel_y != -524288 && accel_z != -524288) {
+			// check if any values are going to exceed int16 limits
+			static constexpr int16_t max_accel = INT16_MAX;
+			static constexpr int16_t min_accel = INT16_MIN;
+
+			if (accel_x >= max_accel || accel_x <= min_accel) {
+				scale_20bit = true;
+			}
+
+			if (accel_y >= max_accel || accel_y <= min_accel) {
+				scale_20bit = true;
+			}
+
+			if (accel_z >= max_accel || accel_z <= min_accel) {
+				scale_20bit = true;
+			}
+
+			// shift by 2 (2 least significant bits are always 0)
+			accel.x[accel.samples] = accel_x / 4;
+			accel.y[accel.samples] = accel_y / 4;
+			accel.z[accel.samples] = accel_z / 4;
+			accel.samples++;
+		}
+	}
+
+	if (!scale_20bit) {
+		// if highres enabled accel data is always 8192 LSB/g
+		if (!hitl_mode) {
+			_px4_accel.set_scale(CONSTANTS_ONE_G / 8192.f);
+		}
+
+	} else {
+		// 20 bit data scaled to 16 bit (2^4)
+		for (int i = 0; i < samples; i++) {
+			// 20 bit hires mode
+			// Sign extension + Accel [19:12] + Accel [11:4] + Accel [3:2] (20 bit extension byte)
+			// Accel data is 18 bit ()
+			int16_t accel_x = combine(fifo[i].ACCEL_DATA_X1, fifo[i].ACCEL_DATA_X0);
+			int16_t accel_y = combine(fifo[i].ACCEL_DATA_Y1, fifo[i].ACCEL_DATA_Y0);
+			int16_t accel_z = combine(fifo[i].ACCEL_DATA_Z1, fifo[i].ACCEL_DATA_Z0);
+
+			accel.x[i] = accel_x;
+			accel.y[i] = accel_y;
+			accel.z[i] = accel_z;
+		}
+
+		if (!hitl_mode) {
+			_px4_accel.set_scale(CONSTANTS_ONE_G / 2048.f);
+		}
+	}
+
+	// correct frame for publication
+	for (int i = 0; i < accel.samples; i++) {
+		// sensor's frame is +x forward, +y left, +z up
+		//  flip y & z to publish right handed with z down (x forward, y right, z down)
+		accel.x[i] = accel.x[i];
+		accel.y[i] = (accel.y[i] == INT16_MIN) ? INT16_MAX : -accel.y[i];
+		accel.z[i] = (accel.z[i] == INT16_MIN) ? INT16_MAX : -accel.z[i];
+	}
+
+	if (!hitl_mode) {
+		_px4_accel.set_error_count(perf_event_count(_bad_register_perf) + perf_event_count(_bad_transfer_perf) +
+					   perf_event_count(_fifo_empty_perf) + perf_event_count(_fifo_overflow_perf));
+	}
+
+	if (accel.samples > 0) {
+		if (!hitl_mode) {
+			_px4_accel.updateFIFO(accel);
+		}
+	}
+}
+
+void ICM42688P::ProcessGyro(const hrt_abstime &timestamp_sample, const FIFO::DATA fifo[], const uint8_t samples)
+{
+	sensor_gyro_fifo_s gyro{};
+	gyro.timestamp_sample = timestamp_sample;
+	gyro.samples = 0;
+	gyro.dt = FIFO_SAMPLE_DT;
+
+	// 20-bits of gyroscope data
+	bool scale_20bit = false;
+
+	// first pass
+	for (int i = 0; i < samples; i++) {
+		// 20 bit hires mode
+		// Gyro [19:12] + Gyro [11:4] + Gyro [3:0] (bottom 4 bits of 20 bit extension byte)
+		int32_t gyro_x = reassemble_20bit(fifo[i].GYRO_DATA_X1, fifo[i].GYRO_DATA_X0, fifo[i].Ext_Accel_X_Gyro_X & 0x0F);
+		int32_t gyro_y = reassemble_20bit(fifo[i].GYRO_DATA_Y1, fifo[i].GYRO_DATA_Y0, fifo[i].Ext_Accel_Y_Gyro_Y & 0x0F);
+		int32_t gyro_z = reassemble_20bit(fifo[i].GYRO_DATA_Z1, fifo[i].GYRO_DATA_Z0, fifo[i].Ext_Accel_Z_Gyro_Z & 0x0F);
+
+		// check if any values are going to exceed int16 limits
+		static constexpr int16_t max_gyro = INT16_MAX;
+		static constexpr int16_t min_gyro = INT16_MIN;
+
+		if (gyro_x >= max_gyro || gyro_x <= min_gyro) {
+			scale_20bit = true;
+		}
+
+		if (gyro_y >= max_gyro || gyro_y <= min_gyro) {
+			scale_20bit = true;
+		}
+
+		if (gyro_z >= max_gyro || gyro_z <= min_gyro) {
+			scale_20bit = true;
+		}
+
+		gyro.x[gyro.samples] = gyro_x / 2;
+		gyro.y[gyro.samples] = gyro_y / 2;
+		gyro.z[gyro.samples] = gyro_z / 2;
+		gyro.samples++;
+	}
+
+	if (!scale_20bit) {
+		// if highres enabled gyro data is always 131 LSB/dps
+		if (!hitl_mode) {
+			_px4_gyro.set_scale(math::radians(1.f / 131.f));
+		}
+
+	} else {
+		// 20 bit data scaled to 16 bit (2^4)
+		for (int i = 0; i < samples; i++) {
+			gyro.x[i] = combine(fifo[i].GYRO_DATA_X1, fifo[i].GYRO_DATA_X0);
+			gyro.y[i] = combine(fifo[i].GYRO_DATA_Y1, fifo[i].GYRO_DATA_Y0);
+			gyro.z[i] = combine(fifo[i].GYRO_DATA_Z1, fifo[i].GYRO_DATA_Z0);
+		}
+
+		if (!hitl_mode) {
+			_px4_gyro.set_scale(math::radians(2000.f / 32768.f));
+		}
+	}
+
+	// correct frame for publication
+	for (int i = 0; i < gyro.samples; i++) {
+		// sensor's frame is +x forward, +y left, +z up
+		//  flip y & z to publish right handed with z down (x forward, y right, z down)
+		gyro.x[i] = gyro.x[i];
+		gyro.y[i] = (gyro.y[i] == INT16_MIN) ? INT16_MAX : -gyro.y[i];
+		gyro.z[i] = (gyro.z[i] == INT16_MIN) ? INT16_MAX : -gyro.z[i];
+	}
+
+	if (!hitl_mode) {
+		_px4_gyro.set_error_count(perf_event_count(_bad_register_perf) + perf_event_count(_bad_transfer_perf) +
+					  perf_event_count(_fifo_empty_perf) + perf_event_count(_fifo_overflow_perf));
+	}
+
+	if (gyro.samples > 0) {
+		if (!hitl_mode) {
+			_px4_gyro.updateFIFO(gyro);
+		}
+	}
+}
+
+bool ICM42688P::ProcessTemperature(const FIFO::DATA fifo[], const uint8_t samples)
+{
+	int16_t temperature[FIFO_MAX_SAMPLES];
+	float temperature_sum{0};
+
+	int valid_samples = 0;
+
+	for (int i = 0; i < samples; i++) {
+		const int16_t t = combine(fifo[i].TEMP_DATA1, fifo[i].TEMP_DATA0);
+
+		// sample invalid if -32768
+		if (t != -32768) {
+			temperature_sum += t;
+			temperature[valid_samples] = t;
+			valid_samples++;
+		}
+	}
+
+	if (valid_samples > 0) {
+		const float temperature_avg = temperature_sum / valid_samples;
+
+		for (int i = 0; i < valid_samples; i++) {
+			// temperature changing wildly is an indication of a transfer error
+			if (fabsf(temperature[i] - temperature_avg) > 1000) {
+				perf_count(_bad_transfer_perf);
+				return false;
+			}
+		}
+
+		// use average temperature reading
+		const float TEMP_degC = (temperature_avg / TEMPERATURE_SENSITIVITY) + TEMPERATURE_OFFSET;
+
+		if (PX4_ISFINITE(TEMP_degC)) {
+			if (!hitl_mode) {
+				_px4_accel.set_temperature(TEMP_degC);
+				_px4_gyro.set_temperature(TEMP_degC);
+				return true;
+			}
+
+		} else {
+			perf_count(_bad_transfer_perf);
+		}
+	}
+
+	return false;
+}
@@ -0,0 +1,233 @@
+/****************************************************************************
+ *
+ *   Copyright (c) 2020 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.
+ *
+ ****************************************************************************/
+
+/**
+ * @file ICM42688P.hpp
+ *
+ * Driver for the Invensense ICM42688P connected via SPI.
+ *
+ */
+
+#pragma once
+
+#include "InvenSense_ICM42688P_registers.hpp"
+
+#include <drivers/drv_hrt.h>
+#include <lib/drivers/accelerometer/PX4Accelerometer.hpp>
+#include <lib/drivers/device/spi.h>
+#include <lib/drivers/gyroscope/PX4Gyroscope.hpp>
+#include <lib/geo/geo.h>
+#include <lib/perf/perf_counter.h>
+#include <px4_platform_common/atomic.h>
+#include <px4_platform_common/i2c_spi_buses.h>
+#include <uORB/topics/sensor_accel_fifo.h>
+#include <uORB/topics/sensor_gyro_fifo.h>
+#include <memory>
+
+using namespace InvenSense_ICM42688P;
+
+extern bool hitl_mode;
+
+class ICM42688P : public device::SPI, public I2CSPIDriver<ICM42688P>
+{
+public:
+	// ICM42688P(I2CSPIBusOption bus_option, int bus, uint32_t device, enum Rotation rotation, int bus_frequency,
+	// 	  spi_mode_e spi_mode, spi_drdy_gpio_t drdy_gpio);
+	ICM42688P(const I2CSPIDriverConfig &config);
+	~ICM42688P() override;
+
+	// static I2CSPIDriverBase *instantiate(const BusCLIArguments &cli, const BusInstanceIterator &iterator,
+	// 				     int runtime_instance);
+	static void print_usage();
+
+	void RunImpl();
+
+	int init() override;
+	void print_status() override;
+
+private:
+	void exit_and_cleanup() override;
+
+	// Sensor Configuration
+	static constexpr float IMU_ODR{8000.f}; // 8kHz accel & gyro ODR configured
+	static constexpr float FIFO_SAMPLE_DT{1e6f / IMU_ODR};
+	static constexpr float GYRO_RATE{1e6f / FIFO_SAMPLE_DT};
+	static constexpr float ACCEL_RATE{1e6f / FIFO_SAMPLE_DT};
+
+	// maximum FIFO samples per transfer is limited to the size of sensor_accel_fifo/sensor_gyro_fifo
+	// static constexpr uint32_t FIFO_MAX_SAMPLES{math::min(math::min(FIFO::SIZE / sizeof(FIFO::DATA), sizeof(sensor_gyro_fifo_s::x) / sizeof(sensor_gyro_fifo_s::x[0])), sizeof(sensor_accel_fifo_s::x) / sizeof(sensor_accel_fifo_s::x[0]) * (int)(GYRO_RATE / ACCEL_RATE))};
+	static constexpr uint32_t FIFO_MAX_SAMPLES{10};
+
+	// Transfer data
+	struct FIFOTransferBuffer {
+		uint8_t cmd{static_cast<uint8_t>(Register::BANK_0::INT_STATUS) | DIR_READ};
+		uint8_t INT_STATUS{0};
+		uint8_t FIFO_COUNTH{0};
+		uint8_t FIFO_COUNTL{0};
+		FIFO::DATA f[FIFO_MAX_SAMPLES] {};
+	};
+	// ensure no struct padding
+	static_assert(sizeof(FIFOTransferBuffer) == (4 + FIFO_MAX_SAMPLES *sizeof(FIFO::DATA)),
+		      "Invalid FIFOTransferBuffer size");
+
+	struct register_bank0_config_t {
+		Register::BANK_0 reg;
+		uint8_t set_bits{0};
+		uint8_t clear_bits{0};
+	};
+
+	struct register_bank1_config_t {
+		Register::BANK_1 reg;
+		uint8_t set_bits{0};
+		uint8_t clear_bits{0};
+	};
+
+	struct register_bank2_config_t {
+		Register::BANK_2 reg;
+		uint8_t set_bits{0};
+		uint8_t clear_bits{0};
+	};
+
+	int probe() override;
+
+	bool Reset();
+
+	bool Configure();
+	void ConfigureSampleRate(int sample_rate);
+	void ConfigureFIFOWatermark(uint8_t samples);
+
+	void SelectRegisterBank(enum REG_BANK_SEL_BIT bank, bool force = false);
+	void SelectRegisterBank(Register::BANK_0 reg) { SelectRegisterBank(REG_BANK_SEL_BIT::USER_BANK_0); }
+	void SelectRegisterBank(Register::BANK_1 reg) { SelectRegisterBank(REG_BANK_SEL_BIT::USER_BANK_1); }
+	void SelectRegisterBank(Register::BANK_2 reg) { SelectRegisterBank(REG_BANK_SEL_BIT::USER_BANK_2); }
+
+	static int DataReadyInterruptCallback(int irq, void *context, void *arg);
+	void DataReady();
+	bool DataReadyInterruptConfigure();
+	bool DataReadyInterruptDisable();
+
+	template <typename T> bool RegisterCheck(const T &reg_cfg);
+	template <typename T> uint8_t RegisterRead(T reg);
+	template <typename T> void RegisterWrite(T reg, uint8_t value);
+	template <typename T> void RegisterSetAndClearBits(T reg, uint8_t setbits, uint8_t clearbits);
+	template <typename T> void RegisterSetBits(T reg, uint8_t setbits) { RegisterSetAndClearBits(reg, setbits, 0); }
+	template <typename T> void RegisterClearBits(T reg, uint8_t clearbits) { RegisterSetAndClearBits(reg, 0, clearbits); }
+
+	uint16_t FIFOReadCount();
+	bool FIFORead(const hrt_abstime &timestamp_sample, uint16_t samples);
+	void FIFOReset();
+
+	void ProcessIMU(const hrt_abstime &timestamp_sample, const FIFO::DATA &fifo);
+	void ProcessAccel(const hrt_abstime &timestamp_sample, const FIFO::DATA fifo[], const uint8_t samples);
+	void ProcessGyro(const hrt_abstime &timestamp_sample, const FIFO::DATA fifo[], const uint8_t samples);
+	bool ProcessTemperature(const FIFO::DATA fifo[], const uint8_t samples);
+
+	const spi_drdy_gpio_t _drdy_gpio;
+
+	// std::shared_ptr<PX4Accelerometer> _px4_accel;
+	// std::shared_ptr<PX4Gyroscope> _px4_gyro;
+	PX4Accelerometer _px4_accel;
+	PX4Gyroscope _px4_gyro;
+
+	perf_counter_t _bad_register_perf{perf_alloc(PC_COUNT, MODULE_NAME": bad register")};
+	perf_counter_t _bad_transfer_perf{perf_alloc(PC_COUNT, MODULE_NAME": bad transfer")};
+	perf_counter_t _fifo_empty_perf{perf_alloc(PC_COUNT, MODULE_NAME": FIFO empty")};
+	perf_counter_t _fifo_overflow_perf{perf_alloc(PC_COUNT, MODULE_NAME": FIFO overflow")};
+	perf_counter_t _fifo_reset_perf{perf_alloc(PC_COUNT, MODULE_NAME": FIFO reset")};
+	perf_counter_t _drdy_missed_perf{nullptr};
+
+	hrt_abstime _reset_timestamp{0};
+	hrt_abstime _last_config_check_timestamp{0};
+	hrt_abstime _temperature_update_timestamp{0};
+	int _failure_count{0};
+
+	enum REG_BANK_SEL_BIT _last_register_bank {REG_BANK_SEL_BIT::USER_BANK_0};
+
+	px4::atomic<uint32_t> _drdy_fifo_read_samples{0};
+	bool _data_ready_interrupt_enabled{false};
+
+	enum class STATE : uint8_t {
+		RESET,
+		WAIT_FOR_RESET,
+		CONFIGURE,
+		FIFO_READ,
+	} _state{STATE::RESET};
+
+	uint16_t _fifo_empty_interval_us{1250}; // default 1250 us / 800 Hz transfer interval
+	uint32_t _fifo_gyro_samples{static_cast<uint32_t>(_fifo_empty_interval_us / (1000000 / GYRO_RATE))};
+
+	uint8_t _checked_register_bank0{0};
+	static constexpr uint8_t size_register_bank0_cfg{12};
+	register_bank0_config_t _register_bank0_cfg[size_register_bank0_cfg] {
+		// Register                              | Set bits, Clear bits
+		{ Register::BANK_0::INT_CONFIG,           INT_CONFIG_BIT::INT1_MODE | INT_CONFIG_BIT::INT1_DRIVE_CIRCUIT, INT_CONFIG_BIT::INT1_POLARITY },
+		{ Register::BANK_0::FIFO_CONFIG,          FIFO_CONFIG_BIT::FIFO_MODE_STOP_ON_FULL, 0 },
+		{ Register::BANK_0::GYRO_CONFIG0,         GYRO_CONFIG0_BIT::GYRO_FS_SEL_2000_DPS | GYRO_CONFIG0_BIT::GYRO_ODR_8KHZ_SET, GYRO_CONFIG0_BIT::GYRO_ODR_8KHZ_CLEAR },
+		{ Register::BANK_0::ACCEL_CONFIG0,        ACCEL_CONFIG0_BIT::ACCEL_FS_SEL_16G | ACCEL_CONFIG0_BIT::ACCEL_ODR_8KHZ_SET, ACCEL_CONFIG0_BIT::ACCEL_ODR_8KHZ_CLEAR },
+		{ Register::BANK_0::GYRO_CONFIG1,         0, GYRO_CONFIG1_BIT::GYRO_UI_FILT_ORD },
+		{ Register::BANK_0::GYRO_ACCEL_CONFIG0,   0, GYRO_ACCEL_CONFIG0_BIT::ACCEL_UI_FILT_BW | GYRO_ACCEL_CONFIG0_BIT::GYRO_UI_FILT_BW },
+		{ Register::BANK_0::ACCEL_CONFIG1,        0, ACCEL_CONFIG1_BIT::ACCEL_UI_FILT_ORD },
+		{ Register::BANK_0::FIFO_CONFIG1,         FIFO_CONFIG1_BIT::FIFO_WM_GT_TH | FIFO_CONFIG1_BIT::FIFO_HIRES_EN | FIFO_CONFIG1_BIT::FIFO_TEMP_EN | FIFO_CONFIG1_BIT::FIFO_GYRO_EN | FIFO_CONFIG1_BIT::FIFO_ACCEL_EN, 0 },
+		{ Register::BANK_0::FIFO_CONFIG2,         0, 0 }, // FIFO_WM[7:0] set at runtime
+		{ Register::BANK_0::FIFO_CONFIG3,         0, 0 }, // FIFO_WM[11:8] set at runtime
+		{ Register::BANK_0::INT_CONFIG0,          INT_CONFIG0_BIT::CLEAR_ON_FIFO_READ, 0 },
+		{ Register::BANK_0::INT_SOURCE0,          INT_SOURCE0_BIT::FIFO_THS_INT1_EN, 0 },
+	};
+
+	uint8_t _checked_register_bank1{0};
+	static constexpr uint8_t size_register_bank1_cfg{4};
+	register_bank1_config_t _register_bank1_cfg[size_register_bank1_cfg] {
+		// Register                              | Set bits, Clear bits
+		{ Register::BANK_1::GYRO_CONFIG_STATIC2,  0, GYRO_CONFIG_STATIC2_BIT::GYRO_NF_DIS | GYRO_CONFIG_STATIC2_BIT::GYRO_AAF_DIS },
+		{ Register::BANK_1::GYRO_CONFIG_STATIC3,  GYRO_CONFIG_STATIC3_BIT::GYRO_AAF_DELT_SET, GYRO_CONFIG_STATIC3_BIT::GYRO_AAF_DELT_CLEAR},
+		{ Register::BANK_1::GYRO_CONFIG_STATIC4,  GYRO_CONFIG_STATIC4_BIT::GYRO_AAF_DELTSQR_LOW_SET, GYRO_CONFIG_STATIC4_BIT::GYRO_AAF_DELTSQR_LOW_CLEAR},
+		{ Register::BANK_1::GYRO_CONFIG_STATIC5,  GYRO_CONFIG_STATIC5_BIT::GYRO_AAF_BITSHIFT_SET | GYRO_CONFIG_STATIC5_BIT::GYRO_AAF_DELTSQR_HIGH_SET, GYRO_CONFIG_STATIC5_BIT::GYRO_AAF_BITSHIFT_CLEAR | GYRO_CONFIG_STATIC5_BIT::GYRO_AAF_DELTSQR_HIGH_CLEAR},
+	};
+
+	uint8_t _checked_register_bank2{0};
+	static constexpr uint8_t size_register_bank2_cfg{3};
+	register_bank2_config_t _register_bank2_cfg[size_register_bank2_cfg] {
+		// Register                              | Set bits, Clear bits
+		{ Register::BANK_2::ACCEL_CONFIG_STATIC2, ACCEL_CONFIG_STATIC2_BIT::ACCEL_AAF_DELT_SET, ACCEL_CONFIG_STATIC2_BIT::ACCEL_AAF_DELT_CLEAR | ACCEL_CONFIG_STATIC2_BIT::ACCEL_AAF_DIS },
+		{ Register::BANK_2::ACCEL_CONFIG_STATIC3, ACCEL_CONFIG_STATIC3_BIT::ACCEL_AAF_DELTSQR_LOW_SET, ACCEL_CONFIG_STATIC3_BIT::ACCEL_AAF_DELTSQR_LOW_CLEAR },
+		{ Register::BANK_2::ACCEL_CONFIG_STATIC4, ACCEL_CONFIG_STATIC4_BIT::ACCEL_AAF_BITSHIFT_SET | ACCEL_CONFIG_STATIC4_BIT::ACCEL_AAF_DELTSQR_HIGH_SET, ACCEL_CONFIG_STATIC4_BIT::ACCEL_AAF_BITSHIFT_CLEAR | ACCEL_CONFIG_STATIC4_BIT::ACCEL_AAF_DELTSQR_HIGH_CLEAR },
+	};
+
+	uint32_t _temperature_samples{0};
+
+	// Support for the IMU server
+	// uint32_t _imu_server_index{0};
+	// imu_server_s _imu_server_data;
+	// uORB::Publication<imu_server_s> _imu_server_pub{ORB_ID(imu_server)};
+
+};
@@ -0,0 +1,430 @@
+/****************************************************************************
+ *
+ *   Copyright (c) 2020 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.
+ *
+ ****************************************************************************/
+
+/**
+ * @file InvenSense_ICM42688P_registers.hpp
+ *
+ * Invensense ICM-42688-P registers.
+ *
+ */
+
+#pragma once
+
+#include <cstdint>
+
+namespace InvenSense_ICM42688P
+{
+// TODO: move to a central header
+static constexpr uint8_t Bit0 = (1 << 0);
+static constexpr uint8_t Bit1 = (1 << 1);
+static constexpr uint8_t Bit2 = (1 << 2);
+static constexpr uint8_t Bit3 = (1 << 3);
+static constexpr uint8_t Bit4 = (1 << 4);
+static constexpr uint8_t Bit5 = (1 << 5);
+static constexpr uint8_t Bit6 = (1 << 6);
+static constexpr uint8_t Bit7 = (1 << 7);
+
+static constexpr uint32_t SPI_SPEED = 24 * 1000 * 1000; // 24 MHz SPI
+static constexpr uint8_t DIR_READ = 0x80;
+
+static constexpr uint8_t WHOAMI = 0x47;
+
+static constexpr float TEMPERATURE_SENSITIVITY = 132.48f; // LSB/C
+static constexpr float TEMPERATURE_OFFSET = 25.f; // C
+
+namespace Register
+{
+
+enum class BANK_0 : uint8_t {
+	DEVICE_CONFIG      = 0x11,
+
+	INT_CONFIG         = 0x14,
+
+	FIFO_CONFIG        = 0x16,
+
+	TEMP_DATA1         = 0x1D,
+	TEMP_DATA0         = 0x1E,
+
+	INT_STATUS         = 0x2D,
+	FIFO_COUNTH        = 0x2E,
+	FIFO_COUNTL        = 0x2F,
+	FIFO_DATA          = 0x30,
+
+	SIGNAL_PATH_RESET  = 0x4B,
+	INTF_CONFIG0       = 0x4C,
+	INTF_CONFIG1       = 0x4D,
+	PWR_MGMT0          = 0x4E,
+	GYRO_CONFIG0       = 0x4F,
+	ACCEL_CONFIG0      = 0x50,
+	GYRO_CONFIG1       = 0x51,
+	GYRO_ACCEL_CONFIG0 = 0x52,
+	ACCEL_CONFIG1      = 0x53,
+
+	FIFO_CONFIG1       = 0x5F,
+	FIFO_CONFIG2       = 0x60,
+	FIFO_CONFIG3       = 0x61,
+
+	INT_CONFIG0        = 0x63,
+
+	INT_SOURCE0        = 0x65,
+
+	SELF_TEST_CONFIG   = 0x70,
+
+	WHO_AM_I           = 0x75,
+	REG_BANK_SEL       = 0x76,
+};
+
+enum class BANK_1 : uint8_t {
+	GYRO_CONFIG_STATIC2 = 0x0B,
+	GYRO_CONFIG_STATIC3 = 0x0C,
+	GYRO_CONFIG_STATIC4 = 0x0D,
+	GYRO_CONFIG_STATIC5 = 0x0E,
+	INTF_CONFIG5        = 0x7B,
+};
+enum class BANK_2 : uint8_t {
+	ACCEL_CONFIG_STATIC2 = 0x03,
+	ACCEL_CONFIG_STATIC3 = 0x04,
+	ACCEL_CONFIG_STATIC4 = 0x05,
+};
+
+};
+
+//---------------- BANK0 Register bits
+
+// DEVICE_CONFIG
+enum DEVICE_CONFIG_BIT : uint8_t {
+	SOFT_RESET_CONFIG = Bit0, //
+};
+
+// INT_CONFIG
+enum INT_CONFIG_BIT : uint8_t {
+	INT1_MODE           = Bit2,
+	INT1_DRIVE_CIRCUIT  = Bit1,
+	INT1_POLARITY       = Bit0,
+};
+
+// FIFO_CONFIG
+enum FIFO_CONFIG_BIT : uint8_t {
+	// 7:6 FIFO_MODE
+	FIFO_MODE_STOP_ON_FULL = Bit7 | Bit6, // 11: STOP-on-FULL Mode
+};
+
+// INT_STATUS
+enum INT_STATUS_BIT : uint8_t {
+	RESET_DONE_INT = Bit4,
+	DATA_RDY_INT   = Bit3,
+	FIFO_THS_INT   = Bit2,
+	FIFO_FULL_INT  = Bit1,
+};
+
+// SIGNAL_PATH_RESET
+enum SIGNAL_PATH_RESET_BIT : uint8_t {
+	ABORT_AND_RESET = Bit3,
+	FIFO_FLUSH      = Bit1,
+};
+
+// PWR_MGMT0
+enum PWR_MGMT0_BIT : uint8_t {
+	GYRO_MODE_LOW_NOISE  = Bit3 | Bit2, // 11: Places gyroscope in Low Noise (LN) Mode
+	ACCEL_MODE_LOW_NOISE = Bit1 | Bit0, // 11: Places accelerometer in Low Noise (LN) Mode
+};
+
+// GYRO_CONFIG0
+enum GYRO_CONFIG0_BIT : uint8_t {
+	// 7:5 GYRO_FS_SEL
+	GYRO_FS_SEL_2000_DPS = 0, // 0b000 = ±2000dps (default)
+	GYRO_FS_SEL_1000_DPS = Bit5,
+	GYRO_FS_SEL_500_DPS  = Bit6,
+	GYRO_FS_SEL_250_DPS  = Bit6 | Bit5,
+	GYRO_FS_SEL_125_DPS  = Bit7,
+
+
+	// 3:0 GYRO_ODR
+	//  0001: 32kHz
+	GYRO_ODR_32KHZ_SET   = Bit0,
+	GYRO_ODR_32KHZ_CLEAR = Bit3 | Bit2 | Bit0,
+	//  0010: 16kHz
+	GYRO_ODR_16KHZ_SET   = Bit1,
+	GYRO_ODR_16KHZ_CLEAR = Bit3 | Bit2 | Bit0,
+	//  0011: 8kHz
+	GYRO_ODR_8KHZ_SET    = Bit1 | Bit0,
+	GYRO_ODR_8KHZ_CLEAR  = Bit3 | Bit2,
+	//  0110: 1kHz (default)
+	GYRO_ODR_1KHZ_SET    = Bit2 | Bit1,
+	GYRO_ODR_1KHZ_CLEAR  = Bit3 | Bit0,
+};
+
+// ACCEL_CONFIG0
+enum ACCEL_CONFIG0_BIT : uint8_t {
+	// 7:5 ACCEL_FS_SEL
+	ACCEL_FS_SEL_16G = 0, // 000: ±16g (default)
+	ACCEL_FS_SEL_8G  = Bit5,
+	ACCEL_FS_SEL_4G  = Bit6,
+	ACCEL_FS_SEL_2G  = Bit6 | Bit5,
+
+
+	// 3:0 ACCEL_ODR
+	//  0001: 32kHz
+	ACCEL_ODR_32KHZ_SET   = Bit0,
+	ACCEL_ODR_32KHZ_CLEAR = Bit3 | Bit2 | Bit0,
+	//  0010: 16kHz
+	ACCEL_ODR_16KHZ_SET   = Bit1,
+	ACCEL_ODR_16KHZ_CLEAR = Bit3 | Bit2 | Bit0,
+	//  0011: 8kHz
+	ACCEL_ODR_8KHZ_SET    = Bit1 | Bit0,
+	ACCEL_ODR_8KHZ_CLEAR  = Bit3 | Bit2,
+	//  0110: 1kHz (default)
+	ACCEL_ODR_1KHZ_SET    = Bit2 | Bit1,
+	ACCEL_ODR_1KHZ_CLEAR  = Bit3 | Bit0,
+};
+
+// GYRO_CONFIG1
+enum GYRO_CONFIG1_BIT : uint8_t {
+	GYRO_UI_FILT_ORD = Bit3 | Bit2, // 00: 1st Order
+};
+
+// GYRO_ACCEL_CONFIG0
+enum GYRO_ACCEL_CONFIG0_BIT : uint8_t {
+	// 7:4 ACCEL_UI_FILT_BW
+	ACCEL_UI_FILT_BW = Bit7 | Bit6 | Bit5 | Bit4, // 0: BW=ODR/2
+
+	// 3:0 GYRO_UI_FILT_BW
+	GYRO_UI_FILT_BW  = Bit3 | Bit2 | Bit1 | Bit0, // 0: BW=ODR/2
+};
+
+// ACCEL_CONFIG1
+enum ACCEL_CONFIG1_BIT : uint8_t {
+	ACCEL_UI_FILT_ORD = Bit4 | Bit3, // 00: 1st Order
+};
+
+// FIFO_CONFIG1
+enum FIFO_CONFIG1_BIT : uint8_t {
+	FIFO_RESUME_PARTIAL_RD = Bit6,
+	FIFO_WM_GT_TH          = Bit5,
+	FIFO_HIRES_EN          = Bit4,
+	FIFO_TEMP_EN           = Bit2,
+	FIFO_GYRO_EN           = Bit1,
+	FIFO_ACCEL_EN          = Bit0,
+};
+
+// INT_CONFIG0
+enum INT_CONFIG0_BIT : uint8_t {
+	// 3:2 FIFO_THS_INT_CLEAR
+	CLEAR_ON_FIFO_READ = Bit3,
+};
+
+// INT_SOURCE0
+enum INT_SOURCE0_BIT : uint8_t {
+	UI_FSYNC_INT1_EN   = Bit6,
+	PLL_RDY_INT1_EN    = Bit5,
+	RESET_DONE_INT1_EN = Bit4,
+	UI_DRDY_INT1_EN    = Bit3,
+	FIFO_THS_INT1_EN   = Bit2, // FIFO threshold interrupt routed to INT1
+	FIFO_FULL_INT1_EN  = Bit1,
+	UI_AGC_RDY_INT1_EN = Bit0,
+};
+
+// REG_BANK_SEL
+enum REG_BANK_SEL_BIT : uint8_t {
+	USER_BANK_0 = 0,           // 0: Select USER BANK 0.
+	USER_BANK_1 = Bit0,        // 1: Select USER BANK 1.
+	USER_BANK_2 = Bit1,        // 2: Select USER BANK 2.
+	USER_BANK_3 = Bit1 | Bit0, // 3: Select USER BANK 3.
+};
+
+
+//---------------- BANK1 Register bits
+
+// GYRO_CONFIG_STATIC2
+enum GYRO_CONFIG_STATIC2_BIT : uint8_t {
+	GYRO_AAF_DIS = Bit1,
+	GYRO_NF_DIS  = Bit0,
+};
+
+// GYRO_CONFIG_STATIC3
+enum GYRO_CONFIG_STATIC3_BIT : uint8_t {
+
+	// 585 Hz
+	GYRO_AAF_DELT_SET = Bit3 | Bit2 | Bit0, //13
+	GYRO_AAF_DELT_CLEAR = Bit5 | Bit4 | Bit1,
+
+	// 213 Hz
+	// GYRO_AAF_DELT_SET = Bit2 | Bit0, //5
+	// GYRO_AAF_DELT_CLEAR = Bit5 | Bit4 | Bit3 | Bit1,
+
+	// 126 Hz
+	//GYRO_AAF_DELT_SET = Bit1 | Bit0, //3
+	//GYRO_AAF_DELT_CLEAR = Bit5 | Bit4 | Bit3 | Bit2,
+
+	// 42 Hz
+	// GYRO_AAF_DELT_SET = Bit0, //1
+	// GYRO_AAF_DELT_CLEAR = Bit5 | Bit4 | Bit3 | Bit2 | Bit1,
+
+};
+
+// GYRO_CONFIG_STATIC4
+enum GYRO_CONFIG_STATIC4_BIT : uint8_t {
+
+	// 585 Hz
+	GYRO_AAF_DELTSQR_LOW_SET = Bit7 | Bit5 | Bit3 | Bit1, //170
+	GYRO_AAF_DELTSQR_LOW_CLEAR = Bit6 | Bit4 | Bit2 | Bit0,
+
+	// 213 Hz
+	// GYRO_AAF_DELTSQR_LOW_SET = Bit4 | Bit3 | Bit0, //25
+	// GYRO_AAF_DELTSQR_LOW_CLEAR = Bit7 | Bit6 | Bit5 | Bit2 | Bit1,
+
+	// 126 Hz
+	//GYRO_AAF_DELTSQR_LOW_SET = Bit3 | Bit0, //9
+	//GYRO_AAF_DELTSQR_LOW_CLEAR = Bit7 | Bit6 | Bit5 | Bit4 | Bit2 | Bit1,
+
+	// 42 Hz
+	// GYRO_AAF_DELTSQR_LOW_SET = Bit0, //1
+	// GYRO_AAF_DELTSQR_LOW_CLEAR = Bit7 | Bit6 | Bit5 | Bit4 | Bit3 | Bit2 | Bit1,
+};
+
+// GYRO_CONFIG_STATIC5
+enum GYRO_CONFIG_STATIC5_BIT : uint8_t {
+
+	// 585 Hz
+	GYRO_AAF_DELTSQR_HIGH_SET = 0,
+	GYRO_AAF_DELTSQR_HIGH_CLEAR = Bit3 | Bit2 | Bit1 | Bit0,
+	GYRO_AAF_BITSHIFT_SET = Bit7, // 8 << 4
+	GYRO_AAF_BITSHIFT_CLEAR = Bit6 | Bit5 | Bit4,
+
+	// 213 Hz
+	// GYRO_AAF_DELTSQR_HIGH_SET = 0,
+	// GYRO_AAF_DELTSQR_HIGH_CLEAR = Bit3 | Bit2 | Bit1 | Bit0,
+	// GYRO_AAF_BITSHIFT_SET = Bit7 | Bit5, //10
+	// GYRO_AAF_BITSHIFT_CLEAR = Bit6 | Bit4,
+
+	// 126 Hz
+	// GYRO_AAF_BITSHIFT_SET = Bit7 | Bit6, //12
+	// GYRO_AAF_BITSHIFT_CLEAR = Bit5 | Bit4,
+
+	// 42 Hz
+	// GYRO_AAF_BITSHIFT_SET = Bit7 | Bit6 | Bit5 | Bit4, //15
+	// GYRO_AAF_BITSHIFT_CLEAR = 0,
+
+
+};
+
+
+//---------------- BANK2 Register bits
+
+// ACCEL_CONFIG_STATIC2
+enum ACCEL_CONFIG_STATIC2_BIT : uint8_t {
+	ACCEL_AAF_DIS  = Bit0,
+	ACCEL_AAF_DELT = Bit3 | Bit1,
+
+	// 213 Hz
+	ACCEL_AAF_DELT_SET = Bit3 | Bit1, //5
+	ACCEL_AAF_DELT_CLEAR = Bit6 | Bit5 | Bit4 | Bit2,
+
+	// 42 Hz
+	// ACCEL_AAF_DELT_SET = Bit1, //1
+	// ACCEL_AAF_DELT_CLEAR = Bit6 | Bit5 | Bit4 | Bit3 | Bit2,
+};
+
+// ACCEL_CONFIG_STATIC3
+enum ACCEL_CONFIG_STATIC3_BIT : uint8_t {
+	ACCEL_AAF_DELTSQR_LOW = Bit4 | Bit3 | Bit0,
+	// 213 Hz
+	ACCEL_AAF_DELTSQR_LOW_SET = Bit4 | Bit3 | Bit0, //25
+	ACCEL_AAF_DELTSQR_LOW_CLEAR = Bit7 | Bit6 | Bit5 | Bit2 | Bit1,
+
+	// 42 Hz
+	// ACCEL_AAF_DELTSQR_LOW_SET = Bit0, //1
+	// ACCEL_AAF_DELTSQR_LOW_CLEAR = Bit7 | Bit6 | Bit5 | Bit4 | Bit3 | Bit2 | Bit1,
+
+};
+
+// ACCEL_CONFIG_STATIC4
+enum ACCEL_CONFIG_STATIC4_BIT : uint8_t {
+	ACCEL_AAF_BITSHIFT     = Bit7 | Bit5,
+	ACCEL_AAF_DELTSQR_HIGH = 0,
+	// 213 Hz
+	ACCEL_AAF_BITSHIFT_SET = Bit7 | Bit5, //10
+	ACCEL_AAF_BITSHIFT_CLEAR = Bit6 | Bit4,
+
+	// 42 Hz
+	// ACCEL_AAF_BITSHIFT_SET = Bit7 | Bit6 | Bit5 | Bit4, //15
+	// ACCEL_AAF_BITSHIFT_CLEAR = 0,
+
+	ACCEL_AAF_DELTSQR_HIGH_SET = 0,
+	ACCEL_AAF_DELTSQR_HIGH_CLEAR = Bit3 | Bit2 | Bit1 | Bit0,
+};
+
+
+namespace FIFO
+{
+static constexpr size_t SIZE = 2048;
+
+// FIFO_DATA layout when FIFO_CONFIG1 has FIFO_GYRO_EN and FIFO_ACCEL_EN set
+
+// Packet 4
+struct DATA {
+	uint8_t FIFO_Header;
+	uint8_t ACCEL_DATA_X1; // Accel X [19:12]
+	uint8_t ACCEL_DATA_X0; // Accel X [11:4]
+	uint8_t ACCEL_DATA_Y1; // Accel Y [19:12]
+	uint8_t ACCEL_DATA_Y0; // Accel Y [11:4]
+	uint8_t ACCEL_DATA_Z1; // Accel Z [19:12]
+	uint8_t ACCEL_DATA_Z0; // Accel Z [11:4]
+	uint8_t GYRO_DATA_X1;  // Gyro X [19:12]
+	uint8_t GYRO_DATA_X0;  // Gyro X [11:4]
+	uint8_t GYRO_DATA_Y1;  // Gyro Y [19:12]
+	uint8_t GYRO_DATA_Y0;  // Gyro Y [11:4]
+	uint8_t GYRO_DATA_Z1;  // Gyro Z [19:12]
+	uint8_t GYRO_DATA_Z0;  // Gyro Z [11:4]
+	uint8_t TEMP_DATA1;    // Temperature[15:8]
+	uint8_t TEMP_DATA0;    // Temperature[7:0]
+	uint8_t TimeStamp_h;   // TimeStamp[15:8]
+	uint8_t TimeStamp_l;   // TimeStamp[7:0]
+	uint8_t Ext_Accel_X_Gyro_X; // Accel X [3:0] Gyro X [3:0]
+	uint8_t Ext_Accel_Y_Gyro_Y; // Accel Y [3:0] Gyro Y [3:0]
+	uint8_t Ext_Accel_Z_Gyro_Z; // Accel Z [3:0] Gyro Z [3:0]
+};
+
+// With FIFO_ACCEL_EN and FIFO_GYRO_EN header should be 8’b_0110_10xx
+enum FIFO_HEADER_BIT : uint8_t {
+	HEADER_MSG             = Bit7, // 1: FIFO is empty
+	HEADER_ACCEL           = Bit6, // 1: Packet is sized so that accel data have location in the packet, FIFO_ACCEL_EN must be 1
+	HEADER_GYRO            = Bit5, // 1: Packet is sized so that gyro data have location in the packet, FIFO_GYRO_EN must be1
+	HEADER_20              = Bit4, // 1: Packet has a new and valid sample of extended 20-bit data for gyro and/or accel
+	HEADER_TIMESTAMP_FSYNC = Bit3 | Bit2,
+	HEADER_ODR_ACCEL       = Bit1, // 1: The ODR for accel is different for this accel data packet compared to the previous accel packet
+	HEADER_ODR_GYRO        = Bit0, // 1: The ODR for gyro is different for this gyro data packet compared to the previous gyro packet
+};
+
+}
+} // namespace InvenSense_ICM42688P
@@ -0,0 +1,116 @@
+/****************************************************************************
+ *
+ *   Copyright (c) 2020 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.
+ *
+ ****************************************************************************/
+
+#include "ICM42688P.hpp"
+
+#include <px4_platform_common/getopt.h>
+#include <px4_platform_common/module.h>
+#include <string>
+
+void ICM42688P::print_usage()
+{
+	PRINT_MODULE_USAGE_NAME("icm42688p", "driver");
+	PRINT_MODULE_USAGE_SUBCATEGORY("imu");
+	PRINT_MODULE_USAGE_COMMAND("start");
+	PRINT_MODULE_USAGE_PARAMS_I2C_SPI_DRIVER(false, true);
+	PRINT_MODULE_USAGE_PARAM_INT('R', 0, 0, 35, "Rotation", true);
+	PRINT_MODULE_USAGE_DEFAULT_COMMANDS();
+}
+
+// I2CSPIDriverBase *ICM42688P::instantiate(const BusCLIArguments &cli, const BusInstanceIterator &iterator,
+// 		int runtime_instance)
+// {
+// 	ICM42688P *instance = new ICM42688P(iterator.configuredBusOption(), iterator.bus(), iterator.devid(), cli.rotation,
+// 					    cli.bus_frequency, cli.spi_mode, iterator.DRDYGPIO());
+//
+// 	if (!instance) {
+// 		PX4_ERR("alloc failed");
+// 		return nullptr;
+// 	}
+//
+// 	if (OK != instance->init()) {
+// 		delete instance;
+// 		return nullptr;
+// 	}
+//
+// 	return instance;
+// }
+
+extern "C" int icm42688p_main(int argc, char *argv[])
+{
+
+	for (int i = 0; i <= argc - 1; i++) {
+		if (std::string(argv[i]) == "-h") {
+			argv[i] = 0;
+			hitl_mode = true;
+			break;
+		}
+	}
+
+	int ch;
+	using ThisDriver = ICM42688P;
+	BusCLIArguments cli{false, true};
+	cli.default_spi_frequency = SPI_SPEED;
+
+	while ((ch = cli.getOpt(argc, argv, "R:")) != EOF) {
+		switch (ch) {
+		case 'R':
+			cli.rotation = (enum Rotation)atoi(cli.optArg());
+			break;
+		}
+	}
+
+	const char *verb = cli.optArg();
+
+	if (!verb) {
+		ThisDriver::print_usage();
+		return -1;
+	}
+
+	BusInstanceIterator iterator(MODULE_NAME, cli, DRV_IMU_DEVTYPE_ICM42688P);
+
+	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;
+}
@@ -1,6 +1,6 @@
 /****************************************************************************
 *
- *   Copyright (C) 2019 PX4 Development Team. All rights reserved.
+ *   Copyright (C) 2020 PX4 Development Team. All rights reserved.
 *
 * Redistribution and use in source and binary forms, with or without
 * modification, are permitted provided that the following conditions
@@ -31,15 +31,10 @@
 *
 ****************************************************************************/

-#include <gtest/gtest.h>
-#include <AngularVelocityControl.hpp>
+#include <px4_arch/spi_hw_description.h>
+#include <px4_platform_common/spi.h>
+#include <drivers/drv_sensor.h>

-using namespace matrix;
-
-TEST(AngularVelocityControlTest, AllZeroCase)
-{
-	AngularVelocityControl control;
-	control.update(Vector3f(), Vector3f(), Vector3f(), 0.f, false);
-	Vector3f torque = control.getTorqueSetpoint();
-	EXPECT_EQ(torque, Vector3f());
-}
+constexpr px4_spi_bus_t px4_spi_buses[SPI_BUS_MAX_BUS_ITEMS] = {
+	initSPIBus(1, {initSPIDevice(DRV_IMU_DEVTYPE_ICM42688P), }),
+};
@@ -1,5 +1,5 @@
 CONFIG_PLATFORM_POSIX=y
-CONFIG_BOARD_LINUX=y
+CONFIG_BOARD_LINUX_TARGET=y
 CONFIG_BOARD_TOOLCHAIN="aarch64-linux-gnu"
 CONFIG_MODULES_MUORB_APPS=y
 CONFIG_SYSTEMCMDS_PERF=y
@@ -7,3 +7,5 @@ CONFIG_SYSTEMCMDS_UORB=y
 CONFIG_ORB_COMMUNICATOR=y
 CONFIG_SYSTEMCMDS_PARAM=y
 CONFIG_DRIVERS_QSHELL_POSIX=y
+CONFIG_SYSTEMCMDS_TOPIC_LISTENER=y
+CONFIG_SYSTEMCMDS_BSONDUMP=y
@@ -17,3 +17,5 @@ if [ $TESTMODE = "ON" ]; then
 fi

 muorb start
+
+qshell icm42688p start -s
@@ -79,6 +79,7 @@ CONFIG_MODULES_GIMBAL=y
 CONFIG_MODULES_VTOL_ATT_CONTROL=y
 CONFIG_SYSTEMCMDS_ACTUATOR_TEST=y
 CONFIG_SYSTEMCMDS_BL_UPDATE=y
+CONFIG_SYSTEMCMDS_BSONDUMP=y
 CONFIG_SYSTEMCMDS_DMESG=y
 CONFIG_SYSTEMCMDS_DUMPFILE=y
 CONFIG_SYSTEMCMDS_GPIO=y
@@ -77,8 +77,9 @@ CONFIG_MODULES_UUV_ATT_CONTROL=y
 CONFIG_MODULES_UUV_POS_CONTROL=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_BSONDUMP=y
 CONFIG_SYSTEMCMDS_DMESG=y
 CONFIG_SYSTEMCMDS_DUMPFILE=y
 CONFIG_SYSTEMCMDS_GPIO=y
@@ -77,8 +77,9 @@ CONFIG_MODULES_UUV_ATT_CONTROL=y
 CONFIG_MODULES_UUV_POS_CONTROL=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_BSONDUMP=y
 CONFIG_SYSTEMCMDS_DMESG=y
 CONFIG_SYSTEMCMDS_DUMPFILE=y
 CONFIG_SYSTEMCMDS_GPIO=y
@@ -78,6 +78,7 @@ CONFIG_MODULES_UUV_POS_CONTROL=y
 CONFIG_MODULES_VTOL_ATT_CONTROL=y
 CONFIG_SYSTEMCMDS_ACTUATOR_TEST=y
 CONFIG_SYSTEMCMDS_BL_UPDATE=y
+CONFIG_SYSTEMCMDS_BSONDUMP=y
 CONFIG_SYSTEMCMDS_DMESG=y
 CONFIG_SYSTEMCMDS_DUMPFILE=y
 CONFIG_SYSTEMCMDS_GPIO=y
@@ -21,7 +21,6 @@ CONFIG_DRIVERS_IRLOCK=y
 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
@@ -79,6 +78,7 @@ CONFIG_MODULES_UUV_POS_CONTROL=y
 CONFIG_MODULES_VTOL_ATT_CONTROL=y
 CONFIG_SYSTEMCMDS_ACTUATOR_TEST=y
 CONFIG_SYSTEMCMDS_BL_UPDATE=y
+CONFIG_SYSTEMCMDS_BSONDUMP=y
 CONFIG_SYSTEMCMDS_DMESG=y
 CONFIG_SYSTEMCMDS_DUMPFILE=y
 CONFIG_SYSTEMCMDS_GPIO=y
@@ -78,6 +78,7 @@ CONFIG_MODULES_UUV_POS_CONTROL=y
 CONFIG_MODULES_VTOL_ATT_CONTROL=y
 CONFIG_SYSTEMCMDS_ACTUATOR_TEST=y
 CONFIG_SYSTEMCMDS_BL_UPDATE=y
+CONFIG_SYSTEMCMDS_BSONDUMP=y
 CONFIG_SYSTEMCMDS_DMESG=y
 CONFIG_SYSTEMCMDS_DUMPFILE=y
 CONFIG_SYSTEMCMDS_GPIO=y
@@ -79,8 +79,9 @@ CONFIG_MODULES_UUV_ATT_CONTROL=y
 CONFIG_MODULES_UUV_POS_CONTROL=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_BSONDUMP=y
 CONFIG_SYSTEMCMDS_DMESG=y
 CONFIG_SYSTEMCMDS_DUMPFILE=y
 CONFIG_SYSTEMCMDS_GPIO=y
@@ -80,8 +80,9 @@ CONFIG_MODULES_UUV_ATT_CONTROL=y
 CONFIG_MODULES_UUV_POS_CONTROL=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_BSONDUMP=y
 CONFIG_SYSTEMCMDS_DUMPFILE=y
 CONFIG_SYSTEMCMDS_GPIO=y
 CONFIG_SYSTEMCMDS_HARDFAULT_LOG=y
@@ -23,14 +23,12 @@ CONFIG_COMMON_LIGHT=y
 CONFIG_DRIVERS_LIGHTS_RGBLED_PWM=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_OUT=y
 CONFIG_DRIVERS_RC_INPUT=y
 CONFIG_DRIVERS_ROBOCLAW=y
-CONFIG_DRIVERS_RPM=y
 CONFIG_DRIVERS_SAFETY_BUTTON=y
 CONFIG_DRIVERS_SMART_BATTERY_BATMON=y
 CONFIG_COMMON_TELEMETRY=y
@@ -78,6 +76,7 @@ CONFIG_MODULES_SIMULATION_SENSOR_MAG_SIM=y
 CONFIG_MODULES_SIMULATION_SIMULATOR_SIH=y
 CONFIG_MODULES_TEMPERATURE_COMPENSATION=y
 CONFIG_MODULES_VTOL_ATT_CONTROL=y
+CONFIG_SYSTEMCMDS_BSONDUMP=y
 CONFIG_SYSTEMCMDS_DUMPFILE=y
 CONFIG_SYSTEMCMDS_I2CDETECT=y
 CONFIG_SYSTEMCMDS_LED_CONTROL=y
@@ -24,14 +24,12 @@ CONFIG_DRIVERS_LIGHTS_RGBLED_NCP5623C=y
 CONFIG_DRIVERS_LIGHTS_RGBLED_PWM=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_OUT=y
 CONFIG_DRIVERS_RC_INPUT=y
 CONFIG_DRIVERS_ROBOCLAW=y
-CONFIG_DRIVERS_RPM=y
 CONFIG_DRIVERS_SAFETY_BUTTON=y
 CONFIG_DRIVERS_SMART_BATTERY_BATMON=y
 CONFIG_COMMON_TELEMETRY=y
@@ -80,6 +78,7 @@ CONFIG_MODULES_SIMULATION_SENSOR_MAG_SIM=y
 CONFIG_MODULES_SIMULATION_SIMULATOR_SIH=y
 CONFIG_MODULES_TEMPERATURE_COMPENSATION=y
 CONFIG_MODULES_VTOL_ATT_CONTROL=y
+CONFIG_SYSTEMCMDS_BSONDUMP=y
 CONFIG_SYSTEMCMDS_DUMPFILE=y
 CONFIG_SYSTEMCMDS_I2CDETECT=y
 CONFIG_SYSTEMCMDS_LED_CONTROL=y
@@ -31,6 +31,7 @@ CONFIG_MODULES_DATAMAN=y
 CONFIG_MODULES_EKF2=y
 CONFIG_MODULES_EVENTS=y
 CONFIG_MODULES_FLIGHT_MODE_MANAGER=y
+CONFIG_MODULES_GIMBAL=y
 CONFIG_MODULES_GYRO_CALIBRATION=y
 CONFIG_MODULES_GYRO_FFT=y
 CONFIG_MODULES_LAND_DETECTOR=y
@@ -49,7 +50,7 @@ CONFIG_MODULES_MICRODDS_CLIENT=y
 CONFIG_MODULES_NAVIGATOR=y
 CONFIG_MODULES_RC_UPDATE=y
 CONFIG_MODULES_SENSORS=y
-CONFIG_MODULES_GIMBAL=y
+CONFIG_SYSTEMCMDS_BSONDUMP=y
 CONFIG_SYSTEMCMDS_DMESG=y
 CONFIG_SYSTEMCMDS_DUMPFILE=y
 CONFIG_SYSTEMCMDS_I2CDETECT=y
@@ -15,8 +15,6 @@ CONFIG_MODULES_SENSORS=y
 CONFIG_SYSTEMCMDS_I2CDETECT=y
 CONFIG_SYSTEMCMDS_LED_CONTROL=y
 CONFIG_SYSTEMCMDS_MFT=y
-CONFIG_SYSTEMCMDS_MIXER=y
-CONFIG_SYSTEMCMDS_MOTOR_TEST=y
 CONFIG_SYSTEMCMDS_MTD=y
 CONFIG_SYSTEMCMDS_PARAM=y
 CONFIG_SYSTEMCMDS_REBOOT=y
@@ -2,14 +2,14 @@
 CONFIG_BOARD_SERIAL_GPS1="/dev/ttyS1"
 CONFIG_BOARD_SERIAL_RC="/dev/ttyS5"
 CONFIG_BOARD_SERIAL_TEL1="/dev/ttyS2"
+CONFIG_BOARD_UAVCAN_INTERFACES=1
 CONFIG_COMMON_LIGHT=y
 CONFIG_DRIVERS_GPS=y
 CONFIG_DRIVERS_IRLOCK=y
 CONFIG_DRIVERS_MAGNETOMETER_ISENTEK_IST8310=y
 CONFIG_DRIVERS_MAGNETOMETER_LIS3MDL=y
-CONFIG_DRIVERS_OSD=y
 CONFIG_DRIVERS_RC_INPUT=y
-CONFIG_DRIVERS_RPM=y
+CONFIG_DRIVERS_UAVCAN=y
 CONFIG_EXAMPLES_FAKE_GPS=y
 CONFIG_MODULES_AIRSPEED_SELECTOR=y
 CONFIG_MODULES_ATTITUDE_ESTIMATOR_Q=y
@@ -38,7 +38,6 @@ CONFIG_MODULES_MC_RATE_CONTROL=y
 CONFIG_MODULES_NAVIGATOR=y
 CONFIG_MODULES_RC_UPDATE=y
 CONFIG_MODULES_ROVER_POS_CONTROL=y
-CONFIG_MODULES_SIH=y
 CONFIG_MODULES_TEMPERATURE_COMPENSATION=y
 CONFIG_MODULES_VTOL_ATT_CONTROL=y
 CONFIG_SYSTEMCMDS_ACTUATOR_TEST=y
@@ -47,7 +46,6 @@ CONFIG_SYSTEMCMDS_DUMPFILE=y
 CONFIG_SYSTEMCMDS_NETMAN=y
 CONFIG_SYSTEMCMDS_NSHTERM=y
 CONFIG_SYSTEMCMDS_PERF=y
-CONFIG_SYSTEMCMDS_PWM=y
 CONFIG_SYSTEMCMDS_REFLECT=y
 CONFIG_SYSTEMCMDS_SERIAL_TEST=y
 CONFIG_SYSTEMCMDS_TUNE_CONTROL=y
@@ -15,3 +15,11 @@ param set-default MAV_1_REMOTE_PRT 14550
 param set-default MAV_1_UDP_PRT 14550

 param set-default SENS_EXT_I2C_PRB 0
+
+if param greater -s UAVCAN_ENABLE 0
+then
+	ifup can0
+	ifup can1
+	ifup can2
+	ifup can3
+fi
@@ -3,6 +3,16 @@
 # see the file kconfig-language.txt in the NuttX tools repository.
 #

-if ARCH_BOARD_MR_CANHUBK3
+config BOARD_HAS_PROBES
+	bool "Board provides GPIO or other Hardware for signaling to timing analyze."
+	default y
+	---help---
+		This board provides GPIO PWM-CH0-7 as PROBE_1-8 to provide timing signals from selected drivers.

-endif # ARCH_BOARD_MR_CANHUBK3
+config BOARD_USE_PROBES
+	bool "Enable the use the board provided PWM-CH0-7 as PROBE_1-8"
+	default n
+	depends on BOARD_HAS_PROBES
+
+	---help---
+		Select to use GPIO PWM-CH0-7 to provide timing signals from selected drivers.
@@ -321,4 +321,41 @@
 #define PIN_EMAC_MII_RMII_MDIO    PIN_EMAC_MII_RMII_MDIO_2     /* PTD16 */
 #define PIN_EMAC_MII_RMII_TX_CLK  PIN_EMAC_MII_RMII_TX_CLK_2   /* PTD6 */

+
+/* Board provides GPIO or other Hardware for signaling to timing analyzer */
+
+#if defined(CONFIG_BOARD_USE_PROBES)
+# include "s32k3xx_pin.h"
+# include "hardware/s32k3xx_pinmux.h"
+# define PROBE_N(n) (1<<((n)-1))
+# define PROBE_1    (PIN_PTB12 | GPIO_OUTPUT)  /* PWM-0 */
+# define PROBE_2    (PIN_PTB13 | GPIO_OUTPUT)  /* PWM-1 */
+# define PROBE_3    (PIN_PTB14 | GPIO_OUTPUT)  /* PWM-2 */
+# define PROBE_4    (PIN_PTB15 | GPIO_OUTPUT)  /* PWM-3 */
+# define PROBE_5    (PIN_PTB16 | GPIO_OUTPUT)  /* PWM-4 */
+# define PROBE_6    (PIN_PTB17 | GPIO_OUTPUT)  /* PWM-5 */
+# define PROBE_7    (PIN_PTA17 | GPIO_OUTPUT)  /* PWM-6 */
+# define PROBE_8    (PIN_PTE7  | GPIO_OUTPUT)  /* PWM-7 */
+
+# define PROBE_INIT(mask) \
+	do { \
+		if ((mask)& PROBE_N(1)) { s32k3xx_pinconfig(PROBE_1); } \
+		if ((mask)& PROBE_N(2)) { s32k3xx_pinconfig(PROBE_2); } \
+		if ((mask)& PROBE_N(3)) { s32k3xx_pinconfig(PROBE_3); } \
+		if ((mask)& PROBE_N(4)) { s32k3xx_pinconfig(PROBE_4); } \
+		if ((mask)& PROBE_N(5)) { s32k3xx_pinconfig(PROBE_5); } \
+		if ((mask)& PROBE_N(6)) { s32k3xx_pinconfig(PROBE_6); } \
+		if ((mask)& PROBE_N(7)) { s32k3xx_pinconfig(PROBE_7); } \
+		if ((mask)& PROBE_N(8)) { s32k3xx_pinconfig(PROBE_8); } \
+	} while(0)
+
+# define PROBE(n,s)  do {s32k3xx_gpiowrite(PROBE_##n,(s));}while(0)
+# define PROBE_MARK(n) PROBE(n,false);PROBE(n,true)
+#else
+# define PROBE_INIT(mask)
+# define PROBE(n,s)
+# define PROBE_MARK(n)
+#endif
+
+
 #endif  /* __BOARDS_ARM_S32K3XX_MR_CANHUBK3_INCLUDE_BOARD_H */
@@ -78,6 +78,7 @@ CONFIG_DEBUG_FEATURES=y
 CONFIG_DEBUG_FULLOPT=y
 CONFIG_DEBUG_HARDFAULT_ALERT=y
 CONFIG_DEBUG_SYMBOLS=y
+CONFIG_DEBUG_TCBINFO=y
 CONFIG_DEFAULT_SMALL=y
 CONFIG_DEV_FIFO_SIZE=0
 CONFIG_DEV_PIPE_MAXSIZE=1024
@@ -103,16 +104,17 @@ CONFIG_HAVE_CXX=y
 CONFIG_HAVE_CXXINITIALIZE=y
 CONFIG_I2C=y
 CONFIG_I2C_RESET=y
-CONFIG_IDLETHREAD_STACKSIZE=800
+CONFIG_IDLETHREAD_STACKSIZE=900
 CONFIG_INIT_ENTRYPOINT="nsh_main"
 CONFIG_INIT_STACKSIZE=2944
 CONFIG_IOB_NBUFFERS=24
 CONFIG_IOB_THROTTLE=0
 CONFIG_IPCFG_BINARY=y
 CONFIG_IPCFG_CHARDEV=y
-CONFIG_IPCFG_PATH="/fs/qspi/mtd_net"
+CONFIG_IPCFG_PATH="/mnt/qspi/mtd_net"
 CONFIG_LIBC_FLOATINGPOINT=y
 CONFIG_LIBC_LONG_LONG=y
+CONFIG_LIBC_MAX_EXITFUNS=1
 CONFIG_LIBC_STRERROR=y
 CONFIG_LPI2C0_DMA=y
 CONFIG_LPI2C1_DMA=y
@@ -145,16 +147,21 @@ CONFIG_NET_ARP_IPIN=y
 CONFIG_NET_ARP_SEND=y
 CONFIG_NET_BROADCAST=y
 CONFIG_NET_CAN=y
+CONFIG_NET_CAN_EXTID=y
+CONFIG_NET_CAN_NOTIFIER=y
 CONFIG_NET_CAN_RAW_FILTER_MAX=1
+CONFIG_NET_CAN_RAW_TX_DEADLINE=y
 CONFIG_NET_CAN_SOCK_OPTS=y
 CONFIG_NET_ETH_PKTSIZE=1518
 CONFIG_NET_ICMP=y
 CONFIG_NET_ICMP_SOCKET=y
+CONFIG_NET_NACTIVESOCKETS=16
 CONFIG_NET_SOLINGER=y
 CONFIG_NET_TCP=y
 CONFIG_NET_TCPBACKLOG=y
 CONFIG_NET_TCP_DELAYED_ACK=y
 CONFIG_NET_TCP_WRITE_BUFFERS=y
+CONFIG_NET_TIMESTAMP=y
 CONFIG_NET_UDP=y
 CONFIG_NET_UDP_CHECKSUMS=y
 CONFIG_NET_UDP_WRITE_BUFFERS=y
@@ -177,6 +184,7 @@ CONFIG_NSH_VARS=y
 CONFIG_PIPES=y
 CONFIG_PREALLOC_TIMERS=50
 CONFIG_PRIORITY_INHERITANCE=y
+CONFIG_PTHREAD_MUTEX_ROBUST=y
 CONFIG_PTHREAD_STACK_MIN=512
 CONFIG_RAM_SIZE=272000
 CONFIG_RAM_START=0x20400000
@@ -205,6 +213,7 @@ CONFIG_S32K3XX_LPSPI2_PINCFG=3
 CONFIG_S32K3XX_LPSPI3=y
 CONFIG_S32K3XX_LPSPI3_PINCFG=3
 CONFIG_S32K3XX_LPSPI4=y
+CONFIG_S32K3XX_LPSPI4_DMA=y
 CONFIG_S32K3XX_LPSPI4_PINCFG=3
 CONFIG_S32K3XX_LPSPI5=y
 CONFIG_S32K3XX_LPSPI5_DMA=y
@@ -220,6 +229,7 @@ CONFIG_S32K3XX_LPUART9=y
 CONFIG_S32K3XX_LPUART_INVERT=y
 CONFIG_S32K3XX_LPUART_SINGLEWIRE=y
 CONFIG_S32K3XX_QSPI=y
+CONFIG_S32K3XX_WKPUINTS=y
 CONFIG_SCHED_HPWORK=y
 CONFIG_SCHED_HPWORKPRIORITY=249
 CONFIG_SCHED_HPWORKSTACKSIZE=1280
@@ -82,10 +82,6 @@ __BEGIN_DECLS

 #define BOARD_NUMBER_I2C_BUSES  2

-/* Count of peripheral clock user configurations */
-
-#define NUM_OF_PERIPHERAL_CLOCKS_0 28
-
 /* Timer I/O PWM and capture */

 #define DIRECT_PWM_OUTPUT_CHANNELS  8
@@ -104,7 +100,7 @@ __BEGIN_DECLS

 /* User peripheral configuration structure 0 */

-extern const struct peripheral_clock_config_s g_peripheral_clockconfig0[NUM_OF_PERIPHERAL_CLOCKS_0];
+extern const struct peripheral_clock_config_s g_peripheral_clockconfig0[];

 /****************************************************************************
 * Public Function Prototypes
@@ -156,7 +156,6 @@ const struct clock_configuration_s g_initial_clkconfig = {
 	},
 	.pcc                 =
 	{
-		.count             = NUM_OF_PERIPHERAL_CLOCKS_0,   /* Number of peripheral clock configurations */
 		.pclks             = g_peripheral_clockconfig0,    /* Peripheral clock configurations */
 	},
 };
@@ -41,7 +41,7 @@
 * This is needed to establish the initial peripheral clocking.
 */

-const struct peripheral_clock_config_s g_peripheral_clockconfig0[NUM_OF_PERIPHERAL_CLOCKS_0] = {
+const struct peripheral_clock_config_s g_peripheral_clockconfig0[] = {
 	{
 		.clkname = FLEXCAN0_CLK,
 #ifdef CONFIG_S32K3XX_FLEXCAN0
@@ -260,6 +260,10 @@ const struct peripheral_clock_config_s g_peripheral_clockconfig0[NUM_OF_PERIPHER
 	},
 };

+unsigned int const num_of_peripheral_clocks_0 =
+	sizeof(g_peripheral_clockconfig0) /
+	sizeof(g_peripheral_clockconfig0[0]);
+
 /****************************************************************************
 * Public Functions
 ****************************************************************************/
@@ -71,10 +71,10 @@ constexpr px4_spi_bus_t px4_spi_buses[SPI_BUS_MAX_BUS_ITEMS] = {
 		initSPIDevice(SPIDEV_FLASH(0), SPI::CS{GPIO::PortD, GPIO::Pin17})
 	}),
 	initSPIBus(SPI::Bus::SPI4, {
-		initSPIDevice(DRV_IMU_DEVTYPE_ICM42688P, SPI::CS{GPIO::PortA, GPIO::Pin16}, SPI::DRDY{GPIO::PortA, GPIO::Pin15, 551})
+		initSPIDevice(DRV_IMU_DEVTYPE_ICM42688P, SPI::CS{GPIO::PortA, GPIO::Pin16}, SPI::DRDY{PIN_WKPU20})
 	}),
 	initSPIBus(SPI::Bus::SPI5, {
-		initSPIDevice(DRV_IMU_DEVTYPE_ICM20649, SPI::CS{GPIO::PortA, GPIO::Pin14}, SPI::DRDY{GPIO::PortA, GPIO::Pin13, 549})
+		initSPIDevice(DRV_IMU_DEVTYPE_ICM20649, SPI::CS{GPIO::PortA, GPIO::Pin14}, SPI::DRDY{PIN_WKPU4})
 	}),
 };

@@ -26,9 +26,11 @@ CONFIG_BCH=y
 CONFIG_BOARDCTL_RESET=y
 CONFIG_BOARD_LOOPSPERMSEC=3997
 CONFIG_BUILTIN=y
+CONFIG_DEBUG_FEATURES=y
 CONFIG_DEBUG_FULLOPT=y
 CONFIG_DEBUG_HARDFAULT_ALERT=y
 CONFIG_DEBUG_SYMBOLS=y
+CONFIG_DEBUG_TCBINFO=y
 CONFIG_DISABLE_MQUEUE=y
 CONFIG_DISABLE_POSIX_TIMERS=y
 CONFIG_EXAMPLES_HELLO=y
@@ -44,7 +46,7 @@ CONFIG_I2CTOOL_MAXADDR=0x7f
 CONFIG_I2CTOOL_MAXBUS=0
 CONFIG_I2CTOOL_MINADDR=0x00
 CONFIG_INIT_ENTRYPOINT="nsh_main"
-CONFIG_INIT_STACKSIZE=2176
+CONFIG_INIT_STACKSIZE=2276
 CONFIG_INTELHEX_BINARY=y
 CONFIG_LIBC_STRERROR=y
 CONFIG_LPI2C0_DMA=y
@@ -88,11 +88,6 @@ __BEGIN_DECLS

 /* SPI chip selects */

-/* Count of peripheral clock user configurations */
-
-#define NUM_OF_PERIPHERAL_CLOCKS_0 18
-
-
 /* High-resolution timer */
 #define HRT_TIMER              5  /* FTM timer for the HRT */
 #define HRT_TIMER_CHANNEL      0  /* Use capture/compare channel 0 */
@@ -197,7 +197,6 @@ const struct clock_configuration_s g_initial_clkconfig = {
 	},
 	.pcc             =
 	{
-		.count         = NUM_OF_PERIPHERAL_CLOCKS_0,       /* Number peripheral clock configurations */
 		.pclks         = g_peripheral_clockconfig0,        /* Peripheral clock configurations */
 	},
 	.pmc             =
@@ -200,3 +200,7 @@ const struct peripheral_clock_config_s g_peripheral_clockconfig0[] = {
 		.clkgate = true,
 	},
 };
+
+unsigned int const num_of_peripheral_clocks_0 =
+	sizeof(g_peripheral_clockconfig0) /
+	sizeof(g_peripheral_clockconfig0[0]);
@@ -83,6 +83,7 @@ CONFIG_MODULES_UUV_POS_CONTROL=y
 CONFIG_MODULES_VTOL_ATT_CONTROL=y
 CONFIG_SYSTEMCMDS_ACTUATOR_TEST=y
 CONFIG_SYSTEMCMDS_BL_UPDATE=y
+CONFIG_SYSTEMCMDS_BSONDUMP=y
 CONFIG_SYSTEMCMDS_DUMPFILE=y
 CONFIG_SYSTEMCMDS_GPIO=y
 CONFIG_SYSTEMCMDS_HARDFAULT_LOG=y
@@ -25,7 +25,6 @@ CONFIG_DRIVERS_IRLOCK=y
 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
@@ -84,6 +83,7 @@ CONFIG_MODULES_UUV_POS_CONTROL=y
 CONFIG_MODULES_VTOL_ATT_CONTROL=y
 CONFIG_SYSTEMCMDS_ACTUATOR_TEST=y
 CONFIG_SYSTEMCMDS_BL_UPDATE=y
+CONFIG_SYSTEMCMDS_BSONDUMP=y
 CONFIG_SYSTEMCMDS_DUMPFILE=y
 CONFIG_SYSTEMCMDS_GPIO=y
 CONFIG_SYSTEMCMDS_HARDFAULT_LOG=y
@@ -25,15 +25,12 @@ CONFIG_DRIVERS_IRLOCK=y
 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_PWM_INPUT=y
 CONFIG_DRIVERS_PWM_OUT=y
-CONFIG_MODULES_SIMULATION_PWM_OUT_SIM=y
 CONFIG_DRIVERS_PX4IO=y
 CONFIG_DRIVERS_ROBOCLAW=y
-CONFIG_DRIVERS_RPM=y
 CONFIG_DRIVERS_SMART_BATTERY_BATMON=y
 CONFIG_COMMON_TELEMETRY=y
 CONFIG_DRIVERS_TONE_ALARM=y
@@ -43,6 +40,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
@@ -51,6 +49,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
@@ -66,7 +65,6 @@ 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
@@ -79,10 +77,10 @@ CONFIG_MODULES_SIMULATION_SIMULATOR_SIH=y
 CONFIG_MODULES_TEMPERATURE_COMPENSATION=y
 CONFIG_MODULES_UUV_ATT_CONTROL=y
 CONFIG_MODULES_UUV_POS_CONTROL=y
-CONFIG_MODULES_GIMBAL=y
 CONFIG_MODULES_VTOL_ATT_CONTROL=y
-CONFIG_SYSTEMCMDS_BL_UPDATE=y
 CONFIG_SYSTEMCMDS_ACTUATOR_TEST=y
+CONFIG_SYSTEMCMDS_BL_UPDATE=y
+CONFIG_SYSTEMCMDS_BSONDUMP=y
 CONFIG_SYSTEMCMDS_DUMPFILE=y
 CONFIG_SYSTEMCMDS_GPIO=y
 CONFIG_SYSTEMCMDS_HARDFAULT_LOG=y
@@ -11,28 +11,25 @@ CONFIG_DRIVERS_IMU_BOSCH_BMI055=n
 CONFIG_DRIVERS_IMU_INVENSENSE_ICM20602=n
 CONFIG_DRIVERS_IMU_INVENSENSE_ICM20948=n
 CONFIG_DRIVERS_IRLOCK=n
-CONFIG_DRIVERS_OSD=n
 CONFIG_DRIVERS_PCA9685=n
 CONFIG_DRIVERS_PCA9685_PWM_OUT=n
 CONFIG_DRIVERS_POWER_MONITOR_INA226=n
 CONFIG_DRIVERS_PWM_INPUT=n
 CONFIG_DRIVERS_ROBOCLAW=n
-CONFIG_DRIVERS_RPM=n
 CONFIG_DRIVERS_SMART_BATTERY_BATMON=n
 CONFIG_DRIVERS_UAVCAN=n
 CONFIG_MODULES_ATTITUDE_ESTIMATOR_Q=n
 CONFIG_MODULES_CAMERA_FEEDBACK=n
 CONFIG_MODULES_ESC_BATTERY=n
+CONFIG_MODULES_GIMBAL=n
 CONFIG_MODULES_GYRO_CALIBRATION=n
 CONFIG_MODULES_GYRO_FFT=n
 CONFIG_MODULES_LANDING_TARGET_ESTIMATOR=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_MODULES_GIMBAL=n
 CONFIG_SYSTEMCMDS_SERIAL_TEST=n
 CONFIG_BOARD_CONSTRAINED_FLASH=y
 CONFIG_BOARD_TESTING=y
@@ -86,6 +86,7 @@ CONFIG_MODULES_UUV_POS_CONTROL=y
 CONFIG_MODULES_VTOL_ATT_CONTROL=y
 CONFIG_SYSTEMCMDS_ACTUATOR_TEST=y
 CONFIG_SYSTEMCMDS_BL_UPDATE=y
+CONFIG_SYSTEMCMDS_BSONDUMP=y
 CONFIG_SYSTEMCMDS_DMESG=y
 CONFIG_SYSTEMCMDS_DUMPFILE=y
 CONFIG_SYSTEMCMDS_GPIO=y
@@ -3,7 +3,6 @@ CONFIG_DRIVERS_IMU_ANALOG_DEVICES_ADIS16448=n
 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
@@ -6,7 +6,6 @@ CONFIG_BOARD_SERIAL_GPS2="/dev/ttyS7"
 CONFIG_BOARD_SERIAL_TEL1="/dev/ttyS6"
 CONFIG_BOARD_SERIAL_TEL2="/dev/ttyS4"
 CONFIG_BOARD_SERIAL_TEL3="/dev/ttyS1"
-CONFIG_BOARD_SERIAL_PPB="/dev/ttyS3"
 CONFIG_DRIVERS_ADC_ADS1115=y
 CONFIG_DRIVERS_ADC_BOARD_ADC=y
 CONFIG_DRIVERS_BAROMETER_BMP388=y
@@ -86,6 +85,7 @@ CONFIG_MODULES_TEMPERATURE_COMPENSATION=y
 CONFIG_MODULES_VTOL_ATT_CONTROL=y
 CONFIG_SYSTEMCMDS_ACTUATOR_TEST=y
 CONFIG_SYSTEMCMDS_BL_UPDATE=y
+CONFIG_SYSTEMCMDS_BSONDUMP=y
 CONFIG_SYSTEMCMDS_DMESG=y
 CONFIG_SYSTEMCMDS_GPIO=y
 CONFIG_SYSTEMCMDS_HARDFAULT_LOG=y
@@ -154,6 +154,7 @@ CONFIG_NET_BROADCAST=y
 CONFIG_NET_ETH_PKTSIZE=1518
 CONFIG_NET_ICMP=y
 CONFIG_NET_ICMP_SOCKET=y
+CONFIG_NET_NACTIVESOCKETS=16
 CONFIG_NET_SOLINGER=y
 CONFIG_NET_TCP=y
 CONFIG_NET_TCPBACKLOG=y
@@ -68,6 +68,7 @@ CONFIG_MODULES_TEMPERATURE_COMPENSATION=y
 CONFIG_MODULES_VTOL_ATT_CONTROL=y
 CONFIG_SYSTEMCMDS_ACTUATOR_TEST=y
 CONFIG_SYSTEMCMDS_BL_UPDATE=y
+CONFIG_SYSTEMCMDS_BSONDUMP=y
 CONFIG_SYSTEMCMDS_DMESG=y
 CONFIG_SYSTEMCMDS_HARDFAULT_LOG=y
 CONFIG_SYSTEMCMDS_I2CDETECT=y
@@ -78,6 +78,7 @@ CONFIG_MODULES_TEMPERATURE_COMPENSATION=y
 CONFIG_MODULES_VTOL_ATT_CONTROL=y
 CONFIG_SYSTEMCMDS_ACTUATOR_TEST=y
 CONFIG_SYSTEMCMDS_BL_UPDATE=y
+CONFIG_SYSTEMCMDS_BSONDUMP=y
 CONFIG_SYSTEMCMDS_DMESG=y
 CONFIG_SYSTEMCMDS_DUMPFILE=y
 CONFIG_SYSTEMCMDS_GPIO=y
@@ -6,6 +6,7 @@ CONFIG_BOARD_SERIAL_GPS2="/dev/ttyS7"
 CONFIG_BOARD_SERIAL_TEL1="/dev/ttyS6"
 CONFIG_BOARD_SERIAL_TEL2="/dev/ttyS4"
 CONFIG_BOARD_SERIAL_TEL3="/dev/ttyS1"
+CONFIG_BOARD_SERIAL_EXT2="/dev/ttyS3"
 CONFIG_DRIVERS_ADC_BOARD_ADC=y
 CONFIG_DRIVERS_BAROMETER_BMP388=y
 CONFIG_DRIVERS_BAROMETER_INVENSENSE_ICP201XX=y
@@ -73,6 +74,7 @@ CONFIG_MODULES_TEMPERATURE_COMPENSATION=y
 CONFIG_MODULES_VTOL_ATT_CONTROL=y
 CONFIG_SYSTEMCMDS_ACTUATOR_TEST=y
 CONFIG_SYSTEMCMDS_BL_UPDATE=y
+CONFIG_SYSTEMCMDS_BSONDUMP=y
 CONFIG_SYSTEMCMDS_DMESG=y
 CONFIG_SYSTEMCMDS_GPIO=y
 CONFIG_SYSTEMCMDS_HARDFAULT_LOG=y
@@ -52,21 +52,37 @@ then
 	# Internal SPI bus ICM20649
 	icm20649 -s -R 6 start
 else
-# Internal SPI BMI088
-	bmi088 -A -R 4 -s start
-	bmi088 -G -R 4 -s start
+	# Internal SPI BMI088
+	if ver hwtypecmp V6X009010 V6X010010
+	then
+		bmi088 -A -R 2 -s start
+		bmi088 -G -R 2 -s start
+	else
+		bmi088 -A -R 4 -s start
+		bmi088 -G -R 4 -s start
+	fi
 fi

 # Internal SPI bus ICM42688p
-icm42688p -R 6 -s start
+if ver hwtypecmp V6X009010 V6X010010
+then
+	icm42688p -R 4 -s start
+else
+	icm42688p -R 6 -s start
+fi

 if ver hwtypecmp V6X000003 V6X001003 V6X003003 V6X000004 V6X001004 V6X004003 V6X004004 V6X005003 V6X005004
 then
 	# Internal SPI bus ICM-42670-P (hard-mounted)
 	icm42670p -R 10 -s start
 else
-	# Internal SPI bus ICM-20649 (hard-mounted)
-	icm20649 -R 14 -s start
+	if ver hwtypecmp V6X009010 V6X010010
+	then
+		icm20602 -R 8 -s start
+	else
+		# Internal SPI bus ICM-20649 (hard-mounted)
+		icm20649 -R 14 -s start
+	fi
 fi

 # Internal magnetometer on I2c
@@ -74,7 +90,7 @@ if ver hwtypecmp V6X002001
 then
 	rm3100 -I -b 4 start
 else
-	bmm150 -I start
+	bmm150 -I -R 6 start
 fi

 # External compass on GPS1/I2C1 (the 3rd external bus): standard Holybro Pixhawk 4 or CUAV V5 GPS/compass puck (with lights, safety button, and buzzer)
@@ -88,7 +104,7 @@ else
 	bmp388 -I -a 0x77 start
 fi

-if ver hwtypecmp V6X000000 V6X001000
+if param compare SENS_INT_BARO_EN 1
 then
 	bmp388 -I start
 else
@@ -98,17 +98,17 @@ CONFIG_STM32H7_OTGFS=y
 CONFIG_STM32H7_PROGMEM=y
 CONFIG_STM32H7_SERIAL_DISABLE_REORDERING=y
 CONFIG_STM32H7_TIM1=y
-CONFIG_STM32H7_UART7=y
+CONFIG_STM32H7_UART5=y
 CONFIG_SYSTEMTICK_HOOK=y
 CONFIG_SYSTEM_CDCACM=y
 CONFIG_TASK_NAME_SIZE=24
 CONFIG_TTY_SIGINT=y
 CONFIG_TTY_SIGINT_CHAR=0x03
 CONFIG_TTY_SIGTSTP=y
-CONFIG_UART7_RXBUFSIZE=512
-CONFIG_UART7_RXDMA=y
-CONFIG_UART7_TXBUFSIZE=512
-CONFIG_UART7_TXDMA=y
+CONFIG_UART5_RXBUFSIZE=512
+CONFIG_UART5_RXDMA=y
+CONFIG_UART5_TXBUFSIZE=512
+CONFIG_UART5_TXDMA=y
 CONFIG_USBDEV=y
 CONFIG_USBDEV_BUSPOWERED=y
 CONFIG_USBDEV_MAXPOWER=500
@@ -158,6 +158,7 @@ CONFIG_NET_BROADCAST=y
 CONFIG_NET_ETH_PKTSIZE=1518
 CONFIG_NET_ICMP=y
 CONFIG_NET_ICMP_SOCKET=y
+CONFIG_NET_NACTIVESOCKETS=16
 CONFIG_NET_SOLINGER=y
 CONFIG_NET_TCP=y
 CONFIG_NET_TCPBACKLOG=y
@@ -215,8 +215,7 @@
 #define GPIO_HW_VER_SENSE      /* PH3 */  GPIO_ADC3_INP14
 #define HW_INFO_INIT_PREFIX    "V6X"

-#define BOARD_NUM_SPI_CFG_HW_VERSIONS 9 // Rev 0 and Rev 3,4 Sensor sets
-
+#define BOARD_NUM_SPI_CFG_HW_VERSIONS 11 // Rev 0 and Rev 3,4 Sensor sets
 //                 Base/FMUM
 #define V6X00   HW_VER_REV(0x0,0x0) // FMUV6X,                 Rev 0
 #define V6X01   HW_VER_REV(0x0,0x1) // FMUV6X,     BMI388 I2C2 Rev 1
@@ -234,6 +233,11 @@
 #define V6X51   HW_VER_REV(0x5,0x1) // FMUV6X,     BMI388 I2C2    HB Mini Rev 1
 #define V6X53   HW_VER_REV(0x5,0x3) // FMUV6X,     Sensor Set     HB Mini Rev 3
 #define V6X54   HW_VER_REV(0x5,0x4) // FMUV6X,     Sensor Set     HB Mini Rev 4
+#define V6X90   HW_VER_REV(0x9,0x0) //                         Rev 0
+#define V6X0910   HW_VER_REV(0x9,0x10)  // FMUV6X,     rev from EEPROM     Auterion Skynode ver9
+#define V6X1010   HW_VER_REV(0x10,0x10) // FMUV6X,     rev from EEPROM     Auterion Skynode ver10
+
+

 #define UAVCAN_NUM_IFACES_RUNTIME  1

@@ -155,6 +155,8 @@ static px4_hw_mft_list_entry_t mft_lists[] = {
 	{V6X04, hw_mft_list_v0600, arraySize(hw_mft_list_v0600)}, // BMP388 moved to I2C2, Sensor Set 4
 	{V6X14, hw_mft_list_v0610, arraySize(hw_mft_list_v0610)}, // No PX4IO BMP388 moved to I2C2, Sensor Set 4
 	{V6X21, hw_mft_list_v0600, arraySize(hw_mft_list_v0600)},
+	{V6X0910, hw_mft_list_v0600, arraySize(hw_mft_list_v0600)}, // FMUV6X,     rev from EEPROM     Auterion Skynode ver9
+	{V6X1010, hw_mft_list_v0600, arraySize(hw_mft_list_v0600)}, // FMUV6X,     rev from EEPROM     Auterion Skynode ver10
 };

 /************************************************************************************
@@ -270,6 +270,52 @@ constexpr px4_spi_bus_all_hw_t px4_spi_buses_all_hw[BOARD_NUM_SPI_CFG_HW_VERSION
 			initSPIConfigExternal(SPI::CS{GPIO::PortA, GPIO::Pin15}, SPI::DRDY{GPIO::PortD, GPIO::Pin12}),
 		}),
 	}),
+	initSPIHWVersion(V6X0910, {
+		initSPIBus(SPI::Bus::SPI1, {
+			initSPIDevice(DRV_IMU_DEVTYPE_ICM20602, SPI::CS{GPIO::PortI, GPIO::Pin9}, SPI::DRDY{GPIO::PortF, GPIO::Pin2}),
+		}, {GPIO::PortI, GPIO::Pin11}),
+		initSPIBus(SPI::Bus::SPI2, {
+			initSPIDevice(DRV_IMU_DEVTYPE_ICM42688P, SPI::CS{GPIO::PortH, GPIO::Pin5}, SPI::DRDY{GPIO::PortA, GPIO::Pin10}),
+		}, {GPIO::PortF, GPIO::Pin4}),
+		initSPIBus(SPI::Bus::SPI3, {
+			initSPIDevice(DRV_GYR_DEVTYPE_BMI088, SPI::CS{GPIO::PortI, GPIO::Pin8}, SPI::DRDY{GPIO::PortI, GPIO::Pin7}),
+			initSPIDevice(DRV_ACC_DEVTYPE_BMI088, SPI::CS{GPIO::PortI, GPIO::Pin4}),
+		}, {GPIO::PortE, GPIO::Pin7}),
+		//  initSPIBus(SPI::Bus::SPI4, {
+		//    // no devices
+		// TODO: if enabled, remove GPIO_VDD_3V3_SENSORS4_EN from board_config.h
+		//  }, {GPIO::PortG, GPIO::Pin8}),
+		initSPIBus(SPI::Bus::SPI5, {
+			initSPIDevice(SPIDEV_FLASH(0), SPI::CS{GPIO::PortG, GPIO::Pin7})
+		}),
+		initSPIBusExternal(SPI::Bus::SPI6, {
+			initSPIConfigExternal(SPI::CS{GPIO::PortI, GPIO::Pin10}, SPI::DRDY{GPIO::PortD, GPIO::Pin11}),
+			initSPIConfigExternal(SPI::CS{GPIO::PortA, GPIO::Pin15}, SPI::DRDY{GPIO::PortD, GPIO::Pin12}),
+		}),
+	}),
+	initSPIHWVersion(V6X1010, {
+		initSPIBus(SPI::Bus::SPI1, {
+			initSPIDevice(DRV_IMU_DEVTYPE_ICM20602, SPI::CS{GPIO::PortI, GPIO::Pin9}, SPI::DRDY{GPIO::PortF, GPIO::Pin2}),
+		}, {GPIO::PortI, GPIO::Pin11}),
+		initSPIBus(SPI::Bus::SPI2, {
+			initSPIDevice(DRV_IMU_DEVTYPE_ICM42688P, SPI::CS{GPIO::PortH, GPIO::Pin5}, SPI::DRDY{GPIO::PortA, GPIO::Pin10}),
+		}, {GPIO::PortF, GPIO::Pin4}),
+		initSPIBus(SPI::Bus::SPI3, {
+			initSPIDevice(DRV_GYR_DEVTYPE_BMI088, SPI::CS{GPIO::PortI, GPIO::Pin8}, SPI::DRDY{GPIO::PortI, GPIO::Pin7}),
+			initSPIDevice(DRV_ACC_DEVTYPE_BMI088, SPI::CS{GPIO::PortI, GPIO::Pin4}),
+		}, {GPIO::PortE, GPIO::Pin7}),
+		//  initSPIBus(SPI::Bus::SPI4, {
+		//    // no devices
+		// TODO: if enabled, remove GPIO_VDD_3V3_SENSORS4_EN from board_config.h
+		//  }, {GPIO::PortG, GPIO::Pin8}),
+		initSPIBus(SPI::Bus::SPI5, {
+			initSPIDevice(SPIDEV_FLASH(0), SPI::CS{GPIO::PortG, GPIO::Pin7})
+		}),
+		initSPIBusExternal(SPI::Bus::SPI6, {
+			initSPIConfigExternal(SPI::CS{GPIO::PortI, GPIO::Pin10}, SPI::DRDY{GPIO::PortD, GPIO::Pin11}),
+			initSPIConfigExternal(SPI::CS{GPIO::PortA, GPIO::Pin15}, SPI::DRDY{GPIO::PortD, GPIO::Pin12}),
+		}),
+	}),
 };

 static constexpr bool unused = validateSPIConfig(px4_spi_buses_all_hw);
@@ -1,5 +1,5 @@
 CONFIG_PLATFORM_POSIX=y
-CONFIG_BOARD_LINUX=y
+CONFIG_BOARD_LINUX_TARGET=y
 CONFIG_BOARD_TOOLCHAIN="arm-linux-gnueabihf"
 CONFIG_BOARD_ARCHITECTURE="cortex-a53"
 CONFIG_BOARD_TESTING=y
@@ -62,6 +62,7 @@ CONFIG_MODULES_TEMPERATURE_COMPENSATION=y
 CONFIG_MODULES_UUV_ATT_CONTROL=y
 CONFIG_MODULES_UUV_POS_CONTROL=y
 CONFIG_MODULES_VTOL_ATT_CONTROL=y
+CONFIG_SYSTEMCMDS_BSONDUMP=y
 CONFIG_SYSTEMCMDS_DYN=y
 CONFIG_SYSTEMCMDS_LED_CONTROL=y
 CONFIG_SYSTEMCMDS_PARAM=y
@@ -49,6 +49,7 @@ CONFIG_MODULES_UUV_ATT_CONTROL=y
 CONFIG_MODULES_UUV_POS_CONTROL=y
 CONFIG_MODULES_VTOL_ATT_CONTROL=y
 CONFIG_SYSTEMCMDS_ACTUATOR_TEST=y
+CONFIG_SYSTEMCMDS_BSONDUMP=y
 CONFIG_SYSTEMCMDS_DYN=y
 CONFIG_SYSTEMCMDS_FAILURE=y
 CONFIG_SYSTEMCMDS_LED_CONTROL=y
@@ -1,5 +1,5 @@
 CONFIG_PLATFORM_POSIX=y
-CONFIG_BOARD_LINUX=y
+CONFIG_BOARD_LINUX_TARGET=y
 CONFIG_BOARD_TOOLCHAIN="arm-linux-gnueabihf"
 CONFIG_BOARD_ARCHITECTURE="cortex-a53"
 CONFIG_BOARD_TESTING=y
@@ -59,6 +59,7 @@ CONFIG_MODULES_SIMULATION_SIMULATOR_SIH=y
 CONFIG_MODULES_TEMPERATURE_COMPENSATION=y
 CONFIG_MODULES_GIMBAL=y
 CONFIG_MODULES_VTOL_ATT_CONTROL=y
+CONFIG_SYSTEMCMDS_BSONDUMP=y
 CONFIG_SYSTEMCMDS_DYN=y
 CONFIG_SYSTEMCMDS_LED_CONTROL=y
 CONFIG_SYSTEMCMDS_PARAM=y
@@ -6,7 +6,7 @@ CONFIG_BOARD_SERIAL_GPS2="/dev/ttyS7"
 CONFIG_BOARD_SERIAL_TEL1="/dev/ttyS6"
 CONFIG_BOARD_SERIAL_TEL2="/dev/ttyS4"
 CONFIG_BOARD_SERIAL_TEL3="/dev/ttyS1"
-CONFIG_BOARD_SERIAL_PPB="/dev/ttyS3"
+CONFIG_BOARD_SERIAL_EXT2="/dev/ttyS3"
 CONFIG_DRIVERS_ADC_BOARD_ADC=y
 CONFIG_COMMON_BAROMETERS=y
 CONFIG_DRIVERS_BATT_SMBUS=y
@@ -83,6 +83,7 @@ CONFIG_MODULES_TEMPERATURE_COMPENSATION=y
 CONFIG_MODULES_VTOL_ATT_CONTROL=y
 CONFIG_SYSTEMCMDS_ACTUATOR_TEST=y
 CONFIG_SYSTEMCMDS_BL_UPDATE=y
+CONFIG_SYSTEMCMDS_BSONDUMP=y
 CONFIG_SYSTEMCMDS_DMESG=y
 CONFIG_SYSTEMCMDS_DUMPFILE=y
 CONFIG_SYSTEMCMDS_GPIO=y
@@ -78,6 +78,7 @@ CONFIG_MODULES_UUV_POS_CONTROL=y
 CONFIG_MODULES_VTOL_ATT_CONTROL=y
 CONFIG_SYSTEMCMDS_ACTUATOR_TEST=y
 CONFIG_SYSTEMCMDS_BL_UPDATE=y
+CONFIG_SYSTEMCMDS_BSONDUMP=y
 CONFIG_SYSTEMCMDS_DUMPFILE=y
 CONFIG_SYSTEMCMDS_GPIO=y
 CONFIG_SYSTEMCMDS_HARDFAULT_LOG=y
@@ -78,6 +78,7 @@ CONFIG_MODULES_UUV_POS_CONTROL=y
 CONFIG_MODULES_VTOL_ATT_CONTROL=y
 CONFIG_SYSTEMCMDS_ACTUATOR_TEST=y
 CONFIG_SYSTEMCMDS_BL_UPDATE=y
+CONFIG_SYSTEMCMDS_BSONDUMP=y
 CONFIG_SYSTEMCMDS_DUMPFILE=y
 CONFIG_SYSTEMCMDS_GPIO=y
 CONFIG_SYSTEMCMDS_HARDFAULT_LOG=y
@@ -33,10 +33,12 @@ 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_EVENTS=y
 CONFIG_MODULES_FLIGHT_MODE_MANAGER=y
+CONFIG_MODULES_GIMBAL=y
 CONFIG_MODULES_GYRO_CALIBRATION=y
 CONFIG_MODULES_GYRO_FFT=y
 CONFIG_MODULES_LAND_DETECTOR=y
@@ -52,7 +54,6 @@ 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_SENSORS=y
@@ -62,9 +63,9 @@ CONFIG_MODULES_SIMULATION_SENSOR_GPS_SIM=y
 CONFIG_MODULES_SIMULATION_SENSOR_MAG_SIM=y
 CONFIG_MODULES_SIMULATION_SIMULATOR_SIH=y
 CONFIG_MODULES_TEMPERATURE_COMPENSATION=y
-CONFIG_MODULES_GIMBAL=y
-CONFIG_SYSTEMCMDS_BL_UPDATE=y
 CONFIG_SYSTEMCMDS_ACTUATOR_TEST=y
+CONFIG_SYSTEMCMDS_BL_UPDATE=y
+CONFIG_SYSTEMCMDS_BSONDUMP=y
 CONFIG_SYSTEMCMDS_DMESG=y
 CONFIG_SYSTEMCMDS_DUMPFILE=y
 CONFIG_SYSTEMCMDS_GPIO=y
@@ -301,8 +301,8 @@ if(EXISTS ${BOARD_DEFCONFIG})
 	if(SERIAL_WIFI)
        list(APPEND board_serial_ports WIFI:${SERIAL_WIFI})
 	endif()
-	if(SERIAL_PPB)
-        list(APPEND board_serial_ports PPB:${SERIAL_PPB})
+	if(SERIAL_EXT2)
+        list(APPEND board_serial_ports EXT2:${SERIAL_EXT2})
 	endif()


@@ -362,7 +362,7 @@ if(EXISTS ${BOARD_DEFCONFIG})
        add_definitions( ${COMPILE_DEFINITIONS})
 	endif()

-	if(LINUX)
+	if(LINUX_TARGET)
        add_definitions( "-D__PX4_LINUX" )
 	endif()

@@ -182,8 +182,8 @@ set(msg_files
 	TrajectoryWaypoint.msg
 	TransponderReport.msg
 	TuneControl.msg
-	UavcanParameterRequest.msg
-	UavcanParameterValue.msg
+	ParameterRequest.msg
+	ParameterValue.msg
 	UlogStream.msg
 	UlogStreamAck.msg
 	UwbDistance.msg
@@ -219,6 +219,14 @@ set(msg_files
 	WheelEncoders.msg
 	Wind.msg
 	YawEstimatorStatus.msg
+
+        Parameter.msg
+
+        SrvParameterGetRequest.msg
+        SrvParameterGetResponse.msg
+
+        SrvParameterSetRequest.msg
+        SrvParameterSetResponse.msg
 )
 list(SORT msg_files)

@@ -0,0 +1,12 @@
+char[17] name                   # parameter name
+int16 index		# -1 if the param_id field should be used as identifier
+int16 index_used		# -1 if the param_id field should be used as identifier
+
+uint8 TYPE_INVALID = 0
+uint8 TYPE_BOOL    = 1
+uint8 TYPE_INT32   = 2
+uint8 TYPE_FLOAT32 = 3
+uint8 type                # parameter type
+
+int32 int32_value               # current value if param_type is int-like
+float32 float32_value           # current value if param_type is float-like
@@ -0,0 +1,27 @@
+# parameter request type
+uint64 timestamp		# time since system start (microseconds)
+
+uint8 MESSAGE_TYPE_INVALID            = 0
+uint8 MESSAGE_TYPE_PARAM_REQUEST_READ = 1
+uint8 MESSAGE_TYPE_PARAM_REQUEST_LIST = 2
+uint8 MESSAGE_TYPE_PARAM_SET          = 3
+uint8 message_type                        # message type
+
+uint8 NODE_ID_ALL = 0		# MAV_COMP_ID_ALL
+uint8 node_id			# node ID
+
+char[17] name                   # parameter name
+int16 param_index		# -1 if the param_id field should be used as identifier
+
+uint8 TYPE_INVALID = 0
+uint8 TYPE_BOOL    = 1
+uint8 TYPE_INT32   = 2
+uint8 TYPE_FLOAT32 = 3
+uint8 type                # parameter type
+
+int32 int32_value               # current value if param_type is int-like
+float32 float32_value           # current value if param_type is float-like
+
+uint8 ORB_QUEUE_LENGTH = 8
+
+# TOPICS parameter_request uavcan_parameter_request
@@ -4,11 +4,6 @@ uint64 timestamp		# time since system start (microseconds)

 uint32 instance		# Instance count - constantly incrementing

-uint32 get_count
-uint32 set_count
-uint32 find_count
-uint32 export_count
+uint16 count

-uint16 active
-uint16 changed
-uint16 custom_default
+Parameter changed_param
@@ -0,0 +1,22 @@
+# parameter response type
+uint64 timestamp		# time since system start (microseconds)
+
+uint8 node_id			# requester node ID
+
+char[17] param_name		# parameter name
+
+int16 param_index		# parameter index, if known
+
+uint16 param_count		# number of parameters exposed by the node
+
+uint8 type                      # parameter type
+
+int32 int32_value               # current value if param_type is int-like
+float32 float32_value           # current value if param_type is float-like
+
+
+uint64 timestamp_requested
+
+uint8 ORB_QUEUE_LENGTH = 8
+
+# TOPICS parameter_value uavcan_parameter_value
@@ -0,0 +1,6 @@
+uint64 timestamp              # time since system start (microseconds)
+
+char[17] name                 # parameter name
+int16 index                   # -1 if the param_id field should be used as identifier
+
+uint8 ORB_QUEUE_LENGTH = 16
@@ -0,0 +1,11 @@
+uint64 timestamp              # time since system start (microseconds)
+uint64 timestamp_requested
+
+Parameter parameter
+
+uint8 RESULT_GET_SUCCESS             = 0
+uint8 RESULT_GET_FAILED              = 1
+uint8 RESULT_ERROR_INVALID_PARAMETER = 2
+uint8 result
+
+uint8 ORB_QUEUE_LENGTH = 16
@@ -0,0 +1,5 @@
+uint64 timestamp              # time since system start (microseconds)
+
+Parameter parameter
+
+uint8 ORB_QUEUE_LENGTH = 8
@@ -0,0 +1,11 @@
+uint64 timestamp              # time since system start (microseconds)
+uint64 timestamp_requested
+
+Parameter parameter
+
+uint8 RESULT_SET_SUCCESS             = 0
+uint8 RESULT_SET_FAILED              = 1
+uint8 RESULT_ERROR_INVALID_PARAMETER = 2
+uint8 result
+
+uint8 ORB_QUEUE_LENGTH = 8
@@ -1,23 +0,0 @@
-# UAVCAN-MAVLink parameter bridge request type
-uint64 timestamp		# time since system start (microseconds)
-
-uint8 MESSAGE_TYPE_PARAM_REQUEST_READ = 20	# MAVLINK_MSG_ID_PARAM_REQUEST_READ
-uint8 MESSAGE_TYPE_PARAM_REQUEST_LIST = 21	# MAVLINK_MSG_ID_PARAM_REQUEST_LIST
-uint8 MESSAGE_TYPE_PARAM_SET = 23		# MAVLINK_MSG_ID_PARAM_SET
-uint8 message_type				# MAVLink message type: PARAM_REQUEST_READ, PARAM_REQUEST_LIST, PARAM_SET
-
-uint8 NODE_ID_ALL = 0		# MAV_COMP_ID_ALL
-uint8 node_id			# UAVCAN node ID mapped from MAVLink component ID
-
-char[17] param_id		# MAVLink/UAVCAN parameter name
-int16 param_index		# -1 if the param_id field should be used as identifier
-
-uint8 PARAM_TYPE_UINT8 = 1	# MAV_PARAM_TYPE_UINT8
-uint8 PARAM_TYPE_INT64 = 8	# MAV_PARAM_TYPE_INT64
-uint8 PARAM_TYPE_REAL32 = 9	# MAV_PARAM_TYPE_REAL32
-uint8 param_type		# MAVLink parameter type
-
-int64 int_value			# current value if param_type is int-like
-float32 real_value		# current value if param_type is float-like
-
-uint8 ORB_QUEUE_LENGTH = 4
@@ -1,9 +0,0 @@
-# UAVCAN-MAVLink parameter bridge response type
-uint64 timestamp		# time since system start (microseconds)
-uint8 node_id			# UAVCAN node ID mapped from MAVLink component ID
-char[17] param_id		# MAVLink/UAVCAN parameter name
-int16 param_index		# parameter index, if known
-uint16 param_count		# number of parameters exposed by the node
-uint8 param_type		# MAVLink parameter type
-int64 int_value			# current value if param_type is int-like
-float32 real_value		# current value if param_type is float-like
@@ -118,7 +118,6 @@ public:

 	Param()
 	{
-		param_set_used(handle());
 		update();
 	}

@@ -146,12 +145,6 @@ public:

 	void set(float val) { _val = val; }

-	void reset()
-	{
-		param_reset_no_notification(handle());
-		update();
-	}
-
 	bool update() { return param_get(handle(), &_val) == 0; }

 	param_t handle() const { return param_handle(p); }
@@ -170,7 +163,6 @@ public:
 	Param(float &external_val)
 		: _val(external_val)
 	{
-		param_set_used(handle());
 		update();
 	}

@@ -198,12 +190,6 @@ public:

 	void set(float val) { _val = val; }

-	void reset()
-	{
-		param_reset_no_notification(handle());
-		update();
-	}
-
 	bool update() { return param_get(handle(), &_val) == 0; }

 	param_t handle() const { return param_handle(p); }
@@ -220,7 +206,6 @@ public:

 	Param()
 	{
-		param_set_used(handle());
 		update();
 	}

@@ -248,12 +233,6 @@ public:

 	void set(int32_t val) { _val = val; }

-	void reset()
-	{
-		param_reset_no_notification(handle());
-		update();
-	}
-
 	bool update() { return param_get(handle(), &_val) == 0; }

 	param_t handle() const { return param_handle(p); }
@@ -272,7 +251,6 @@ public:
 	Param(int32_t &external_val)
 		: _val(external_val)
 	{
-		param_set_used(handle());
 		update();
 	}

@@ -300,12 +278,6 @@ public:

 	void set(int32_t val) { _val = val; }

-	void reset()
-	{
-		param_reset_no_notification(handle());
-		update();
-	}
-
 	bool update() { return param_get(handle(), &_val) == 0; }

 	param_t handle() const { return param_handle(p); }
@@ -322,7 +294,6 @@ public:

 	Param()
 	{
-		param_set_used(handle());
 		update();
 	}

@@ -358,12 +329,6 @@ public:

 	void set(bool val) { _val = val; }

-	void reset()
-	{
-		param_reset_no_notification(handle());
-		update();
-	}
-
 	bool update()
 	{
 		int32_t value_int;
--- a/Show More
+++ b/Show More
Author	SHA1	Message	Date
Daniel Agar	5247db7647	parameters: new parameter server backend - parameter backend is now running in a wq and accessible over pub/sub - parameters library can call into backend directly (with locking), but operations will gradually start moving to pub/sub - majority of parameters migrated as is, only small changes to parameter autosave and parameter notification that are now performed from the WQ thread	2022-12-20 09:51:48 -05:00
Daniel Agar	278bce7446	systemcmds/bsondump: new command line utility (extracted from parameters)	2022-12-19 21:46:37 -05:00
Daniel Agar	b6157c89a0	parameters: eliminate param_reset(param_t param) from public C API	2022-12-19 16:50:09 -05:00
Peter van der Perk	b8e07cc52c	Fix Ethernet Socket Defaults	2022-12-19 13:24:55 -05:00
Silvan Fuhrer	9c66f1b14a	AirspeedValidator: Remvoe airspeed variance after boot check (#20678 ) This checks was introduced to catch the (unlikely) case of the driver publishing a stale value that is not actually from a current measurement right after boot. This was done by declaring it invalid if there is no update of the measurement within the first 10s after boot. Practice though has shown that around 0, many airspeed sensors have such a low resolution that the reported airspeed value is often at the exact same value for longer periods of time on totally healthy sensors, and thus we trigger some false positive failure detections. Given that this failure mode (driver publishing stale data) is quite rare (if it doesn't receive new data it should stop publishing), I remove this check here again. When in aerodynamic flight mode and armed, there is still the data stuck check that can trigger if there is no update for 2s. Signed-off-by: Silvan Fuhrer <silvan@auterion.com>	2022-12-19 11:36:48 +01:00
Igor Misic	df441ac202	drivers/gps: add param for enabling protocols at i2c interface	2022-12-19 08:52:36 +01:00
tanja	e5255b173a	v6x: use param SENS_INT_BARO_EN to start internal bmp388	2022-12-19 08:50:31 +01:00
tanja	987de56af2	v6x: rev10 has second PM	2022-12-19 08:50:31 +01:00
tanja	09039faf0a	v6x: change bootloader UART7 to UART5	2022-12-19 08:50:31 +01:00
Eric Katzfey	b0580d88e1	Added muorb aggregator from DSP to Apps direction	2022-12-16 19:43:00 -05:00
Peter van der Perk	d945e87e4f	S32K3 DRDY less ambigious	2022-12-16 12:02:44 -05:00
Peter van der Perk	05dd43a8de	S32K3 Correct pinirq pinset for external interrupt	2022-12-16 12:02:44 -05:00
RomanBapst	ffdf186598	vtol_att_control: reset transition timer states when starting a transition Signed-off-by: RomanBapst <bapstroman@gmail.com>	2022-12-16 11:36:00 +03:00
frederictaillandier	84c5ce3a53	removing catkin sitl_gazebo submodule tests as it has also been removed in PX4-Autopilot Updating the submodule https://github.com/PX4/PX4-SITL_gazebo/pull/934	2022-12-16 08:11:28 +01:00
JaeyoungLim	9db133b13d	Remove angular velocity controller module	2022-12-16 07:52:20 +01:00
Peter van der Perk	52c0cba24b	NuttX with TXAVAL merged nxp_ucans32k146:Use txavail NuttX Fix Stack Size Expose some K1 debug features	2022-12-15 08:29:10 -05:00
RomanBapst	48e8477799	vtol_att_control: disable maximum height for quadchute by default Signed-off-by: RomanBapst <bapstroman@gmail.com>	2022-12-15 15:40:53 +03:00
RomanBapst	9ed51ba8ed	quadchute: introduced parameter to specify maximum height Signed-off-by: RomanBapst <bapstroman@gmail.com>	2022-12-15 15:40:53 +03:00
Peter van der Perk	f3fe10f63e	S32K Dynamic periphclocks	2022-12-14 10:44:42 -05:00
Marcell Rausch	6c7702b906	mavlink: Keep sending GPS_RAW_INT/GPS2_RAW.hpp streams on GPS failure * GPS_RAW_INT: Only send no gps messages if gps has ever been present * GPS2_RAW: Keep in sync with GPS_RAW_INT Signed-off-by: Marcell Rausch <marcell@auterion.com>	2022-12-14 09:12:39 -05:00
David Sidrane	96362bfb52	nxp_mr-canhubk3 Add PROBEs for Debugging	2022-12-14 07:34:23 -05:00
Beat Küng	5217bedd4b	commander: make SYS_HAS_MAG a count param and ensure system has N calibrated + enabled mags	2022-12-14 07:55:04 +01:00
Tanja Baumann	d75e61eef6	v6x: fix rc.board_sensors ver command (#20763 )	2022-12-14 07:53:12 +01:00
Daniel Agar	4d318ebd30	mavlink: add initial mavlink OPEN_DRONE_ID_SYSTEM stream	2022-12-13 19:39:27 -05:00
Daniel Agar	696eeb9a49	ekf2: update EV height state machine (small piece of EV overhaul) - respect new EKF2_EV_CTRL parameter for VPOS usage - EV hgt rotate EV position before usage (there's often a small offset in frames) - EV hgt reset use proper EV velocity body frame - try to keep EV hgt and EV vel state machines consistent - small incremental piece of https://github.com/PX4/PX4-Autopilot/pull/19128	2022-12-13 13:29:18 -05:00
Daniel Agar	805ffa9d0b	ekf2: push mag cal reset to delayed time horizon	2022-12-13 10:24:02 -05:00
David Sidrane	57b82af3a0	Hard fault on Port E config (CAN0) due to not sizing the table correctly @PetervdPerk-NXP bit by the non-auto sizing table bug again! Symptom: Hard fault on Port E config (CAN0)	2022-12-13 10:09:40 -05:00
PX4 BuildBot	8b02b6b661	Update submodule mavlink to latest Tue Dec 13 12:38:57 UTC 2022 - mavlink in PX4/Firmware (8e1e5c4faa7749f6bc16650c7452af249a7dcc3a): https://github.com/mavlink/mavlink/commit/081120844300d68e90c8c4d40ab4662cb3eb604c - mavlink current upstream: https://github.com/mavlink/mavlink/commit/0c7792edfeee7cf08e531cb4ce5b4f01854e5724 - Changes: https://github.com/mavlink/mavlink/compare/081120844300d68e90c8c4d40ab4662cb3eb604c...0c7792edfeee7cf08e531cb4ce5b4f01854e5724 0c7792ed 2022-12-08 olliw42 - Gimbal Device: Adding more invalid and bitmask attributes (II) (#1927) 771926e6 2022-12-07 olliw42 - gimbal manager cap flags: resolve duplication mistake 2bc32222 2022-12-07 olliw42 - add gimbal device flags to low word of gimbal manager flags (#1928)	2022-12-13 09:06:14 -05:00
Beat Küng	685d5cb473	fix kconfig: rename LINUX to LINUX_TARGET LINUX is defined by cmake >= 3.25: https://cmake.org/cmake/help/latest/variable/LINUX.html	2022-12-13 09:05:18 -05:00
Roman Bapst	257d4e473b	fixed tailsitter transitions (#20756 ) Signed-off-by: RomanBapst <bapstroman@gmail.com>	2022-12-13 10:38:38 +01:00
tanja	aba11ce920	rename all instances of serial PPB to EXT2	2022-12-13 08:09:15 +01:00
tanja	ba344231d2	v6x: configure SPI busses for board 0x010	2022-12-13 08:09:15 +01:00
tanja	ed0f602ce8	v6x: Adapt sensor set for board 0x010 upstream	2022-12-13 08:09:15 +01:00
tanja	aec4b93527	v6x: Add EXT2 instance on ttyS3	2022-12-13 08:09:15 +01:00
Eric Katzfey	e17ddcc0e5	Qurt platform custom icm42688p IMU driver (#20753 ) - first version of IMU driver for the VOXL 2 platform (Qurt) - this is a customized version of the Invensense ICM42688P driver, it is currently in the VOXL 2 board directory	2022-12-12 22:02:23 -05:00
Peter van der Perk	33e39d68f7	DroneCAN SocketCAN driver add FMU support	2022-12-12 20:06:13 -05:00
Eric Katzfey	9b3feee6ee	worker and HRT threads for Qurt platform (#20739 ) * Getting work manager and hrt threads ready for Qurt platform	2022-12-12 14:25:28 -05:00
Daniel Agar	dc0823062e	github actions: disable vtol_standard address sanitizer test - unfortunately this is often too slow for github actions	2022-12-12 14:23:13 -05:00
bresch	41abf695a8	ekf2: use more efficient covariance update computation KSK' is faster to compute than KHP and is mathematically equivalent	2022-12-12 12:31:54 -05:00
Daniel Agar	61fa28c0d6	Tools/generate_board_targets_json.py exclude px4_ros2_default for now	2022-12-12 11:46:36 -05:00
dirksavage88	5b667cf4ba	Uavcannode hygrometer support: temp and humidity Signed-off-by: dirksavage88 <dirksavage88@gmail.com>	2022-12-12 10:54:08 -05:00
Eric Katzfey	a5e4295029	lib/drivers: device drivers library for Qurt platform (#20741 )	2022-12-10 19:31:06 -05:00
Noah Bliss	d8bfee517a	boards: beaglebone blue point to a newer version of librobotcontrol (#20740 ) - Fixes PRU path issues found in old versions of the library.	2022-12-10 17:27:32 -05:00