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-27 21:40:06 +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
536 changed files with 32304 additions and 12411 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

@@ -47,6 +47,8 @@ GTAGS
 *.files
 *.includes

+Tools/jlink-nuttx.so
+
 # CLion ignores
 .idea
 cmake-build-*/
@@ -21,11 +21,11 @@
 [submodule "platforms/nuttx/NuttX/nuttx"]
 	path = platforms/nuttx/NuttX/nuttx
 	url = https://github.com/PX4/NuttX.git
-	branch = px4_firmware_nuttx-10.1.0+
+	branch = px4_firmware_nuttx-10.3.0+
 [submodule "platforms/nuttx/NuttX/apps"]
 	path = platforms/nuttx/NuttX/apps
 	url = https://github.com/PX4/NuttX-apps.git
-	branch = px4_firmware_nuttx-10.1.0+
+	branch = px4_firmware_nuttx-10.3.0+
 [submodule "Tools/flightgear_bridge"]
 	path = Tools/simulation/flightgear/flightgear_bridge
 	url = https://github.com/PX4/PX4-FlightGear-Bridge.git
@@ -133,6 +133,5 @@
        "workbench.settings.enableNaturalLanguageSearch": false,
        "yaml.schemas": {
                "${workspaceFolder}/validation/module_schema.yaml": "${workspaceFolder}/src/modules/*/module.yaml"
-        },
-        "cortex-debug.openocdPath": "${env:PICO_SDK_PATH}/../openocd/src/openocd"       // Added for rp2040
+        }
 }
@@ -1,6 +1,6 @@
 ############################################################################
 #
-# Copyright (c) 2017 - 2019 PX4 Development Team. All rights reserved.
+# Copyright (c) 2017 - 2022 PX4 Development Team. All rights reserved.
 #
 # Redistribution and use in source and binary forms, with or without
 # modification, are permitted provided that the following conditions
