split drivers/rc_input per protocol

mavlink_shell: fix stall on CubeOrange
On CubeOrange where no console is configured by default, starting MAVLink shell just stalls, and doesn't work. Also, logfile download has been reported not to work, and again, seems to work with this change. Signed-off-by: Julian Oes <julian@oes.ch>
2026-05-24 01:37:35 +08:00 · 2022-12-23 18:49:16 -05:00 · 2022-12-23 11:00:20 -05:00 · 2022-12-23 10:28:19 -05:00 · 2022-12-23 10:28:19 -05:00 · 2022-12-23 10:28:19 -05:00
538 changed files with 22662 additions and 12683 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" || true' // expected to fail after erase

  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

@@ -119,7 +119,9 @@
                "utility": "cpp",
                "valarray": "cpp",
                "variant": "cpp",
-                "vector": "cpp"
+                "vector": "cpp",
+                "Jenkinsfile*": "groovy",
+                "*.sdf": "xml"
        },
        "search.exclude": {
                "${workspaceFolder}/build": true
@@ -133,6 +135,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
@@ -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
@@ -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
@@ -29,7 +29,6 @@ set LOGGER_ARGS ""
 set LOGGER_BUF 8
 set PARAM_FILE ""
 set PARAM_BACKUP_FILE ""
-set RC_INPUT_ARGS ""
 set SDCARD_AVAILABLE no
 set SDCARD_EXT_PATH /fs/microsd/ext_autostart
 set SDCARD_FORMAT no
@@ -130,7 +129,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 +142,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

@@ -411,8 +410,10 @@ else
 	#
 	. ${R}etc/init.d/rc.serial

-	# Must be started after the serial config is read
-	rc_input start $RC_INPUT_ARGS
+	if param compare -s RC_PPM_EN 1
+	then
+		ppm_in start
+	fi

 	#
 	# Play the startup tune (if not disabled or there is an error)
@@ -544,7 +545,6 @@ unset LOGGER_ARGS
 unset LOGGER_BUF
 unset PARAM_FILE
 unset PARAM_BACKUP_FILE
-unset RC_INPUT_ARGS
 unset SDCARD_AVAILABLE
 unset SDCARD_EXT_PATH
 unset SDCARD_FORMAT
@@ -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 = [
@@ -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>
@@ -4,6 +4,7 @@ CONFIG_BOARD_CONSTRAINED_MEMORY=y
 CONFIG_BOARD_SERIAL_GPS1="/dev/ttyS3"
 CONFIG_BOARD_SERIAL_TEL1="/dev/ttyS1"
 CONFIG_BOARD_SERIAL_TEL2="/dev/ttyS2"
+CONFIG_BOARD_SERIAL_RC="/dev/ttyS0"
 CONFIG_DRIVERS_ADC_ADS1115=y
 CONFIG_DRIVERS_ADC_BOARD_ADC=y
 CONFIG_COMMON_BAROMETERS=y
@@ -28,7 +29,8 @@ CONFIG_DRIVERS_PCA9685_PWM_OUT=y
 CONFIG_DRIVERS_POWER_MONITOR_INA226=y
 CONFIG_DRIVERS_PWM_INPUT=y
 CONFIG_DRIVERS_PWM_OUT=y
-CONFIG_DRIVERS_RC_INPUT=y
+CONFIG_COMMON_RC=y
+CONFIG_DRIVERS_RC_PPM_IN=y
 CONFIG_DRIVERS_ROBOCLAW=y
 CONFIG_DRIVERS_RPM=y
 CONFIG_DRIVERS_SMART_BATTERY_BATMON=y
@@ -79,8 +81,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
@@ -7,9 +7,9 @@ CONFIG_BOARD_SERIAL_TEL1="/dev/ttyS6"
 CONFIG_BOARD_SERIAL_TEL2="/dev/ttyS4"
 CONFIG_BOARD_SERIAL_TEL3="/dev/ttyS1"
 CONFIG_BOARD_SERIAL_TEL4="/dev/ttyS3"
+CONFIG_BOARD_SERIAL_RC="/dev/ttyS5"
 CONFIG_DRIVERS_ADC_BOARD_ADC=y
 CONFIG_DRIVERS_BAROMETER_BMP388=y
-CONFIG_DRIVERS_BAROMETER_MS5611=y
 CONFIG_DRIVERS_BATT_SMBUS=y
 CONFIG_DRIVERS_CAMERA_CAPTURE=y
 CONFIG_DRIVERS_CAMERA_TRIGGER=y
@@ -28,7 +28,8 @@ CONFIG_DRIVERS_POWER_MONITOR_INA228=y
 CONFIG_DRIVERS_POWER_MONITOR_INA238=y
 CONFIG_DRIVERS_PWM_OUT=y
 CONFIG_DRIVERS_PX4IO=y
-CONFIG_DRIVERS_RC_INPUT=y
+CONFIG_COMMON_RC=y
+CONFIG_DRIVERS_RC_PPM_IN=y
 CONFIG_DRIVERS_SAFETY_BUTTON=y
 CONFIG_DRIVERS_TONE_ALARM=y
 CONFIG_DRIVERS_UAVCAN=y
@@ -66,10 +67,10 @@ CONFIG_MODULES_ROVER_POS_CONTROL=y
 CONFIG_MODULES_SENSORS=y
 CONFIG_MODULES_SIMULATION_PWM_OUT_SIM=y
 CONFIG_MODULES_SIMULATION_SIMULATOR_SIH=y
-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
@@ -16,6 +16,13 @@ param set-default SENS_EN_INA238 0
 param set-default SENS_EN_INA228 0
 param set-default SENS_EN_INA226 1

+param set-default SENS_EN_THERMAL 1
+param set-default SENS_TEMP_ID 2818058
+param set-default SENS_IMU_TEMP 10.0
+#param set-default SENS_IMU_TEMP_FF 0.0
+#param set-default SENS_IMU_TEMP_I 0.025
+#param set-default SENS_IMU_TEMP_P 1.0
+
 if ver hwtypecmp ARKV6X001000 ARKV6X001001 ARKV6X001002 ARKV6X001003 ARKV6X001004 ARKV6X001005 ARKV6X001006 ARKV6X001007
 then
 	param set-default SYS_USE_IO 0
@@ -8,6 +8,7 @@ if ver hwtypecmp ARKV6X005000 ARKV6X005001 ARKV6X005002 ARKV6X005003 ARKV6X00500
 then
 	set HAVE_PM2 no
 fi
+
 if param compare -s ADC_ADS1115_EN 1
 then
 	ads1115 start -X
@@ -15,7 +16,6 @@ else
 	board_adc start
 fi

-
 if param compare SENS_EN_INA226 1
 then
 	# Start Digital power monitors
@@ -56,16 +56,10 @@ icm42688p -R 9 -s -b 2 start
 # Internal SPI bus ICM42688p
 icm42688p -R 6 -s -b 3 start

-# Internal magnetometer on I2c
+# 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 1 -R 10 start
-
-# Possible internal Baro
+# Internal Baro on I2C
 bmp388 -I start

-# Baro on I2C3
-ms5611 -X start
-
 unset HAVE_PM2
@@ -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
@@ -323,7 +323,6 @@
 #define GPIO_PPM_IN             /* PI5 T8C1 */ GPIO_TIM8_CH1IN_2

 /* RC Serial port */
-
 #define RC_SERIAL_PORT                     "/dev/ttyS5"

 /* Input Capture Channels. */
@@ -145,10 +145,6 @@
 #define HRT_TIMER               8  /* use timer8 for the HRT */
 #define HRT_TIMER_CHANNEL       3  /* use capture/compare channel 3 */

-/* RC Serial port */
-#define RC_SERIAL_PORT                     "/dev/ttyS5"
-#define RC_SERIAL_SINGLEWIRE
-
 /* power on/off */
 #define MS_PWR_BUTTON_DOWN 750
 #define KEY_AD_GPIO    (GPIO_INPUT|GPIO_PULLUP|GPIO_EXTI|GPIO_PORTC|GPIO_PIN4)
@@ -6,6 +6,7 @@ CONFIG_BOARD_SERIAL_TEL1="/dev/ttyS0"
 CONFIG_BOARD_SERIAL_TEL2="/dev/ttyS1"
 CONFIG_BOARD_SERIAL_TEL3="/dev/ttyS2"
 CONFIG_BOARD_SERIAL_TEL4="/dev/ttyS3"
+CONFIG_BOARD_SERIAL_RC="/dev/ttyS4"
 CONFIG_DRIVERS_ADC_ADS1115=y
 CONFIG_DRIVERS_ADC_BOARD_ADC=y
 CONFIG_COMMON_BAROMETERS=y
@@ -22,14 +23,12 @@ CONFIG_DRIVERS_IRLOCK=y
 CONFIG_COMMON_LIGHT=y
 CONFIG_COMMON_MAGNETOMETER=y
 CONFIG_COMMON_OPTICAL_FLOW=y
-CONFIG_DRIVERS_OSD=y
 CONFIG_DRIVERS_PCA9685=y
 CONFIG_DRIVERS_PCA9685_PWM_OUT=y
 CONFIG_DRIVERS_POWER_MONITOR_INA226=y
 CONFIG_DRIVERS_PWM_OUT=y
-CONFIG_DRIVERS_RC_INPUT=y
+CONFIG_COMMON_RC=y
 CONFIG_DRIVERS_ROBOCLAW=y
-CONFIG_DRIVERS_RPM=y
 CONFIG_DRIVERS_SMART_BATTERY_BATMON=y
 CONFIG_COMMON_TELEMETRY=y
 CONFIG_DRIVERS_UAVCAN=y
@@ -38,6 +37,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
@@ -46,6 +46,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
@@ -61,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
@@ -74,10 +74,10 @@ CONFIG_MODULES_SIMULATION_SIMULATOR_SIH=y
 CONFIG_MODULES_TEMPERATURE_COMPENSATION=y
 CONFIG_MODULES_UUV_ATT_CONTROL=y
 CONFIG_MODULES_UUV_POS_CONTROL=y
-CONFIG_MODULES_GIMBAL=y
 CONFIG_MODULES_VTOL_ATT_CONTROL=y
-CONFIG_SYSTEMCMDS_BL_UPDATE=y
 CONFIG_SYSTEMCMDS_ACTUATOR_TEST=y
+CONFIG_SYSTEMCMDS_BL_UPDATE=y
+CONFIG_SYSTEMCMDS_BSONDUMP=y
 CONFIG_SYSTEMCMDS_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
@@ -103,7 +103,6 @@
 #define HRT_TIMER_CHANNEL    1  /* use capture/compare channel 3 */

 /* RC Serial port */
-
 #define RC_SERIAL_PORT                     "/dev/ttyS4"

 /* Power switch controls ******************************************************/
@@ -74,7 +74,7 @@ uint32_t px4_arch_adc_sample(uint32_t base_address, unsigned channel)
 		// ADC_SYSFS_PATH
 		char channel_path[strlen(ADC_SYSFS_PATH) + 20] {};

-		if (snprintf(channel_path, sizeof(channel_path), "%s/in_voltage%d_raw", ADC_SYSFS_PATH, channel) == -1) {
+		if (sprintf(channel_path, "%s/in_voltage%d_raw", ADC_SYSFS_PATH, channel) == -1) {
 			PX4_ERR("adc channel: %d\n", channel);
 			return UINT32_MAX; // error
 		}
@@ -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,8 +1,9 @@
 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
+CONFIG_BOARD_SERIAL_RC="/dev/ttyS4"
 CONFIG_DRIVERS_ADC_ADS1115=y
 CONFIG_DRIVERS_ADC_BOARD_ADC=y
 CONFIG_DRIVERS_BAROMETER_BMP280=y
@@ -16,7 +17,7 @@ CONFIG_DRIVERS_IMU_INVENSENSE_ICM20948=y
 CONFIG_DRIVERS_IMU_INVENSENSE_MPU9250=y
 CONFIG_DRIVERS_LINUX_PWM_OUT=y
 CONFIG_DRIVERS_MAGNETOMETER_HMC5883=y
-CONFIG_DRIVERS_RC_INPUT=y
+CONFIG_COMMON_RC=y
 CONFIG_DRIVERS_SMART_BATTERY_BATMON=y
 CONFIG_MODULES_AIRSPEED_SELECTOR=y
 CONFIG_MODULES_ATTITUDE_ESTIMATOR_Q=y
@@ -63,6 +64,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
@@ -31,7 +31,9 @@ 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_COMMON_RC=y
+CONFIG_BOARD_SERIAL_RC="/dev/ttyS5"
+CONFIG_DRIVERS_RC_PPM_IN=y
 CONFIG_DRIVERS_ROBOCLAW=y
 CONFIG_DRIVERS_RPM=y
 CONFIG_DRIVERS_SAFETY_BUTTON=y
@@ -81,8 +83,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
@@ -4,6 +4,7 @@ CONFIG_BOARD_SERIAL_GPS1="/dev/ttyS0"
 CONFIG_BOARD_SERIAL_GPS2="/dev/ttyS2"
 CONFIG_BOARD_SERIAL_TEL1="/dev/ttyS1"
 CONFIG_BOARD_SERIAL_TEL2="/dev/ttyS3"
+CONFIG_BOARD_SERIAL_RC="/dev/ttyS5"
 CONFIG_DRIVERS_ADC_ADS1115=y
 CONFIG_DRIVERS_ADC_BOARD_ADC=y
 CONFIG_COMMON_BAROMETERS=y
@@ -27,14 +28,13 @@ 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_COMMON_RC=y
+CONFIG_DRIVERS_RC_PPM_IN=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 +46,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 +55,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 +71,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 +81,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 +107,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
@@ -16,7 +16,9 @@ CONFIG_DRIVERS_IMU_INVENSENSE_MPU9250=y
 CONFIG_DRIVERS_MAGNETOMETER_AKM_AK8963=y
 CONFIG_DRIVERS_MAGNETOMETER_HMC5883=y
 CONFIG_DRIVERS_PWM_OUT=y
-CONFIG_DRIVERS_RC_INPUT=y
+CONFIG_COMMON_RC=y
+CONFIG_BOARD_SERIAL_RC="/dev/ttyS0"
+CONFIG_DRIVERS_RC_PPM_IN=y
 CONFIG_MODULES_ATTITUDE_ESTIMATOR_Q=y
 CONFIG_MODULES_BATTERY_STATUS=y
 CONFIG_MODULES_COMMANDER=y
@@ -132,7 +132,6 @@
 #define GPIO_PPM_IN                  (GPIO_ALT|GPIO_AF2|GPIO_PULLUP|GPIO_PORTB|GPIO_PIN8)

 #define RC_SERIAL_PORT               "/dev/ttyS0"
-#define BOARD_SUPPORTS_RC_SERIAL_PORT_OUTPUT

 /*
 * One RC_IN
@@ -74,7 +74,7 @@ uint32_t px4_arch_adc_sample(uint32_t base_address, unsigned channel)
 		// ADC_SYSFS_PATH
 		char channel_path[strlen(ADC_SYSFS_PATH) + 5] {};

-		if (snprintf(channel_path, sizeof(channel_path), "%s/ch%d", ADC_SYSFS_PATH, channel) == -1) {
+		if (sprintf(channel_path, "%s/ch%d", ADC_SYSFS_PATH, channel) == -1) {
 			PX4_ERR("adc channel: %d\n", channel);
 			return UINT32_MAX; // error
 		}
@@ -1,8 +1,9 @@
 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
+CONFIG_BOARD_SERIAL_RC="/dev/ttyS4"
 CONFIG_DRIVERS_ADC_ADS1115=y
 CONFIG_DRIVERS_ADC_BOARD_ADC=y
 CONFIG_DRIVERS_BAROMETER_MS5611=y
@@ -18,7 +19,7 @@ CONFIG_DRIVERS_IMU_ST_LSM9DS1=y
 CONFIG_DRIVERS_LINUX_PWM_OUT=y
 CONFIG_DRIVERS_MAGNETOMETER_HMC5883=y
 CONFIG_DRIVERS_MAGNETOMETER_LSM9DS1_MAG=y
-CONFIG_DRIVERS_RC_INPUT=y
+CONFIG_COMMON_RC=y
 CONFIG_DRIVERS_SMART_BATTERY_BATMON=y
 CONFIG_MODULES_AIRSPEED_SELECTOR=y
 CONFIG_MODULES_ATTITUDE_ESTIMATOR_Q=y
@@ -66,6 +67,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
@@ -5,6 +5,7 @@ CONFIG_BOARD_NO_HELP=y
 CONFIG_BOARD_CONSTRAINED_MEMORY=y
 CONFIG_BOARD_SERIAL_URT6="/dev/ttyS2"
 CONFIG_BOARD_SERIAL_TEL2="/dev/ttyS1"
+CONFIG_BOARD_SERIAL_RC="/dev/ttyS0"
 CONFIG_DRIVERS_ADC_BOARD_ADC=y
 CONFIG_DRIVERS_BAROMETER_BMP280=y
 CONFIG_DRIVERS_DSHOT=y
@@ -135,7 +135,6 @@
 #define GPIO_PPM_IN                  (GPIO_ALT|GPIO_AF2|GPIO_PULLUP|GPIO_PORTB|GPIO_PIN8)

 #define RC_SERIAL_PORT               "/dev/ttyS0"
-#define BOARD_SUPPORTS_RC_SERIAL_PORT_OUTPUT

 /*
 * One RC_IN
@@ -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
@@ -12,9 +12,8 @@ CONFIG_DRIVERS_BAROMETER_BMP280=y
 CONFIG_DRIVERS_DSHOT=y
 CONFIG_DRIVERS_IMU_INVENSENSE_ICM20689=y
 CONFIG_DRIVERS_IMU_INVENSENSE_MPU6000=y
-CONFIG_DRIVERS_OSD=y
 CONFIG_DRIVERS_PWM_OUT=y
-CONFIG_DRIVERS_RC_INPUT=y
+CONFIG_COMMON_RC=y
 CONFIG_DRIVERS_TELEMETRY_FRSKY_TELEMETRY=y
 CONFIG_MODULES_ATTITUDE_ESTIMATOR_Q=y
 CONFIG_MODULES_BATTERY_STATUS=y
@@ -111,7 +111,6 @@
 #define HRT_TIMER_CHANNEL       1  /* use capture/compare channel 1 */

 /* RC Serial port */
-
 #define RC_SERIAL_PORT                     "/dev/ttyS4"

 #define GPIO_RSSI_IN                       /* PC5  */ (GPIO_INPUT|GPIO_PULLUP|GPIO_PORTC|GPIO_PIN5)
@@ -18,12 +18,10 @@ CONFIG_DRIVERS_IMU_INVENSENSE_MPU6000=y
 CONFIG_COMMON_LIGHT=y
 CONFIG_COMMON_MAGNETOMETER=y
 CONFIG_COMMON_OPTICAL_FLOW=y
-CONFIG_DRIVERS_OSD=y
 CONFIG_DRIVERS_POWER_MONITOR_INA226=y
 CONFIG_DRIVERS_PWM_OUT=y
-CONFIG_DRIVERS_RC_INPUT=y
+CONFIG_COMMON_RC=y
 CONFIG_DRIVERS_ROBOCLAW=y
-CONFIG_DRIVERS_RPM=y
 CONFIG_DRIVERS_SMART_BATTERY_BATMON=y
 CONFIG_COMMON_TELEMETRY=y
 CONFIG_DRIVERS_TONE_ALARM=y
@@ -71,6 +69,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
@@ -34,7 +34,9 @@ CONFIG_DRIVERS_PWM_INPUT=y
 CONFIG_DRIVERS_PWM_OUT=y
 CONFIG_MODULES_SIMULATION_PWM_OUT_SIM=y
 CONFIG_DRIVERS_PX4IO=y
-CONFIG_DRIVERS_RC_INPUT=y
+CONFIG_COMMON_RC=y
+CONFIG_BOARD_SERIAL_RC="/dev/ttyS4"
+CONFIG_DRIVERS_RC_PPM_IN=y
 CONFIG_DRIVERS_ROBOCLAW=y
 CONFIG_DRIVERS_RPM=y
 CONFIG_DRIVERS_SAFETY_BUTTON=y
@@ -85,8 +87,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
@@ -283,7 +283,6 @@
 #define GPIO_PPM_IN             /* PI5 T8C1 */ GPIO_TIM8_CH1IN_2

 /* RC Serial port */
-
 #define RC_SERIAL_PORT                     "/dev/ttyS4"
 #define RC_SERIAL_SINGLEWIRE

@@ -22,7 +22,8 @@ CONFIG_COMMON_OPTICAL_FLOW=y
 CONFIG_DRIVERS_OSD=y
 CONFIG_DRIVERS_PWM_OUT=y
 CONFIG_DRIVERS_PWM_OUT_SIM=n
-CONFIG_DRIVERS_RC_INPUT=y
+CONFIG_COMMON_RC=y
+CONFIG_BOARD_SERIAL_RC="/dev/ttyS4"
 CONFIG_DRIVERS_RPM=n
 CONFIG_COMMON_TELEMETRY=y
 CONFIG_DRIVERS_TONE_ALARM=y
@@ -159,7 +159,6 @@

 /* RC Serial port */
 #define RC_SERIAL_PORT          "/dev/ttyS4"
-#define BOARD_SUPPORTS_RC_SERIAL_PORT_OUTPUT

 // #define GPIO_RSSI_IN            /* PC5  */ (GPIO_INPUT|GPIO_PULLUP|GPIO_PORTC|GPIO_PIN5)

@@ -21,7 +21,8 @@ CONFIG_COMMON_OPTICAL_FLOW=y
 CONFIG_DRIVERS_OSD=y
 CONFIG_DRIVERS_PWM_OUT=y
 CONFIG_DRIVERS_PWM_OUT_SIM=n
-CONFIG_DRIVERS_RC_INPUT=y
+CONFIG_COMMON_RC=y
+CONFIG_BOARD_SERIAL_RC="/dev/ttyS4"
 CONFIG_DRIVERS_RPM=n
 CONFIG_COMMON_TELEMETRY=y
 CONFIG_DRIVERS_TONE_ALARM=y
@@ -150,7 +150,6 @@

 /* RC Serial port */
 #define RC_SERIAL_PORT          "/dev/ttyS4"
-#define BOARD_SUPPORTS_RC_SERIAL_PORT_OUTPUT

 // #define GPIO_RSSI_IN            /* PC5  */ (GPIO_INPUT|GPIO_PULLUP|GPIO_PORTC|GPIO_PIN5)

@@ -22,7 +22,8 @@ CONFIG_COMMON_OPTICAL_FLOW=y
 CONFIG_DRIVERS_OSD=y
 CONFIG_DRIVERS_PWM_OUT=y
 CONFIG_DRIVERS_PWM_OUT_SIM=n
-CONFIG_DRIVERS_RC_INPUT=y
+CONFIG_COMMON_RC=y
+CONFIG_BOARD_SERIAL_RC="/dev/ttyS4"
 CONFIG_DRIVERS_RPM=n
 CONFIG_COMMON_TELEMETRY=y
 CONFIG_DRIVERS_TONE_ALARM=y
@@ -159,7 +159,6 @@

 /* RC Serial port */
 #define RC_SERIAL_PORT          "/dev/ttyS4"
-#define BOARD_SUPPORTS_RC_SERIAL_PORT_OUTPUT

 // #define GPIO_RSSI_IN            /* PC5  */ (GPIO_INPUT|GPIO_PULLUP|GPIO_PORTC|GPIO_PIN5)

@@ -30,7 +30,9 @@ CONFIG_DRIVERS_PCA9685_PWM_OUT=y
 CONFIG_DRIVERS_POWER_MONITOR_INA226=y
 CONFIG_DRIVERS_POWER_MONITOR_VOXLPM=y
 CONFIG_DRIVERS_PWM_OUT=y
-CONFIG_DRIVERS_RC_INPUT=y
+CONFIG_COMMON_RC=y
+CONFIG_BOARD_SERIAL_RC="/dev/ttyS5"
+CONFIG_DRIVERS_RC_PPM_IN=y
 CONFIG_DRIVERS_ROBOCLAW=y
 CONFIG_DRIVERS_SAFETY_BUTTON=y
 CONFIG_DRIVERS_SMART_BATTERY_BATMON=y
@@ -82,6 +84,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
@@ -182,9 +182,7 @@
 #define GPIO_PPM_IN             /* PI5 T8C1 */ GPIO_TIM8_CH1IN_2

 /* RC Serial port */
-
 #define RC_SERIAL_PORT                     "/dev/ttyS5"
-#define BOARD_SUPPORTS_RC_SERIAL_PORT_OUTPUT

 /* Safety Switch: Enable the FMU to control it as there is no px4io in ModalAI FC-v1 */
 #define GPIO_SAFETY_SWITCH_IN              /* PF3 */ (GPIO_INPUT|GPIO_PULLDOWN|GPIO_PORTF|GPIO_PIN3)
@@ -3,6 +3,7 @@ CONFIG_BOARD_ARCHITECTURE="cortex-m7"
 CONFIG_BOARD_SERIAL_GPS1="/dev/ttyS0"
 CONFIG_BOARD_SERIAL_TEL1="/dev/ttyS6"
 CONFIG_BOARD_SERIAL_TEL2="/dev/ttyS4"
+CONFIG_BOARD_SERIAL_RC="/dev/ttyS5"
 CONFIG_DRIVERS_ACTUATORS_MODALAI_ESC=y
 CONFIG_DRIVERS_ADC_ADS1115=y
 CONFIG_DRIVERS_ADC_BOARD_ADC=y
@@ -29,7 +30,7 @@ CONFIG_DRIVERS_PCA9685_PWM_OUT=y
 CONFIG_DRIVERS_POWER_MONITOR_INA226=y
 CONFIG_DRIVERS_POWER_MONITOR_VOXLPM=y
 CONFIG_DRIVERS_PWM_OUT=y
-CONFIG_DRIVERS_RC_INPUT=y
+CONFIG_COMMON_RC=y
 CONFIG_DRIVERS_ROBOCLAW=y
 CONFIG_COMMON_TELEMETRY=y
 CONFIG_DRIVERS_UAVCAN=y
@@ -76,6 +77,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
@@ -237,7 +237,6 @@
 //#define GPIO_PPM_IN             /* PI5 T8C1 */ GPIO_TIM8_CH1IN_2

 /* RC Serial port */
-
 #define RC_SERIAL_PORT                     "/dev/ttyS5"

 /* Input Capture Channels. */
@@ -3,3 +3,13 @@ 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
+CONFIG_DRIVERS_ACTUATORS_MODALAI_ESC=y
+CONFIG_MODULES_SENSORS=y
+CONFIG_MODULES_EKF2=y
+CONFIG_MODULES_MC_POS_CONTROL=y
+CONFIG_MODULES_MC_ATT_CONTROL=y
+CONFIG_MODULES_MC_RATE_CONTROL=y
+CONFIG_MODULES_MC_HOVER_THRUST_ESTIMATOR=y
+CONFIG_MODULES_LAND_DETECTOR=y
@@ -31,7 +31,16 @@
 #
 ############################################################################

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

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

-using namespace matrix;
-
-TEST(AngularVelocityControlTest, AllZeroCase)
-{
-	AngularVelocityControl control;
-	control.update(Vector3f(), Vector3f(), Vector3f(), 0.f, false);
-	Vector3f torque = control.getTorqueSetpoint();
-	EXPECT_EQ(torque, Vector3f());
-}
+constexpr px4_spi_bus_t px4_spi_buses[SPI_BUS_MAX_BUS_ITEMS] = {
+	initSPIBus(1, {initSPIDevice(DRV_IMU_DEVTYPE_ICM42688P), }),
+};
@@ -1,7 +1,18 @@
 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
+CONFIG_MODULES_LOGGER=y
+CONFIG_MODULES_RC_UPDATE=y
+CONFIG_MODULES_DATAMAN=y
+CONFIG_MODULES_NAVIGATOR=y
+CONFIG_MODULES_COMMANDER=y
+CONFIG_MODULES_FLIGHT_MODE_MANAGER=y
+CONFIG_MODULES_MAVLINK=y
@@ -31,6 +31,12 @@
 #
 ############################################################################

+# Need to make sure that the Linux processor on VOXL2
+# knows about all parameters since it is acting as the
+# parameter server for other processors that may define
+# parameters that it doesn't normally know about.
+set(DISABLE_PARAMS_MODULE_SCOPING TRUE PARENT_SCOPE)
+
 add_library(drivers_board
 	board_config.h
 	init.c
@@ -47,3 +47,6 @@

 #include <system_config.h>
 #include <px4_platform_common/board_common.h>
+
+#define BOARD_OVERRIDE_UUID "MODALAIVOXL20000" // must be of length 16
+#define PX4_SOC_ARCH_ID PX4_SOC_ARCH_ID_VOXL2
@@ -17,3 +17,7 @@ if [ $TESTMODE = "ON" ]; then
 fi

 muorb start
+
+qshell icm42688p start -s
+
+qshell modalai_esc start
@@ -26,7 +26,9 @@ 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_COMMON_RC=y
+CONFIG_BOARD_SERIAL_RC="/dev/ttyS3"
+CONFIG_DRIVERS_RC_PPM_IN=y
 CONFIG_DRIVERS_ROBOCLAW=y
 CONFIG_DRIVERS_RPM=y
 CONFIG_DRIVERS_SMART_BATTERY_BATMON=y
@@ -79,6 +81,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
@@ -26,7 +26,9 @@ 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_COMMON_RC=y
+CONFIG_BOARD_SERIAL_RC="/dev/ttyS3"
+CONFIG_DRIVERS_RC_PPM_IN=y
 CONFIG_DRIVERS_ROBOCLAW=y
 CONFIG_DRIVERS_RPM=y
 CONFIG_DRIVERS_SAFETY_BUTTON=y
@@ -77,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
@@ -25,7 +25,9 @@ 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_COMMON_RC=y
+CONFIG_BOARD_SERIAL_RC="/dev/ttyS3"
+CONFIG_DRIVERS_RC_PPM_IN=y
 CONFIG_DRIVERS_ROBOCLAW=y
 CONFIG_DRIVERS_RPM=y
 CONFIG_DRIVERS_SAFETY_BUTTON=y
@@ -77,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
@@ -26,7 +26,9 @@ 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_COMMON_RC=y
+CONFIG_BOARD_SERIAL_RC="/dev/ttyS3"
+CONFIG_DRIVERS_RC_PPM_IN=y
 CONFIG_DRIVERS_ROBOCLAW=y
 CONFIG_DRIVERS_SMART_BATTERY_BATMON=y
 CONFIG_COMMON_TELEMETRY=y
@@ -78,6 +80,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,12 +21,13 @@ 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
 CONFIG_DRIVERS_PWM_OUT=y
-CONFIG_DRIVERS_RC_INPUT=y
+CONFIG_COMMON_RC=y
+CONFIG_BOARD_SERIAL_RC="/dev/ttyS3"
+CONFIG_DRIVERS_RC_PPM_IN=y
 CONFIG_DRIVERS_ROBOCLAW=y
 CONFIG_DRIVERS_SMART_BATTERY_BATMON=y
 CONFIG_COMMON_TELEMETRY=y
@@ -79,6 +80,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
@@ -26,7 +26,9 @@ 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_COMMON_RC=y
+CONFIG_BOARD_SERIAL_RC="/dev/ttyS4"
+CONFIG_DRIVERS_RC_PPM_IN=y
 CONFIG_DRIVERS_ROBOCLAW=y
 CONFIG_DRIVERS_SMART_BATTERY_BATMON=y
 CONFIG_COMMON_TELEMETRY=y
@@ -78,6 +80,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
@@ -118,7 +118,6 @@

 /* RC Serial port */
 #define RC_SERIAL_PORT          "/dev/ttyS4"
-#define BOARD_SUPPORTS_RC_SERIAL_PORT_OUTPUT

 #define GPIO_RSSI_IN            /* PC1  */ (GPIO_INPUT|GPIO_PULLUP|GPIO_PORTC|GPIO_PIN1)

@@ -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
--- a/Show More
+++ b/Show More
Author	SHA1	Message	Date
Daniel Agar	ad7b74847e	split drivers/rc_input per protocol	2022-12-23 18:49:16 -05:00
Julian Oes	bb591c6c29	mavlink_shell: fix stall on CubeOrange On CubeOrange where no console is configured by default, starting MAVLink shell just stalls, and doesn't work. Also, logfile download has been reported not to work, and again, seems to work with this change. Signed-off-by: Julian Oes <julian@oes.ch>	2022-12-23 11:00:20 -05:00
bresch	86a3d2459a	ekf2: remove old yaw estimator generated code	2022-12-23 10:28:19 -05:00
bresch	0388c161e7	ekf2: compare yaw estimator sympy vs symforce	2022-12-23 10:28:19 -05:00
bresch	7c33019510	EKF2: move yaw estimator to symforce	2022-12-23 10:28:19 -05:00
alexklimaj	2f1b3142a6	Decrease PAW3902 and PAA3905 backup schedule to 200ms	2022-12-22 20:46:19 -05:00
Eric Katzfey	c3e70b03aa	Add more to Voxl2 build and fix associated build errors (#20821 ) - Do not pull in PWM parameters when DISABLE_PARAMS_MODULE_SCOPING is TRUE since VOXL2 has no PWM nor any of the required timer_config files that go along with that - Replace non-standard M_PI constants with PX4 defined M_PI_F constants - Include missing header file for function hrt_absolute_time declaration - Add new PX4_SOC_ARCH_ID for the VOXL2 board	2022-12-22 15:44:19 -05:00
alexklimaj	35a9dba6e6	Enable ARKV6X heater	2022-12-22 15:12:32 -05:00
Daniel Agar	03c0808ae6	vscode/settings.json: add .sdf and Jenkinsfile associations	2022-12-21 16:46:25 -05:00
Silvan Fuhrer	aff3f2e77f	boards: disable gyro fft module for v5 and v5x to safe flash Signed-off-by: Silvan Fuhrer <silvan@auterion.com>	2022-12-21 10:17:59 -05:00
Daniel Agar	f2cd7667dc	systemcmds/bsondump: new command line utility (extracted from parameters)	2022-12-21 10:14:00 -05:00
Konrad	f5524fa605	TECS: Combine both airspeed and airspeed derivative filters in TECS into one MIMO filter using a steady state Kalman filter.	2022-12-21 09:04:19 +01:00
Konrad	08c36612b3	TECS: Updated throttle control for airspeed sensorless vehicles. It includes the P gain controller instead of feedforward only. I term is still disabled.	2022-12-21 09:04:19 +01:00
Konrad	77539d4dac	TECS: Rearrange setpoint input. If an altitude rate is given, use this as a feedforward term in the altitude control. If an altitude setpoint is given use a reference model to get a smooth altitude setpoint.	2022-12-21 09:04:19 +01:00
Konrad	8c6dfc840b	TECS: Fix bug to reset airspeed derivative and energy rate low pass filters at every time step.	2022-12-21 09:04:19 +01:00
Konrad	7a3e0f53c2	TECS: Replaced old tecs by cleaned up version.	2022-12-21 09:04:19 +01:00
Konrad	991689d3cd	TECS: Add new tecs library in parallel to old tecs in the position control library for comparison.	2022-12-21 09:04:19 +01:00
Konrad	c64e111d8e	TECS: Rearranged the TECS library into submodules.	2022-12-21 09:04:19 +01:00
Daniel Agar	54a32eb2f7	ekf2: EV overhaul yaw and position fusion (#20501 ) - move EV yaw and EV position to new state machines - EV yaw and EV pos now configured via EKF2_EV_CTRL (migrated from EKF2_AID_MASK) - new EV position offset estimator to enable EV position while GPS position is active (no more EV pos delta fusion) - yaw_align now strictly means north (no more rotate external vision aid mask) - automatic switching between EV yaw, and yaw align north based on GPS quality	2022-12-20 10:23:56 -05:00
Silvan Fuhrer	20342216e2	Airspeed Selector: use better density source and only save scale parameter if valid (#20764 ) * AirspeedSelector: use vehicle_air_data.rho for calculating groundspeed-wind CAS Previously the vehicle_air_data.temperature and pressure was used, instead of the density field directly. Only makes a difference if there is an airspeed sensor connected to provide the air temperature. * AirspeedSelector: only safe estimated scale in param if airspeed is valid * AirspeedSelector: remove 0.01 cliff for saving learned scale to param Signed-off-by: Silvan Fuhrer <silvan@auterion.com>	2022-12-20 15:52:54 +01:00
David Sidrane	efbb2cf2e3	nxp_ucans32k146:Add LPSPI DMA nxp_ucans32k146:Add Dready	2022-12-20 09:31:05 -05:00
David Sidrane	b7ea31ceed	s32k1xx:Add gpiosetevent	2022-12-20 09:31:05 -05:00
David Sidrane	da536b3a82	nxp_ucans32k146:Add PROBEs for debugging	2022-12-20 09:29:07 -05:00
David Sidrane	e8aa54e7bb	nxp_ucans32k146:Use GPIO based RTS (Buffer not character)	2022-12-20 09:29:07 -05:00
David Sidrane	fe7d761a11	nxp_ucans32k146:Use serial DMA	2022-12-20 09:29:07 -05:00
David Sidrane	39822ef5a1	nxp_mr-canhubk:lpuart0 use Serial HW flow control	2022-12-20 09:27:40 -05:00
David Sidrane	6a7f5a339b	nxp_mr-canhubk3:fmu use Serial DMA	2022-12-20 09:27:40 -05:00
David Sidrane	022e941ebe	NuttX with s32k3 Serial DMA	2022-12-20 09:27:40 -05:00
Igor Misic	52275923ad	adsb: add support for callsign	2022-12-20 08:18:09 +01:00
Eric Katzfey	678607117a	Qurt UART ESC driver support (#20784 )	2022-12-20 01:25:12 -05:00
PX4 BuildBot	da7d52e302	Update submodule libevents to latest Tue Dec 20 00:39:10 UTC 2022 - libevents in PX4/Firmware (26f3fea7ebb328ef58d4d592dae0559c91f13c1c): https://github.com/mavlink/libevents/commit/0c8bc543db2f8c78f59214d5bcf959bdadd96677 - libevents current upstream: https://github.com/mavlink/libevents/commit/8d9c5551273a52e22253ea6abf28d9e4b05e0ab7 - Changes: https://github.com/mavlink/libevents/compare/0c8bc543db2f8c78f59214d5bcf959bdadd96677...8d9c5551273a52e22253ea6abf28d9e4b05e0ab7 8d9c555 2022-12-13 Beat Küng - README: clarify use of component ID	2022-12-20 01:24:15 -05:00
Peter van der Perk	b8e07cc52c	Fix Ethernet Socket Defaults	2022-12-19 13:24:55 -05:00
Silvan Fuhrer	9c66f1b14a	AirspeedValidator: Remvoe airspeed variance after boot check (#20678 ) This checks was introduced to catch the (unlikely) case of the driver publishing a stale value that is not actually from a current measurement right after boot. This was done by declaring it invalid if there is no update of the measurement within the first 10s after boot. Practice though has shown that around 0, many airspeed sensors have such a low resolution that the reported airspeed value is often at the exact same value for longer periods of time on totally healthy sensors, and thus we trigger some false positive failure detections. Given that this failure mode (driver publishing stale data) is quite rare (if it doesn't receive new data it should stop publishing), I remove this check here again. When in aerodynamic flight mode and armed, there is still the data stuck check that can trigger if there is no update for 2s. Signed-off-by: Silvan Fuhrer <silvan@auterion.com>	2022-12-19 11:36:48 +01:00
Igor Misic	df441ac202	drivers/gps: add param for enabling protocols at i2c interface	2022-12-19 08:52:36 +01:00
tanja	e5255b173a	v6x: use param SENS_INT_BARO_EN to start internal bmp388	2022-12-19 08:50:31 +01:00
tanja	987de56af2	v6x: rev10 has second PM	2022-12-19 08:50:31 +01:00
tanja	09039faf0a	v6x: change bootloader UART7 to UART5	2022-12-19 08:50:31 +01:00
Eric Katzfey	b0580d88e1	Added muorb aggregator from DSP to Apps direction	2022-12-16 19:43:00 -05:00
Peter van der Perk	d945e87e4f	S32K3 DRDY less ambigious	2022-12-16 12:02:44 -05:00
Peter van der Perk	05dd43a8de	S32K3 Correct pinirq pinset for external interrupt	2022-12-16 12:02:44 -05:00
RomanBapst	ffdf186598	vtol_att_control: reset transition timer states when starting a transition Signed-off-by: RomanBapst <bapstroman@gmail.com>	2022-12-16 11:36:00 +03:00
frederictaillandier	84c5ce3a53	removing catkin sitl_gazebo submodule tests as it has also been removed in PX4-Autopilot Updating the submodule https://github.com/PX4/PX4-SITL_gazebo/pull/934	2022-12-16 08:11:28 +01:00
JaeyoungLim	9db133b13d	Remove angular velocity controller module	2022-12-16 07:52:20 +01:00
Peter van der Perk	52c0cba24b	NuttX with TXAVAL merged nxp_ucans32k146:Use txavail NuttX Fix Stack Size Expose some K1 debug features	2022-12-15 08:29:10 -05:00
RomanBapst	48e8477799	vtol_att_control: disable maximum height for quadchute by default Signed-off-by: RomanBapst <bapstroman@gmail.com>	2022-12-15 15:40:53 +03:00
RomanBapst	9ed51ba8ed	quadchute: introduced parameter to specify maximum height Signed-off-by: RomanBapst <bapstroman@gmail.com>	2022-12-15 15:40:53 +03:00
Peter van der Perk	f3fe10f63e	S32K Dynamic periphclocks	2022-12-14 10:44:42 -05:00
Marcell Rausch	6c7702b906	mavlink: Keep sending GPS_RAW_INT/GPS2_RAW.hpp streams on GPS failure * GPS_RAW_INT: Only send no gps messages if gps has ever been present * GPS2_RAW: Keep in sync with GPS_RAW_INT Signed-off-by: Marcell Rausch <marcell@auterion.com>	2022-12-14 09:12:39 -05:00
David Sidrane	96362bfb52	nxp_mr-canhubk3 Add PROBEs for Debugging	2022-12-14 07:34:23 -05:00
Beat Küng	5217bedd4b	commander: make SYS_HAS_MAG a count param and ensure system has N calibrated + enabled mags	2022-12-14 07:55:04 +01:00
Tanja Baumann	d75e61eef6	v6x: fix rc.board_sensors ver command (#20763 )	2022-12-14 07:53:12 +01:00
Daniel Agar	4d318ebd30	mavlink: add initial mavlink OPEN_DRONE_ID_SYSTEM stream	2022-12-13 19:39:27 -05:00
Daniel Agar	696eeb9a49	ekf2: update EV height state machine (small piece of EV overhaul) - respect new EKF2_EV_CTRL parameter for VPOS usage - EV hgt rotate EV position before usage (there's often a small offset in frames) - EV hgt reset use proper EV velocity body frame - try to keep EV hgt and EV vel state machines consistent - small incremental piece of https://github.com/PX4/PX4-Autopilot/pull/19128	2022-12-13 13:29:18 -05:00
Daniel Agar	805ffa9d0b	ekf2: push mag cal reset to delayed time horizon	2022-12-13 10:24:02 -05:00
David Sidrane	57b82af3a0	Hard fault on Port E config (CAN0) due to not sizing the table correctly @PetervdPerk-NXP bit by the non-auto sizing table bug again! Symptom: Hard fault on Port E config (CAN0)	2022-12-13 10:09:40 -05:00
PX4 BuildBot	8b02b6b661	Update submodule mavlink to latest Tue Dec 13 12:38:57 UTC 2022 - mavlink in PX4/Firmware (8e1e5c4faa7749f6bc16650c7452af249a7dcc3a): https://github.com/mavlink/mavlink/commit/081120844300d68e90c8c4d40ab4662cb3eb604c - mavlink current upstream: https://github.com/mavlink/mavlink/commit/0c7792edfeee7cf08e531cb4ce5b4f01854e5724 - Changes: https://github.com/mavlink/mavlink/compare/081120844300d68e90c8c4d40ab4662cb3eb604c...0c7792edfeee7cf08e531cb4ce5b4f01854e5724 0c7792ed 2022-12-08 olliw42 - Gimbal Device: Adding more invalid and bitmask attributes (II) (#1927) 771926e6 2022-12-07 olliw42 - gimbal manager cap flags: resolve duplication mistake 2bc32222 2022-12-07 olliw42 - add gimbal device flags to low word of gimbal manager flags (#1928)	2022-12-13 09:06:14 -05:00
Beat Küng	685d5cb473	fix kconfig: rename LINUX to LINUX_TARGET LINUX is defined by cmake >= 3.25: https://cmake.org/cmake/help/latest/variable/LINUX.html	2022-12-13 09:05:18 -05:00
Roman Bapst	257d4e473b	fixed tailsitter transitions (#20756 ) Signed-off-by: RomanBapst <bapstroman@gmail.com>	2022-12-13 10:38:38 +01:00
tanja	aba11ce920	rename all instances of serial PPB to EXT2	2022-12-13 08:09:15 +01:00
tanja	ba344231d2	v6x: configure SPI busses for board 0x010	2022-12-13 08:09:15 +01:00
tanja	ed0f602ce8	v6x: Adapt sensor set for board 0x010 upstream	2022-12-13 08:09:15 +01:00
tanja	aec4b93527	v6x: Add EXT2 instance on ttyS3	2022-12-13 08:09:15 +01:00
Eric Katzfey	e17ddcc0e5	Qurt platform custom icm42688p IMU driver (#20753 ) - first version of IMU driver for the VOXL 2 platform (Qurt) - this is a customized version of the Invensense ICM42688P driver, it is currently in the VOXL 2 board directory	2022-12-12 22:02:23 -05:00
Peter van der Perk	33e39d68f7	DroneCAN SocketCAN driver add FMU support	2022-12-12 20:06:13 -05:00
Eric Katzfey	9b3feee6ee	worker and HRT threads for Qurt platform (#20739 ) * Getting work manager and hrt threads ready for Qurt platform	2022-12-12 14:25:28 -05:00
Daniel Agar	dc0823062e	github actions: disable vtol_standard address sanitizer test - unfortunately this is often too slow for github actions	2022-12-12 14:23:13 -05:00
bresch	41abf695a8	ekf2: use more efficient covariance update computation KSK' is faster to compute than KHP and is mathematically equivalent	2022-12-12 12:31:54 -05:00
Daniel Agar	61fa28c0d6	Tools/generate_board_targets_json.py exclude px4_ros2_default for now	2022-12-12 11:46:36 -05:00
dirksavage88	5b667cf4ba	Uavcannode hygrometer support: temp and humidity Signed-off-by: dirksavage88 <dirksavage88@gmail.com>	2022-12-12 10:54:08 -05:00
Eric Katzfey	a5e4295029	lib/drivers: device drivers library for Qurt platform (#20741 )	2022-12-10 19:31:06 -05:00
Noah Bliss	d8bfee517a	boards: beaglebone blue point to a newer version of librobotcontrol (#20740 ) - Fixes PRU path issues found in old versions of the library.	2022-12-10 17:27:32 -05:00
alexklimaj	29ade6c472	Increase SPI stacks by 16 bytes	2022-12-10 10:54:20 -05:00
Zachary Lowell	e4f641e9b5	Qurt qshell implementation (#20736 )	2022-12-09 16:49:41 -05:00
Daniel Agar	8cf13d50a8	ekf2: EKFGSF_yaw minor cleanup (#20510 ) - don't store unnecessary IMU copy in class, pass it through where required - remove custom pi constants - remove unused fields from ahrs_ekf_gsf (vel_NE, fuse_gps, accel_dt) - explicitly initialize everything in header - apply PX4 code style - set GPS velocity before yaw estimator update, not after	2022-12-09 13:24:00 -05:00
Zachary Lowell	643eed51cb	Qurt lightweight parameter implementation (#20735 )	2022-12-09 09:55:49 -08:00
Daniel Agar	f3884d5835	Update world_magnetic_model to latest Fri Dec 9 11:14:09 UTC 2022 Co-authored-by: PX4 BuildBot <bot@px4.io>	2022-12-09 11:40:30 -05:00
Daniel Agar	41fa53605c	boards: rebuild STM32H7 bootloaders and px4-io-v2	2022-12-09 11:02:21 -05:00
Peter van der Perk	5b7a2230fc	Update NuttX kernel	2022-12-09 06:36:40 -05:00
Peter van der Perk	3b5f0e21bc	Add Sysview support in conjunction with PX4 cpuload note driver	2022-12-09 06:36:40 -05:00
Beat Küng	a502146d73	board_hw_rev_ver: fix hex printf	2022-12-09 07:52:50 +01:00
Beat Küng	8bde2a7a28	board_hw_rev_ver.c: check for 'rv == OK' before reading the revision Otherwise a potential failure reading the hw version can get overwritten.	2022-12-09 07:52:50 +01:00
Daniel Agar	8114aad983	initial minimal PX4_ROS2 platform and px4_ros2_default build (#20689 ) - new ROS2 platform in PX4 intended for creating configs that build and run entirely in ROS2 - PX4_CONFIG defaults to px4_ros2_default if no config specified and in a colcon workspace with ROS_VERSION=2 - currently doesn't do much other than allow you to build px4 msgs interface package	2022-12-08 23:03:44 -05:00
Roman Bapst	cfb670fbb3	Refactor quadchute logic (#20704 ) * moved computation of _time_since_trans_start into base class Signed-off-by: RomanBapst <bapstroman@gmail.com> * refactor quadchute logic - move entired logic into VtolType class - split into smaller functions Signed-off-by: RomanBapst <bapstroman@gmail.com> * Update src/modules/vtol_att_control/vtol_type.h Co-authored-by: Silvan Fuhrer <silvan@auterion.com> Signed-off-by: RomanBapst <bapstroman@gmail.com> Co-authored-by: Silvan Fuhrer <silvan@auterion.com>	2022-12-08 11:56:20 +03:00
Daniel Agar	2b1d8c1d8e	mavlink switch to common dialect by default and update to latest	2022-12-07 15:46:04 -05:00
Daniel Agar	72cb4bee01	SITL gz fixes and init cleanup (#20725 ) - remove broken gz version handling and remove x500-legacy - fix all shellcheck warnings - prepare for Gazebo Garden compatibility (needs more work)	2022-12-07 15:41:13 -05:00
Beniamino Pozzan	f2c71a8874	microdds_client: added environment variable for defining namespace Signed-off-by: Beniamino Pozzan <beniamino.pozzan@phd.unipd.it>	2022-12-07 14:36:23 -05:00
Beniamino Pozzan	5a2e41c4e4	microdds_client: add XRCE_DDS_KEY parameter Multiple agents can connect to the same client For sitl builds, if the intance number is different from zero, XRCE_DDS_KEY is set to the instance number and the microdds_client is automatically started with namespace px4_"instance_number" and udp port 8888 If the instance number is equal to zero XRCE_DDS_KEY is left untouched and the microdds_client is automatically started without namespace and udp port 8888 Signed-off-by: Beniamino Pozzan <beniamino.pozzan@phd.unipd.it>	2022-12-07 14:36:23 -05:00
Beniamino Pozzan	a92897fb58	microdds_client: add namespace to partecipant name The partecipant name is modified into "client_namespace"/px4_micro_xrce_dds For sitl builds the microdds_client is automatically started with namespace px4_"instance_number" and udp port 8888+"intance_number" Signed-off-by: Beniamino Pozzan <beniamino.pozzan@phd.unipd.it>	2022-12-07 14:36:23 -05:00
Daniel Agar	8eb2a0a3ec	ekf2: delete orientation_covariances_euler() - usage replaced with now public calcRotVecVariances()	2022-12-07 12:41:27 -05:00
Mathieu Bresciani	a187bb3b80	ekf2: predict covariance before predicting state (#20715 ) * To predict the covariance, the jacobian from the previous to the current state is required.	2022-12-07 09:34:19 -05:00
Daniel Agar	c12d9124bd	Update submodule GPS drivers to latest Wed Dec 7 12:38:31 UTC 2022 - GPS drivers in PX4/Firmware (4eb0a0598048df07c213659944b2720ee42e1cc2): https://github.com/PX4/PX4-GPSDrivers/commit/b49a6c257371abae6e26e0a3e0fc04b963b2f13d - GPS drivers current upstream: https://github.com/PX4/PX4-GPSDrivers/commit/b76fc1df45952abbc76c7eb9f5a5ab3ae994bef2 - Changes: https://github.com/PX4/PX4-GPSDrivers/compare/b49a6c257371abae6e26e0a3e0fc04b963b2f13d...b76fc1df45952abbc76c7eb9f5a5ab3ae994bef2 b76fc1d 2022-11-21 qst0528 - ashtech: fix position accuracy loss caused by misplaced integer cast (#118) Co-authored-by: PX4 BuildBot <bot@px4.io>	2022-12-07 09:16:31 -05:00
PX4 BuildBot	db791fa317	Update submodule sitl_gazebo to latest Wed Dec 7 12:38:25 UTC 2022 - sitl_gazebo in PX4/Firmware (`b3cebc6686`): https://github.com/PX4/PX4-SITL_gazebo/commit/b38e701ec4230a6105ace872447c0f63bc181d41 - sitl_gazebo current upstream: https://github.com/PX4/PX4-SITL_gazebo/commit/1b56329b73f68f1480e58b1137b3e5169c7453d1 - Changes: https://github.com/PX4/PX4-SITL_gazebo/compare/b38e701ec4230a6105ace872447c0f63bc181d41...1b56329b73f68f1480e58b1137b3e5169c7453d1 1b56329 2022-11-22 Frederic Taillandier - adding a gazebo position sniffer to get position at a high rate from another program (#928)	2022-12-07 09:16:00 -05:00