@@ -143,25 +143,17 @@ define_property(GLOBAL PROPERTY PX4_SRC_FILES
 # configuration
 #

-set(CONFIG "px4_sitl_default" CACHE STRING "desired configuration")
-
 include(px4_add_module)
 set(config_module_list)
 set(config_kernel_list)

 # Find Python
-# If using catkin, Python 2 is found since it points
-# to the Python libs installed with the ROS distro
-if (NOT CATKIN_DEVEL_PREFIX)
-	find_package(PythonInterp 3)
-	# We have a custom error message to tell users how to install python3.
-	if (NOT PYTHONINTERP_FOUND)
-		message(FATAL_ERROR "Python 3 not found. Please install Python 3:\n"
-			"    Ubuntu: sudo apt install python3 python3-dev python3-pip\n"
-			"    macOS: brew install python")
-	endif()
-else()
-	find_package(PythonInterp REQUIRED)
+find_package(PythonInterp 3)
+# We have a custom error message to tell users how to install python3.
+if(NOT PYTHONINTERP_FOUND)
+	message(FATAL_ERROR "Python 3 not found. Please install Python 3:\n"
+		"    Ubuntu: sudo apt install python3 python3-dev python3-pip\n"
+		"    macOS: brew install python")
 endif()

 option(PYTHON_COVERAGE "Python code coverage" OFF)
@@ -207,6 +199,11 @@ if(EXISTS "${PX4_SOURCE_DIR}/platforms/${PX4_PLATFORM}/cmake/init.cmake")
 	include(init)
 endif()

+#=============================================================================
+# project definition
+#
+project(px4 CXX C ASM)
+
 # CMake build type (Debug Release RelWithDebInfo MinSizeRel Coverage)
 if(NOT CMAKE_BUILD_TYPE)
 	if(${PX4_PLATFORM} STREQUAL "nuttx")
@@ -235,11 +232,6 @@ endif()
 set_property(CACHE CMAKE_BUILD_TYPE PROPERTY STRINGS "Debug;Release;RelWithDebInfo;MinSizeRel;Coverage;AddressSanitizer;UndefinedBehaviorSanitizer")
 message(STATUS "cmake build type: ${CMAKE_BUILD_TYPE}")

-#=============================================================================
-# project definition
-#
-project(px4 CXX C ASM)
-
 # Check if LTO option and check if toolchain supports it
 if(LTO)
    include(CheckIPOSupported)
@@ -256,15 +248,9 @@ set(CMAKE_C_STANDARD 11)
 set(CMAKE_C_STANDARD_REQUIRED ON)
 set(CMAKE_EXPORT_COMPILE_COMMANDS ON)

-# For the catkin build process, unset build of dynamically-linked binaries
-# and do not change CMAKE_RUNTIME_OUTPUT_DIRECTORY
-if (NOT CATKIN_DEVEL_PREFIX)
-	set(CMAKE_RUNTIME_OUTPUT_DIRECTORY ${PX4_BINARY_DIR})
-	set(CMAKE_RUNTIME_OUTPUT_DIRECTORY_DEBUG ${PX4_BINARY_DIR})
-	set(CMAKE_RUNTIME_OUTPUT_DIRECTORY_RELEASE ${PX4_BINARY_DIR})
-else()
-	SET(BUILD_SHARED_LIBS OFF)
-endif()
+set(CMAKE_RUNTIME_OUTPUT_DIRECTORY ${PX4_BINARY_DIR})
+set(CMAKE_RUNTIME_OUTPUT_DIRECTORY_DEBUG ${PX4_BINARY_DIR})
+set(CMAKE_RUNTIME_OUTPUT_DIRECTORY_RELEASE ${PX4_BINARY_DIR})

 #=============================================================================

@@ -309,21 +295,6 @@ endif()

 include(ccache)

-#=============================================================================
-# find programs and packages
-#
-
-# see if catkin was invoked to build this
-if (CATKIN_DEVEL_PREFIX)
-	message(STATUS "catkin ENABLED")
-	find_package(catkin REQUIRED)
-	if (catkin_FOUND)
-		catkin_package()
-	else()
-		message(FATAL_ERROR "catkin not found")
-	endif()
-endif()
-
 #=============================================================================
 # get chip and chip manufacturer
 #
@@ -450,10 +421,10 @@ add_subdirectory(src/lib/metadata EXCLUDE_FROM_ALL)
 add_subdirectory(src/lib/parameters EXCLUDE_FROM_ALL)

 if(${PX4_PLATFORM} STREQUAL "nuttx" AND NOT CONFIG_BUILD_FLAT)
-target_link_libraries(parameters_interface INTERFACE usr_parameters)
-target_link_libraries(kernel_parameters_interface INTERFACE parameters)
+	target_link_libraries(parameters_interface INTERFACE usr_parameters)
+	target_link_libraries(kernel_parameters_interface INTERFACE parameters)
 else()
-target_link_libraries(parameters_interface INTERFACE parameters)
+	target_link_libraries(parameters_interface INTERFACE parameters)
 endif()

 # firmware added last to generate the builtin for included modules
@@ -486,17 +457,13 @@ include(doxygen)
 include(metadata)
 include(package)

-# print size
-add_custom_target(size
-	COMMAND size $<TARGET_FILE:px4>
-	DEPENDS px4
-	WORKING_DIRECTORY ${PX4_BINARY_DIR}
-	USES_TERMINAL
-	)
-
 # install python requirements using configured python
 add_custom_target(install_python_requirements
 	COMMAND ${PYTHON_EXECUTABLE} -m pip install --requirement ${PX4_SOURCE_DIR}/Tools/setup/requirements.txt
 	DEPENDS ${PX4_SOURCE_DIR}/Tools/setup/requirements.txt
 	USES_TERMINAL
 )
+
+if(EXISTS "${PX4_SOURCE_DIR}/platforms/${PX4_PLATFORM}/cmake/finalize.cmake")
+	include(finalize)
+endif()
@@ -17,6 +17,8 @@ menu "Toolchain"
            bool "posix"
        config PLATFORM_QURT
            bool "qurt"
+        config PLATFORM_ROS2
+            bool "ros2"
    endchoice

    config BOARD_PLATFORM
@@ -24,6 +26,7 @@ menu "Toolchain"
        default "nuttx" if PLATFORM_NUTTX
        default "posix" if PLATFORM_POSIX
        default "qurt" if PLATFORM_QURT
+        default "ros2" if PLATFORM_ROS2

    config BOARD_LOCKSTEP
        bool "Force enable lockstep"
@@ -37,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
@@ -324,6 +324,10 @@ px4io_update:
 	cp build/cubepilot_io-v2_default/cubepilot_io-v2_default.bin boards/cubepilot/cubeyellow/extras/cubepilot_io-v2_default.bin
 	git status

+jlink-nuttx:
+	$(CC) -shared -fPIC platforms/nuttx/NuttX/nuttx/tools/jlink-nuttx.c -o Tools/jlink-nuttx.so
+
+
 bootloaders_update: ark_fmu-v6x_bootloader cuav_nora_bootloader cuav_x7pro_bootloader cubepilot_cubeorange_bootloader holybro_durandal-v1_bootloader holybro_kakuteh7_bootloader matek_h743_bootloader matek_h743-mini_bootloader matek_h743-slim_bootloader modalai_fc-v2_bootloader mro_ctrl-zero-classic_bootloader mro_ctrl-zero-h7_bootloader mro_ctrl-zero-h7-oem_bootloader mro_pixracerpro_bootloader px4_fmu-v6c_bootloader px4_fmu-v6u_bootloader px4_fmu-v6x_bootloader
 	git status

@@ -31,6 +31,11 @@ then
 	set PARAM_FILE /dev/eeeprom0
 fi

+if mft query -q -k MTD -s MTD_PARAMETERS -v /mnt/qspi/params
+then
+	set PARAM_FILE /mnt/qspi/params
+fi
+
 #
 # Load parameters.
 #
@@ -38,8 +38,6 @@ param set-default MIS_TAKEOFF_ALT 30
 param set-default NAV_ACC_RAD 15
 param set-default NAV_DLL_ACT 2

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

 param set-default CA_AIRFRAME 1
@@ -3,7 +3,7 @@

 udp_offboard_port_local=$((14580+px4_instance))
 udp_offboard_port_remote=$((14540+px4_instance))
-[ $px4_instance -gt 9 ] && udp_offboard_port_remote=14549 # use the same ports for more than 10 instances to avoid port overlaps
+[ "$px4_instance" -gt 9 ] && udp_offboard_port_remote=14549 # use the same ports for more than 10 instances to avoid port overlaps
 udp_onboard_payload_port_local=$((14280+px4_instance))
 udp_onboard_payload_port_remote=$((14030+px4_instance))
 udp_onboard_gimbal_port_local=$((13030+px4_instance))
@@ -13,108 +13,97 @@ if [ "$PX4_SIMULATOR" = "sihsim" ] || [ "$(param show -q SYS_AUTOSTART)" -eq "0"
 		sensor_mag_sim start

 	else
-		echo "ERROR  [init] simulator_sih failed to start"
+		echo "ERROR [init] simulator_sih failed to start"
 		exit 1
 	fi

 elif [ "$PX4_SIMULATOR" = "gz" ]; then

 	# source generated gazebo_env.sh for IGN_GAZEBO_RESOURCE_PATH
-	if [ -f gazebo_env.sh ]; then
+	if [ -f ./gazebo_env.sh ]; then
 		. ./gazebo_env.sh
+
 	elif [ -f ../gazebo_env.sh ]; then
 		. ../gazebo_env.sh
 	fi

-# shellcheck disable=SC2236
-	if [ ! -z $PX4_GZ_VERBOSE ]; then
-		if [ "$PX4_GZ_VERBOSE" -le "4" ] && [ "$PX4_GZ_VERBOSE" -ge "1" ]; then
-			gz_verbose=$PX4_GZ_VERBOSE
-		else
-			gz_verbose=4
-			echo "WARN  [init] PX4_GZ_VERBOSE was passed: $PX4_GZ_VERBOSE, not in range [1,4], defaulting to: $gz_verbose."
-		fi
-		echo "INFO  [init] PX4_GZ_VERBOSE set to $gz_verbose."
-	else
-		gz_verbose=1
-		echo "INFO  [init] PX4_GZ_VERBOSE not explicitly set, defaulting to: $gz_verbose."
-	fi
+	gz_command="ign" # "ign" before garden
+	gz_sub_command="gazebo" # "gazebo" before garden

-	gz_world=$( ign topic -l | grep -m 1 -e "/world/.*/clock" | sed 's/\/world\///g; s/\/clock//g' )
-	
-	gz_version_major=$( ign gazebo --versions | sed 's/\..*//g' )
-	gz_version_minor=$( ign gazebo --versions | sed 's/'"${gz_version_major}"\.'//; s/\..*//g' )
-	gz_version_point=$( ign gazebo --versions | sed 's/'"${gz_version_major}"\.'//; s/'"${gz_version_minor}"\.'//')
+	# look for running ${gz_command} gazebo world
+	gz_world=$( ${gz_command} topic -l | grep -m 1 -e "/world/.*/clock" | sed 's/\/world\///g; s/\/clock//g' )

-	if [ "$gz_version_major" -gt "6" ] || { [ "$gz_version_major" -eq "6" ] && [ "$gz_version_minor" -gt "12" ]; } || { [ "$gz_version_major" -eq "6" ] && [ "$gz_version_minor" -eq "12" ] && [ "$gz_version_point" -gt "0" ]; }; then
-		echo "INFO  [init] using latest version of MultiCopterMotor plugin."
-	else
-		echo "WARN  [init] using older version of MultiCopterMotor plugin, please update to latest gazebo > 6.12.0."
-		if [ "$PX4_SIM_MODEL" = "x500" ]; then
-			PX4_SIM_MODEL="x500-Legacy"
-			echo "WARN  [init] setting PX4_SIM_MODEL -> $PX4_SIM_MODEL from x500 till gazebo > 6.12.0"
-		fi
-	fi
+	# shellcheck disable=SC2153
+	if [ -z "${gz_world}" ] && [ -n "${PX4_GZ_WORLDS}" ] && [ -n "${PX4_GZ_WORLD}" ]; then

-	if [ -z $gz_world ]; then
+		echo "INFO  [init] starting gazebo with world: ${PX4_GZ_WORLDS}/${PX4_GZ_WORLD}.sdf"

-		# starting ign gazebo with ${PX4_GZ_WORLD} world
-		echo "INFO  [init] starting ign gazebo"
-# shellcheck disable=SC2153
-		ign gazebo --verbose=$gz_verbose -r -s "${PX4_GZ_WORLDS}/${PX4_GZ_WORLD}.sdf" &
+		${gz_command} ${gz_sub_command} --verbose=1 -r -s "${PX4_GZ_WORLDS}/${PX4_GZ_WORLD}.sdf" &

-		if [ -z $HEADLESS ]; then
+		if [ -z "${HEADLESS}" ]; then
 			# HEADLESS not set, starting ign gazebo gui
-			ign gazebo -g &
+			${gz_command} gazebo -g &
 		fi
+
 	else
-		echo "INFO  [init] ign gazebo already running world: $gz_world"
-		PX4_GZ_WORLD=$gz_world
+		echo "INFO  [init] gazebo already running world: ${gz_world}"
+		PX4_GZ_WORLD=${gz_world}
 	fi

-# shellcheck disable=SC2236
-	if [ ! -z $PX4_GZ_MODEL ] && [ -z $PX4_GZ_MODEL_NAME ]; then
+	# start gz_bridge
+	if [ -n "${PX4_GZ_MODEL}" ] && [ -z "${PX4_GZ_MODEL_NAME}" ]; then
+		# model specified, gz_bridge will spawn model

-# shellcheck disable=SC2236
-		if [ ! -z $PX4_GZ_MODEL_POSE ]; then
+		if [ -n "${PX4_GZ_MODEL_POSE}" ]; then
 			# Clean potential input line formatting.
-			model_pose="$( echo ${PX4_GZ_MODEL_POSE} | sed -e 's/^[ \t]*//; s/[ \t]*$//; s/,/ /g; s/  / /g; s/ /,/g' )"
+			model_pose="$( echo "${PX4_GZ_MODEL_POSE}" | sed -e 's/^[ \t]*//; s/[ \t]*$//; s/,/ /g; s/  / /g; s/ /,/g' )"
 			echo "INFO  [init] PX4_GZ_MODEL_POSE set, spawning at: ${model_pose}"
+
 		else
 			echo "WARN  [init] PX4_GZ_MODEL_POSE not set, spawning at origin."
 			model_pose="0,0,0,0,0,0"
 		fi
+
 		# start gz bridge with pose arg.
-		if gz_bridge start -p "${model_pose}"  -m "${PX4_GZ_MODEL}" -w "${PX4_GZ_WORLD}" -i "${px4_instance}"; then
+		if gz_bridge start -p "${model_pose}" -m "${PX4_GZ_MODEL}" -w "${PX4_GZ_WORLD}" -i "${px4_instance}"; then
 			sensor_baro_sim start
 			sensor_gps_sim start
 			sensor_mag_sim start
+
 		else
-			echo "ERROR  [init] ign gazebo failed to start"
+			echo "ERROR [init] gazebo failed to start"
 			exit 1
 		fi
-	elif [ ! -z $PX4_GZ_MODEL_NAME ] && [ -z $PX4_GZ_MODEL ]; then
+
+	elif [ -n "${PX4_GZ_MODEL_NAME}" ] && [ -z "${PX4_GZ_MODEL}" ]; then
+		# model name specificed, gz_bridge will attach to existing model
+
 		if gz_bridge start -n "${PX4_GZ_MODEL_NAME}" -w "${PX4_GZ_WORLD}"; then
 			sensor_baro_sim start
 			sensor_gps_sim start
 			sensor_mag_sim start
+
 		else
-			echo "ERROR  [init] ign gazebo failed to start"
+			echo "ERROR [init] gazebo failed to start"
 			exit 1
 		fi
-	elif [ ! -z $PX4_SIM_MODEL ] && [ -z $PX4_GZ_MODEL_NAME ] && [ -z $PX4_GZ_MODEL ]; then
+
+	elif [ -n "${PX4_SIM_MODEL}" ] && [ -z "${PX4_GZ_MODEL_NAME}" ] && [ -z "${PX4_GZ_MODEL}" ]; then
+
 		echo "WARN  [init] PX4_GZ_MODEL_NAME or PX4_GZ_MODEL not set using PX4_SIM_MODEL."
+
 		if gz_bridge start -m "${PX4_SIM_MODEL}" -w "${PX4_GZ_WORLD}" -i "${px4_instance}"; then
 			sensor_baro_sim start
 			sensor_gps_sim start
 			sensor_mag_sim start
+
 		else
-			echo "ERROR  [init] ign gazebo failed to start"
+			echo "ERROR [init] gazebo failed to start"
 			exit 1
 		fi

 	else
-		echo "ERROR  [init] failed to pass only PX4_GZ_MODEL_NAME or PX4_GZ_MODEL"
+		echo "ERROR [init] failed to pass only PX4_GZ_MODEL_NAME or PX4_GZ_MODEL"
 		exit 1
 	fi

@@ -131,13 +120,13 @@ else
 			echo "PX4 SIM HOST: localhost"
 			simulator_mavlink start -c $simulator_tcp_port
 		else
-			echo "PX4 SIM HOST: $PX4_SIM_HOST_ADDR"
-			simulator_mavlink start -t $PX4_SIM_HOST_ADDR $simulator_tcp_port
+			echo "PX4 SIM HOST: ${PX4_SIM_HOST_ADDR}"
+			simulator_mavlink start -t "${PX4_SIM_HOST_ADDR}" "${simulator_tcp_port}"
 		fi

 	else
-		echo "PX4 SIM HOST: $PX4_SIM_HOSTNAME"
-		simulator_mavlink start -h $PX4_SIM_HOSTNAME $simulator_tcp_port
+		echo "PX4 SIM HOST: ${PX4_SIM_HOSTNAME}"
+		simulator_mavlink start -h "${PX4_SIM_HOSTNAME}" "${simulator_tcp_port}"
 	fi

 fi
@@ -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
@@ -260,7 +262,19 @@ fi
 navigator start

 # Try to start the microdds_client with UDP transport if module exists
-microdds_client start -t udp -p 8888
+microdds_ns=""
+if [ "$px4_instance" -ne "0" ]
+then
+	# Assign new xrce dds key based on instance number and set default namespace
+	param set XRCE_DDS_KEY ${px4_instance}
+	microdds_ns="-n px4_$px4_instance"
+fi
+if [ -n "$PX4_MICRODDS_NS" ]
+then
+	# Override namespace if environment variable is defined
+	microdds_ns="-n $PX4_MICRODDS_NS"
+fi
+microdds_client start -t udp -p 8888 $microdds_ns

 if param greater -s MNT_MODE_IN -1
 then
@@ -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,
        },
@@ -1,11 +0,0 @@
-<?xml version="1.0"?>
-<model>
-  <name>x500-Legacy</name>
-  <version>1.0</version>
-  <sdf version="1.9">model.sdf</sdf>
-  <author>
-    <name>Benjamin Perseghetti</name>
-    <email>bperseghetti@rudislabs.com</email>
-  </author>
-  <description>Model of the NXP HoverGames Drone development kit (KIT-HGDRONEK66). The PX4 software compatible kit provides mechanical, RC remote and other components needed to evaluate the RDDRONE-FMUK66 reference design. The FMU includes 100Base-T1 Automotive Ethernet, dual CAN transceivers, as well as SE050 secure element, and works with add on boards NavQPlus, MR-T1ETH8, MR-T1ADAPT, and CAN-nodes such as UCANS32K1SIC.  Kit may be used with, and contains the components needed for the HoverGames.com coding challenges.</description>
-</model>
@@ -1,76 +0,0 @@
-<?xml version="1.0" encoding="UTF-8"?>
-<sdf version='1.9'>
-  <model name='x500-Legacy'>
-    <include merge='true'>
-      <uri>model://x500-NoPlugin</uri>
-    </include>
-    <plugin filename="ignition-gazebo-multicopter-motor-model-system" name="ignition::gazebo::systems::MulticopterMotorModel">
-      <robotNamespace>/x500-Legacy_0</robotNamespace>
-      <jointName>rotor_0_joint</jointName>
-      <linkName>rotor_0</linkName>
-      <turningDirection>ccw</turningDirection>
-      <timeConstantUp>0.0125</timeConstantUp>
-      <timeConstantDown>0.025</timeConstantDown>
-      <maxRotVelocity>1000.0</maxRotVelocity>
-      <motorConstant>8.54858e-06</motorConstant>
-      <momentConstant>0.016</momentConstant>
-      <commandSubTopic>command/motor_speed</commandSubTopic>
-      <motorNumber>0</motorNumber>
-      <rotorDragCoefficient>8.06428e-05</rotorDragCoefficient>
-      <rollingMomentCoefficient>1e-06</rollingMomentCoefficient>
-      <rotorVelocitySlowdownSim>10</rotorVelocitySlowdownSim>
-      <motorType>velocity</motorType>
-    </plugin>
-    <plugin filename="ignition-gazebo-multicopter-motor-model-system" name="ignition::gazebo::systems::MulticopterMotorModel">
-      <robotNamespace>/x500-Legacy_0</robotNamespace>
-      <jointName>rotor_1_joint</jointName>
-      <linkName>rotor_1</linkName>
-      <turningDirection>ccw</turningDirection>
-      <timeConstantUp>0.0125</timeConstantUp>
-      <timeConstantDown>0.025</timeConstantDown>
-      <maxRotVelocity>1000.0</maxRotVelocity>
-      <motorConstant>8.54858e-06</motorConstant>
-      <momentConstant>0.016</momentConstant>
-      <commandSubTopic>command/motor_speed</commandSubTopic>
-      <motorNumber>1</motorNumber>
-      <rotorDragCoefficient>8.06428e-05</rotorDragCoefficient>
-      <rollingMomentCoefficient>1e-06</rollingMomentCoefficient>
-      <rotorVelocitySlowdownSim>10</rotorVelocitySlowdownSim>
-      <motorType>velocity</motorType>
-    </plugin>
-    <plugin filename="ignition-gazebo-multicopter-motor-model-system" name="ignition::gazebo::systems::MulticopterMotorModel">
-      <robotNamespace>/x500-Legacy_0</robotNamespace>
-      <jointName>rotor_2_joint</jointName>
-      <linkName>rotor_2</linkName>
-      <turningDirection>cw</turningDirection>
-      <timeConstantUp>0.0125</timeConstantUp>
-      <timeConstantDown>0.025</timeConstantDown>
-      <maxRotVelocity>1000.0</maxRotVelocity>
-      <motorConstant>8.54858e-06</motorConstant>
-      <momentConstant>0.016</momentConstant>
-      <commandSubTopic>command/motor_speed</commandSubTopic>
-      <motorNumber>2</motorNumber>
-      <rotorDragCoefficient>8.06428e-05</rotorDragCoefficient>
-      <rollingMomentCoefficient>1e-06</rollingMomentCoefficient>
-      <rotorVelocitySlowdownSim>10</rotorVelocitySlowdownSim>
-      <motorType>velocity</motorType>
-    </plugin>
-    <plugin filename="ignition-gazebo-multicopter-motor-model-system" name="ignition::gazebo::systems::MulticopterMotorModel">
-      <robotNamespace>/x500-Legacy_0</robotNamespace>
-      <jointName>rotor_3_joint</jointName>
-      <linkName>rotor_3</linkName>
-      <turningDirection>cw</turningDirection>
-      <timeConstantUp>0.0125</timeConstantUp>
-      <timeConstantDown>0.025</timeConstantDown>
-      <maxRotVelocity>1000.0</maxRotVelocity>
-      <motorConstant>8.54858e-06</motorConstant>
-      <momentConstant>0.016</momentConstant>
-      <commandSubTopic>command/motor_speed</commandSubTopic>
-      <motorNumber>3</motorNumber>
-      <rotorDragCoefficient>8.06428e-05</rotorDragCoefficient>
-      <rollingMomentCoefficient>1e-06</rollingMomentCoefficient>
-      <rotorVelocitySlowdownSim>10</rotorVelocitySlowdownSim>
-      <motorType>velocity</motorType>
-    </plugin>
-  </model>
-</sdf>
@@ -73,4 +73,5 @@ fi
 ant create_run_jar copy_res
 cd out/production

-java -XX:GCTimeRatio=20 -Djava.ext.dirs= -jar jmavsim_run.jar $device $extra_args
+java --add-exports java.base/java.lang=ALL-UNNAMED --add-exports java.desktop/sun.awt=ALL-UNNAMED --add-exports java.desktop/sun.java2d=ALL-UNNAMED \
+	-XX:GCTimeRatio=20 -Djava.ext.dirs= -jar jmavsim_run.jar $device $extra_args
@@ -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
@@ -82,6 +83,4 @@ CONFIG_EXAMPLES_FAKE_MAGNETOMETER=y
 CONFIG_EXAMPLES_HELLO=y
 CONFIG_EXAMPLES_PX4_MAVLINK_DEBUG=y
 CONFIG_EXAMPLES_PX4_SIMPLE_APP=y
-CONFIG_EXAMPLES_ROVER_STEERING_CONTROL=y
-CONFIG_EXAMPLES_UUV_EXAMPLE_APP=y
 CONFIG_EXAMPLES_WORK_ITEM=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
@@ -85,6 +86,4 @@ CONFIG_EXAMPLES_FAKE_MAGNETOMETER=y
 CONFIG_EXAMPLES_HELLO=y
 CONFIG_EXAMPLES_PX4_MAVLINK_DEBUG=y
 CONFIG_EXAMPLES_PX4_SIMPLE_APP=y
-CONFIG_EXAMPLES_ROVER_STEERING_CONTROL=y
-CONFIG_EXAMPLES_UUV_EXAMPLE_APP=y
 CONFIG_EXAMPLES_WORK_ITEM=y
@@ -9,19 +9,14 @@ CONFIG_DRIVERS_ADC_BOARD_ADC=y
 CONFIG_DRIVERS_BAROMETER_BMP280=y
 CONFIG_DRIVERS_DSHOT=y
 CONFIG_DRIVERS_GPS=y
-CONFIG_DRIVERS_IMU_INVENSENSE_ICM20602=n
 CONFIG_DRIVERS_IMU_INVENSENSE_MPU6000=y
-CONFIG_DRIVERS_MAGNETOMETER_QMC5883=y
-CONFIG_DRIVERS_OSD=y
 CONFIG_DRIVERS_PWM_OUT=y
 CONFIG_DRIVERS_RC_INPUT=y
-CONFIG_DRIVERS_TELEMETRY_FRSKY_TELEMETRY=n
-CONFIG_MODULES_ATTITUDE_ESTIMATOR_Q=n
-CONFIG_MODULES_EKF2=y
 CONFIG_MODULES_BATTERY_STATUS=y
 CONFIG_MODULES_COMMANDER=y
 CONFIG_MODULES_CONTROL_ALLOCATOR=y
 CONFIG_MODULES_DATAMAN=y
+CONFIG_MODULES_EKF2=y
 CONFIG_MODULES_FLIGHT_MODE_MANAGER=y
 CONFIG_MODULES_LAND_DETECTOR=y
 CONFIG_MODULES_LOAD_MON=y
@@ -33,14 +28,12 @@ CONFIG_MODULES_MC_RATE_CONTROL=y
 CONFIG_MODULES_NAVIGATOR=y
 CONFIG_MODULES_RC_UPDATE=y
 CONFIG_MODULES_SENSORS=y
-CONFIG_SYSTEMCMDS_HARDFAULT_LOG=y
-CONFIG_SYSTEMCMDS_TOP=y
 CONFIG_SYSTEMCMDS_DMESG=y
+CONFIG_SYSTEMCMDS_HARDFAULT_LOG=y
+CONFIG_SYSTEMCMDS_I2CDETECT=y
+CONFIG_SYSTEMCMDS_PARAM=y
+CONFIG_SYSTEMCMDS_REBOOT=y
+CONFIG_SYSTEMCMDS_TOP=y
 CONFIG_SYSTEMCMDS_TOPIC_LISTENER=y
 CONFIG_SYSTEMCMDS_UORB=y
-CONFIG_SYSTEMCMDS_MIXER=y
-CONFIG_SYSTEMCMDS_PARAM=y
-CONFIG_SYSTEMCMDS_I2CDETECT=y
-CONFIG_SYSTEMCMDS_PWM=y
-CONFIG_SYSTEMCMDS_REBOOT=y
-CONFIG_SYSTEMCMDS_VER=y
+CONFIG_SYSTEMCMDS_VER=y
@@ -6,9 +6,48 @@
 # modifications.
 #
 # CONFIG_DEV_CONSOLE is not set
+# CONFIG_DISABLE_ENVIRON is not set
+# CONFIG_DISABLE_PSEUDOFS_OPERATIONS is not set
+# CONFIG_DISABLE_PTHREAD is not set
 # CONFIG_MMCSD_HAVE_CARDDETECT is not set
 # CONFIG_MMCSD_HAVE_WRITEPROTECT is not set
 # CONFIG_MMCSD_MMCSUPPORT is not set
+# CONFIG_NSH_DISABLEBG is not set
+# CONFIG_NSH_DISABLESCRIPT is not set
+# CONFIG_NSH_DISABLE_CAT is not set
+# CONFIG_NSH_DISABLE_CD is not set
+# CONFIG_NSH_DISABLE_CP is not set
+# CONFIG_NSH_DISABLE_DATE is not set
+# CONFIG_NSH_DISABLE_DF is not set
+# CONFIG_NSH_DISABLE_ECHO is not set
+# CONFIG_NSH_DISABLE_ENV is not set
+# CONFIG_NSH_DISABLE_EXEC is not set
+# CONFIG_NSH_DISABLE_EXIT is not set
+# CONFIG_NSH_DISABLE_FREE is not set
+# CONFIG_NSH_DISABLE_GET is not set
+# CONFIG_NSH_DISABLE_HELP is not set
+# CONFIG_NSH_DISABLE_ITEF is not set
+# CONFIG_NSH_DISABLE_KILL is not set
+# CONFIG_NSH_DISABLE_LOOPS is not set
+# CONFIG_NSH_DISABLE_LS is not set
+# CONFIG_NSH_DISABLE_MKDIR is not set
+# CONFIG_NSH_DISABLE_MKFATFS is not set
+# CONFIG_NSH_DISABLE_MOUNT is not set
+# CONFIG_NSH_DISABLE_MV is not set
+# CONFIG_NSH_DISABLE_PS is not set
+# CONFIG_NSH_DISABLE_PSSTACKUSAGE is not set
+# CONFIG_NSH_DISABLE_PWD is not set
+# CONFIG_NSH_DISABLE_RM is not set
+# CONFIG_NSH_DISABLE_RMDIR is not set
+# CONFIG_NSH_DISABLE_SEMICOLON is not set
+# CONFIG_NSH_DISABLE_SET is not set
+# CONFIG_NSH_DISABLE_SLEEP is not set
+# CONFIG_NSH_DISABLE_SOURCE is not set
+# CONFIG_NSH_DISABLE_TEST is not set
+# CONFIG_NSH_DISABLE_TIME is not set
+# CONFIG_NSH_DISABLE_UMOUNT is not set
+# CONFIG_NSH_DISABLE_UNSET is not set
+# CONFIG_NSH_DISABLE_USLEEP is not set
 # CONFIG_SPI_CALLBACK is not set
 # CONFIG_STM32_CCMEXCLUDE is not set
 CONFIG_ARCH="arm"
@@ -20,15 +59,17 @@ CONFIG_ARCH_CHIP="stm32"
 CONFIG_ARCH_CHIP_STM32=y
 CONFIG_ARCH_CHIP_STM32F405RG=y
 CONFIG_ARCH_INTERRUPTSTACK=512
+CONFIG_ARCH_STACKDUMP=y
 CONFIG_ARMV7M_MEMCPY=y
 CONFIG_ARMV7M_USEBASEPRI=y
+CONFIG_ARM_MPU_EARLY_RESET=y
 CONFIG_BOARDCTL_RESET=y
+CONFIG_BOARD_ASSERT_RESET_VALUE=0
 CONFIG_BOARD_CRASHDUMP=y
 CONFIG_BOARD_CUSTOM_LEDS=y
 CONFIG_BOARD_LOOPSPERMSEC=16717
 CONFIG_BOARD_RESET_ON_ASSERT=2
 CONFIG_BUILTIN=y
-CONFIG_C99_BOOL8=y
 CONFIG_CDCACM=y
 CONFIG_CDCACM_IFLOWCONTROL=y
 CONFIG_CDCACM_PRODUCTID=0x0001
@@ -37,11 +78,14 @@ CONFIG_CDCACM_RXBUFSIZE=600
 CONFIG_CDCACM_TXBUFSIZE=2000
 CONFIG_CDCACM_VENDORID=0x26ac
 CONFIG_CDCACM_VENDORSTR="Flywoo"
+CONFIG_DEBUG_FULLOPT=y
+CONFIG_DEBUG_HARDFAULT_ALERT=y
 CONFIG_DEBUG_SYMBOLS=y
+CONFIG_DEFAULT_SMALL=y
 CONFIG_DEV_FIFO_SIZE=0
+CONFIG_DEV_PIPE_MAXSIZE=1024
 CONFIG_DEV_PIPE_SIZE=70
-CONFIG_DISABLE_MQUEUE=y
-CONFIG_DISABLE_POSIX_TIMERS=y
+CONFIG_EXAMPLES_CALIB_UDELAY=y
 CONFIG_FAT_DMAMEMORY=y
 CONFIG_FAT_LCNAMES=y
 CONFIG_FAT_LFN=y
@@ -52,6 +96,7 @@ CONFIG_FS_CROMFS=y
 CONFIG_FS_FAT=y
 CONFIG_FS_FATTIME=y
 CONFIG_FS_PROCFS=y
+CONFIG_FS_PROCFS_INCLUDE_PROGMEM=y
 CONFIG_FS_PROCFS_REGISTER=y
 CONFIG_FS_ROMFS=y
 CONFIG_GRAN=y
@@ -61,37 +106,27 @@ CONFIG_HAVE_CXXINITIALIZE=y
 CONFIG_I2C=y
 CONFIG_I2C_RESET=y
 CONFIG_IDLETHREAD_STACKSIZE=750
-CONFIG_LIBC_FLOATINGPOINT=y
+CONFIG_INIT_ENTRYPOINT="nsh_main"
+CONFIG_INIT_STACKSIZE=2944
+CONFIG_LIBC_LONG_LONG=y
+CONFIG_LIBC_MAX_EXITFUNS=1
 CONFIG_LIBC_STRERROR=y
 CONFIG_MEMSET_64BIT=y
 CONFIG_MEMSET_OPTSPEED=y
 CONFIG_MMCSD=y
 CONFIG_MM_REGIONS=2
+CONFIG_NAME_MAX=40
 CONFIG_NSH_ARCHINIT=y
+CONFIG_NSH_ARGCAT=y
 CONFIG_NSH_BUILTIN_APPS=y
+CONFIG_NSH_CMDPARMS=y
 CONFIG_NSH_CROMFSETC=y
-CONFIG_NSH_DISABLE_BASENAME=y
-CONFIG_NSH_DISABLE_DD=y
-CONFIG_NSH_DISABLE_DIRNAME=y
-CONFIG_NSH_DISABLE_HEXDUMP=y
-CONFIG_NSH_DISABLE_IFCONFIG=y
-CONFIG_NSH_DISABLE_IFUPDOWN=y
-CONFIG_NSH_DISABLE_LOSETUP=y
-CONFIG_NSH_DISABLE_MKFIFO=y
-CONFIG_NSH_DISABLE_MKRD=y
-CONFIG_NSH_DISABLE_PUT=y
-CONFIG_NSH_DISABLE_REBOOT=y
-CONFIG_NSH_DISABLE_TELNETD=y
-CONFIG_NSH_DISABLE_UNAME=y
-CONFIG_NSH_DISABLE_WGET=y
-CONFIG_NSH_DISABLE_XD=y
 CONFIG_NSH_FATDEVNO=0
-CONFIG_NSH_FILEIOSIZE=512
 CONFIG_NSH_LINELEN=128
 CONFIG_NSH_MAXARGUMENTS=15
 CONFIG_NSH_MMCSDSPIPORTNO=2
 CONFIG_NSH_NESTDEPTH=8
-CONFIG_NSH_READLINE=y
+CONFIG_NSH_QUOTE=y
 CONFIG_NSH_ROMFSETC=y
 CONFIG_NSH_ROMFSSECTSIZE=128
 CONFIG_NSH_STRERROR=y
@@ -99,23 +134,24 @@ 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=196608
 CONFIG_RAM_START=0x20000000
 CONFIG_RAW_BINARY=y
 CONFIG_RTC_DATETIME=y
-CONFIG_SCHED_ATEXIT=y
 CONFIG_SCHED_HPWORK=y
 CONFIG_SCHED_HPWORKPRIORITY=249
 CONFIG_SCHED_HPWORKSTACKSIZE=1280
 CONFIG_SCHED_INSTRUMENTATION=y
 CONFIG_SCHED_INSTRUMENTATION_EXTERNAL=y
+CONFIG_SCHED_INSTRUMENTATION_SWITCH=y
 CONFIG_SCHED_LPWORK=y
 CONFIG_SCHED_LPWORKPRIORITY=50
 CONFIG_SCHED_LPWORKSTACKSIZE=1632
 CONFIG_SCHED_WAITPID=y
-CONFIG_SDCLONE_DISABLE=y
 CONFIG_SEM_PREALLOCHOLDERS=32
+CONFIG_SERIAL_IFLOWCONTROL_WATERMARKS=y
 CONFIG_SERIAL_TERMIOS=y
 CONFIG_SIG_DEFAULT=y
 CONFIG_SIG_SIGALRM_ACTION=y
@@ -156,7 +192,6 @@ CONFIG_STM32_SPI1_DMA_BUFFER=1024
 CONFIG_STM32_SPI2=y
 CONFIG_STM32_SPI2_DMA=y
 CONFIG_STM32_SPI3=y
-CONFIG_STM32_SPI_DMA=y
 CONFIG_STM32_SPI_DMATHRESHOLD=8
 CONFIG_STM32_TIM1=y
 CONFIG_STM32_TIM5=y
@@ -183,5 +218,3 @@ CONFIG_USBDEV=y
 CONFIG_USBDEV_BUSPOWERED=y
 CONFIG_USBDEV_MAXPOWER=500
 CONFIG_USEC_PER_TICK=1000
-CONFIG_USERMAIN_STACKSIZE=2944
-CONFIG_USER_ENTRYPOINT="nsh_main"
@@ -51,7 +51,7 @@
 #include <nuttx/usb/usbdev.h>
 #include <nuttx/usb/usbdev_trace.h>

-#include <arm_arch.h>
+#include <arm_internal.h>
 #include <stm32.h>
 #include "board_config.h"

@@ -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
@@ -15,32 +15,26 @@ CONFIG_ARCH_CHIP="stm32"
 CONFIG_ARCH_CHIP_STM32=y
 CONFIG_ARCH_CHIP_STM32F100C8=y
 CONFIG_ARMV7M_USEBASEPRI=y
+CONFIG_ARM_MPU_EARLY_RESET=y
 CONFIG_BOARD_LOOPSPERMSEC=2000
-CONFIG_C99_BOOL8=y
 CONFIG_DEBUG_FULLOPT=y
 CONFIG_DEBUG_SYMBOLS=y
 CONFIG_DEFAULT_SMALL=y
 CONFIG_DISABLE_MOUNTPOINT=y
-CONFIG_DISABLE_MQUEUE=y
-CONFIG_DISABLE_PTHREAD=y
 CONFIG_FDCLONE_DISABLE=y
 CONFIG_FDCLONE_STDIO=y
 CONFIG_HAVE_CXX=y
 CONFIG_HAVE_CXXINITIALIZE=y
-CONFIG_IDLETHREAD_STACKSIZE=280
-CONFIG_MAX_TASKS=2
-CONFIG_MAX_WDOGPARMS=2
+CONFIG_IDLETHREAD_STACKSIZE=316
+CONFIG_INIT_ENTRYPOINT="user_start"
+CONFIG_INIT_STACKSIZE=1100
 CONFIG_MM_FILL_ALLOCATIONS=y
 CONFIG_MM_SMALL=y
 CONFIG_NAME_MAX=12
-CONFIG_NFILE_DESCRIPTORS=3
-CONFIG_NFILE_STREAMS=0
 CONFIG_PREALLOC_TIMERS=0
-CONFIG_PREALLOC_WDOGS=1
 CONFIG_RAM_SIZE=8192
 CONFIG_RAM_START=0x20000000
 CONFIG_RAW_BINARY=y
-CONFIG_SDCLONE_DISABLE=y
 CONFIG_SERIAL_TERMIOS=y
 CONFIG_STACK_COLORATION=y
 CONFIG_START_DAY=30
@@ -65,6 +59,3 @@ CONFIG_USART3_RXBUFSIZE=64
 CONFIG_USART3_RXDMA=y
 CONFIG_USART3_TXBUFSIZE=64
 CONFIG_USEC_PER_TICK=1000
-CONFIG_USERMAIN_STACKSIZE=1100
-CONFIG_USER_ENTRYPOINT="user_start"
-CONFIG_WDOG_INTRESERVE=0
@@ -72,7 +72,6 @@ SECTIONS
 		*(.gcc_except_table)
 		*(.gnu.linkonce.r.*)
 		_etext = ABSOLUTE(.);
-
 	} > flash

 	/*
@@ -47,8 +47,6 @@
 #include <nuttx/compiler.h>
 #include <stdint.h>

-
-
 /******************************************************************************
 * Definitions
 ******************************************************************************/
@@ -156,6 +154,4 @@

 #define BOARD_NUM_IO_TIMERS 3

-#define BOARD_DISABLE_I2C_SPI
-
 #include <px4_platform_common/board_common.h>
@@ -32,7 +32,7 @@
 ****************************************************************************/

 /**
- * @file px4iov2_init.c
+ * @file init.c
 *
 * PX4FMU-specific early startup code.  This file implements the
 * stm32_boardinitialize() function that is called during cpu startup.
@@ -84,11 +84,9 @@

 __EXPORT void stm32_boardinitialize(void)
 {
-
 	/* configure GPIOs */

 	/* Set up for sensing HW */
-
 	stm32_configgpio(GPIO_SENSE_PC14_DN);
 	stm32_configgpio(GPIO_SENSE_PC15_UP);

@@ -3,3 +3,5 @@ CONFIG_BOARD_TOOLCHAIN="qurt"
 CONFIG_MODULES_MUORB_SLPI=y
 CONFIG_SYSTEMCMDS_UORB=y
 CONFIG_ORB_COMMUNICATOR=y
+CONFIG_SYSTEMCMDS_PARAM=y
+CONFIG_DRIVERS_QSHELL_QURT=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;
+}
@@ -0,0 +1,40 @@
+/****************************************************************************
+ *
+ *   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 <px4_arch/spi_hw_description.h>
+#include <px4_platform_common/spi.h>
+#include <drivers/drv_sensor.h>
+
+constexpr px4_spi_bus_t px4_spi_buses[SPI_BUS_MAX_BUS_ITEMS] = {
+	initSPIBus(1, {initSPIDevice(DRV_IMU_DEVTYPE_ICM42688P), }),
+};
@@ -1,7 +1,11 @@
 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
 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
@@ -0,0 +1,4 @@
+#
+# For a description of the syntax of this configuration file,
+# see the file kconfig-language.txt in the NuttX tools repository.
+#
@@ -0,0 +1,28 @@
+CONFIG_BOARD_TOOLCHAIN="arm-none-eabi"
+CONFIG_BOARD_ARCHITECTURE="cortex-m7"
+CONFIG_BOARD_ROMFSROOT="cannode"
+CONFIG_BOARD_ETHERNET=y
+CONFIG_DRIVERS_BAROMETER_BMP388=y
+CONFIG_DRIVERS_IMU_INVENSENSE_ICM20649=y
+CONFIG_DRIVERS_IMU_INVENSENSE_ICM42688P=y
+CONFIG_DRIVERS_MAGNETOMETER_BOSCH_BMM150=y
+CONFIG_DRIVERS_PWM_OUT=y
+CONFIG_MODULES_CONTROL_ALLOCATOR=y
+CONFIG_MODULES_DATAMAN=y
+CONFIG_MODULES_EKF2=y
+CONFIG_MODULES_MAVLINK=y
+CONFIG_MODULES_SENSORS=y
+CONFIG_SYSTEMCMDS_I2CDETECT=y
+CONFIG_SYSTEMCMDS_LED_CONTROL=y
+CONFIG_SYSTEMCMDS_MFT=y
+CONFIG_SYSTEMCMDS_MTD=y
+CONFIG_SYSTEMCMDS_PARAM=y
+CONFIG_SYSTEMCMDS_REBOOT=y
+CONFIG_SYSTEMCMDS_SD_BENCH=y
+CONFIG_SYSTEMCMDS_SD_STRESS=y
+CONFIG_SYSTEMCMDS_SYSTEM_TIME=y
+CONFIG_SYSTEMCMDS_TOP=y
+CONFIG_SYSTEMCMDS_TOPIC_LISTENER=y
+CONFIG_SYSTEMCMDS_UORB=y
+CONFIG_SYSTEMCMDS_VER=y
+CONFIG_SYSTEMCMDS_WORK_QUEUE=y
@@ -0,0 +1,13 @@
+{
+    "board_id": 34,
+    "magic": "PX4FWv1",
+    "description": "Firmware for the ucans32k146 board",
+    "image": "",
+    "build_time": 0,
+    "summary": "UCANS32K146",
+    "version": "0.1",
+    "image_size": 0,
+    "image_maxsize": 0,
+    "git_identity": "",
+    "board_revision": 0
+}
@@ -0,0 +1,51 @@
+# CONFIG_BOARD_ROMFSROOT is not set
+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_RC_INPUT=y
+CONFIG_DRIVERS_UAVCAN=y
+CONFIG_EXAMPLES_FAKE_GPS=y
+CONFIG_MODULES_AIRSPEED_SELECTOR=y
+CONFIG_MODULES_ATTITUDE_ESTIMATOR_Q=y
+CONFIG_MODULES_CAMERA_FEEDBACK=y
+CONFIG_MODULES_COMMANDER=y
+CONFIG_MODULES_ESC_BATTERY=y
+CONFIG_MODULES_EVENTS=y
+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_GYRO_CALIBRATION=y
+CONFIG_MODULES_GYRO_FFT=y
+CONFIG_MODULES_LANDING_TARGET_ESTIMATOR=y
+CONFIG_MODULES_LAND_DETECTOR=y
+CONFIG_MODULES_LOAD_MON=y
+CONFIG_MODULES_LOCAL_POSITION_ESTIMATOR=y
+CONFIG_MODULES_LOGGER=y
+CONFIG_MODULES_MAG_BIAS_ESTIMATOR=y
+CONFIG_MODULES_MANUAL_CONTROL=y
+CONFIG_MODULES_MC_ATT_CONTROL=y
+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_NAVIGATOR=y
+CONFIG_MODULES_RC_UPDATE=y
+CONFIG_MODULES_ROVER_POS_CONTROL=y
+CONFIG_MODULES_TEMPERATURE_COMPENSATION=y
+CONFIG_MODULES_VTOL_ATT_CONTROL=y
+CONFIG_SYSTEMCMDS_ACTUATOR_TEST=y
+CONFIG_SYSTEMCMDS_DMESG=y
+CONFIG_SYSTEMCMDS_DUMPFILE=y
+CONFIG_SYSTEMCMDS_NETMAN=y
+CONFIG_SYSTEMCMDS_NSHTERM=y
+CONFIG_SYSTEMCMDS_PERF=y
+CONFIG_SYSTEMCMDS_REFLECT=y
+CONFIG_SYSTEMCMDS_SERIAL_TEST=y
+CONFIG_SYSTEMCMDS_TUNE_CONTROL=y
@@ -0,0 +1,25 @@
+#!/bin/sh
+#
+# board specific defaults
+#------------------------------------------------------------------------------
+
+# FIXME TELEM
+
+# Mavlink ethernet (CFG 1000)
+param set-default MAV_1_CONFIG 1000
+param set-default MAV_1_BROADCAST 1
+param set-default MAV_1_MODE 0
+param set-default MAV_1_RADIO_CTL 0
+param set-default MAV_1_RATE 100000
+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
@@ -0,0 +1,4 @@
+#!/bin/sh
+#
+# board specific MAVLink startup script.
+#------------------------------------------------------------------------------
@@ -0,0 +1,29 @@
+#!/bin/sh
+#
+# NXP MR-CANHUBK3 specific board sensors init
+#------------------------------------------------------------------------------
+
+#board_adc start FIXME no ADC drivers
+
+#FMUv5Xbase board orientation
+
+# Internal SPI bus ICM20649
+icm20649 -s -R 6 start
+
+# Internal SPI bus ICM42688p
+icm42688p -R 6 -s start
+
+# Internal magnetometer on I2c
+bmm150 -I start
+
+# 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)
+ist8310 -X -b 2 -R 10 start
+
+# External compass on GPS1/I2C1 (the 3rd external bus): Drotek RTK GPS with LIS3MDL Compass
+lis3mdl -X -b 2 -R 2 start
+
+# Disable startup of internal baros if param is set to false
+if param compare SENS_INT_BARO_EN 1
+then
+	bmp388 -I -a 0x77 start
+fi
@@ -0,0 +1,18 @@
+#
+# For a description of the syntax of this configuration file,
+# see the file kconfig-language.txt in the NuttX tools repository.
+#
+
+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.
+
+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.
@@ -0,0 +1,361 @@
+/****************************************************************************
+ * boards/arm/s32k3xx/mr-canhubk3/include/board.h
+ *
+ * Licensed to the Apache Software Foundation (ASF) under one or more
+ * contributor license agreements.  See the NOTICE file distributed with
+ * this work for additional information regarding copyright ownership.  The
+ * ASF licenses this file to you under the Apache License, Version 2.0 (the
+ * "License"); you may not use this file except in compliance with the
+ * License.  You may obtain a copy of the License at
+ *
+ *   http://www.apache.org/licenses/LICENSE-2.0
+ *
+ * Unless required by applicable law or agreed to in writing, software
+ * distributed under the License is distributed on an "AS IS" BASIS, WITHOUT
+ * WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.  See the
+ * License for the specific language governing permissions and limitations
+ * under the License.
+ *
+ ****************************************************************************/
+
+/* Copyright 2022 NXP */
+
+#ifndef __BOARDS_ARM_S32K3XX_MR_CANHUBK3_INCLUDE_BOARD_H
+#define __BOARDS_ARM_S32K3XX_MR_CANHUBK3_INCLUDE_BOARD_H
+
+/****************************************************************************
+ * Included Files
+ ****************************************************************************/
+
+#include <nuttx/config.h>
+
+/****************************************************************************
+ * Pre-processor Definitions
+ ****************************************************************************/
+
+/* Clocking *****************************************************************/
+
+/* The MR-CANHUBK3 is fitted with a 16 MHz crystal */
+
+#define BOARD_XTAL_FREQUENCY  16000000
+
+/* The S32K344 will run at 160 MHz */
+
+#define MR_CANHUBK3_SYSCLK_FREQUENCY  160000000
+
+/* MX25L QuadSPI Flash ******************************************************/
+
+#ifdef CONFIG_S32K3XX_QSPI
+#  ifdef CONFIG_MTD_MX25RXX
+#    define HAVE_MX25L
+#      ifdef CONFIG_FS_LITTLEFS
+#        define HAVE_MX25L_LITTLEFS
+#      else
+#        ifdef CONFIG_FS_NXFFS
+#          define HAVE_MX25L_NXFFS
+#        else
+#          define HAVE_MX25L_CHARDEV
+#        endif
+#      endif
+#  endif
+#endif
+
+#define MX25L_MTD_MINOR    0
+#define MX25L_SMART_MINOR  0
+
+/* LED definitions **********************************************************/
+
+/* The MR-CANHUBK3 has one RGB LED:
+ *
+ *   RedLED    PTE14  (FXIO D7 / EMIOS0 CH19)
+ *   GreenLED  PTA27  (FXIO D5 / EMIOS1 CH10 / EMIOS2 CH10)
+ *   BlueLED   PTE12  (FXIO D8 / EMIOS1 CH5)
+ *
+ * If CONFIG_ARCH_LEDS is not defined, then the user can control the LEDs in
+ * any way.  The following definitions are used to access individual RGB
+ * components.
+ *
+ * The RGB components could, alternatively be controlled through PWM using
+ * the common RGB LED driver.
+ */
+
+/* LED index values for use with board_userled() */
+
+#define BOARD_LED_R       0
+#define BOARD_LED_G       1
+#define BOARD_LED_B       2
+#define BOARD_NLEDS       3
+
+/* LED bits for use with board_userled_all() */
+
+#define BOARD_LED_R_BIT   (1 << BOARD_LED_R)
+#define BOARD_LED_G_BIT   (1 << BOARD_LED_G)
+#define BOARD_LED_B_BIT   (1 << BOARD_LED_B)
+
+/* If CONFIG_ARCH_LEDs is defined, then NuttX will control the LEDs on board
+ * the MR-CANHUBK3.  The following definitions describe how NuttX controls
+ * the LEDs:
+ *
+ *      SYMBOL            Meaning                         LED state
+ *                                                        RED    GREEN  BLUE
+ *      ----------------  -----------------------------  -------------------
+ */
+
+#define LED_STARTED       1 /* NuttX has been started     OFF    OFF    OFF */
+#define LED_HEAPALLOCATE  2 /* Heap has been allocated    OFF    OFF    ON  */
+#define LED_IRQSENABLED   0 /* Interrupts enabled         OFF    OFF    ON  */
+#define LED_STACKCREATED  3 /* Idle stack created         OFF    ON     OFF */
+#define LED_INIRQ         0 /* In an interrupt           (No change)        */
+#define LED_SIGNAL        0 /* In a signal handler       (No change)        */
+#define LED_ASSERTION     0 /* An assertion failed       (No change)        */
+#define LED_PANIC         4 /* The system has crashed     FLASH  OFF    OFF */
+#undef  LED_IDLE            /* S32K344 is in sleep mode  (Not used)         */
+
+/* Button definitions *******************************************************/
+
+/* The MR-CANHUBK3 supports two buttons:
+ *
+ *   SW1  PTD15  (EIRQ31)
+ *   SW2  PTA25  (EIRQ5 / WKPU34)
+ */
+
+#define BUTTON_SW1        0
+#define BUTTON_SW2        1
+#define NUM_BUTTONS       2
+
+#define BUTTON_SW1_BIT    (1 << BUTTON_SW1)
+#define BUTTON_SW2_BIT    (1 << BUTTON_SW2)
+
+/* UART selections **********************************************************/
+
+/* By default, the serial console will be provided on the DCD-LZ UART
+ * (available on the 7-pin DCD-LZ debug connector P6):
+ *
+ *   DCD-LZ UART RX  PTA8  (LPUART2_RX)
+ *   DCD-LZ UART TX  PTA9  (LPUART2_TX)
+ */
+
+#define PIN_LPUART2_RX    (PIN_LPUART2_RX_1 | PIN_INPUT_PULLUP)   /* PTA8 */
+#define PIN_LPUART2_TX    PIN_LPUART2_TX_1   /* PTA9 */
+
+/* LPUART0   P2 UART (with flow control) connector */
+
+#define PIN_LPUART0_CTS   PIN_LPUART0_CTS_1  /* PTA0 */
+#define PIN_LPUART0_RTS   PIN_LPUART0_RTS_1  /* PTA1 */
+#define PIN_LPUART0_RX    (PIN_LPUART0_RX_1 | PIN_INPUT_PULLUP)   /* PTA2 */
+#define PIN_LPUART0_TX    PIN_LPUART0_TX_1   /* PTA3 */
+
+/* LPUART1   P5 UART (with flow control) connector */
+
+#define PIN_LPUART1_CTS   PIN_LPUART1_CTS_2  /* PTE2 */
+#define PIN_LPUART1_RTS   PIN_LPUART1_RTS_2  /* PTE6 */
+#define PIN_LPUART1_RX    (PIN_LPUART1_RX_3 | PIN_INPUT_PULLUP)   /* PTC6 */
+#define PIN_LPUART1_TX    PIN_LPUART1_TX_3   /* PTC7 */
+
+/* LPUART9   P24 UART connector */
+
+#define PIN_LPUART9_RX    (PIN_LPUART9_RX_1 | PIN_INPUT_PULLUP)   /* PTB2 */
+#define PIN_LPUART9_TX    PIN_LPUART9_TX_1   /* PTB3 */
+
+/* LPUART10  P24 UART connector */
+
+#define PIN_LPUART10_RX   (PIN_LPUART10_RX_1 | PIN_INPUT_PULLUP)  /* PTC12 */
+#define PIN_LPUART10_TX   PIN_LPUART10_TX_1  /* PTC13 */
+
+/* LPUART13  P25 UART connector */
+
+#define PIN_LPUART13_RX   (PIN_LPUART13_RX_1 | PIN_INPUT_PULLUP)  /* PTB19 */
+#define PIN_LPUART13_TX   PIN_LPUART13_TX_1  /* PTB18 */
+
+/* LPUART14  P25 UART connector */
+
+#define PIN_LPUART14_RX   (PIN_LPUART14_RX_1 | PIN_INPUT_PULLUP)  /* PTB21 */
+#define PIN_LPUART14_TX   PIN_LPUART14_TX_1  /* PTB20 */
+
+/* SPI selections ***********************************************************/
+
+/* LPSPI1  P1A external SPI connector */
+
+#define PIN_LPSPI1_SCK    PIN_LPSPI1_SCK_3   /* PTA28 */
+#define PIN_LPSPI1_MISO   PIN_LPSPI1_SOUT_2  /* PTA30 */
+#define PIN_LPSPI1_MOSI   PIN_LPSPI1_SIN_3   /* PTA29 */
+#define PIN_LPSPI1_PCS0   PIN_LPSPI1_PCS0_2  /* PTA21 */
+#define PIN_LPSPI1_PCS1   PIN_LPSPI1_PCS1_6  /* PTE4  */
+
+#define PIN_LPSPI1_PCS    (PIN_PTA21 | GPIO_LOWDRIVE | GPIO_OUTPUT_ONE)  /* PTA21 */
+
+/* LPSPI2  P1B external SPI connector */
+
+#define PIN_LPSPI2_SCK    PIN_LPSPI2_SCK_1   /* PTB29 */
+#define PIN_LPSPI2_MISO   PIN_LPSPI2_SOUT_3  /* PTB27 */
+#define PIN_LPSPI2_MOSI   PIN_LPSPI2_SIN_2   /* PTB28 */
+#define PIN_LPSPI2_PCS0   PIN_LPSPI2_PCS0_2  /* PTB25 */
+#define PIN_LPSPI2_PCS1   PIN_LPSPI2_PCS1_3  /* PTC19 */
+
+#define PIN_LPSPI2_PCS    (PIN_PTB25 | GPIO_LOWDRIVE | GPIO_OUTPUT_ONE)  /* PTB25 */
+
+/* LPSPI3  FS26 Safety SBC */
+
+#define PIN_LPSPI3_SCK    PIN_LPSPI3_SCK_2   /* PTD1  */
+#define PIN_LPSPI3_MISO   PIN_LPSPI3_SOUT_2  /* PTD0  */
+#define PIN_LPSPI3_MOSI   PIN_LPSPI3_SIN_3   /* PTE10 */
+#define PIN_LPSPI3_PCS    (PIN_PTD17 | GPIO_LOWDRIVE | GPIO_OUTPUT_ONE) /* PTD17 */
+
+/* LPSPI4  P8B I/O connector / P26 external IMU connector */
+
+#define PIN_LPSPI4_SCK    PIN_LPSPI4_SCK_1   /* PTB10 */
+#define PIN_LPSPI4_MISO   PIN_LPSPI4_SOUT_1  /* PTB9  */
+#define PIN_LPSPI4_MOSI   PIN_LPSPI4_SIN_1   /* PTB11 */
+#define PIN_LPSPI4_PCS0   PIN_LPSPI4_PCS0_1  /* PTB8  */
+#define PIN_LPSPI4_PCS3   PIN_LPSPI4_PCS3_1  /* PTA16 */
+
+#define PIN_LPSPI4_PCS    (PIN_PTA16 | GPIO_LOWDRIVE | GPIO_OUTPUT_ONE)  /* PTA16 */
+
+/* LPSPI5  P26 external IMU connector */
+
+#define PIN_LPSPI5_SCK    PIN_LPSPI5_SCK_3   /* PTD26 */
+#define PIN_LPSPI5_MISO   PIN_LPSPI5_SOUT_2  /* PTD27 */
+#define PIN_LPSPI5_MOSI   PIN_LPSPI5_SIN_3   /* PTD28 */
+#define PIN_LPSPI5_PCS1   PIN_LPSPI5_PCS1_1  /* PTA14 */
+
+#define PIN_LPSPI5_PCS    (PIN_PTA14 | GPIO_LOWDRIVE | GPIO_OUTPUT_ONE)  /* PTA14 */
+
+/*      PIN_LPSPI5_PCS2                         PTD29 */
+
+/* I2C selections ***********************************************************/
+
+/* LPI2C0  P4 LCD header / P26 external IMU connector */
+
+#define PIN_LPI2C0_SCL    PIN_LPI2C0_SCL_2   /* PTD14 */
+#define PIN_LPI2C0_SDA    PIN_LPI2C0_SDA_2   /* PTD13 */
+
+/* LPI2C1  P3 external I2C connector / SE050 EdgeLock Secure Element */
+
+#define PIN_LPI2C1_SCL    PIN_LPI2C1_SCL_4   /* PTD9 */
+#define PIN_LPI2C1_SDA    PIN_LPI2C1_SDA_4   /* PTD8 */
+
+/* CAN selections ***********************************************************/
+
+/* CAN0  TJA1443 CAN transceiver */
+
+#define PIN_CAN0_RX       PIN_CAN0_RX_1      /* PTA6 */
+#define PIN_CAN0_TX       PIN_CAN0_TX_1      /* PTA7 */
+#define PIN_CAN0_STB      (PIN_PTC21 | GPIO_OUTPUT)
+#define CAN0_STB_OUT      1
+#define PIN_CAN0_ENABLE   (PIN_PTC24 | GPIO_OUTPUT)
+#define CAN0_ENABLE_OUT   1
+#define PIN_CAN0_LED      (PIN_PTC18 | GPIO_OUTPUT)
+#define CAN0_LED_OUT      0
+#define PIN_CAN0_ERRN     (PIN_PTC20 | GPIO_INPUT)
+
+/* CAN1  TJA1443 CAN transceiver */
+
+#define PIN_CAN1_RX       PIN_CAN1_RX_4      /* PTC9 */
+#define PIN_CAN1_TX       PIN_CAN1_TX_4      /* PTC8 */
+#define PIN_CAN1_STB      (PIN_PTD2  | GPIO_OUTPUT)
+#define CAN1_STB_OUT      1
+#define PIN_CAN1_ENABLE   (PIN_PTD23 | GPIO_OUTPUT)
+#define CAN1_ENABLE_OUT   1
+#define PIN_CAN1_LED      (PIN_PTE5  | GPIO_OUTPUT)
+#define CAN1_LED_OUT      0
+#define PIN_CAN1_ERRN     (PIN_PTD3  | GPIO_INPUT)
+
+/* CAN2  TJA1463 CAN transceiver */
+
+#define PIN_CAN2_RX       PIN_CAN2_RX_5      /* PTE25 */
+#define PIN_CAN2_TX       PIN_CAN2_TX_5      /* PTE24 */
+#define PIN_CAN2_STB      (PIN_PTD22 | GPIO_OUTPUT)
+#define CAN2_STB_OUT      1
+#define PIN_CAN2_ENABLE   (PIN_PTD4  | GPIO_OUTPUT)
+#define CAN2_ENABLE_OUT   1
+#define PIN_CAN2_LED      (PIN_PTD20 | GPIO_OUTPUT)
+#define CAN2_LED_OUT      0
+#define PIN_CAN2_ERRN     (PIN_PTD21 | GPIO_INPUT)
+
+/* CAN3  TJA1463 CAN transceiver */
+
+#define PIN_CAN3_RX       PIN_CAN3_RX_2      /* PTC29 */
+#define PIN_CAN3_TX       PIN_CAN3_TX_2      /* PTC28 */
+#define PIN_CAN3_STB      (PIN_PTB1  | GPIO_OUTPUT)
+#define CAN3_STB_OUT      1
+#define PIN_CAN3_ENABLE   (PIN_PTB0  | GPIO_OUTPUT)
+#define CAN3_ENABLE_OUT   1
+#define PIN_CAN3_LED      (PIN_PTB24 | GPIO_OUTPUT)
+#define CAN3_LED_OUT      0
+#define PIN_CAN3_ERRN     (PIN_PTC27 | GPIO_INPUT)
+
+/* CAN4  TJA1153 CAN transceiver */
+
+#define PIN_CAN4_RX       PIN_CAN4_RX_2      /* PTC31 */
+#define PIN_CAN4_TX       PIN_CAN4_TX_2      /* PTC30 */
+#define PIN_CAN4_STB      (PIN_PTC25 | GPIO_OUTPUT)
+#define CAN4_STB_OUT      0
+#define PIN_CAN4_ENABLE   (PIN_PTC26 | GPIO_OUTPUT)
+#define CAN4_ENABLE_OUT   1
+#define PIN_CAN4_LED      (PIN_PTB26 | GPIO_OUTPUT)
+#define CAN4_LED_OUT      0
+#define PIN_CAN4_ERRN     (PIN_PTC23 | GPIO_INPUT)
+
+/* CAN5  TJA1153 CAN transceiver */
+
+#define PIN_CAN5_RX       PIN_CAN5_RX_1      /* PTC11 */
+#define PIN_CAN5_TX       PIN_CAN5_TX_1      /* PTC10 */
+#define PIN_CAN5_STB      (PIN_PTE17 | GPIO_OUTPUT)
+#define CAN5_STB_OUT      0
+#define PIN_CAN5_ENABLE   (PIN_PTD30 | GPIO_OUTPUT)
+#define CAN5_ENABLE_OUT   1
+#define PIN_CAN5_LED      (PIN_PTD31 | GPIO_OUTPUT)
+#define CAN5_LED_OUT      0
+#define PIN_CAN5_ERRN     (PIN_PTD24 | GPIO_INPUT)
+
+/* ENET selections **********************************************************/
+
+#define PIN_EMAC_MII_RMII_TXD0    PIN_EMAC_MII_RMII_TXD0_1     /* PTB5  */
+#define PIN_EMAC_MII_RMII_TXD1    PIN_EMAC_MII_RMII_TXD1_1     /* PTB4  */
+#define PIN_EMAC_MII_RMII_TX_EN   PIN_EMAC_MII_RMII_TX_EN_3    /* PTE9  */
+#define PIN_EMAC_MII_RMII_RXD0    PIN_EMAC_MII_RMII_RXD0_1     /* PTC0  */
+#define PIN_EMAC_MII_RMII_RXD1    PIN_EMAC_MII_RMII_RXD1_2     /* PTC1  */
+#define PIN_EMAC_MII_RMII_RX_DV   PIN_EMAC_MII_RMII_RX_DV_1    /* PTC15 */
+#define PIN_EMAC_MII_RMII_RX_ER   PIN_EMAC_MII_RMII_RX_ER_1    /* PTC14 */
+#define PIN_EMAC_MII_RMII_MDC     PIN_EMAC_MII_RMII_MDC_3      /* PTC8  */
+#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 */
@@ -0,0 +1,257 @@
+#
+# This file is autogenerated: PLEASE DO NOT EDIT IT.
+#
+# You can use "make menuconfig" to make any modifications to the installed .config file.
+# You can then do "make savedefconfig" to generate a new defconfig file that includes your
+# modifications.
+#
+# CONFIG_DISABLE_ENVIRON is not set
+# CONFIG_DISABLE_PSEUDOFS_OPERATIONS is not set
+# CONFIG_DISABLE_PTHREAD is not set
+# CONFIG_MMCSD_HAVE_CARDDETECT is not set
+# CONFIG_MMCSD_HAVE_WRITEPROTECT is not set
+# CONFIG_MMCSD_MMCSUPPORT is not set
+# CONFIG_NSH_DISABLEBG is not set
+# CONFIG_NSH_DISABLESCRIPT is not set
+# CONFIG_NSH_DISABLE_ARP is not set
+# CONFIG_NSH_DISABLE_CAT is not set
+# CONFIG_NSH_DISABLE_CD is not set
+# CONFIG_NSH_DISABLE_CP is not set
+# CONFIG_NSH_DISABLE_DATE is not set
+# CONFIG_NSH_DISABLE_DF is not set
+# CONFIG_NSH_DISABLE_ECHO is not set
+# CONFIG_NSH_DISABLE_ENV is not set
+# CONFIG_NSH_DISABLE_EXEC is not set
+# CONFIG_NSH_DISABLE_EXPORT is not set
+# CONFIG_NSH_DISABLE_FREE is not set
+# CONFIG_NSH_DISABLE_GET is not set
+# CONFIG_NSH_DISABLE_HELP is not set
+# CONFIG_NSH_DISABLE_IFCONFIG is not set
+# CONFIG_NSH_DISABLE_IFUPDOWN is not set
+# CONFIG_NSH_DISABLE_ITEF is not set
+# CONFIG_NSH_DISABLE_KILL is not set
+# CONFIG_NSH_DISABLE_LOOPS is not set
+# CONFIG_NSH_DISABLE_LS is not set
+# CONFIG_NSH_DISABLE_MKDIR is not set
+# CONFIG_NSH_DISABLE_MKFATFS is not set
+# CONFIG_NSH_DISABLE_MOUNT is not set
+# CONFIG_NSH_DISABLE_MV is not set
+# CONFIG_NSH_DISABLE_NSLOOKUP is not set
+# CONFIG_NSH_DISABLE_PS is not set
+# CONFIG_NSH_DISABLE_PSSTACKUSAGE is not set
+# CONFIG_NSH_DISABLE_PWD is not set
+# CONFIG_NSH_DISABLE_RM is not set
+# CONFIG_NSH_DISABLE_RMDIR is not set
+# CONFIG_NSH_DISABLE_SEMICOLON is not set
+# CONFIG_NSH_DISABLE_SET is not set
+# CONFIG_NSH_DISABLE_SLEEP is not set
+# CONFIG_NSH_DISABLE_SOURCE is not set
+# CONFIG_NSH_DISABLE_TELNETD is not set
+# CONFIG_NSH_DISABLE_TEST is not set
+# CONFIG_NSH_DISABLE_TIME is not set
+# CONFIG_NSH_DISABLE_UMOUNT is not set
+# CONFIG_NSH_DISABLE_UNSET is not set
+# CONFIG_NSH_DISABLE_USLEEP is not set
+# CONFIG_SPI_CALLBACK is not set
+CONFIG_ALLOW_GPL_COMPONENTS=y
+CONFIG_ARCH="arm"
+CONFIG_ARCH_BOARD_CUSTOM=y
+CONFIG_ARCH_BOARD_CUSTOM_DIR="../../../../boards/nxp/mr-canhubk3/nuttx-config"
+CONFIG_ARCH_BOARD_CUSTOM_DIR_RELPATH=y
+CONFIG_ARCH_BOARD_CUSTOM_NAME="px4"
+CONFIG_ARCH_CHIP="s32k3xx"
+CONFIG_ARCH_CHIP_S32K344=y
+CONFIG_ARCH_CHIP_S32K3XX=y
+CONFIG_ARCH_STACKDUMP=y
+CONFIG_ARMV7M_DCACHE=y
+CONFIG_ARMV7M_DCACHE_WRITETHROUGH=y
+CONFIG_ARMV7M_DTCM=y
+CONFIG_ARMV7M_ICACHE=y
+CONFIG_ARMV7M_MEMCPY=y
+CONFIG_ARMV7M_USEBASEPRI=y
+CONFIG_BOARDCTL_RESET=y
+CONFIG_BOARD_LOOPSPERMSEC=14539
+CONFIG_BUILTIN=y
+CONFIG_DEBUG_BUSFAULT=y
+CONFIG_DEBUG_ERROR=y
+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
+CONFIG_DEV_PIPE_SIZE=70
+CONFIG_FAT_DMAMEMORY=y
+CONFIG_FAT_LCNAMES=y
+CONFIG_FAT_LFN=y
+CONFIG_FAT_LFN_ALIAS_HASH=y
+CONFIG_FDCLONE_STDIO=y
+CONFIG_FS26_SPI_FREQUENCY=5000000
+CONFIG_FSUTILS_IPCFG=y
+CONFIG_FS_CROMFS=y
+CONFIG_FS_FAT=y
+CONFIG_FS_FATTIME=y
+CONFIG_FS_LITTLEFS=y
+CONFIG_FS_PROCFS=y
+CONFIG_FS_PROCFS_MAX_TASKS=64
+CONFIG_FS_PROCFS_REGISTER=y
+CONFIG_FS_ROMFS=y
+CONFIG_GRAN=y
+CONFIG_GRAN_INTR=y
+CONFIG_HAVE_CXX=y
+CONFIG_HAVE_CXXINITIALIZE=y
+CONFIG_I2C=y
+CONFIG_I2C_RESET=y
+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="/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
+CONFIG_LPUART2_SERIAL_CONSOLE=y
+CONFIG_MEMSET_64BIT=y
+CONFIG_MEMSET_OPTSPEED=y
+CONFIG_MMCSD=y
+CONFIG_MMCSD_SPICLOCK=7500000
+CONFIG_MM_REGIONS=2
+CONFIG_MTD=y
+CONFIG_MTD_MX25RXX=y
+CONFIG_MTD_PARTITION=y
+CONFIG_MTD_PARTITION_NAMES=y
+CONFIG_MX25RXX_LXX=y
+CONFIG_MX25RXX_PAGE128=y
+CONFIG_MX25RXX_QSPI_FREQUENCY=80000000
+CONFIG_NET=y
+CONFIG_NETDB_DNSCLIENT=y
+CONFIG_NETDB_DNSCLIENT_ENTRIES=8
+CONFIG_NETDB_DNSSERVER_NOADDR=y
+CONFIG_NETDEV_LATEINIT=y
+CONFIG_NETINIT_DHCPC=y
+CONFIG_NETINIT_DNS=y
+CONFIG_NETINIT_DNSIPADDR=0XC0A800FE
+CONFIG_NETINIT_DRIPADDR=0XC0A800FE
+CONFIG_NETINIT_THREAD=y
+CONFIG_NETINIT_THREAD_PRIORITY=49
+CONFIG_NETUTILS_TELNETD=y
+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
+CONFIG_NSH_ARCHINIT=y
+CONFIG_NSH_ARGCAT=y
+CONFIG_NSH_BUILTIN_APPS=y
+CONFIG_NSH_CMDPARMS=y
+CONFIG_NSH_CROMFSETC=y
+CONFIG_NSH_LINELEN=128
+CONFIG_NSH_MAXARGUMENTS=15
+CONFIG_NSH_MMCSDSPIPORTNO=1
+CONFIG_NSH_NESTDEPTH=8
+CONFIG_NSH_QUOTE=y
+CONFIG_NSH_ROMFSETC=y
+CONFIG_NSH_ROMFSSECTSIZE=128
+CONFIG_NSH_STRERROR=y
+CONFIG_NSH_TELNET=y
+CONFIG_NSH_TELNET_LOGIN=y
+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
+CONFIG_RAW_BINARY=y
+CONFIG_S32K3XX_DTCM_HEAP=y
+CONFIG_S32K3XX_EDMA=y
+CONFIG_S32K3XX_EDMA_EDBG=y
+CONFIG_S32K3XX_EDMA_NTCD=64
+CONFIG_S32K3XX_EIRQINTS=y
+CONFIG_S32K3XX_ENET=y
+CONFIG_S32K3XX_ENET_NTXBUFFERS=4
+CONFIG_S32K3XX_FLEXCAN0=y
+CONFIG_S32K3XX_FLEXCAN1=y
+CONFIG_S32K3XX_FLEXCAN2=y
+CONFIG_S32K3XX_FLEXCAN3=y
+CONFIG_S32K3XX_FS26=y
+CONFIG_S32K3XX_GPIOIRQ=y
+CONFIG_S32K3XX_LPI2C0=y
+CONFIG_S32K3XX_LPI2C1=y
+CONFIG_S32K3XX_LPI2C_DMA=y
+CONFIG_S32K3XX_LPSPI1=y
+CONFIG_S32K3XX_LPSPI1_PINCFG=3
+CONFIG_S32K3XX_LPSPI2=y
+CONFIG_S32K3XX_LPSPI2_DMA=y
+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
+CONFIG_S32K3XX_LPSPI5_PINCFG=3
+CONFIG_S32K3XX_LPSPI_DMA=y
+CONFIG_S32K3XX_LPUART0=y
+CONFIG_S32K3XX_LPUART10=y
+CONFIG_S32K3XX_LPUART13=y
+CONFIG_S32K3XX_LPUART14=y
+CONFIG_S32K3XX_LPUART1=y
+CONFIG_S32K3XX_LPUART2=y
+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
+CONFIG_SCHED_INSTRUMENTATION=y
+CONFIG_SCHED_INSTRUMENTATION_EXTERNAL=y
+CONFIG_SCHED_INSTRUMENTATION_SWITCH=y
+CONFIG_SCHED_LPWORK=y
+CONFIG_SCHED_LPWORKPRIORITY=50
+CONFIG_SCHED_LPWORKSTACKSIZE=1768
+CONFIG_SCHED_WAITPID=y
+CONFIG_SEM_PREALLOCHOLDERS=32
+CONFIG_SERIAL_TERMIOS=y
+CONFIG_SIG_DEFAULT=y
+CONFIG_SIG_SIGALRM_ACTION=y
+CONFIG_SIG_SIGUSR1_ACTION=y
+CONFIG_SIG_SIGUSR2_ACTION=y
+CONFIG_SIG_SIGWORK=4
+CONFIG_STACK_COLORATION=y
+CONFIG_START_DAY=30
+CONFIG_START_MONTH=11
+CONFIG_STDIO_BUFFER_SIZE=256
+CONFIG_SYSTEM_DHCPC_RENEW=y
+CONFIG_SYSTEM_NSH=y
+CONFIG_SYSTEM_PING=y
+CONFIG_TASK_NAME_SIZE=24
@@ -0,0 +1,162 @@
+/****************************************************************************
+ * boards/arm/s32k3xx/mr-canhubk3/scripts/flash.ld
+ *
+ * Licensed to the Apache Software Foundation (ASF) under one or more
+ * contributor license agreements.  See the NOTICE file distributed with
+ * this work for additional information regarding copyright ownership.  The
+ * ASF licenses this file to you under the Apache License, Version 2.0 (the
+ * "License"); you may not use this file except in compliance with the
+ * License.  You may obtain a copy of the License at
+ *
+ *   http://www.apache.org/licenses/LICENSE-2.0
+ *
+ * Unless required by applicable law or agreed to in writing, software
+ * distributed under the License is distributed on an "AS IS" BASIS, WITHOUT
+ * WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.  See the
+ * License for the specific language governing permissions and limitations
+ * under the License.
+ *
+ ****************************************************************************/
+
+/* Copyright 2022 NXP */
+
+/* TO DO: ADD DESCRIPTION
+ *
+ *   0x00400000 - 0x007fffff  4194304  Program Flash (last 64K sBAF)
+ *   0x10000000 - 0x1003ffff   262144  Data Flash (last 32K HSE_NVM)
+ *   0x20400000 - 0x20408000    32768  Standby RAM_0 (32K)
+ *   0x20400000 - 0x20427fff   163840  SRAM_0
+ *   0x20428000 - 0x2044ffff   163840  SRAM_1
+ *
+ *   Last  48 KB of SRAM_1 reserved by HSE Firmware
+ *   Last 128 KB of CODE_FLASH_3 reserved by HSE Firmware
+ *   Last 128 KB of DATA_FLASH reserved by HSE Firmware (not supported in this linker file)
+ *
+ *   Note Standby RAM and SRAM overlaps in NuttX since we dont use the Standby functionality
+ *
+ */
+
+MEMORY
+{
+  BOOT_HEADER (R)   : ORIGIN = 0x00400000, LENGTH = 0x00001000  /* 0x00400000 - 0x00400fff */
+  flash       (rx)  : ORIGIN = 0x00401000, LENGTH = 0x003cffff  /* 0x00401000 - (0x007fffff - 0x20000 (128 KB) = 0x007dffff) */
+  sram0_stdby (rwx) : ORIGIN = 0x20400000, LENGTH = 32K
+  sram        (rwx) : ORIGIN = 0x20400000, LENGTH = 272K
+  itcm        (rwx) : ORIGIN = 0x00000000, LENGTH = 64K
+  dtcm        (rwx) : ORIGIN = 0x20000000, LENGTH = 128K
+}
+
+OUTPUT_ARCH(arm)
+EXTERN(_vectors)
+EXTERN(boot_header)
+ENTRY(_stext)
+
+SECTIONS
+{
+
+  .boot_header :
+  {
+    KEEP(*(.boot_header))
+  } > BOOT_HEADER
+
+  .text :
+  {
+    _stext = ABSOLUTE(.);
+    *(.vectors)
+    *(.text.__start)
+    *(.text .text.*)
+    *(.fixup)
+    *(.gnu.warning)
+    *(.rodata .rodata.*)
+    *(.gnu.linkonce.t.*)
+    *(.glue_7)
+    *(.glue_7t)
+    *(.got)
+    *(.gcc_except_table)
+    *(.gnu.linkonce.r.*)
+    _etext = ABSOLUTE(.);
+  } > flash
+
+  .init_section :
+  {
+    _sinit = ABSOLUTE(.);
+    KEEP(*(.init_array .init_array.*))
+    _einit = ABSOLUTE(.);
+  } > flash
+
+  .ARM.extab :
+  {
+    *(.ARM.extab*)
+  } >flash
+
+  .ARM.exidx :
+  {
+    __exidx_start = ABSOLUTE(.);
+    *(.ARM.exidx*)
+    __exidx_end = ABSOLUTE(.);
+  } >flash
+
+  /* Due ECC initialization sequence __data_start__ and __data_end__ should be aligned on 8 bytes */
+  .data :
+  {
+    . = ALIGN(8);
+    _sdata = ABSOLUTE(.);
+    *(.data .data.*)
+    *(.gnu.linkonce.d.*)
+    CONSTRUCTORS
+    . = ALIGN(8);
+    _edata = ABSOLUTE(.);
+  } > sram AT > flash
+
+  _eronly = LOADADDR(.data);
+
+  .ramfunc ALIGN(8):
+  {
+    _sramfuncs = ABSOLUTE(.);
+    *(.ramfunc  .ramfunc.*)
+    _eramfuncs = ABSOLUTE(.);
+  } > sram AT > flash
+
+  _framfuncs = LOADADDR(.ramfunc);
+
+  /* Due ECC initialization sequence __bss_start__ and __bss_end__ should be aligned on 8 bytes */
+  .bss :
+  {
+    . = ALIGN(8);
+    _sbss = ABSOLUTE(.);
+    *(.bss .bss.*)
+    *(.gnu.linkonce.b.*)
+    *(COMMON)
+    . = ALIGN(8);
+    _ebss = ABSOLUTE(.);
+  } > sram
+
+  CM7_0_START_ADDRESS = ORIGIN(flash);
+
+  /* Stabs debugging sections. */
+
+  .stab 0 : { *(.stab) }
+  .stabstr 0 : { *(.stabstr) }
+  .stab.excl 0 : { *(.stab.excl) }
+  .stab.exclstr 0 : { *(.stab.exclstr) }
+  .stab.index 0 : { *(.stab.index) }
+  .stab.indexstr 0 : { *(.stab.indexstr) }
+  .comment 0 : { *(.comment) }
+  .debug_abbrev 0 : { *(.debug_abbrev) }
+  .debug_info 0 : { *(.debug_info) }
+  .debug_line 0 : { *(.debug_line) }
+  .debug_pubnames 0 : { *(.debug_pubnames) }
+  .debug_aranges 0 : { *(.debug_aranges) }
+
+  SRAM_BASE_ADDR         = ORIGIN(sram);
+  SRAM_END_ADDR          = ORIGIN(sram) + LENGTH(sram);
+  SRAM_STDBY_BASE_ADDR   = ORIGIN(sram0_stdby);
+  SRAM_STDBY_END_ADDR    = ORIGIN(sram0_stdby) + LENGTH(sram0_stdby);
+  SRAM_INIT_END_ADDR     = ORIGIN(sram) + 320K;
+  ITCM_BASE_ADDR         = ORIGIN(itcm);
+  ITCM_END_ADDR          = ORIGIN(itcm) + LENGTH(itcm);
+  DTCM_BASE_ADDR         = ORIGIN(dtcm);
+  DTCM_END_ADDR          = ORIGIN(dtcm) + LENGTH(dtcm);
+  FLASH_BASE_ADDR        = ORIGIN(BOOT_HEADER);
+  FLASH_END_ADDR         = ORIGIN(flash) + LENGTH(flash);
+}
--- a/Show More
+++ b/Show More