diff --git a/.ci/Jenkinsfile-hardware b/.ci/Jenkinsfile-hardware
index 9bfb23f68d..66f8992fc4 100644
--- a/.ci/Jenkinsfile-hardware
+++ b/.ci/Jenkinsfile-hardware
@@ -903,12 +903,11 @@ void printTopics() {
   sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-*` --cmd "listener rate_ctrl_status" || true'
   sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-*` --cmd "listener safety" || true'
   sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-*` --cmd "listener sensor_accel" || true'
-  sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-*` --cmd "listener sensor_accel_fifo" || true'
+  sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-*` --cmd "listener sensor_imu_fifo" || true'
   sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-*` --cmd "listener sensor_baro" || true'
   sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-*` --cmd "listener sensor_combined" || true'
   sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-*` --cmd "listener sensor_gyro" || true'
   sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-*` --cmd "listener sensor_gyro_fft" || true'
-  sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-*` --cmd "listener sensor_gyro_fifo" || true'
   sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-*` --cmd "listener sensor_mag" || true'
   sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-*` --cmd "listener sensor_preflight_mag" || true'
   sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-*` --cmd "listener sensor_selection" || true'
diff --git a/msg/CMakeLists.txt b/msg/CMakeLists.txt
index 8ad5e81dbd..68cd487267 100644
--- a/msg/CMakeLists.txt
+++ b/msg/CMakeLists.txt
@@ -144,7 +144,6 @@ set(msg_files
 	rtl_time_estimate.msg
 	satellite_info.msg
 	sensor_accel.msg
-	sensor_accel_fifo.msg
 	sensor_baro.msg
 	sensor_combined.msg
 	sensor_correction.msg
@@ -152,8 +151,8 @@ set(msg_files
 	sensor_gps.msg
 	sensor_gyro.msg
 	sensor_gyro_fft.msg
-	sensor_gyro_fifo.msg
 	sensor_hygrometer.msg
+	sensor_imu_fifo.msg
 	sensor_mag.msg
 	sensor_optical_flow.msg
 	sensor_preflight_mag.msg
diff --git a/msg/sensor_accel.msg b/msg/sensor_accel.msg
index e47d813a7a..ddcfb2bce6 100644
--- a/msg/sensor_accel.msg
+++ b/msg/sensor_accel.msg
@@ -7,12 +7,10 @@ float32 x                 # acceleration in the FRD board frame X-axis in m/s^2
 float32 y                 # acceleration in the FRD board frame Y-axis in m/s^2
 float32 z                 # acceleration in the FRD board frame Z-axis in m/s^2
 
+float32 range # TODO
+
 float32 temperature       # temperature in degrees Celsius
 
 uint32 error_count
 
-uint8[3] clip_counter     # clip count per axis in the sample period
-
-uint8 samples             # number of raw samples that went into this message
-
 uint8 ORB_QUEUE_LENGTH = 8
diff --git a/msg/sensor_accel_fifo.msg b/msg/sensor_accel_fifo.msg
deleted file mode 100644
index 1eae5dd149..0000000000
--- a/msg/sensor_accel_fifo.msg
+++ /dev/null
@@ -1,13 +0,0 @@
-uint64 timestamp          # time since system start (microseconds)
-uint64 timestamp_sample
-
-uint32 device_id          # unique device ID for the sensor that does not change between power cycles
-
-float32 dt                # delta time between samples (microseconds)
-float32 scale
-
-uint8 samples             # number of valid samples
-
-int16[32] x               # acceleration in the FRD board frame X-axis in m/s^2
-int16[32] y               # acceleration in the FRD board frame Y-axis in m/s^2
-int16[32] z               # acceleration in the FRD board frame Z-axis in m/s^2
diff --git a/msg/sensor_gyro.msg b/msg/sensor_gyro.msg
index b906127b58..85d1a1fb0d 100644
--- a/msg/sensor_gyro.msg
+++ b/msg/sensor_gyro.msg
@@ -7,12 +7,10 @@ float32 x                 # angular velocity in the FRD board frame X-axis in ra
 float32 y                 # angular velocity in the FRD board frame Y-axis in rad/s
 float32 z                 # angular velocity in the FRD board frame Z-axis in rad/s
 
+float32 range             # dynamic range in rad/s
+
 float32 temperature       # temperature in degrees Celsius
 
 uint32 error_count
 
-uint8[3] clip_counter     # clip count per axis in the sample period
-
-uint8 samples             # number of raw samples that went into this message
-
 uint8 ORB_QUEUE_LENGTH = 8
diff --git a/msg/sensor_gyro_fifo.msg b/msg/sensor_gyro_fifo.msg
deleted file mode 100644
index 2e77ef07eb..0000000000
--- a/msg/sensor_gyro_fifo.msg
+++ /dev/null
@@ -1,15 +0,0 @@
-uint64 timestamp          # time since system start (microseconds)
-uint64 timestamp_sample
-
-uint32 device_id          # unique device ID for the sensor that does not change between power cycles
-
-float32 dt                # delta time between samples (microseconds)
-float32 scale
-
-uint8 samples             # number of valid samples
-
-int16[32] x               # angular velocity in the FRD board frame X-axis in rad/s
-int16[32] y               # angular velocity in the FRD board frame Y-axis in rad/s
-int16[32] z               # angular velocity in the FRD board frame Z-axis in rad/s
-
-uint8 ORB_QUEUE_LENGTH = 4
diff --git a/msg/sensor_imu_fifo.msg b/msg/sensor_imu_fifo.msg
new file mode 100644
index 0000000000..46552217e2
--- /dev/null
+++ b/msg/sensor_imu_fifo.msg
@@ -0,0 +1,26 @@
+uint64 timestamp          # time since system start (microseconds)
+uint64 timestamp_sample
+
+uint32 device_id          # unique device ID for the sensor that does not change between power cycles
+
+float32 dt           # delta time between samples (microseconds)
+uint8 samples        # number of valid samples
+
+float32 accel_scale
+float32 gyro_scale
+
+uint8 FIFO_SIZE = 24
+
+int16[24] accel_x         # gyro in the FRD board frame X-axis
+int16[24] accel_y         # gyro in the FRD board frame Y-axis
+int16[24] accel_z         # gyro in the FRD board frame Z-axis
+
+int16[24] gyro_x          # gyro in the FRD board frame X-axis
+int16[24] gyro_y          # gyro in the FRD board frame Y-axis
+int16[24] gyro_z          # gyro in the FRD board frame Z-axis
+
+float32 temperature       # temperature in degrees Celsius
+
+uint32 error_count
+
+uint8 ORB_QUEUE_LENGTH = 8
diff --git a/posix-configs/SITL/init/test/test_imu_filtering b/posix-configs/SITL/init/test/test_imu_filtering
index b64c8cc5d9..ec8482855b 100644
--- a/posix-configs/SITL/init/test/test_imu_filtering
+++ b/posix-configs/SITL/init/test/test_imu_filtering
@@ -49,7 +49,7 @@ logger off
 
 sensors status
 listener sensor_gyro
-listener sensor_gyro_fifo
+listener sensor_imu_fifo
 listener sensor_gyro_fft
 perf
 
diff --git a/src/drivers/imu/adis16477/ADIS16477.cpp b/src/drivers/imu/adis16477/ADIS16477.cpp
index 98c9787eac..654a199968 100644
--- a/src/drivers/imu/adis16477/ADIS16477.cpp
+++ b/src/drivers/imu/adis16477/ADIS16477.cpp
@@ -56,8 +56,7 @@ using namespace time_literals;
 ADIS16477::ADIS16477(const I2CSPIDriverConfig &config) :
 	SPI(config),
 	I2CSPIDriver(config),
-	_px4_accel(get_device_id(), config.rotation),
-	_px4_gyro(get_device_id(), config.rotation),
+	_rotation(config.rotation),
 	_sample_perf(perf_alloc(PC_ELAPSED, MODULE_NAME": read")),
 	_bad_transfers(perf_alloc(PC_COUNT, MODULE_NAME": bad transfers")),
 	_drdy_gpio(config.drdy_gpio)
@@ -67,8 +66,8 @@ ADIS16477::ADIS16477(const I2CSPIDriverConfig &config) :
 	px4_arch_configgpio(GPIO_SPI1_RESET_ADIS16477);
 #endif // GPIO_SPI1_RESET_ADIS16477
 
-	_px4_accel.set_scale(1.25f * CONSTANTS_ONE_G / 1000.0f); // accel 1.25 mg/LSB
-	_px4_gyro.set_scale(math::radians(0.025f)); // gyro 0.025 °/sec/LSB
+	_accel_scale = 1.25f * CONSTANTS_ONE_G / 1000.0f; // accel 1.25 mg/LSB
+	_gyro_scale = math::radians(0.025f); // gyro 0.025 °/sec/LSB
 }
 
 ADIS16477::~ADIS16477()
@@ -366,11 +365,31 @@ ADIS16477::measure()
 
 	// temperature 1 LSB = 0.1°C
 	const float temperature = adis_report.temp * 0.1f;
-	_px4_accel.set_temperature(temperature);
-	_px4_gyro.set_temperature(temperature);
 
-	_px4_accel.update(timestamp_sample, adis_report.accel_x, adis_report.accel_y, adis_report.accel_z);
-	_px4_gyro.update(timestamp_sample, adis_report.gyro_x, adis_report.gyro_y, adis_report.gyro_z);
+
+	// sensor_accel
+	sensor_accel_s sensor_accel{};
+	sensor_accel.timestamp_sample = timestamp_sample;
+	sensor_accel.device_id = get_device_id();
+	sensor_accel.x = adis_report.accel_x;
+	sensor_accel.y = adis_report.accel_y;
+	sensor_accel.z = adis_report.accel_z;
+	sensor_accel.range = 16.f * CONSTANTS_ONE_G;
+	sensor_accel.temperature = temperature;
+	sensor_accel.timestamp = hrt_absolute_time();
+	_sensor_accel_pub.publish(sensor_accel);
+
+	// sensor_gyro
+	sensor_gyro_s sensor_gyro{};
+	sensor_gyro.timestamp_sample = timestamp_sample;
+	sensor_gyro.device_id = get_device_id();
+	sensor_gyro.x = adis_report.gyro_x;
+	sensor_gyro.y = adis_report.gyro_y;
+	sensor_gyro.z = adis_report.gyro_z;
+	sensor_gyro.range = math::radians(2000.f);
+	sensor_gyro.temperature = temperature;
+	sensor_gyro.timestamp = hrt_absolute_time();
+	_sensor_gyro_pub.publish(sensor_gyro);
 
 	perf_end(_sample_perf);
 
diff --git a/src/drivers/imu/adis16477/ADIS16477.hpp b/src/drivers/imu/adis16477/ADIS16477.hpp
index c99de49d6b..26a2a13d5b 100644
--- a/src/drivers/imu/adis16477/ADIS16477.hpp
+++ b/src/drivers/imu/adis16477/ADIS16477.hpp
@@ -41,8 +41,6 @@
 #include <drivers/device/spi.h>
 #include <geo/geo.h>
 #include <lib/conversion/rotation.h>
-#include <lib/drivers/accelerometer/PX4Accelerometer.hpp>
-#include <lib/drivers/gyroscope/PX4Gyroscope.hpp>
 #include <perf/perf_counter.h>
 #include <px4_platform_common/getopt.h>
 #include <px4_platform_common/i2c_spi_buses.h>
@@ -66,9 +64,6 @@ protected:
 
 private:
 
-	PX4Accelerometer	_px4_accel;
-	PX4Gyroscope		_px4_gyro;
-
 	perf_counter_t		_sample_perf;
 	perf_counter_t		_bad_transfers;
 
diff --git a/src/drivers/imu/adis16477/CMakeLists.txt b/src/drivers/imu/adis16477/CMakeLists.txt
index fb12c05ec5..19b735da88 100644
--- a/src/drivers/imu/adis16477/CMakeLists.txt
+++ b/src/drivers/imu/adis16477/CMakeLists.txt
@@ -40,6 +40,4 @@ px4_add_module(
 		adis16477_main.cpp
 	DEPENDS
 		px4_work_queue
-		drivers_accelerometer
-		drivers_gyroscope
 	)
diff --git a/src/drivers/imu/adis16497/ADIS16497.cpp b/src/drivers/imu/adis16497/ADIS16497.cpp
index 9b39f4a071..9dc2b1a02f 100644
--- a/src/drivers/imu/adis16497/ADIS16497.cpp
+++ b/src/drivers/imu/adis16497/ADIS16497.cpp
@@ -73,8 +73,7 @@ using namespace time_literals;
 ADIS16497::ADIS16497(const I2CSPIDriverConfig &config) :
 	SPI(config),
 	I2CSPIDriver(config),
-	_px4_accel(get_device_id(), config.rotation),
-	_px4_gyro(get_device_id(), config.rotation),
+	_rotation(config.rotation),
 	_sample_perf(perf_alloc(PC_ELAPSED, MODULE_NAME": read")),
 	_bad_transfers(perf_alloc(PC_COUNT, MODULE_NAME": bad transfers")),
 	_drdy_gpio(config.drdy_gpio)
@@ -308,23 +307,23 @@ ADIS16497::self_test()
 bool
 ADIS16497::set_measurement_range(uint16_t model)
 {
-	_px4_accel.set_scale(1.25f * CONSTANTS_ONE_G / 1000.0f); // 1.25 mg/LSB
-	_px4_accel.set_range(40.0f * CONSTANTS_ONE_G); // 40g
+	_accel_scale = 1.25f * CONSTANTS_ONE_G / 1000.0f; // 1.25 mg/LSB
+	_accel_range = 40.0f * CONSTANTS_ONE_G; // 40g
 
 	switch (model) {
 	case RANG_MDL_1BMLZ:
-		_px4_gyro.set_scale(math::radians(0.00625f)); // 0.00625 °/sec/LSB
-		_px4_gyro.set_range(math::radians(125.0f)); // 125 °/s
+		_gyro_scale = math::radians(0.00625f); // 0.00625 °/sec/LSB
+		_gyro_range = math::radians(125.0f); // 125 °/s
 		break;
 
 	case RANG_MDL_2BMLZ:
-		_px4_gyro.set_scale(math::radians(0.025f)); // 0.025 °/sec/LSB
-		_px4_gyro.set_range(math::radians(450.0f)); // 450 °/s
+		_gyro_scale = math::radians(0.025f); // 0.025 °/sec/LSB
+		_gyro_range = math::radians(450.0f); // 450 °/s
 		break;
 
 	case RANG_MDL_3BMLZ:
-		_px4_gyro.set_scale(math::radians(0.1f)); // 0.1 °/sec/LSB
-		_px4_gyro.set_range(math::radians(2000.0f)); // 2000 °/s
+		_gyro_scale = math::radians(0.1f); // 0.1 °/sec/LSB
+		_gyro_range = math::radians(2000.0f); // 2000 °/s
 		break;
 
 	default:
@@ -470,26 +469,44 @@ ADIS16497::measure()
 	}
 
 	const uint64_t error_count = perf_event_count(_bad_transfers);
-	_px4_accel.set_error_count(error_count);
-	_px4_gyro.set_error_count(error_count);
-
 	const float temperature = (int16_t(adis_report.TEMP_OUT) * 0.0125f) + 25.0f; // 1 LSB = 0.0125°C, 0x0000 at 25°C
-	_px4_accel.set_temperature(temperature);
-	_px4_gyro.set_temperature(temperature);
+
 
 	// TODO check data counter here to see if we're missing samples/getting repeated samples
 	{
 		float xraw_f = (int32_t(adis_report.X_ACCEL_OUT) << 16 | adis_report.X_ACCEL_LOW) / 65536.0f;
 		float yraw_f = (int32_t(adis_report.Y_ACCEL_OUT) << 16 | adis_report.Y_ACCEL_LOW) / 65536.0f;
 		float zraw_f = (int32_t(adis_report.Z_ACCEL_OUT) << 16 | adis_report.Z_ACCEL_LOW) / 65536.0f;
-		_px4_accel.update(timestamp_sample, xraw_f, yraw_f, zraw_f);
+
+		sensor_accel_s sensor_accel{};
+		sensor_accel.timestamp_sample = timestamp_sample;
+		sensor_accel.device_id = get_device_id();
+		sensor_accel.x = xraw_f;
+		sensor_accel.y = yraw_f;
+		sensor_accel.z = zraw_f;
+		sensor_accel.range = 16.f * CONSTANTS_ONE_G;
+		sensor_accel.temperature = temperature;
+		sensor_accel.error_count = error_count;
+		sensor_accel.timestamp = hrt_absolute_time();
+		_sensor_accel_pub.publish(sensor_accel);
 	}
 
 	{
 		float xraw_f = (int32_t(adis_report.X_GYRO_OUT) << 16 | adis_report.X_GYRO_LOW) / 65536.0f;
 		float yraw_f = (int32_t(adis_report.Y_GYRO_OUT) << 16 | adis_report.Y_GYRO_LOW) / 65536.0f;
 		float zraw_f = (int32_t(adis_report.Z_GYRO_OUT) << 16 | adis_report.Z_GYRO_LOW) / 65536.0f;
-		_px4_gyro.update(timestamp_sample, xraw_f, yraw_f, zraw_f);
+
+		sensor_gyro_s sensor_gyro{};
+		sensor_gyro.timestamp_sample = timestamp_sample;
+		sensor_gyro.device_id = get_device_id();
+		sensor_gyro.x = xraw_f;
+		sensor_gyro.y = yraw_f;
+		sensor_gyro.z = zraw_f;
+		sensor_gyro.range = math::radians(2000.f);
+		sensor_gyro.temperature = temperature;
+		sensor_gyro.error_count = error_count;
+		sensor_gyro.timestamp = hrt_absolute_time();
+		_sensor_gyro_pub.publish(sensor_gyro);
 	}
 
 	perf_end(_sample_perf);
diff --git a/src/drivers/imu/adis16497/ADIS16497.hpp b/src/drivers/imu/adis16497/ADIS16497.hpp
index 828e53697c..02518594d8 100644
--- a/src/drivers/imu/adis16497/ADIS16497.hpp
+++ b/src/drivers/imu/adis16497/ADIS16497.hpp
@@ -41,8 +41,6 @@
 #include <drivers/device/spi.h>
 #include <geo/geo.h>
 #include <lib/conversion/rotation.h>
-#include <lib/drivers/accelerometer/PX4Accelerometer.hpp>
-#include <lib/drivers/gyroscope/PX4Gyroscope.hpp>
 #include <perf/perf_counter.h>
 #include <px4_platform_common/getopt.h>
 #include <px4_platform_common/i2c_spi_buses.h>
@@ -102,9 +100,6 @@ protected:
 
 private:
 
-	PX4Accelerometer	_px4_accel;
-	PX4Gyroscope		_px4_gyro;
-
 	perf_counter_t		_sample_perf;
 	perf_counter_t		_bad_transfers;
 
diff --git a/src/drivers/imu/adis16497/CMakeLists.txt b/src/drivers/imu/adis16497/CMakeLists.txt
index 027d89280a..d0cf6258bf 100644
--- a/src/drivers/imu/adis16497/CMakeLists.txt
+++ b/src/drivers/imu/adis16497/CMakeLists.txt
@@ -40,6 +40,5 @@ px4_add_module(
 		ADIS16497.hpp
 		adis16497_main.cpp
 	DEPENDS
-		drivers_accelerometer
-		drivers_gyroscope
+		px4_work_queue
 	)
diff --git a/src/drivers/imu/analog_devices/adis16448/ADIS16448.cpp b/src/drivers/imu/analog_devices/adis16448/ADIS16448.cpp
index 5566a60db7..d49786e500 100644
--- a/src/drivers/imu/analog_devices/adis16448/ADIS16448.cpp
+++ b/src/drivers/imu/analog_devices/adis16448/ADIS16448.cpp
@@ -1,6 +1,6 @@
 /****************************************************************************
  *
- *   Copyright (c) 2021 PX4 Development Team. All rights reserved.
+ *   Copyright (c) 2021-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
@@ -99,9 +99,8 @@ ADIS16448::ADIS16448(const I2CSPIDriverConfig &config) :
 	SPI(config),
 	I2CSPIDriver(config),
 	_drdy_gpio(config.drdy_gpio), // TODO: DRDY disabled
-	_px4_accel(get_device_id(), config.rotation),
-	_px4_gyro(get_device_id(), config.rotation),
-	_px4_mag(get_device_id(), config.rotation)
+	_px4_mag(get_device_id(), config.rotation),
+	_rotation(config.rotation)
 {
 	if (_drdy_gpio != 0) {
 		_drdy_missed_perf = perf_alloc(PC_COUNT, MODULE_NAME": DRDY missed");
@@ -392,15 +391,10 @@ void ADIS16448::RunImpl()
 				if (publish_data) {
 
 					const uint32_t error_count = perf_event_count(_bad_register_perf) + perf_event_count(_bad_transfer_perf);
-					_px4_accel.set_error_count(error_count);
-					_px4_gyro.set_error_count(error_count);
 
 					// temperature 0.07386°C/LSB, 31°C = 0x000
 					const float temperature = (convert12BitToINT16(buffer.TEMP_OUT) * 0.07386f) + 31.f;
 
-					_px4_accel.set_temperature(temperature);
-					_px4_gyro.set_temperature(temperature);
-
 					bool imu_updated = false;
 
 					// sensor's frame is +x forward, +y left, +z up
@@ -430,8 +424,29 @@ void ADIS16448::RunImpl()
 					}
 
 					if (imu_updated) {
-						_px4_accel.update(timestamp_sample, accel_x, accel_y, accel_z);
-						_px4_gyro.update(timestamp_sample, gyro_x, gyro_y, gyro_z);
+						sensor_accel_s sensor_accel{};
+						sensor_accel.timestamp_sample = timestamp_sample;
+						sensor_accel.device_id = get_device_id();
+						sensor_accel.x = accel_x;
+						sensor_accel.y = accel_y;
+						sensor_accel.z = accel_z;
+						sensor_accel.range = 16.f * CONSTANTS_ONE_G;
+						sensor_accel.temperature = temperature;
+						sensor_accel.error_count = error_count;
+						sensor_accel.timestamp = hrt_absolute_time();
+						_sensor_accel_pub.publish(sensor_accel);
+
+						sensor_gyro_s sensor_gyro{};
+						sensor_gyro.timestamp_sample = timestamp_sample;
+						sensor_gyro.device_id = get_device_id();
+						sensor_gyro.x = gyro_x;
+						sensor_gyro.y = gyro_y;
+						sensor_gyro.z = gyro_z;
+						sensor_gyro.range = math::radians(2000.f);
+						sensor_gyro.temperature = temperature;
+						sensor_gyro.error_count = error_count;
+						sensor_gyro.timestamp = hrt_absolute_time();
+						_sensor_gyro_pub.publish(sensor_gyro);
 					}
 
 					// DIAG_STAT bit 7: New data, xMAGN_OUT/BARO_OUT
@@ -534,12 +549,12 @@ bool ADIS16448::Configure()
 		}
 	}
 
-	_px4_accel.set_scale(0.833f * 1e-3f * CONSTANTS_ONE_G); // 0.833 mg/LSB
-	_px4_gyro.set_scale(math::radians(0.04f));              // 0.04 °/sec/LSB
+	_accel_scale = 0.833f * 1e-3f * CONSTANTS_ONE_G; // 0.833 mg/LSB
+	_gyro_scale = math::radians(0.04f);              // 0.04 °/sec/LSB
 	_px4_mag.set_scale(142.9f * 1e-6f);                     // μgauss/LSB
 
-	_px4_accel.set_range(18.f * CONSTANTS_ONE_G);
-	_px4_gyro.set_range(math::radians(1000.f));
+	_accel_range = 18.f * CONSTANTS_ONE_G;
+	_gyro_range = math::radians(1000.f);
 
 	return success;
 }
diff --git a/src/drivers/imu/analog_devices/adis16448/ADIS16448.hpp b/src/drivers/imu/analog_devices/adis16448/ADIS16448.hpp
index a28f6f2706..2e5d31e5e1 100644
--- a/src/drivers/imu/analog_devices/adis16448/ADIS16448.hpp
+++ b/src/drivers/imu/analog_devices/adis16448/ADIS16448.hpp
@@ -1,6 +1,6 @@
 /****************************************************************************
  *
- *   Copyright (c) 2021 PX4 Development Team. All rights reserved.
+ *   Copyright (c) 2021-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
@@ -44,16 +44,17 @@
 
 #include <drivers/drv_hrt.h>
 #include <lib/drivers/device/spi.h>
-#include <lib/drivers/accelerometer/PX4Accelerometer.hpp>
-#include <lib/drivers/gyroscope/PX4Gyroscope.hpp>
 #include <lib/drivers/magnetometer/PX4Magnetometer.hpp>
-#include <uORB/PublicationMulti.hpp>
-#include <uORB/topics/sensor_baro.h>
 #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/PublicationMulti.hpp>
+#include <uORB/topics/sensor_accel.h>
+#include <uORB/topics/sensor_baro.h>
+#include <uORB/topics/sensor_gyro.h>
+
 using namespace Analog_Devices_ADIS16448;
 
 class ADIS16448 : public device::SPI, public I2CSPIDriver<ADIS16448>
@@ -98,11 +99,17 @@ private:
 
 	const spi_drdy_gpio_t _drdy_gpio;
 
-	PX4Accelerometer _px4_accel;
-	PX4Gyroscope _px4_gyro;
 	PX4Magnetometer _px4_mag;
 
+	uORB::PublicationMulti<sensor_accel_s> _sensor_accel_pub{ORB_ID(sensor_accel)};
 	uORB::PublicationMulti<sensor_baro_s> _sensor_baro_pub{ORB_ID(sensor_baro)};
+	uORB::PublicationMulti<sensor_gyro_s> _sensor_gyro_pub{ORB_ID(sensor_gyro)};
+
+	const enum Rotation _rotation;
+	float _accel_scale{0.f};
+	float _accel_range{0.f};
+	float _gyro_scale{0.f};
+	float _gyro_range{0.f};
 
 	perf_counter_t _reset_perf{perf_alloc(PC_COUNT, MODULE_NAME": reset")};
 	perf_counter_t _bad_register_perf{perf_alloc(PC_COUNT, MODULE_NAME": bad register")};
diff --git a/src/drivers/imu/analog_devices/adis16448/CMakeLists.txt b/src/drivers/imu/analog_devices/adis16448/CMakeLists.txt
index b8f8d690fc..dc774257cc 100644
--- a/src/drivers/imu/analog_devices/adis16448/CMakeLists.txt
+++ b/src/drivers/imu/analog_devices/adis16448/CMakeLists.txt
@@ -43,8 +43,6 @@ px4_add_module(
 		adis16448_main.cpp
 		Analog_Devices_ADIS16448_registers.hpp
 	DEPENDS
-		drivers_accelerometer
-		drivers_gyroscope
 		drivers_magnetometer
 		px4_work_queue
 	)
diff --git a/src/drivers/imu/analog_devices/adis16470/ADIS16470.cpp b/src/drivers/imu/analog_devices/adis16470/ADIS16470.cpp
index 1e73a9d9f7..d3b2b7bc23 100644
--- a/src/drivers/imu/analog_devices/adis16470/ADIS16470.cpp
+++ b/src/drivers/imu/analog_devices/adis16470/ADIS16470.cpp
@@ -44,8 +44,7 @@ ADIS16470::ADIS16470(const I2CSPIDriverConfig &config) :
 	SPI(config),
 	I2CSPIDriver(config),
 	_drdy_gpio(config.drdy_gpio),
-	_px4_accel(get_device_id(), config.rotation),
-	_px4_gyro(get_device_id(), config.rotation)
+	_rotation(config.rotation)
 {
 	if (_drdy_gpio != 0) {
 		_drdy_missed_perf = perf_alloc(PC_COUNT, MODULE_NAME": DRDY missed");
@@ -296,8 +295,6 @@ void ADIS16470::RunImpl()
 
 				// temperature 1 LSB = 0.1°C
 				const float temperature = buffer.TEMP_OUT * 0.1f;
-				_px4_accel.set_temperature(temperature);
-				_px4_gyro.set_temperature(temperature);
 
 
 				int16_t accel_x = buffer.X_ACCL_OUT;
@@ -306,10 +303,17 @@ void ADIS16470::RunImpl()
 
 				// 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 == INT16_MIN) ? INT16_MAX : -accel_y;
-				accel_z = (accel_z == INT16_MIN) ? INT16_MAX : -accel_z;
-
-				_px4_accel.update(timestamp_sample, accel_x, accel_y, accel_z);
+				sensor_accel_s sensor_accel{};
+				sensor_accel.timestamp_sample = timestamp_sample;
+				sensor_accel.device_id = get_device_id();
+				sensor_accel.x = accel_x;
+				sensor_accel.y = math::negate(accel_y);
+				sensor_accel.z = math::negate(accel_z);
+				sensor_accel.range = 16.f * CONSTANTS_ONE_G;
+				sensor_accel.temperature = temperature;
+				sensor_accel.error_count = error_count;
+				sensor_accel.timestamp = hrt_absolute_time();
+				_sensor_accel_pub.publish(sensor_accel);
 
 
 				int16_t gyro_x = buffer.X_GYRO_OUT;
@@ -317,9 +321,17 @@ void ADIS16470::RunImpl()
 				int16_t gyro_z = buffer.Z_GYRO_OUT;
 				// 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_y = (gyro_y == INT16_MIN) ? INT16_MAX : -gyro_y;
-				gyro_z = (gyro_z == INT16_MIN) ? INT16_MAX : -gyro_z;
-				_px4_gyro.update(timestamp_sample, gyro_x, gyro_y, gyro_z);
+				sensor_gyro_s sensor_gyro{};
+				sensor_gyro.timestamp_sample = timestamp_sample;
+				sensor_gyro.device_id = get_device_id();
+				sensor_gyro.x = gyro_x;
+				sensor_gyro.y = math::negate(gyro_y);
+				sensor_gyro.z = math::negate(gyro_z);
+				sensor_gyro.range = math::radians(2000.f);
+				sensor_gyro.temperature = temperature;
+				sensor_gyro.error_count = error_count;
+				sensor_gyro.timestamp = hrt_absolute_time();
+				_sensor_gyro_pub.publish(sensor_gyro);
 
 				success = true;
 
@@ -376,12 +388,12 @@ bool ADIS16470::Configure()
 	}
 
 	// accel: ±40 g, 800 LSB/g (16-bit format)
-	_px4_accel.set_range(40.f * CONSTANTS_ONE_G);
-	_px4_accel.set_scale(CONSTANTS_ONE_G / 800.f); // scaling 800 LSB/g -> m/s^2 per LSB
+	//_accel_range = 40.f * CONSTANTS_ONE_G;
+	_accel_scale = CONSTANTS_ONE_G / 800.f; // scaling 800 LSB/g -> m/s^2 per LSB
 
 	// gyro: ±2000 °/sec, 10 LSB/°/sec (16-bit format)
-	_px4_gyro.set_range(math::radians(2000.f));
-	_px4_gyro.set_scale(math::radians(1.f / 10.f)); // scaling 10 LSB/°/sec -> rad/s per LSB
+	//_gyro_range = math::radians(2000.f);
+	_gyro_scale = math::radians(1.f / 10.f); // scaling 10 LSB/°/sec -> rad/s per LSB
 
 	return success;
 }
diff --git a/src/drivers/imu/analog_devices/adis16470/ADIS16470.hpp b/src/drivers/imu/analog_devices/adis16470/ADIS16470.hpp
index 40c6fab14f..237a331e6e 100644
--- a/src/drivers/imu/analog_devices/adis16470/ADIS16470.hpp
+++ b/src/drivers/imu/analog_devices/adis16470/ADIS16470.hpp
@@ -1,6 +1,6 @@
 /****************************************************************************
  *
- *   Copyright (c) 2021 PX4 Development Team. All rights reserved.
+ *   Copyright (c) 2021-2022 PX4 Development Team. All rights reserved.
  *
  * Redistribution and use in source and binary forms, with or without
  * modification, are permitted provided that the following conditions
@@ -43,14 +43,15 @@
 #include "Analog_Devices_ADIS16470_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/PublicationMulti.hpp>
+#include <uORB/topics/sensor_imu_fifo.h>
+
 using namespace Analog_Devices_ADIS16470;
 
 class ADIS16470 : public device::SPI, public I2CSPIDriver<ADIS16470>
@@ -94,8 +95,11 @@ private:
 
 	const spi_drdy_gpio_t _drdy_gpio;
 
-	PX4Accelerometer _px4_accel;
-	PX4Gyroscope _px4_gyro;
+	uORB::PublicationMulti<sensor_imu_fifo_s> _sensor_imu_fifo_pub{ORB_ID(sensor_imu_fifo)};
+
+	const enum Rotation _rotation;
+	float _accel_scale{0.f};
+	float _gyro_scale{0.f};
 
 	perf_counter_t _reset_perf{perf_alloc(PC_COUNT, MODULE_NAME": reset")};
 	perf_counter_t _bad_register_perf{perf_alloc(PC_COUNT, MODULE_NAME": bad register")};
diff --git a/src/drivers/imu/analog_devices/adis16470/CMakeLists.txt b/src/drivers/imu/analog_devices/adis16470/CMakeLists.txt
index 6dd5aec2c9..51f332cb4d 100644
--- a/src/drivers/imu/analog_devices/adis16470/CMakeLists.txt
+++ b/src/drivers/imu/analog_devices/adis16470/CMakeLists.txt
@@ -35,13 +35,12 @@ px4_add_module(
 	MODULE drivers__imu__analog_devices__adis16470
 	MAIN adis16470
 	COMPILE_FLAGS
+		-Wno-error
 	SRCS
 		ADIS16470.cpp
 		ADIS16470.hpp
 		adis16470_main.cpp
 		Analog_Devices_ADIS16470_registers.hpp
 	DEPENDS
-		drivers_accelerometer
-		drivers_gyroscope
 		px4_work_queue
 	)
diff --git a/src/drivers/imu/bosch/bmi055/BMI055_Accelerometer.cpp b/src/drivers/imu/bosch/bmi055/BMI055_Accelerometer.cpp
index 77cf537302..71c585e199 100644
--- a/src/drivers/imu/bosch/bmi055/BMI055_Accelerometer.cpp
+++ b/src/drivers/imu/bosch/bmi055/BMI055_Accelerometer.cpp
@@ -1,6 +1,6 @@
 /****************************************************************************
  *
- *   Copyright (c) 2020-2021 PX4 Development Team. All rights reserved.
+ *   Copyright (c) 2020-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
@@ -42,13 +42,13 @@ namespace Bosch::BMI055::Accelerometer
 
 BMI055_Accelerometer::BMI055_Accelerometer(const I2CSPIDriverConfig &config) :
 	BMI055(config),
-	_px4_accel(get_device_id(), config.rotation)
+	_rotation(config.rotation)
 {
 	if (config.drdy_gpio != 0) {
 		_drdy_missed_perf = perf_alloc(PC_COUNT, MODULE_NAME"_accel: DRDY missed");
 	}
 
-	ConfigureSampleRate(_px4_accel.get_max_rate_hz());
+	ConfigureSampleRate(RATE);
 }
 
 BMI055_Accelerometer::~BMI055_Accelerometer()
@@ -121,8 +121,8 @@ void BMI055_Accelerometer::RunImpl()
 				ScheduleDelayed(100_ms);
 
 			} else {
-				PX4_DEBUG("Reset not complete, check again in 10 ms");
-				ScheduleDelayed(10_ms);
+				PX4_DEBUG("Reset not complete, check again in 100 ms");
+				ScheduleDelayed(100_ms);
 			}
 		}
 
@@ -162,62 +162,78 @@ void BMI055_Accelerometer::RunImpl()
 		break;
 
 	case STATE::FIFO_READ: {
-			hrt_abstime timestamp_sample = 0;
+			hrt_abstime timestamp_sample = now;
+			uint8_t samples = 0;
 
 			if (_data_ready_interrupt_enabled) {
 				// scheduled from interrupt if _drdy_timestamp_sample was set as expected
 				const hrt_abstime drdy_timestamp_sample = _drdy_timestamp_sample.fetch_and(0);
 
-				if ((now - drdy_timestamp_sample) < _fifo_empty_interval_us) {
+				if ((drdy_timestamp_sample != 0) && (now < drdy_timestamp_sample + _fifo_empty_interval_us)) {
 					timestamp_sample = drdy_timestamp_sample;
+					samples = _fifo_samples;
 
 				} else {
 					perf_count(_drdy_missed_perf);
 				}
 
 				// push backup schedule back
-				ScheduleDelayed(_fifo_empty_interval_us * 2);
+				ScheduleDelayed(_fifo_empty_interval_us * 3);
 			}
 
-			// always check current FIFO status/count
-			bool success = false;
-			const uint8_t FIFO_STATUS = RegisterRead(Register::FIFO_STATUS);
+			if (samples == 0) {
+				// always check current FIFO status/count
+				const uint8_t FIFO_STATUS = RegisterRead(Register::FIFO_STATUS);
 
-			if (FIFO_STATUS & FIFO_STATUS_BIT::fifo_overrun) {
-				FIFOReset();
-				perf_count(_fifo_overflow_perf);
-
-			} else {
-				const uint8_t fifo_frame_counter = FIFO_STATUS & FIFO_STATUS_BIT::fifo_frame_counter;
-
-				if (fifo_frame_counter > FIFO_MAX_SAMPLES) {
-					// not technically an overflow, but more samples than we expected or can publish
+				if (FIFO_STATUS & FIFO_STATUS_BIT::fifo_overrun) {
 					FIFOReset();
 					perf_count(_fifo_overflow_perf);
 
-				} else if (fifo_frame_counter == 0) {
-					perf_count(_fifo_empty_perf);
+				} else {
+					const uint8_t fifo_frame_counter = FIFO_STATUS & FIFO_STATUS_BIT::fifo_frame_counter;
 
-				} else if (fifo_frame_counter >= 1) {
+					if (fifo_frame_counter > FIFO_MAX_SAMPLES) {
+						FIFOReset();
+						perf_count(_fifo_overflow_perf);
 
-					uint8_t samples = fifo_frame_counter;
+					} else if (fifo_frame_counter == 0) {
+						perf_count(_fifo_empty_perf);
 
-					// tolerate minor jitter, leave sample to next iteration if behind by only 1
-					if (samples == _fifo_samples + 1) {
-						// sample timestamp set from data ready already corresponds to _fifo_samples
-						if (timestamp_sample == 0) {
-							timestamp_sample = now - static_cast<int>(FIFO_SAMPLE_DT);
+					} else if (fifo_frame_counter >= 1) {
+
+						samples = fifo_frame_counter;
+
+						if (samples > _fifo_samples) {
+							// grab desired number of samples, but reschedule next cycle sooner
+							const int extra_samples = samples - _fifo_samples;
+							samples = _fifo_samples;
+
+							if (_fifo_samples > extra_samples) {
+								// reschedule to run when a total of _fifo_gyro_samples should be available in the FIFO
+								const uint32_t reschedule_delay_us = (_fifo_samples - extra_samples) * static_cast<int>(FIFO_SAMPLE_DT);
+								ScheduleOnInterval(_fifo_empty_interval_us, reschedule_delay_us);
+
+							} else {
+								// otherwise reschedule to run immediately
+								ScheduleOnInterval(_fifo_empty_interval_us);
+							}
+
+						} else if (samples < _fifo_samples) {
+							// reschedule next cycle to catch the desired number of samples
+							ScheduleOnInterval(_fifo_empty_interval_us, (_fifo_samples - samples) * static_cast<int>(FIFO_SAMPLE_DT));
 						}
-
-						samples--;
 					}
+				}
+			}
 
-					if (FIFORead((timestamp_sample == 0) ? now : timestamp_sample, samples)) {
-						success = true;
+			bool success = false;
 
-						if (_failure_count > 0) {
-							_failure_count--;
-						}
+			if (samples == _fifo_samples) {
+				if (FIFORead(timestamp_sample, samples)) {
+					success = true;
+
+					if (_failure_count > 0) {
+						_failure_count--;
 					}
 				}
 			}
@@ -227,13 +243,14 @@ void BMI055_Accelerometer::RunImpl()
 
 				// full reset if things are failing consistently
 				if (_failure_count > 10) {
+					PX4_DEBUG("Full reset because things are failing consistently");
 					Reset();
 					return;
 				}
 			}
 
+			// check configuration registers periodically or immediately following any failure
 			if (!success || hrt_elapsed_time(&_last_config_check_timestamp) > 100_ms) {
-				// check configuration registers periodically or immediately following any failure
 				if (RegisterCheck(_register_cfg[_checked_register])) {
 					_last_config_check_timestamp = now;
 					_checked_register = (_checked_register + 1) % size_register_cfg;
@@ -241,6 +258,7 @@ void BMI055_Accelerometer::RunImpl()
 				} else {
 					// register check failed, force reset
 					perf_count(_bad_register_perf);
+					PX4_DEBUG("Force reset because register 0x%02hhX check failed ", (uint8_t)_register_cfg[_checked_register].reg);
 					Reset();
 				}
 
@@ -263,23 +281,23 @@ void BMI055_Accelerometer::ConfigureAccel()
 
 	switch (PMU_RANGE) {
 	case range_2g_set:
-		_px4_accel.set_scale(CONSTANTS_ONE_G / 1024.f); // 1024 LSB/g, 0.98mg/LSB
-		_px4_accel.set_range(2.f * CONSTANTS_ONE_G);
+		_accel_scale = CONSTANTS_ONE_G / 1024.f; // 1024 LSB/g, 0.98mg/LSB
+		_accel_range = 2.f * CONSTANTS_ONE_G;
 		break;
 
 	case range_4g_set:
-		_px4_accel.set_scale(CONSTANTS_ONE_G / 512.f); // 512 LSB/g, 1.95mg/LSB
-		_px4_accel.set_range(4.f * CONSTANTS_ONE_G);
+		_accel_scale = CONSTANTS_ONE_G / 512.f; // 512 LSB/g, 1.95mg/LSB
+		_accel_range = 4.f * CONSTANTS_ONE_G;
 		break;
 
 	case range_8g_set:
-		_px4_accel.set_scale(CONSTANTS_ONE_G / 256.f); // 256 LSB/g, 3.91mg/LSB
-		_px4_accel.set_range(8.f * CONSTANTS_ONE_G);
+		_accel_scale = CONSTANTS_ONE_G / 256.f; // 256 LSB/g, 3.91mg/LSB
+		_accel_range = 8.f * CONSTANTS_ONE_G;
 		break;
 
 	case range_16g_set:
-		_px4_accel.set_scale(CONSTANTS_ONE_G / 128.f); // 128 LSB/g, 7.81mg/LSB
-		_px4_accel.set_range(16.f * CONSTANTS_ONE_G);
+		_accel_scale = CONSTANTS_ONE_G / 128.f; // 128 LSB/g, 7.81mg/LSB
+		_accel_range = 16.f * CONSTANTS_ONE_G;
 		break;
 	}
 }
@@ -349,7 +367,7 @@ bool BMI055_Accelerometer::DataReadyInterruptConfigure()
 	}
 
 	// Setup data ready on falling edge
-	return px4_arch_gpiosetevent(_drdy_gpio, false, true, true, &DataReadyInterruptCallback, this) == 0;
+	return (px4_arch_gpiosetevent(_drdy_gpio, false, true, true, &DataReadyInterruptCallback, this) == 0);
 }
 
 bool BMI055_Accelerometer::DataReadyInterruptDisable()
@@ -358,7 +376,7 @@ bool BMI055_Accelerometer::DataReadyInterruptDisable()
 		return false;
 	}
 
-	return px4_arch_gpiosetevent(_drdy_gpio, false, false, false, nullptr, nullptr) == 0;
+	return (px4_arch_gpiosetevent(_drdy_gpio, false, false, false, nullptr, nullptr) == 0);
 }
 
 bool BMI055_Accelerometer::RegisterCheck(const register_config_t &reg_cfg)
@@ -407,40 +425,66 @@ void BMI055_Accelerometer::RegisterSetAndClearBits(Register reg, uint8_t setbits
 
 bool BMI055_Accelerometer::FIFORead(const hrt_abstime &timestamp_sample, uint8_t samples)
 {
-	FIFOTransferBuffer buffer{};
-	const size_t transfer_size = math::min(samples * sizeof(FIFO::DATA) + 1, FIFO::SIZE);
+	// Transfer data
+	struct FIFOTransferBuffer {
+		uint8_t cmd{static_cast<uint8_t>(Register::FIFO_DATA) | DIR_READ};
+		FIFO::DATA f[FIFO_MAX_SAMPLES] {};
+	} buffer{};
+
+	// cmd + (samples * FIFO::DATA)
+	const size_t transfer_size = 1 + math::min(samples * sizeof(FIFO::DATA), FIFO::SIZE);
 
 	if (transfer((uint8_t *)&buffer, (uint8_t *)&buffer, transfer_size) != PX4_OK) {
 		perf_count(_bad_transfer_perf);
 		return false;
 	}
 
-	sensor_accel_fifo_s accel{};
-	accel.timestamp_sample = timestamp_sample;
-	accel.samples = samples;
-	accel.dt = FIFO_SAMPLE_DT;
+	if (samples > 0) {
 
-	for (int i = 0; i < samples; i++) {
-		const FIFO::DATA &fifo_sample = buffer.f[i];
+		sensor_accel_s sensor_accel{};
+		sensor_accel.timestamp_sample = timestamp_sample;
+		sensor_accel.device_id = get_device_id();
 
-		// acc_x_msb<11:4> + acc_x_lsb<3:0>
-		const int16_t accel_x = combine(fifo_sample.ACCD_X_MSB, fifo_sample.ACCD_X_LSB) >> 4;
-		const int16_t accel_y = combine(fifo_sample.ACCD_Y_MSB, fifo_sample.ACCD_Y_LSB) >> 4;
-		const int16_t accel_z = combine(fifo_sample.ACCD_Z_MSB, fifo_sample.ACCD_Z_LSB) >> 4;
+		float accel_sum[3] {};
 
-		// 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;
-		accel.y[i] = (accel_y == INT16_MIN) ? INT16_MAX : -accel_y;
-		accel.z[i] = (accel_z == INT16_MIN) ? INT16_MAX : -accel_z;
+		for (int i = 0; i < samples; i++) {
+			const FIFO::DATA &fifo_sample = buffer.f[i];
+
+			// acc_x_msb<11:4> + acc_x_lsb<3:0>
+			int16_t accel_x = combine(fifo_sample.ACCD_X_MSB, fifo_sample.ACCD_X_LSB) >> 4;
+			int16_t accel_y = combine(fifo_sample.ACCD_Y_MSB, fifo_sample.ACCD_Y_LSB) >> 4;
+			int16_t accel_z = combine(fifo_sample.ACCD_Z_MSB, fifo_sample.ACCD_Z_LSB) >> 4;
+
+			// 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 = accel_x;
+			accel_y = math::negate(accel_y);
+			accel_z = math::negate(accel_z);
+
+			accel_sum[0] += accel_x;
+			accel_sum[1] += accel_y;
+			accel_sum[2] += accel_z;
+		}
+
+
+		sensor_accel.x = (accel_sum[0] / samples) * _accel_scale;
+		sensor_accel.y = (accel_sum[1] / samples) * _accel_scale;
+		sensor_accel.z = (accel_sum[2] / samples) * _accel_scale;
+
+		rotate_3f(_rotation, sensor_accel.x, sensor_accel.y, sensor_accel.z);
+
+		sensor_accel.range = _accel_range;
+		sensor_accel.temperature = _temperature;
+		sensor_accel.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);
+
+		sensor_accel.timestamp = hrt_absolute_time();
+		_sensor_accel_pub.publish(sensor_accel);
+
+		return true;
 	}
 
-	_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));
-
-	_px4_accel.updateFIFO(accel);
-
-	return true;
+	return false;
 }
 
 void BMI055_Accelerometer::FIFOReset()
@@ -472,7 +516,7 @@ void BMI055_Accelerometer::UpdateTemperature()
 	float temperature = static_cast<int8_t>(RegisterRead(Register::ACCD_TEMP)) * 0.5f + 23.f;
 
 	if (PX4_ISFINITE(temperature)) {
-		_px4_accel.set_temperature(temperature);
+		_temperature = temperature;
 
 	} else {
 		perf_count(_bad_transfer_perf);
diff --git a/src/drivers/imu/bosch/bmi055/BMI055_Accelerometer.hpp b/src/drivers/imu/bosch/bmi055/BMI055_Accelerometer.hpp
index 31906c8d96..07cc6f1d1b 100644
--- a/src/drivers/imu/bosch/bmi055/BMI055_Accelerometer.hpp
+++ b/src/drivers/imu/bosch/bmi055/BMI055_Accelerometer.hpp
@@ -1,6 +1,6 @@
 /****************************************************************************
  *
- *   Copyright (c) 2020-2021 PX4 Development Team. All rights reserved.
+ *   Copyright (c) 2020-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
@@ -35,10 +35,11 @@
 
 #include "BMI055.hpp"
 
-#include <lib/drivers/accelerometer/PX4Accelerometer.hpp>
-
 #include "Bosch_BMI055_Accelerometer_Registers.hpp"
 
+#include <uORB/PublicationMulti.hpp>
+#include <uORB/topics/sensor_accel.h>
+
 namespace Bosch::BMI055::Accelerometer
 {
 
@@ -58,15 +59,7 @@ private:
 	static constexpr uint32_t RATE{2000}; // 2000 Hz
 	static constexpr float FIFO_SAMPLE_DT{1e6f / RATE};
 
-	static constexpr int32_t FIFO_MAX_SAMPLES{math::min(FIFO::SIZE / sizeof(FIFO::DATA), sizeof(sensor_accel_fifo_s::x) / sizeof(sensor_accel_fifo_s::x[0]))};
-
-	// Transfer data
-	struct FIFOTransferBuffer {
-		uint8_t cmd{static_cast<uint8_t>(Register::FIFO_DATA) | DIR_READ};
-		FIFO::DATA f[FIFO_MAX_SAMPLES] {};
-	};
-	// ensure no struct padding
-	static_assert(sizeof(FIFOTransferBuffer) == (1 + FIFO_MAX_SAMPLES *sizeof(FIFO::DATA)));
+	static constexpr int32_t FIFO_MAX_SAMPLES{FIFO::SIZE / sizeof(FIFO::DATA)};
 
 	struct register_config_t {
 		Register reg;
@@ -97,7 +90,13 @@ private:
 
 	void UpdateTemperature();
 
-	PX4Accelerometer _px4_accel;
+	uORB::PublicationMulti<sensor_accel_s> _sensor_accel_pub{ORB_ID(sensor_accel)};
+
+	const enum Rotation _rotation;
+	float _accel_scale{0.f};
+	float _accel_range{0.f};
+
+	float _temperature{NAN};
 
 	perf_counter_t _bad_register_perf{perf_alloc(PC_COUNT, MODULE_NAME"_accel: bad register")};
 	perf_counter_t _bad_transfer_perf{perf_alloc(PC_COUNT, MODULE_NAME"_accel: bad transfer")};
diff --git a/src/drivers/imu/bosch/bmi055/BMI055_Gyroscope.cpp b/src/drivers/imu/bosch/bmi055/BMI055_Gyroscope.cpp
index d2ea514530..0f7296cedd 100644
--- a/src/drivers/imu/bosch/bmi055/BMI055_Gyroscope.cpp
+++ b/src/drivers/imu/bosch/bmi055/BMI055_Gyroscope.cpp
@@ -1,6 +1,6 @@
 /****************************************************************************
  *
- *   Copyright (c) 2020-2021 PX4 Development Team. All rights reserved.
+ *   Copyright (c) 2020-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
@@ -33,8 +33,6 @@
 
 #include "BMI055_Gyroscope.hpp"
 
-#include <px4_platform/board_dma_alloc.h>
-
 using namespace time_literals;
 
 namespace Bosch::BMI055::Gyroscope
@@ -42,13 +40,13 @@ namespace Bosch::BMI055::Gyroscope
 
 BMI055_Gyroscope::BMI055_Gyroscope(const I2CSPIDriverConfig &config) :
 	BMI055(config),
-	_px4_gyro(get_device_id(), config.rotation)
+	_rotation(config.rotation)
 {
 	if (config.drdy_gpio != 0) {
 		_drdy_missed_perf = perf_alloc(PC_COUNT, MODULE_NAME"_gyro: DRDY missed");
 	}
 
-	ConfigureSampleRate(_px4_gyro.get_max_rate_hz());
+	ConfigureSampleRate(RATE);
 }
 
 BMI055_Gyroscope::~BMI055_Gyroscope()
@@ -121,8 +119,8 @@ void BMI055_Gyroscope::RunImpl()
 				ScheduleDelayed(100_ms);
 
 			} else {
-				PX4_DEBUG("Reset not complete, check again in 10 ms");
-				ScheduleDelayed(10_ms);
+				PX4_DEBUG("Reset not complete, check again in 100 ms");
+				ScheduleDelayed(100_ms);
 			}
 		}
 
@@ -162,62 +160,78 @@ void BMI055_Gyroscope::RunImpl()
 		break;
 
 	case STATE::FIFO_READ: {
-			hrt_abstime timestamp_sample = 0;
+			hrt_abstime timestamp_sample = now;
+			uint8_t samples = 0;
 
 			if (_data_ready_interrupt_enabled) {
 				// scheduled from interrupt if _drdy_timestamp_sample was set as expected
 				const hrt_abstime drdy_timestamp_sample = _drdy_timestamp_sample.fetch_and(0);
 
-				if ((now - drdy_timestamp_sample) < _fifo_empty_interval_us) {
+				if ((drdy_timestamp_sample != 0) && (now < drdy_timestamp_sample + _fifo_empty_interval_us)) {
 					timestamp_sample = drdy_timestamp_sample;
+					samples = _fifo_samples;
 
 				} else {
 					perf_count(_drdy_missed_perf);
 				}
 
 				// push backup schedule back
-				ScheduleDelayed(_fifo_empty_interval_us * 2);
+				ScheduleDelayed(_fifo_empty_interval_us * 3);
 			}
 
-			// always check current FIFO status/count
-			bool success = false;
-			const uint8_t FIFO_STATUS = RegisterRead(Register::FIFO_STATUS);
+			if (samples == 0) {
+				// always check current FIFO status/count
+				const uint8_t FIFO_STATUS = RegisterRead(Register::FIFO_STATUS);
 
-			if (FIFO_STATUS & FIFO_STATUS_BIT::fifo_overrun) {
-				FIFOReset();
-				perf_count(_fifo_overflow_perf);
-
-			} else {
-				const uint8_t fifo_frame_counter = FIFO_STATUS & FIFO_STATUS_BIT::fifo_frame_counter;
-
-				if (fifo_frame_counter > FIFO_MAX_SAMPLES) {
-					// not technically an overflow, but more samples than we expected or can publish
+				if (FIFO_STATUS & FIFO_STATUS_BIT::fifo_overrun) {
 					FIFOReset();
 					perf_count(_fifo_overflow_perf);
 
-				} else if (fifo_frame_counter == 0) {
-					perf_count(_fifo_empty_perf);
+				} else {
+					const uint8_t fifo_frame_counter = FIFO_STATUS & FIFO_STATUS_BIT::fifo_frame_counter;
 
-				} else if (fifo_frame_counter >= 1) {
+					if (fifo_frame_counter > FIFO_MAX_SAMPLES) {
+						FIFOReset();
+						perf_count(_fifo_overflow_perf);
 
-					uint8_t samples = fifo_frame_counter;
+					} else if (fifo_frame_counter == 0) {
+						perf_count(_fifo_empty_perf);
 
-					// tolerate minor jitter, leave sample to next iteration if behind by only 1
-					if (samples == _fifo_samples + 1) {
-						// sample timestamp set from data ready already corresponds to _fifo_samples
-						if (timestamp_sample == 0) {
-							timestamp_sample = now - static_cast<int>(FIFO_SAMPLE_DT);
+					} else if (fifo_frame_counter >= 1) {
+
+						samples = fifo_frame_counter;
+
+						if (samples > _fifo_samples) {
+							// grab desired number of samples, but reschedule next cycle sooner
+							const int extra_samples = samples - _fifo_samples;
+							samples = _fifo_samples;
+
+							if (_fifo_samples > extra_samples) {
+								// reschedule to run when a total of _fifo_gyro_samples should be available in the FIFO
+								const uint32_t reschedule_delay_us = (_fifo_samples - extra_samples) * static_cast<int>(FIFO_SAMPLE_DT);
+								ScheduleOnInterval(_fifo_empty_interval_us, reschedule_delay_us);
+
+							} else {
+								// otherwise reschedule to run immediately
+								ScheduleOnInterval(_fifo_empty_interval_us);
+							}
+
+						} else if (samples < _fifo_samples) {
+							// reschedule next cycle to catch the desired number of samples
+							ScheduleOnInterval(_fifo_empty_interval_us, (_fifo_samples - samples) * static_cast<int>(FIFO_SAMPLE_DT));
 						}
-
-						samples--;
 					}
+				}
+			}
 
-					if (FIFORead((timestamp_sample == 0) ? now : timestamp_sample, samples)) {
-						success = true;
+			bool success = false;
 
-						if (_failure_count > 0) {
-							_failure_count--;
-						}
+			if (samples == _fifo_samples) {
+				if (FIFORead(timestamp_sample, samples)) {
+					success = true;
+
+					if (_failure_count > 0) {
+						_failure_count--;
 					}
 				}
 			}
@@ -227,13 +241,14 @@ void BMI055_Gyroscope::RunImpl()
 
 				// full reset if things are failing consistently
 				if (_failure_count > 10) {
+					PX4_DEBUG("Full reset because things are failing consistently");
 					Reset();
 					return;
 				}
 			}
 
+			// check configuration registers periodically or immediately following any failure
 			if (!success || hrt_elapsed_time(&_last_config_check_timestamp) > 100_ms) {
-				// check configuration registers periodically or immediately following any failure
 				if (RegisterCheck(_register_cfg[_checked_register])) {
 					_last_config_check_timestamp = now;
 					_checked_register = (_checked_register + 1) % size_register_cfg;
@@ -241,6 +256,7 @@ void BMI055_Gyroscope::RunImpl()
 				} else {
 					// register check failed, force reset
 					perf_count(_bad_register_perf);
+					PX4_DEBUG("Force reset because register 0x%02hhX check failed ", (uint8_t)_register_cfg[_checked_register].reg);
 					Reset();
 				}
 			}
@@ -256,28 +272,28 @@ void BMI055_Gyroscope::ConfigureGyro()
 
 	switch (RANGE) {
 	case gyro_range_2000_dps:
-		_px4_gyro.set_scale(math::radians(1.f / 16.384f));
-		_px4_gyro.set_range(math::radians(2000.f));
+		_gyro_scale = math::radians(1.f / 16.384f);
+		_gyro_range = math::radians(2000.f);
 		break;
 
 	case gyro_range_1000_dps:
-		_px4_gyro.set_scale(math::radians(1.f / 32.768f));
-		_px4_gyro.set_range(math::radians(1000.f));
+		_gyro_scale = math::radians(1.f / 32.768f);
+		_gyro_range = math::radians(1000.f);
 		break;
 
 	case gyro_range_500_dps:
-		_px4_gyro.set_scale(math::radians(1.f / 65.536f));
-		_px4_gyro.set_range(math::radians(500.f));
+		_gyro_scale = math::radians(1.f / 65.536f);
+		_gyro_range = math::radians(500.f);
 		break;
 
 	case gyro_range_250_dps:
-		_px4_gyro.set_scale(math::radians(1.f / 131.072f));
-		_px4_gyro.set_range(math::radians(250.f));
+		_gyro_scale = math::radians(1.f / 131.072f);
+		_gyro_range = math::radians(250.f);
 		break;
 
 	case gyro_range_125_dps:
-		_px4_gyro.set_scale(math::radians(1.f / 262.144f));
-		_px4_gyro.set_range(math::radians(125.f));
+		_gyro_scale = math::radians(1.f / 262.144f);
+		_gyro_range = math::radians(125.f);
 		break;
 	}
 }
@@ -347,7 +363,7 @@ bool BMI055_Gyroscope::DataReadyInterruptConfigure()
 	}
 
 	// Setup data ready on falling edge
-	return px4_arch_gpiosetevent(_drdy_gpio, false, true, true, &DataReadyInterruptCallback, this) == 0;
+	return (px4_arch_gpiosetevent(_drdy_gpio, false, true, false, &DataReadyInterruptCallback, this) == 0);
 }
 
 bool BMI055_Gyroscope::DataReadyInterruptDisable()
@@ -356,7 +372,7 @@ bool BMI055_Gyroscope::DataReadyInterruptDisable()
 		return false;
 	}
 
-	return px4_arch_gpiosetevent(_drdy_gpio, false, false, false, nullptr, nullptr) == 0;
+	return (px4_arch_gpiosetevent(_drdy_gpio, false, false, false, nullptr, nullptr) == 0);
 }
 
 bool BMI055_Gyroscope::RegisterCheck(const register_config_t &reg_cfg)
@@ -405,37 +421,57 @@ void BMI055_Gyroscope::RegisterSetAndClearBits(Register reg, uint8_t setbits, ui
 
 bool BMI055_Gyroscope::FIFORead(const hrt_abstime &timestamp_sample, uint8_t samples)
 {
-	FIFOTransferBuffer buffer{};
-	const size_t transfer_size = math::min(samples * sizeof(FIFO::DATA) + 1, FIFO::SIZE);
+	// Transfer data
+	struct FIFOTransferBuffer {
+		uint8_t cmd{static_cast<uint8_t>(Register::FIFO_DATA) | DIR_READ};
+		FIFO::DATA f[FIFO_MAX_SAMPLES] {};
+	} buffer{};
+
+	// cmd + samples * FIFO::DATA
+	const size_t transfer_size = 1 + math::min(samples * sizeof(FIFO::DATA), FIFO::SIZE);
 
 	if (transfer((uint8_t *)&buffer, (uint8_t *)&buffer, transfer_size) != PX4_OK) {
 		perf_count(_bad_transfer_perf);
 		return false;
 	}
 
-	sensor_gyro_fifo_s gyro{};
-	gyro.timestamp_sample = timestamp_sample;
-	gyro.samples = samples;
-	gyro.dt = FIFO_SAMPLE_DT;
+	sensor_gyro_s sensor_gyro{};
+	sensor_gyro.timestamp_sample = timestamp_sample;
+	sensor_gyro.device_id = get_device_id();
+
+	float gyro_sum[3] {};
 
 	for (int i = 0; i < samples; i++) {
 		const FIFO::DATA &fifo_sample = buffer.f[i];
 
-		const int16_t gyro_x = combine(fifo_sample.RATE_X_MSB, fifo_sample.RATE_X_LSB);
-		const int16_t gyro_y = combine(fifo_sample.RATE_Y_MSB, fifo_sample.RATE_Y_LSB);
-		const int16_t gyro_z = combine(fifo_sample.RATE_Z_MSB, fifo_sample.RATE_Z_LSB);
+		int16_t gyro_x = combine(fifo_sample.RATE_X_MSB, fifo_sample.RATE_X_LSB);
+		int16_t gyro_y = combine(fifo_sample.RATE_Y_MSB, fifo_sample.RATE_Y_LSB);
+		int16_t gyro_z = combine(fifo_sample.RATE_Z_MSB, fifo_sample.RATE_Z_LSB);
 
 		// 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;
-		gyro.y[i] = (gyro_y == INT16_MIN) ? INT16_MAX : -gyro_y;
-		gyro.z[i] = (gyro_z == INT16_MIN) ? INT16_MAX : -gyro_z;
+		//gyro.x[i] = gyro_x;
+		gyro_y = math::negate(gyro_y);
+		gyro_z = math::negate(gyro_z);
+
+		gyro_sum[0] += gyro_x;
+		gyro_sum[1] += gyro_y;
+		gyro_sum[2] += gyro_z;
 	}
 
-	_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));
+	sensor_gyro.x = gyro_sum[0] * _gyro_scale / samples;
+	sensor_gyro.y = gyro_sum[1] * _gyro_scale / samples;
+	sensor_gyro.z = gyro_sum[2] * _gyro_scale / samples;
 
-	_px4_gyro.updateFIFO(gyro);
+	rotate_3f(_rotation, sensor_gyro.x, sensor_gyro.y, sensor_gyro.z);
+
+	sensor_gyro.range = _gyro_range;
+	sensor_gyro.temperature = NAN;
+	sensor_gyro.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);
+
+	sensor_gyro.timestamp = hrt_absolute_time();
+	_sensor_gyro_pub.publish(sensor_gyro);
 
 	return true;
 }
diff --git a/src/drivers/imu/bosch/bmi055/BMI055_Gyroscope.hpp b/src/drivers/imu/bosch/bmi055/BMI055_Gyroscope.hpp
index 13c0468f0d..5654f6d573 100644
--- a/src/drivers/imu/bosch/bmi055/BMI055_Gyroscope.hpp
+++ b/src/drivers/imu/bosch/bmi055/BMI055_Gyroscope.hpp
@@ -1,6 +1,6 @@
 /****************************************************************************
  *
- *   Copyright (c) 2020-2021 PX4 Development Team. All rights reserved.
+ *   Copyright (c) 2020-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
@@ -35,10 +35,11 @@
 
 #include "BMI055.hpp"
 
-#include <lib/drivers/gyroscope/PX4Gyroscope.hpp>
-
 #include "Bosch_BMI055_Gyroscope_Registers.hpp"
 
+#include <uORB/PublicationMulti.hpp>
+#include <uORB/topics/sensor_gyro.h>
+
 namespace Bosch::BMI055::Gyroscope
 {
 
@@ -58,15 +59,7 @@ private:
 	static constexpr uint32_t RATE{2000}; // 2000 Hz
 	static constexpr float FIFO_SAMPLE_DT{1e6f / RATE};
 
-	static constexpr int32_t FIFO_MAX_SAMPLES{math::min(FIFO::SIZE / sizeof(FIFO::DATA), sizeof(sensor_gyro_fifo_s::x) / sizeof(sensor_gyro_fifo_s::x[0]))};
-
-	// Transfer data
-	struct FIFOTransferBuffer {
-		uint8_t cmd{static_cast<uint8_t>(Register::FIFO_DATA) | DIR_READ};
-		FIFO::DATA f[FIFO_MAX_SAMPLES] {};
-	};
-	// ensure no struct padding
-	static_assert(sizeof(FIFOTransferBuffer) == (1 + FIFO_MAX_SAMPLES *sizeof(FIFO::DATA)));
+	static constexpr int32_t FIFO_MAX_SAMPLES{FIFO::SIZE / sizeof(FIFO::DATA)};
 
 	struct register_config_t {
 		Register reg;
@@ -95,7 +88,11 @@ private:
 	bool FIFORead(const hrt_abstime &timestamp_sample, uint8_t samples);
 	void FIFOReset();
 
-	PX4Gyroscope _px4_gyro;
+	uORB::PublicationMulti<sensor_gyro_s> _sensor_gyro_pub{ORB_ID(sensor_gyro)};
+
+	const enum Rotation _rotation;
+	float _gyro_scale{0.f};
+	float _gyro_range{0.f};
 
 	perf_counter_t _bad_register_perf{perf_alloc(PC_COUNT, MODULE_NAME"_gyro: bad register")};
 	perf_counter_t _bad_transfer_perf{perf_alloc(PC_COUNT, MODULE_NAME"_gyro: bad transfer")};
diff --git a/src/drivers/imu/bosch/bmi055/CMakeLists.txt b/src/drivers/imu/bosch/bmi055/CMakeLists.txt
index bf24faffde..79b3d342fa 100644
--- a/src/drivers/imu/bosch/bmi055/CMakeLists.txt
+++ b/src/drivers/imu/bosch/bmi055/CMakeLists.txt
@@ -35,6 +35,7 @@ px4_add_module(
 	MODULE drivers__imu__bosch__bmi055
 	MAIN bmi055
 	COMPILE_FLAGS
+		-O0
 	SRCS
 		Bosch_BMI055_Accelerometer_Registers.hpp
 		Bosch_BMI055_Gyroscope_Registers.hpp
@@ -48,7 +49,5 @@ px4_add_module(
 
 		bmi055_main.cpp
 	DEPENDS
-		drivers_accelerometer
-		drivers_gyroscope
 		px4_work_queue
 	)
diff --git a/src/drivers/imu/bosch/bmi085/BMI085_Accelerometer.cpp b/src/drivers/imu/bosch/bmi085/BMI085_Accelerometer.cpp
index 8016632f2f..b5b5eb6f86 100644
--- a/src/drivers/imu/bosch/bmi085/BMI085_Accelerometer.cpp
+++ b/src/drivers/imu/bosch/bmi085/BMI085_Accelerometer.cpp
@@ -42,13 +42,13 @@ namespace Bosch::BMI085::Accelerometer
 
 BMI085_Accelerometer::BMI085_Accelerometer(const I2CSPIDriverConfig &config) :
 	BMI085(config),
-	_px4_accel(get_device_id(), config.rotation)
+	_rotation(config.rotation)
 {
 	if (config.drdy_gpio != 0) {
 		_drdy_missed_perf = perf_alloc(PC_COUNT, MODULE_NAME"_accel: DRDY missed");
 	}
 
-	ConfigureSampleRate(_px4_accel.get_max_rate_hz());
+	ConfigureSampleRate(RATE);
 }
 
 BMI085_Accelerometer::~BMI085_Accelerometer()
@@ -280,23 +280,23 @@ void BMI085_Accelerometer::ConfigureAccel()
 
 	switch (ACC_RANGE) {
 	case acc_range_2g:
-		_px4_accel.set_scale(CONSTANTS_ONE_G * (powf(2, ACC_RANGE + 1)) / 32768.f);
-		_px4_accel.set_range(2.f * CONSTANTS_ONE_G);
+		_accel_scale = CONSTANTS_ONE_G * (powf(2, ACC_RANGE + 1)) / 32768.f;
+		_accel_range = 2.f * CONSTANTS_ONE_G;
 		break;
 
 	case acc_range_4g:
-		_px4_accel.set_scale(CONSTANTS_ONE_G * (powf(2, ACC_RANGE + 1)) / 32768.f);
-		_px4_accel.set_range(4.f * CONSTANTS_ONE_G);
+		_accel_scale = CONSTANTS_ONE_G * (powf(2, ACC_RANGE + 1)) / 32768.f;
+		_accel_range = 4.f * CONSTANTS_ONE_G;
 		break;
 
 	case acc_range_8g:
-		_px4_accel.set_scale(CONSTANTS_ONE_G * (powf(2, ACC_RANGE + 1)) / 32768.f);
-		_px4_accel.set_range(8.f * CONSTANTS_ONE_G);
+		_accel_scale = CONSTANTS_ONE_G * (powf(2, ACC_RANGE + 1)) / 32768.f;
+		_accel_range = 8.f * CONSTANTS_ONE_G;
 		break;
 
 	case acc_range_16g:
-		_px4_accel.set_scale(CONSTANTS_ONE_G * (powf(2, ACC_RANGE + 1)) / 32768.f);
-		_px4_accel.set_range(16.f * CONSTANTS_ONE_G);
+		_accel_scale = CONSTANTS_ONE_G * (powf(2, ACC_RANGE + 1)) / 32768.f;
+		_accel_range = 16.f * CONSTANTS_ONE_G;
 		break;
 	}
 }
@@ -452,7 +452,7 @@ uint16_t BMI085_Accelerometer::FIFOReadCount()
 	const uint8_t FIFO_LENGTH_0 = fifo_len_buf[2];        // fifo_byte_counter[7:0]
 	const uint8_t FIFO_LENGTH_1 = fifo_len_buf[3] & 0x3F; // fifo_byte_counter[13:8]
 
-	return combine(FIFO_LENGTH_1, FIFO_LENGTH_0);
+	return (FIFO_LENGTH_1 << 8) + FIFO_LENGTH_0;
 }
 
 bool BMI085_Accelerometer::FIFORead(const hrt_abstime &timestamp_sample, uint8_t samples)
@@ -465,7 +465,7 @@ bool BMI085_Accelerometer::FIFORead(const hrt_abstime &timestamp_sample, uint8_t
 		return false;
 	}
 
-	const size_t fifo_byte_counter = combine(buffer.FIFO_LENGTH_1 & 0x3F, buffer.FIFO_LENGTH_0);
+	const size_t fifo_byte_counter = ((buffer.FIFO_LENGTH_1 & 0x3F) << 8) + buffer.FIFO_LENGTH_0;
 
 	// An empty FIFO corresponds to 0x8000
 	if (fifo_byte_counter == 0x8000) {
@@ -477,15 +477,17 @@ bool BMI085_Accelerometer::FIFORead(const hrt_abstime &timestamp_sample, uint8_t
 		return false;
 	}
 
-	sensor_accel_fifo_s accel{};
-	accel.timestamp_sample = timestamp_sample;
-	accel.samples = 0;
-	accel.dt = FIFO_SAMPLE_DT;
+	sensor_accel_s sensor_accel{};
+	sensor_accel.timestamp_sample = timestamp_sample;
+	sensor_accel.device_id = get_device_id();
 
 	// first find all sensor data frames in the buffer
 	uint8_t *data_buffer = (uint8_t *)&buffer.f[0];
 	unsigned fifo_buffer_index = 0; // start of buffer
 
+	float accel_sum[3] {};
+	int accel_samples = 0;
+
 	while (fifo_buffer_index < math::min(fifo_byte_counter, transfer_size - 4)) {
 		// look for header signature (first 6 bits) followed by two bits indicating the status of INT1 and INT2
 		switch (data_buffer[fifo_buffer_index] & 0xFC) {
@@ -494,16 +496,23 @@ bool BMI085_Accelerometer::FIFORead(const hrt_abstime &timestamp_sample, uint8_t
 				// Frame length: 7 bytes (1 byte header + 6 bytes payload)
 
 				FIFO::DATA *fifo_sample = (FIFO::DATA *)&data_buffer[fifo_buffer_index];
-				const int16_t accel_x = combine(fifo_sample->ACC_X_MSB, fifo_sample->ACC_X_LSB);
-				const int16_t accel_y = combine(fifo_sample->ACC_Y_MSB, fifo_sample->ACC_Y_LSB);
-				const int16_t accel_z = combine(fifo_sample->ACC_Z_MSB, fifo_sample->ACC_Z_LSB);
+				int16_t accel_x = combine(fifo_sample->ACC_X_MSB, fifo_sample->ACC_X_LSB);
+				int16_t accel_y = combine(fifo_sample->ACC_Y_MSB, fifo_sample->ACC_Y_LSB);
+				int16_t accel_z = combine(fifo_sample->ACC_Z_MSB, fifo_sample->ACC_Z_LSB);
 
 				// 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[accel.samples] = accel_x;
-				accel.y[accel.samples] = math::negate(accel_y);
-				accel.z[accel.samples] = math::negate(accel_z);
-				accel.samples++;
+				// accel_x = accel_x;
+				accel_y = math::negate(accel_y);
+				accel_z = math::negate(accel_z);
+
+				rotate_3i(_rotation, accel_x, accel_y, accel_z);
+
+				accel_sum[0] += accel_x;
+				accel_sum[1] += accel_y;
+				accel_sum[2] += accel_z;
+
+				accel_samples++;
 
 				fifo_buffer_index += 7; // move forward to next record
 			}
@@ -543,11 +552,19 @@ bool BMI085_Accelerometer::FIFORead(const hrt_abstime &timestamp_sample, uint8_t
 		}
 	}
 
-	_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) {
+		sensor_accel.x = (accel_sum[0] / accel_samples) * _accel_scale;
+		sensor_accel.y = (accel_sum[1] / accel_samples) * _accel_scale;
+		sensor_accel.z = (accel_sum[2] / accel_samples) * _accel_scale;
+
+		sensor_accel.range = _accel_range;
+		sensor_accel.temperature = _temperature;
+		sensor_accel.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);
+
+		sensor_accel.timestamp = hrt_absolute_time();
+		_sensor_accel_pub.publish(sensor_accel);
 
-	if (accel.samples > 0) {
-		_px4_accel.updateFIFO(accel);
 		return true;
 	}
 
@@ -594,7 +611,7 @@ void BMI085_Accelerometer::UpdateTemperature()
 	float temperature = (Temp_int11 * 0.125f) + 23.f; // Temp_int11 * 0.125°C/LSB + 23°C
 
 	if (PX4_ISFINITE(temperature)) {
-		_px4_accel.set_temperature(temperature);
+		_temperature = temperature;
 
 	} else {
 		perf_count(_bad_transfer_perf);
diff --git a/src/drivers/imu/bosch/bmi085/BMI085_Accelerometer.hpp b/src/drivers/imu/bosch/bmi085/BMI085_Accelerometer.hpp
index b20eb2573b..688bcf6ba2 100644
--- a/src/drivers/imu/bosch/bmi085/BMI085_Accelerometer.hpp
+++ b/src/drivers/imu/bosch/bmi085/BMI085_Accelerometer.hpp
@@ -35,10 +35,11 @@
 
 #include "BMI085.hpp"
 
-#include <lib/drivers/accelerometer/PX4Accelerometer.hpp>
-
 #include "Bosch_BMI085_Accelerometer_Registers.hpp"
 
+#include <uORB/PublicationMulti.hpp>
+#include <uORB/topics/sensor_accel.h>
+
 namespace Bosch::BMI085::Accelerometer
 {
 
@@ -58,7 +59,7 @@ private:
 	static constexpr uint32_t RATE{1600}; // 1600 Hz
 	static constexpr float FIFO_SAMPLE_DT{1e6f / RATE};
 
-	static constexpr int32_t FIFO_MAX_SAMPLES{math::min(FIFO::SIZE / sizeof(FIFO::DATA), sizeof(sensor_accel_fifo_s::x) / sizeof(sensor_accel_fifo_s::x[0]))};
+	static constexpr int32_t FIFO_MAX_SAMPLES{FIFO::SIZE / sizeof(FIFO::DATA)};
 
 	// Transfer data
 	struct FIFOTransferBuffer {
@@ -101,7 +102,13 @@ private:
 
 	void UpdateTemperature();
 
-	PX4Accelerometer _px4_accel;
+	uORB::PublicationMulti<sensor_accel_s> _sensor_accel_pub{ORB_ID(sensor_accel)};
+
+	const enum Rotation _rotation;
+	float _accel_scale{0.f};
+	float _accel_range{0.f};
+
+	float _temperature{NAN};
 
 	perf_counter_t _bad_register_perf{perf_alloc(PC_COUNT, MODULE_NAME"_accel: bad register")};
 	perf_counter_t _bad_transfer_perf{perf_alloc(PC_COUNT, MODULE_NAME"_accel: bad transfer")};
diff --git a/src/drivers/imu/bosch/bmi085/BMI085_Gyroscope.cpp b/src/drivers/imu/bosch/bmi085/BMI085_Gyroscope.cpp
index 6c9288430b..0f721ac93f 100644
--- a/src/drivers/imu/bosch/bmi085/BMI085_Gyroscope.cpp
+++ b/src/drivers/imu/bosch/bmi085/BMI085_Gyroscope.cpp
@@ -33,8 +33,6 @@
 
 #include "BMI085_Gyroscope.hpp"
 
-#include <px4_platform/board_dma_alloc.h>
-
 using namespace time_literals;
 
 namespace Bosch::BMI085::Gyroscope
@@ -42,13 +40,13 @@ namespace Bosch::BMI085::Gyroscope
 
 BMI085_Gyroscope::BMI085_Gyroscope(const I2CSPIDriverConfig &config) :
 	BMI085(config),
-	_px4_gyro(get_device_id(), config.rotation)
+	_rotation(config.rotation)
 {
 	if (config.drdy_gpio != 0) {
 		_drdy_missed_perf = perf_alloc(PC_COUNT, MODULE_NAME"_gyro: DRDY missed");
 	}
 
-	ConfigureSampleRate(_px4_gyro.get_max_rate_hz());
+	ConfigureSampleRate(RATE);
 }
 
 BMI085_Gyroscope::~BMI085_Gyroscope()
@@ -257,28 +255,28 @@ void BMI085_Gyroscope::ConfigureGyro()
 
 	switch (GYRO_RANGE) {
 	case gyro_range_2000_dps:
-		_px4_gyro.set_scale(math::radians(1.f / 16.384f));
-		_px4_gyro.set_range(math::radians(2000.f));
+		_gyro_scale = math::radians(1.f / 16.384f);
+		_gyro_range = math::radians(2000.f);
 		break;
 
 	case gyro_range_1000_dps:
-		_px4_gyro.set_scale(math::radians(1.f / 32.768f));
-		_px4_gyro.set_range(math::radians(1000.f));
+		_gyro_scale = math::radians(1.f / 32.768f);
+		_gyro_range = math::radians(1000.f);
 		break;
 
 	case gyro_range_500_dps:
-		_px4_gyro.set_scale(math::radians(1.f / 65.536f));
-		_px4_gyro.set_range(math::radians(500.f));
+		_gyro_scale = math::radians(1.f / 65.536f);
+		_gyro_range = math::radians(500.f);
 		break;
 
 	case gyro_range_250_dps:
-		_px4_gyro.set_scale(math::radians(1.f / 131.072f));
-		_px4_gyro.set_range(math::radians(250.f));
+		_gyro_scale = math::radians(1.f / 131.072f);
+		_gyro_range = math::radians(250.f);
 		break;
 
 	case gyro_range_125_dps:
-		_px4_gyro.set_scale(math::radians(1.f / 262.144f));
-		_px4_gyro.set_range(math::radians(125.f));
+		_gyro_scale = math::radians(1.f / 262.144f);
+		_gyro_range = math::radians(125.f);
 		break;
 	}
 }
@@ -414,29 +412,43 @@ bool BMI085_Gyroscope::FIFORead(const hrt_abstime &timestamp_sample, uint8_t sam
 		return false;
 	}
 
-	sensor_gyro_fifo_s gyro{};
-	gyro.timestamp_sample = timestamp_sample;
-	gyro.samples = samples;
-	gyro.dt = FIFO_SAMPLE_DT;
+	sensor_gyro_s sensor_gyro{};
+	sensor_gyro.timestamp_sample = timestamp_sample;
+	sensor_gyro.device_id = get_device_id();
+
+	float gyro_sum[3] {};
 
 	for (int i = 0; i < samples; i++) {
 		const FIFO::DATA &fifo_sample = buffer.f[i];
 
-		const int16_t gyro_x = combine(fifo_sample.RATE_X_MSB, fifo_sample.RATE_X_LSB);
-		const int16_t gyro_y = combine(fifo_sample.RATE_Y_MSB, fifo_sample.RATE_Y_LSB);
-		const int16_t gyro_z = combine(fifo_sample.RATE_Z_MSB, fifo_sample.RATE_Z_LSB);
+		int16_t gyro_x = combine(fifo_sample.RATE_X_MSB, fifo_sample.RATE_X_LSB);
+		int16_t gyro_y = combine(fifo_sample.RATE_Y_MSB, fifo_sample.RATE_Y_LSB);
+		int16_t gyro_z = combine(fifo_sample.RATE_Z_MSB, fifo_sample.RATE_Z_LSB);
 
 		// 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;
-		gyro.y[i] = (gyro_y == INT16_MIN) ? INT16_MAX : -gyro_y;
-		gyro.z[i] = (gyro_z == INT16_MIN) ? INT16_MAX : -gyro_z;
+		//gyro.x[i] = gyro_x;
+		gyro_y = math::negate(gyro_y);
+		gyro_z = math::negate(gyro_z);
+
+		rotate_3i(_rotation, gyro_x, gyro_y, gyro_z);
+
+		gyro_sum[0] += gyro_x * _gyro_scale;
+		gyro_sum[1] += gyro_y * _gyro_scale;
+		gyro_sum[2] += gyro_z * _gyro_scale;
 	}
 
-	_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));
+	sensor_gyro.x = gyro_sum[0] / samples;
+	sensor_gyro.y = gyro_sum[1] / samples;
+	sensor_gyro.z = gyro_sum[2] / samples;
 
-	_px4_gyro.updateFIFO(gyro);
+	sensor_gyro.range = _gyro_range;
+	sensor_gyro.temperature = NAN;
+	sensor_gyro.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);
+
+	sensor_gyro.timestamp = hrt_absolute_time();
+	_sensor_gyro_pub.publish(sensor_gyro);
 
 	return true;
 }
diff --git a/src/drivers/imu/bosch/bmi085/BMI085_Gyroscope.hpp b/src/drivers/imu/bosch/bmi085/BMI085_Gyroscope.hpp
index 75d4c03d83..d23abd2b26 100644
--- a/src/drivers/imu/bosch/bmi085/BMI085_Gyroscope.hpp
+++ b/src/drivers/imu/bosch/bmi085/BMI085_Gyroscope.hpp
@@ -35,10 +35,11 @@
 
 #include "BMI085.hpp"
 
-#include <lib/drivers/gyroscope/PX4Gyroscope.hpp>
-
 #include "Bosch_BMI085_Gyroscope_Registers.hpp"
 
+#include <uORB/PublicationMulti.hpp>
+#include <uORB/topics/sensor_gyro.h>
+
 namespace Bosch::BMI085::Gyroscope
 {
 
@@ -58,7 +59,7 @@ private:
 	static constexpr uint32_t RATE{2000}; // 2000 Hz
 	static constexpr float FIFO_SAMPLE_DT{1e6f / RATE};
 
-	static constexpr int32_t FIFO_MAX_SAMPLES{math::min(FIFO::SIZE / sizeof(FIFO::DATA), sizeof(sensor_gyro_fifo_s::x) / sizeof(sensor_gyro_fifo_s::x[0]))};
+	static constexpr int32_t FIFO_MAX_SAMPLES{FIFO::SIZE / sizeof(FIFO::DATA)};
 
 	// Transfer data
 	struct FIFOTransferBuffer {
@@ -95,7 +96,11 @@ private:
 	bool FIFORead(const hrt_abstime &timestamp_sample, uint8_t samples);
 	void FIFOReset();
 
-	PX4Gyroscope _px4_gyro;
+	uORB::PublicationMulti<sensor_gyro_s> _sensor_gyro_pub{ORB_ID(sensor_gyro)};
+
+	const enum Rotation _rotation;
+	float _gyro_scale{0.f};
+	float _gyro_range{0.f};
 
 	perf_counter_t _bad_register_perf{perf_alloc(PC_COUNT, MODULE_NAME"_gyro: bad register")};
 	perf_counter_t _bad_transfer_perf{perf_alloc(PC_COUNT, MODULE_NAME"_gyro: bad transfer")};
diff --git a/src/drivers/imu/bosch/bmi085/CMakeLists.txt b/src/drivers/imu/bosch/bmi085/CMakeLists.txt
index f11c0dab13..abd3fd6d9d 100644
--- a/src/drivers/imu/bosch/bmi085/CMakeLists.txt
+++ b/src/drivers/imu/bosch/bmi085/CMakeLists.txt
@@ -48,7 +48,5 @@ px4_add_module(
 
 		bmi085_main.cpp
 	DEPENDS
-		drivers_accelerometer
-		drivers_gyroscope
 		px4_work_queue
 	)
diff --git a/src/drivers/imu/bosch/bmi088/BMI088.cpp b/src/drivers/imu/bosch/bmi088/BMI088.cpp
index 0c45fd3a2d..df49e8ee8a 100644
--- a/src/drivers/imu/bosch/bmi088/BMI088.cpp
+++ b/src/drivers/imu/bosch/bmi088/BMI088.cpp
@@ -1,6 +1,6 @@
 /****************************************************************************
  *
- *   Copyright (c) 2020-2021 PX4 Development Team. All rights reserved.
+ *   Copyright (c) 2020-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
diff --git a/src/drivers/imu/bosch/bmi088/BMI088.hpp b/src/drivers/imu/bosch/bmi088/BMI088.hpp
index 35187fdb72..59ce4e2569 100644
--- a/src/drivers/imu/bosch/bmi088/BMI088.hpp
+++ b/src/drivers/imu/bosch/bmi088/BMI088.hpp
@@ -1,6 +1,6 @@
 /****************************************************************************
  *
- *   Copyright (c) 2020-2021 PX4 Development Team. All rights reserved.
+ *   Copyright (c) 2020-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
diff --git a/src/drivers/imu/bosch/bmi088/BMI088_Accelerometer.cpp b/src/drivers/imu/bosch/bmi088/BMI088_Accelerometer.cpp
index 8f0dc0068b..5990a64c17 100644
--- a/src/drivers/imu/bosch/bmi088/BMI088_Accelerometer.cpp
+++ b/src/drivers/imu/bosch/bmi088/BMI088_Accelerometer.cpp
@@ -1,6 +1,6 @@
 /****************************************************************************
  *
- *   Copyright (c) 2020-2021 PX4 Development Team. All rights reserved.
+ *   Copyright (c) 2020-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
@@ -42,13 +42,13 @@ namespace Bosch::BMI088::Accelerometer
 
 BMI088_Accelerometer::BMI088_Accelerometer(const I2CSPIDriverConfig &config) :
 	BMI088(config),
-	_px4_accel(get_device_id(), config.rotation)
+	_rotation(config.rotation)
 {
 	if (config.drdy_gpio != 0) {
 		_drdy_missed_perf = perf_alloc(PC_COUNT, MODULE_NAME"_accel: DRDY missed");
 	}
 
-	ConfigureSampleRate(_px4_accel.get_max_rate_hz());
+	ConfigureSampleRate(RATE);
 }
 
 BMI088_Accelerometer::~BMI088_Accelerometer()
@@ -286,23 +286,23 @@ void BMI088_Accelerometer::ConfigureAccel()
 
 	switch (ACC_RANGE) {
 	case acc_range_3g:
-		_px4_accel.set_scale(CONSTANTS_ONE_G * (powf(2, ACC_RANGE + 1) * 1.5f) / 32768.f);
-		_px4_accel.set_range(3.f * CONSTANTS_ONE_G);
+		_accel_scale = CONSTANTS_ONE_G * (powf(2, ACC_RANGE + 1) * 1.5f) / 32768.f;
+		_accel_range = 3.f * CONSTANTS_ONE_G;
 		break;
 
 	case acc_range_6g:
-		_px4_accel.set_scale(CONSTANTS_ONE_G * (powf(2, ACC_RANGE + 1) * 1.5f) / 32768.f);
-		_px4_accel.set_range(6.f * CONSTANTS_ONE_G);
+		_accel_scale = CONSTANTS_ONE_G * (powf(2, ACC_RANGE + 1) * 1.5f) / 32768.f;
+		_accel_range = 6.f * CONSTANTS_ONE_G;
 		break;
 
 	case acc_range_12g:
-		_px4_accel.set_scale(CONSTANTS_ONE_G * (powf(2, ACC_RANGE + 1) * 1.5f) / 32768.f);
-		_px4_accel.set_range(12.f * CONSTANTS_ONE_G);
+		_accel_scale = CONSTANTS_ONE_G * (powf(2, ACC_RANGE + 1) * 1.5f) / 32768.f;
+		_accel_range = 12.f * CONSTANTS_ONE_G;
 		break;
 
 	case acc_range_24g:
-		_px4_accel.set_scale(CONSTANTS_ONE_G * (powf(2, ACC_RANGE + 1) * 1.5f) / 32768.f);
-		_px4_accel.set_range(24.f * CONSTANTS_ONE_G);
+		_accel_scale = CONSTANTS_ONE_G * (powf(2, ACC_RANGE + 1) * 1.5f) / 32768.f;
+		_accel_range = 24.f * CONSTANTS_ONE_G;
 		break;
 	}
 }
@@ -458,7 +458,7 @@ uint16_t BMI088_Accelerometer::FIFOReadCount()
 	const uint8_t FIFO_LENGTH_0 = fifo_len_buf[2];        // fifo_byte_counter[7:0]
 	const uint8_t FIFO_LENGTH_1 = fifo_len_buf[3] & 0x3F; // fifo_byte_counter[13:8]
 
-	return combine(FIFO_LENGTH_1, FIFO_LENGTH_0);
+	return (FIFO_LENGTH_1 << 8) + FIFO_LENGTH_0;
 }
 
 bool BMI088_Accelerometer::FIFORead(const hrt_abstime &timestamp_sample, uint8_t samples)
@@ -471,7 +471,7 @@ bool BMI088_Accelerometer::FIFORead(const hrt_abstime &timestamp_sample, uint8_t
 		return false;
 	}
 
-	const size_t fifo_byte_counter = combine(buffer.FIFO_LENGTH_1 & 0x3F, buffer.FIFO_LENGTH_0);
+	const size_t fifo_byte_counter = ((buffer.FIFO_LENGTH_1 & 0x3F) << 8) + buffer.FIFO_LENGTH_0;
 
 	// An empty FIFO corresponds to 0x8000
 	if (fifo_byte_counter == 0x8000) {
@@ -483,15 +483,17 @@ bool BMI088_Accelerometer::FIFORead(const hrt_abstime &timestamp_sample, uint8_t
 		return false;
 	}
 
-	sensor_accel_fifo_s accel{};
-	accel.timestamp_sample = timestamp_sample;
-	accel.samples = 0;
-	accel.dt = FIFO_SAMPLE_DT;
+	sensor_accel_s sensor_accel{};
+	sensor_accel.timestamp_sample = timestamp_sample;
+	sensor_accel.device_id = get_device_id();
 
 	// first find all sensor data frames in the buffer
 	uint8_t *data_buffer = (uint8_t *)&buffer.f[0];
 	unsigned fifo_buffer_index = 0; // start of buffer
 
+	float accel_sum[3] {};
+	int accel_samples = 0;
+
 	while (fifo_buffer_index < math::min(fifo_byte_counter, transfer_size - 4)) {
 		// look for header signature (first 6 bits) followed by two bits indicating the status of INT1 and INT2
 		switch (data_buffer[fifo_buffer_index] & 0xFC) {
@@ -500,16 +502,23 @@ bool BMI088_Accelerometer::FIFORead(const hrt_abstime &timestamp_sample, uint8_t
 				// Frame length: 7 bytes (1 byte header + 6 bytes payload)
 
 				FIFO::DATA *fifo_sample = (FIFO::DATA *)&data_buffer[fifo_buffer_index];
-				const int16_t accel_x = combine(fifo_sample->ACC_X_MSB, fifo_sample->ACC_X_LSB);
-				const int16_t accel_y = combine(fifo_sample->ACC_Y_MSB, fifo_sample->ACC_Y_LSB);
-				const int16_t accel_z = combine(fifo_sample->ACC_Z_MSB, fifo_sample->ACC_Z_LSB);
+				int16_t accel_x = combine(fifo_sample->ACC_X_MSB, fifo_sample->ACC_X_LSB);
+				int16_t accel_y = combine(fifo_sample->ACC_Y_MSB, fifo_sample->ACC_Y_LSB);
+				int16_t accel_z = combine(fifo_sample->ACC_Z_MSB, fifo_sample->ACC_Z_LSB);
 
 				// 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[accel.samples] = accel_x;
-				accel.y[accel.samples] = (accel_y == INT16_MIN) ? INT16_MAX : -accel_y;
-				accel.z[accel.samples] = (accel_z == INT16_MIN) ? INT16_MAX : -accel_z;
-				accel.samples++;
+				// accel_x = accel_x;
+				accel_y = math::negate(accel_y);
+				accel_z = math::negate(accel_z);
+
+				rotate_3i(_rotation, accel_x, accel_y, accel_z);
+
+				accel_sum[0] += accel_x;
+				accel_sum[1] += accel_y;
+				accel_sum[2] += accel_z;
+
+				accel_samples++;
 
 				fifo_buffer_index += 7; // move forward to next record
 			}
@@ -549,11 +558,19 @@ bool BMI088_Accelerometer::FIFORead(const hrt_abstime &timestamp_sample, uint8_t
 		}
 	}
 
-	_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) {
+		sensor_accel.x = (accel_sum[0] / accel_samples) * _accel_scale;
+		sensor_accel.y = (accel_sum[1] / accel_samples) * _accel_scale;
+		sensor_accel.z = (accel_sum[2] / accel_samples) * _accel_scale;
+
+		sensor_accel.range = _accel_range;
+		sensor_accel.temperature = _temperature;
+		sensor_accel.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);
+
+		sensor_accel.timestamp = hrt_absolute_time();
+		_sensor_accel_pub.publish(sensor_accel);
 
-	if (accel.samples > 0) {
-		_px4_accel.updateFIFO(accel);
 		return true;
 	}
 
@@ -600,7 +617,7 @@ void BMI088_Accelerometer::UpdateTemperature()
 	float temperature = (Temp_int11 * 0.125f) + 23.f; // Temp_int11 * 0.125°C/LSB + 23°C
 
 	if (PX4_ISFINITE(temperature)) {
-		_px4_accel.set_temperature(temperature);
+		_temperature = temperature;
 
 	} else {
 		perf_count(_bad_transfer_perf);
diff --git a/src/drivers/imu/bosch/bmi088/BMI088_Accelerometer.hpp b/src/drivers/imu/bosch/bmi088/BMI088_Accelerometer.hpp
index ccf5f40d3a..63e0ed2709 100644
--- a/src/drivers/imu/bosch/bmi088/BMI088_Accelerometer.hpp
+++ b/src/drivers/imu/bosch/bmi088/BMI088_Accelerometer.hpp
@@ -1,6 +1,6 @@
 /****************************************************************************
  *
- *   Copyright (c) 2020-2021 PX4 Development Team. All rights reserved.
+ *   Copyright (c) 2020-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
@@ -35,10 +35,11 @@
 
 #include "BMI088.hpp"
 
-#include <lib/drivers/accelerometer/PX4Accelerometer.hpp>
-
 #include "Bosch_BMI088_Accelerometer_Registers.hpp"
 
+#include <uORB/PublicationMulti.hpp>
+#include <uORB/topics/sensor_accel.h>
+
 namespace Bosch::BMI088::Accelerometer
 {
 
@@ -58,7 +59,7 @@ private:
 	static constexpr uint32_t RATE{1600}; // 1600 Hz
 	static constexpr float FIFO_SAMPLE_DT{1e6f / RATE};
 
-	static constexpr int32_t FIFO_MAX_SAMPLES{math::min(FIFO::SIZE / sizeof(FIFO::DATA), sizeof(sensor_accel_fifo_s::x) / sizeof(sensor_accel_fifo_s::x[0]))};
+	static constexpr int32_t FIFO_MAX_SAMPLES{FIFO::SIZE / sizeof(FIFO::DATA)};
 
 	// Transfer data
 	struct FIFOTransferBuffer {
@@ -101,7 +102,13 @@ private:
 
 	void UpdateTemperature();
 
-	PX4Accelerometer _px4_accel;
+	uORB::PublicationMulti<sensor_accel_s> _sensor_accel_pub{ORB_ID(sensor_accel)};
+
+	const enum Rotation _rotation;
+	float _accel_scale{0.f};
+	float _accel_range{0.f};
+
+	float _temperature{NAN};
 
 	perf_counter_t _bad_register_perf{perf_alloc(PC_COUNT, MODULE_NAME"_accel: bad register")};
 	perf_counter_t _bad_transfer_perf{perf_alloc(PC_COUNT, MODULE_NAME"_accel: bad transfer")};
diff --git a/src/drivers/imu/bosch/bmi088/BMI088_Gyroscope.cpp b/src/drivers/imu/bosch/bmi088/BMI088_Gyroscope.cpp
index 7dac23e578..03aff967e1 100644
--- a/src/drivers/imu/bosch/bmi088/BMI088_Gyroscope.cpp
+++ b/src/drivers/imu/bosch/bmi088/BMI088_Gyroscope.cpp
@@ -1,6 +1,6 @@
 /****************************************************************************
  *
- *   Copyright (c) 2020-2021 PX4 Development Team. All rights reserved.
+ *   Copyright (c) 2020-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
@@ -33,8 +33,6 @@
 
 #include "BMI088_Gyroscope.hpp"
 
-#include <px4_platform/board_dma_alloc.h>
-
 using namespace time_literals;
 
 namespace Bosch::BMI088::Gyroscope
@@ -42,13 +40,13 @@ namespace Bosch::BMI088::Gyroscope
 
 BMI088_Gyroscope::BMI088_Gyroscope(const I2CSPIDriverConfig &config) :
 	BMI088(config),
-	_px4_gyro(get_device_id(), config.rotation)
+	_rotation(config.rotation)
 {
 	if (config.drdy_gpio != 0) {
 		_drdy_missed_perf = perf_alloc(PC_COUNT, MODULE_NAME"_gyro: DRDY missed");
 	}
 
-	ConfigureSampleRate(_px4_gyro.get_max_rate_hz());
+	ConfigureSampleRate(RATE);
 }
 
 BMI088_Gyroscope::~BMI088_Gyroscope()
@@ -257,28 +255,28 @@ void BMI088_Gyroscope::ConfigureGyro()
 
 	switch (GYRO_RANGE) {
 	case gyro_range_2000_dps:
-		_px4_gyro.set_scale(math::radians(1.f / 16.384f));
-		_px4_gyro.set_range(math::radians(2000.f));
+		_gyro_scale = math::radians(1.f / 16.384f);
+		_gyro_range = math::radians(2000.f);
 		break;
 
 	case gyro_range_1000_dps:
-		_px4_gyro.set_scale(math::radians(1.f / 32.768f));
-		_px4_gyro.set_range(math::radians(1000.f));
+		_gyro_scale = math::radians(1.f / 32.768f);
+		_gyro_range = math::radians(1000.f);
 		break;
 
 	case gyro_range_500_dps:
-		_px4_gyro.set_scale(math::radians(1.f / 65.536f));
-		_px4_gyro.set_range(math::radians(500.f));
+		_gyro_scale = math::radians(1.f / 65.536f);
+		_gyro_range = math::radians(500.f);
 		break;
 
 	case gyro_range_250_dps:
-		_px4_gyro.set_scale(math::radians(1.f / 131.072f));
-		_px4_gyro.set_range(math::radians(250.f));
+		_gyro_scale = math::radians(1.f / 131.072f);
+		_gyro_range = math::radians(250.f);
 		break;
 
 	case gyro_range_125_dps:
-		_px4_gyro.set_scale(math::radians(1.f / 262.144f));
-		_px4_gyro.set_range(math::radians(125.f));
+		_gyro_scale = math::radians(1.f / 262.144f);
+		_gyro_range = math::radians(125.f);
 		break;
 	}
 }
@@ -414,29 +412,43 @@ bool BMI088_Gyroscope::FIFORead(const hrt_abstime &timestamp_sample, uint8_t sam
 		return false;
 	}
 
-	sensor_gyro_fifo_s gyro{};
-	gyro.timestamp_sample = timestamp_sample;
-	gyro.samples = samples;
-	gyro.dt = FIFO_SAMPLE_DT;
+	sensor_gyro_s sensor_gyro{};
+	sensor_gyro.timestamp_sample = timestamp_sample;
+	sensor_gyro.device_id = get_device_id();
+
+	float gyro_sum[3] {};
 
 	for (int i = 0; i < samples; i++) {
 		const FIFO::DATA &fifo_sample = buffer.f[i];
 
-		const int16_t gyro_x = combine(fifo_sample.RATE_X_MSB, fifo_sample.RATE_X_LSB);
-		const int16_t gyro_y = combine(fifo_sample.RATE_Y_MSB, fifo_sample.RATE_Y_LSB);
-		const int16_t gyro_z = combine(fifo_sample.RATE_Z_MSB, fifo_sample.RATE_Z_LSB);
+		int16_t gyro_x = combine(fifo_sample.RATE_X_MSB, fifo_sample.RATE_X_LSB);
+		int16_t gyro_y = combine(fifo_sample.RATE_Y_MSB, fifo_sample.RATE_Y_LSB);
+		int16_t gyro_z = combine(fifo_sample.RATE_Z_MSB, fifo_sample.RATE_Z_LSB);
 
 		// 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;
-		gyro.y[i] = (gyro_y == INT16_MIN) ? INT16_MAX : -gyro_y;
-		gyro.z[i] = (gyro_z == INT16_MIN) ? INT16_MAX : -gyro_z;
+		//gyro.x[i] = gyro_x;
+		gyro_y = math::negate(gyro_y);
+		gyro_z = math::negate(gyro_z);
+
+		rotate_3i(_rotation, gyro_x, gyro_y, gyro_z);
+
+		gyro_sum[0] += gyro_x * _gyro_scale;
+		gyro_sum[1] += gyro_y * _gyro_scale;
+		gyro_sum[2] += gyro_z * _gyro_scale;
 	}
 
-	_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));
+	sensor_gyro.x = gyro_sum[0] / samples;
+	sensor_gyro.y = gyro_sum[1] / samples;
+	sensor_gyro.z = gyro_sum[2] / samples;
 
-	_px4_gyro.updateFIFO(gyro);
+	sensor_gyro.range = _gyro_range;
+	sensor_gyro.temperature = NAN;
+	sensor_gyro.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);
+
+	sensor_gyro.timestamp = hrt_absolute_time();
+	_sensor_gyro_pub.publish(sensor_gyro);
 
 	return true;
 }
diff --git a/src/drivers/imu/bosch/bmi088/BMI088_Gyroscope.hpp b/src/drivers/imu/bosch/bmi088/BMI088_Gyroscope.hpp
index 59d847bf02..879caec6df 100644
--- a/src/drivers/imu/bosch/bmi088/BMI088_Gyroscope.hpp
+++ b/src/drivers/imu/bosch/bmi088/BMI088_Gyroscope.hpp
@@ -1,6 +1,6 @@
 /****************************************************************************
  *
- *   Copyright (c) 2020-2021 PX4 Development Team. All rights reserved.
+ *   Copyright (c) 2020-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
@@ -35,10 +35,11 @@
 
 #include "BMI088.hpp"
 
-#include <lib/drivers/gyroscope/PX4Gyroscope.hpp>
-
 #include "Bosch_BMI088_Gyroscope_Registers.hpp"
 
+#include <uORB/PublicationMulti.hpp>
+#include <uORB/topics/sensor_gyro.h>
+
 namespace Bosch::BMI088::Gyroscope
 {
 
@@ -58,7 +59,7 @@ private:
 	static constexpr uint32_t RATE{2000}; // 2000 Hz
 	static constexpr float FIFO_SAMPLE_DT{1e6f / RATE};
 
-	static constexpr int32_t FIFO_MAX_SAMPLES{math::min(FIFO::SIZE / sizeof(FIFO::DATA), sizeof(sensor_gyro_fifo_s::x) / sizeof(sensor_gyro_fifo_s::x[0]))};
+	static constexpr int32_t FIFO_MAX_SAMPLES{FIFO::SIZE / sizeof(FIFO::DATA)};
 
 	// Transfer data
 	struct FIFOTransferBuffer {
@@ -95,7 +96,11 @@ private:
 	bool FIFORead(const hrt_abstime &timestamp_sample, uint8_t samples);
 	void FIFOReset();
 
-	PX4Gyroscope _px4_gyro;
+	uORB::PublicationMulti<sensor_gyro_s> _sensor_gyro_pub{ORB_ID(sensor_gyro)};
+
+	const enum Rotation _rotation;
+	float _gyro_scale{0.f};
+	float _gyro_range{0.f};
 
 	perf_counter_t _bad_register_perf{perf_alloc(PC_COUNT, MODULE_NAME"_gyro: bad register")};
 	perf_counter_t _bad_transfer_perf{perf_alloc(PC_COUNT, MODULE_NAME"_gyro: bad transfer")};
diff --git a/src/drivers/imu/bosch/bmi088/Bosch_BMI088_Accelerometer_Registers.hpp b/src/drivers/imu/bosch/bmi088/Bosch_BMI088_Accelerometer_Registers.hpp
index 00b8ffe6d6..eb67c4c20f 100644
--- a/src/drivers/imu/bosch/bmi088/Bosch_BMI088_Accelerometer_Registers.hpp
+++ b/src/drivers/imu/bosch/bmi088/Bosch_BMI088_Accelerometer_Registers.hpp
@@ -1,6 +1,6 @@
 /****************************************************************************
  *
- *   Copyright (c) 2020-2021 PX4 Development Team. All rights reserved.
+ *   Copyright (c) 2020-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
diff --git a/src/drivers/imu/bosch/bmi088/Bosch_BMI088_Gyroscope_Registers.hpp b/src/drivers/imu/bosch/bmi088/Bosch_BMI088_Gyroscope_Registers.hpp
index 0f1974f2cb..e290915e35 100644
--- a/src/drivers/imu/bosch/bmi088/Bosch_BMI088_Gyroscope_Registers.hpp
+++ b/src/drivers/imu/bosch/bmi088/Bosch_BMI088_Gyroscope_Registers.hpp
@@ -1,6 +1,6 @@
 /****************************************************************************
  *
- *   Copyright (c) 2020-2021 PX4 Development Team. All rights reserved.
+ *   Copyright (c) 2020-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
diff --git a/src/drivers/imu/bosch/bmi088/CMakeLists.txt b/src/drivers/imu/bosch/bmi088/CMakeLists.txt
index 8326953f07..ced56aa801 100644
--- a/src/drivers/imu/bosch/bmi088/CMakeLists.txt
+++ b/src/drivers/imu/bosch/bmi088/CMakeLists.txt
@@ -1,6 +1,6 @@
 ############################################################################
 #
-#   Copyright (c) 2019-2020 PX4 Development Team. All rights reserved.
+#   Copyright (c) 2019-2022 PX4 Development Team. All rights reserved.
 #
 # Redistribution and use in source and binary forms, with or without
 # modification, are permitted provided that the following conditions
@@ -48,7 +48,5 @@ px4_add_module(
 
 		bmi088_main.cpp
 	DEPENDS
-		drivers_accelerometer
-		drivers_gyroscope
 		px4_work_queue
 	)
diff --git a/src/drivers/imu/bosch/bmi088_i2c/BMI088.cpp b/src/drivers/imu/bosch/bmi088_i2c/BMI088.cpp
index 54a8b2eed2..4fce6d73d0 100644
--- a/src/drivers/imu/bosch/bmi088_i2c/BMI088.cpp
+++ b/src/drivers/imu/bosch/bmi088_i2c/BMI088.cpp
@@ -1,6 +1,6 @@
 /****************************************************************************
  *
- *   Copyright (c) 2020 PX4 Development Team. All rights reserved.
+ *   Copyright (c) 2020-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
@@ -76,10 +76,7 @@ int BMI088::init()
 		return ret;
 	}
 
-	int res = Reset() ? 0 : -1;
-	_state = STATE::SELFTEST;
-
-	return res;
+	return Reset() ? 0 : -1;
 }
 
 bool BMI088::Reset()
diff --git a/src/drivers/imu/bosch/bmi088_i2c/BMI088.hpp b/src/drivers/imu/bosch/bmi088_i2c/BMI088.hpp
index 6c1734a059..ad16b2ee32 100644
--- a/src/drivers/imu/bosch/bmi088_i2c/BMI088.hpp
+++ b/src/drivers/imu/bosch/bmi088_i2c/BMI088.hpp
@@ -1,6 +1,6 @@
 /****************************************************************************
  *
- *   Copyright (c) 2020-2021 PX4 Development Team. All rights reserved.
+ *   Copyright (c) 2020-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
@@ -65,23 +65,16 @@ protected:
 	hrt_abstime _last_config_check_timestamp{0};
 	hrt_abstime _temperature_update_timestamp{0};
 	int _failure_count{0};
-	int _overflow_data_size_count{0};
-	int _overflow_fifo_max_samples_count{0};
-	int _fifo_read_error_count{0};
-	int _empty_count{0};
-	int _total_failure_count{0};
-
 
 	px4::atomic<hrt_abstime> _drdy_timestamp_sample{0};
 	bool _data_ready_interrupt_enabled{false};
 
 	enum class STATE : uint8_t {
-		SELFTEST,
 		RESET,
 		WAIT_FOR_RESET,
 		CONFIGURE,
 		FIFO_READ,
-	} _state{STATE::SELFTEST};
+	} _state{STATE::RESET};
 
 	uint16_t _fifo_empty_interval_us{2500}; // 2500 us / 400 Hz transfer interval
 
diff --git a/src/drivers/imu/bosch/bmi088_i2c/BMI088_Accelerometer.cpp b/src/drivers/imu/bosch/bmi088_i2c/BMI088_Accelerometer.cpp
index 6d7e4d6739..1e70e52cbd 100644
--- a/src/drivers/imu/bosch/bmi088_i2c/BMI088_Accelerometer.cpp
+++ b/src/drivers/imu/bosch/bmi088_i2c/BMI088_Accelerometer.cpp
@@ -1,6 +1,6 @@
 /****************************************************************************
  *
- *   Copyright (c) 2020-2021 PX4 Development Team. All rights reserved.
+ *   Copyright (c) 2020-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
@@ -39,15 +39,16 @@ using namespace time_literals;
 
 namespace Bosch::BMI088::Accelerometer
 {
+
 BMI088_Accelerometer::BMI088_Accelerometer(const I2CSPIDriverConfig &config) :
 	BMI088(config),
-	_px4_accel(get_device_id(), config.rotation)
+	_rotation(config.rotation)
 {
 	if (config.drdy_gpio != 0) {
 		_drdy_missed_perf = perf_alloc(PC_COUNT, MODULE_NAME"_accel: DRDY missed");
 	}
 
-	ConfigureSampleRate(1600);
+	ConfigureSampleRate(RATE);
 }
 
 BMI088_Accelerometer::~BMI088_Accelerometer()
@@ -80,32 +81,21 @@ void BMI088_Accelerometer::print_status()
 	perf_print_counter(_drdy_missed_perf);
 }
 
-uint8_t BMI088_Accelerometer::RegisterRead(Register reg)
-{
-	uint8_t add = static_cast<uint8_t>(reg);
-	uint8_t cmd[2] = {add, 0};
-	transfer(&cmd[0], 1, &cmd[1], 1);
-	return cmd[1];
-}
-
-uint8_t BMI088_Accelerometer::RegisterWrite(Register reg, uint8_t value)
-{
-	uint8_t add = static_cast<uint8_t>(reg);
-	uint8_t cmd[2] = { add, value};
-	return transfer(cmd, sizeof(cmd), nullptr, 0);
-}
-
 int BMI088_Accelerometer::probe()
 {
 	const uint8_t ACC_CHIP_ID = RegisterRead(Register::ACC_CHIP_ID);
 
-	if (ACC_CHIP_ID != ID) {
+	if (ACC_CHIP_ID == ID_088) {
+		DEVICE_DEBUG("BMI088 Accel");
+
+	} else if (ACC_CHIP_ID == ID_090L) {
+		DEVICE_DEBUG("BMI090L Accel");
+
+	} else {
 		DEVICE_DEBUG("unexpected ACC_CHIP_ID 0x%02x", ACC_CHIP_ID);
 		return PX4_ERROR;
 	}
 
-	PX4_WARN("Probe success, ACC_CHIP_ID: 0x%02x", ACC_CHIP_ID);
-
 	return PX4_OK;
 }
 
@@ -114,27 +104,17 @@ void BMI088_Accelerometer::RunImpl()
 	const hrt_abstime now = hrt_absolute_time();
 
 	switch (_state) {
-	case STATE::SELFTEST:
-		//PX4_WARN("Selftest state");
-		//SelfTest();
-		_state = STATE::RESET;
-		ScheduleDelayed(10_ms);
-		break;
-
 	case STATE::RESET:
 		// ACC_SOFTRESET: Writing a value of 0xB6 to this register resets the sensor
 		RegisterWrite(Register::ACC_SOFTRESET, 0xB6);
 		_reset_timestamp = now;
 		_failure_count = 0;
 		_state = STATE::WAIT_FOR_RESET;
-
-
 		ScheduleDelayed(1_ms); // Following a delay of 1 ms, all configuration settings are overwritten with their reset value.
-
 		break;
 
 	case STATE::WAIT_FOR_RESET:
-		if (RegisterRead(Register::ACC_CHIP_ID) == ID) {
+		if ((RegisterRead(Register::ACC_CHIP_ID) == ID_088) || (RegisterRead(Register::ACC_CHIP_ID) == ID_090L)) {
 			// ACC_PWR_CONF: Power on sensor
 			RegisterWrite(Register::ACC_PWR_CONF, 0);
 
@@ -191,36 +171,125 @@ void BMI088_Accelerometer::RunImpl()
 		break;
 
 	case STATE::FIFO_READ: {
-			SimpleFIFORead(now);
+			hrt_abstime timestamp_sample = now;
+			uint8_t samples = 0;
+
+			if (_data_ready_interrupt_enabled) {
+				// scheduled from interrupt if _drdy_timestamp_sample was set as expected
+				const hrt_abstime drdy_timestamp_sample = _drdy_timestamp_sample.fetch_and(0);
+
+				if ((now - drdy_timestamp_sample) < _fifo_empty_interval_us) {
+					timestamp_sample = drdy_timestamp_sample;
+					samples = _fifo_samples;
+
+				} else {
+					perf_count(_drdy_missed_perf);
+				}
+
+				// push backup schedule back
+				ScheduleDelayed(_fifo_empty_interval_us * 2);
+			}
+
+			if (samples == 0) {
+				// check current FIFO count
+				const uint16_t fifo_byte_counter = FIFOReadCount();
+
+				if (fifo_byte_counter >= FIFO::SIZE) {
+					FIFOReset();
+					perf_count(_fifo_overflow_perf);
+
+				} else if ((fifo_byte_counter == 0) || (fifo_byte_counter == 0x8000)) {
+					// An empty FIFO corresponds to 0x8000
+					perf_count(_fifo_empty_perf);
+
+				} else {
+					samples = fifo_byte_counter / sizeof(FIFO::DATA);
+
+					// tolerate minor jitter, leave sample to next iteration if behind by only 1
+					if (samples == _fifo_samples + 1) {
+						timestamp_sample -= static_cast<int>(FIFO_SAMPLE_DT);
+						samples--;
+					}
+
+					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(timestamp_sample, 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;
+				}
+			}
+
+			if (!success || hrt_elapsed_time(&_last_config_check_timestamp) > 100_ms) {
+				// check configuration registers periodically or immediately following any failure
+				if (RegisterCheck(_register_cfg[_checked_register])) {
+					_last_config_check_timestamp = now;
+					_checked_register = (_checked_register + 1) % size_register_cfg;
+
+				} else {
+					// register check failed, force reset
+					perf_count(_bad_register_perf);
+					Reset();
+				}
+
+			} else {
+				// periodically update temperature (~1 Hz)
+				if (hrt_elapsed_time(&_temperature_update_timestamp) >= 1_s) {
+					UpdateTemperature();
+					_temperature_update_timestamp = now;
+				}
+			}
 		}
+
 		break;
 	}
 }
 
 void BMI088_Accelerometer::ConfigureAccel()
 {
-	//PX4_WARN("ConfigureAccel");
 	const uint8_t ACC_RANGE = RegisterRead(Register::ACC_RANGE) & (Bit1 | Bit0);
 
 	switch (ACC_RANGE) {
 	case acc_range_3g:
-		_px4_accel.set_scale(CONSTANTS_ONE_G * (powf(2, ACC_RANGE + 1) * 1.5f) / 32768.f);
-		_px4_accel.set_range(3.f * CONSTANTS_ONE_G);
+		_accel_scale = CONSTANTS_ONE_G * (powf(2, ACC_RANGE + 1) * 1.5f) / 32768.f;
+		_accel_range = 3.f * CONSTANTS_ONE_G;
 		break;
 
 	case acc_range_6g:
-		_px4_accel.set_scale(CONSTANTS_ONE_G * (powf(2, ACC_RANGE + 1) * 1.5f) / 32768.f);
-		_px4_accel.set_range(6.f * CONSTANTS_ONE_G);
+		_accel_scale = CONSTANTS_ONE_G * (powf(2, ACC_RANGE + 1) * 1.5f) / 32768.f;
+		_accel_range = 6.f * CONSTANTS_ONE_G;
 		break;
 
 	case acc_range_12g:
-		_px4_accel.set_scale(CONSTANTS_ONE_G * (powf(2, ACC_RANGE + 1) * 1.5f) / 32768.f);
-		_px4_accel.set_range(12.f * CONSTANTS_ONE_G);
+		_accel_scale = CONSTANTS_ONE_G * (powf(2, ACC_RANGE + 1) * 1.5f) / 32768.f;
+		_accel_range = 12.f * CONSTANTS_ONE_G;
 		break;
 
 	case acc_range_24g:
-		_px4_accel.set_scale(CONSTANTS_ONE_G * (powf(2, ACC_RANGE + 1) * 1.5f) / 32768.f);
-		_px4_accel.set_range(24.f * CONSTANTS_ONE_G);
+		_accel_scale = CONSTANTS_ONE_G * (powf(2, ACC_RANGE + 1) * 1.5f) / 32768.f;
+		_accel_range = 24.f * CONSTANTS_ONE_G;
 		break;
 	}
 }
@@ -231,15 +300,11 @@ void BMI088_Accelerometer::ConfigureSampleRate(int sample_rate)
 	const float min_interval = FIFO_SAMPLE_DT;
 	_fifo_empty_interval_us = math::max(roundf((1e6f / (float)sample_rate) / min_interval) * min_interval, min_interval);
 
-	PX4_WARN("_fifo_empty_interval_us %d", _fifo_empty_interval_us);
 	_fifo_samples = math::min((float)_fifo_empty_interval_us / (1e6f / RATE), (float)FIFO_MAX_SAMPLES);
 
-	PX4_WARN("_fifo_samples %d", _fifo_samples);
 	// recompute FIFO empty interval (us) with actual sample limit
 	_fifo_empty_interval_us = _fifo_samples * (1e6f / RATE);
 
-	PX4_WARN("_fifo_empty_interval_us %d", _fifo_empty_interval_us);
-	//PX4_WARN("_fifo_samples %d", _fifo_samples);
 	ConfigureFIFOWatermark(_fifo_samples);
 }
 
@@ -265,7 +330,6 @@ void BMI088_Accelerometer::ConfigureFIFOWatermark(uint8_t samples)
 
 bool BMI088_Accelerometer::Configure()
 {
-
 	// first set and clear all configured register bits
 	for (const auto &reg_cfg : _register_cfg) {
 		RegisterSetAndClearBits(reg_cfg.reg, reg_cfg.set_bits, reg_cfg.clear_bits);
@@ -293,6 +357,7 @@ int BMI088_Accelerometer::DataReadyInterruptCallback(int irq, void *context, voi
 
 void BMI088_Accelerometer::DataReady()
 {
+	_drdy_timestamp_sample.store(hrt_absolute_time());
 	ScheduleNow();
 }
 
@@ -334,6 +399,20 @@ bool BMI088_Accelerometer::RegisterCheck(const register_config_t &reg_cfg)
 	return success;
 }
 
+uint8_t BMI088_Accelerometer::RegisterRead(Register reg)
+{
+	uint8_t cmd = static_cast<uint8_t>(reg);
+	uint8_t value = 0;
+	transfer(&cmd, 1, &value, 1);
+	return value;
+}
+
+void BMI088_Accelerometer::RegisterWrite(Register reg, uint8_t value)
+{
+	uint8_t cmd[2] { (uint8_t)reg, value };
+	transfer(cmd, sizeof(cmd), nullptr, 0);
+}
+
 void BMI088_Accelerometer::RegisterSetAndClearBits(Register reg, uint8_t setbits, uint8_t clearbits)
 {
 	const uint8_t orig_val = RegisterRead(reg);
@@ -349,10 +428,9 @@ uint16_t BMI088_Accelerometer::FIFOReadCount()
 {
 	// FIFO length registers FIFO_LENGTH_1 and FIFO_LENGTH_0 contain the 14 bit FIFO byte
 	uint8_t fifo_len_buf[2] {};
-	fifo_len_buf[0] = static_cast<uint8_t>(Register::FIFO_LENGTH_0) | DIR_READ;
-	// fifo_len_buf[1] dummy byte
+	uint8_t cmd = static_cast<uint8_t>(Register::FIFO_LENGTH_0);
 
-	if (transfer(&fifo_len_buf[0], 1, &fifo_len_buf[0], 2) != PX4_OK) {
+	if (transfer(&cmd, 1, fifo_len_buf, sizeof(fifo_len_buf)) != PX4_OK) {
 		perf_count(_bad_transfer_perf);
 		return 0;
 	}
@@ -360,21 +438,20 @@ uint16_t BMI088_Accelerometer::FIFOReadCount()
 	const uint8_t FIFO_LENGTH_0 = fifo_len_buf[0];        // fifo_byte_counter[7:0]
 	const uint8_t FIFO_LENGTH_1 = fifo_len_buf[1] & 0x3F; // fifo_byte_counter[13:8]
 
-	return combine(FIFO_LENGTH_1, FIFO_LENGTH_0);
+	return (FIFO_LENGTH_1 << 8) + FIFO_LENGTH_0;
 }
 
 bool BMI088_Accelerometer::FIFORead(const hrt_abstime &timestamp_sample, uint8_t samples)
 {
 	FIFOTransferBuffer buffer{};
-	const size_t transfer_size = math::min(samples * sizeof(FIFO::DATA) + 4, FIFO::SIZE);
+	const size_t transfer_size = math::min(samples * sizeof(FIFO::DATA) + 2, FIFO::SIZE);
 
-	if (transfer((uint8_t *)&buffer, 1, (uint8_t *)&buffer, transfer_size) != PX4_OK) {
+	if (transfer(&buffer.cmd, 1, (uint8_t *)&buffer.FIFO_LENGTH_0, transfer_size) != PX4_OK) {
 		perf_count(_bad_transfer_perf);
 		return false;
 	}
 
-	//PX4_WARN("Accel transfer success");
-	const size_t fifo_byte_counter = combine(buffer.FIFO_LENGTH_1 & 0x3F, buffer.FIFO_LENGTH_0);
+	const size_t fifo_byte_counter = ((buffer.FIFO_LENGTH_1 & 0x3F) << 8) + buffer.FIFO_LENGTH_0;
 
 	// An empty FIFO corresponds to 0x8000
 	if (fifo_byte_counter == 0x8000) {
@@ -386,15 +463,17 @@ bool BMI088_Accelerometer::FIFORead(const hrt_abstime &timestamp_sample, uint8_t
 		return false;
 	}
 
-	sensor_accel_fifo_s accel{};
-	accel.timestamp_sample = timestamp_sample;
-	accel.samples = 0;
-	accel.dt = FIFO_SAMPLE_DT;
+	sensor_accel_s sensor_accel{};
+	sensor_accel.timestamp_sample = timestamp_sample;
+	sensor_accel.device_id = get_device_id();
 
 	// first find all sensor data frames in the buffer
 	uint8_t *data_buffer = (uint8_t *)&buffer.f[0];
 	unsigned fifo_buffer_index = 0; // start of buffer
 
+	float accel_sum[3] {};
+	int accel_samples = 0;
+
 	while (fifo_buffer_index < math::min(fifo_byte_counter, transfer_size - 4)) {
 		// look for header signature (first 6 bits) followed by two bits indicating the status of INT1 and INT2
 		switch (data_buffer[fifo_buffer_index] & 0xFC) {
@@ -403,16 +482,23 @@ bool BMI088_Accelerometer::FIFORead(const hrt_abstime &timestamp_sample, uint8_t
 				// Frame length: 7 bytes (1 byte header + 6 bytes payload)
 
 				FIFO::DATA *fifo_sample = (FIFO::DATA *)&data_buffer[fifo_buffer_index];
-				const int16_t accel_x = combine(fifo_sample->ACC_X_MSB, fifo_sample->ACC_X_LSB);
-				const int16_t accel_y = combine(fifo_sample->ACC_Y_MSB, fifo_sample->ACC_Y_LSB);
-				const int16_t accel_z = combine(fifo_sample->ACC_Z_MSB, fifo_sample->ACC_Z_LSB);
+				int16_t accel_x = combine(fifo_sample->ACC_X_MSB, fifo_sample->ACC_X_LSB);
+				int16_t accel_y = combine(fifo_sample->ACC_Y_MSB, fifo_sample->ACC_Y_LSB);
+				int16_t accel_z = combine(fifo_sample->ACC_Z_MSB, fifo_sample->ACC_Z_LSB);
 
 				// 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[accel.samples] = accel_x;
-				accel.y[accel.samples] = (accel_y == INT16_MIN) ? INT16_MAX : -accel_y;
-				accel.z[accel.samples] = (accel_z == INT16_MIN) ? INT16_MAX : -accel_z;
-				accel.samples++;
+				// accel_x = accel_x;
+				accel_y = math::negate(accel_y);
+				accel_z = math::negate(accel_z);
+
+				rotate_3i(_rotation, accel_x, accel_y, accel_z);
+
+				accel_sum[0] += accel_x;
+				accel_sum[1] += accel_y;
+				accel_sum[2] += accel_z;
+
+				accel_samples++;
 
 				fifo_buffer_index += 7; // move forward to next record
 			}
@@ -452,130 +538,25 @@ bool BMI088_Accelerometer::FIFORead(const hrt_abstime &timestamp_sample, uint8_t
 		}
 	}
 
-	_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) {
+		sensor_accel.x = (accel_sum[0] / accel_samples) * _accel_scale;
+		sensor_accel.y = (accel_sum[1] / accel_samples) * _accel_scale;
+		sensor_accel.z = (accel_sum[2] / accel_samples) * _accel_scale;
+
+		sensor_accel.range = _accel_range;
+		sensor_accel.temperature = _temperature;
+		sensor_accel.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);
+
+		sensor_accel.timestamp = hrt_absolute_time();
+		_sensor_accel_pub.publish(sensor_accel);
 
-	if (accel.samples > 0) {
-		_px4_accel.updateFIFO(accel);
 		return true;
 	}
 
 	return false;
 }
 
-bool BMI088_Accelerometer::SimpleFIFORead(const hrt_abstime &timestamp_sample)
-{
-	sensor_accel_fifo_s accel{};
-	accel.timestamp_sample = timestamp_sample;
-	accel.samples = 0;
-	accel.dt = FIFO_SAMPLE_DT;
-
-	int fifo_fill_level = 0;
-
-	uint8_t data_o[2] = { 0, 0 };
-	uint8_t data_i[1] = {static_cast<uint8_t>(Register::FIFO_LENGTH_0)};
-	data_i[0] = static_cast<uint8_t>(Register::FIFO_LENGTH_0);
-
-	transfer(&data_i[0], 1, &data_o[0], 2);
-	fifo_fill_level = data_o[0] + (data_o[1] << 8);
-
-	if (fifo_fill_level & 0x8000) {
-		return false;
-	}
-
-	int n_frames_to_read = 6;
-
-	// don't read more than 6 frames at a time
-	if (fifo_fill_level > n_frames_to_read * 7) {
-		fifo_fill_level = n_frames_to_read * 7;
-	}
-
-	if (fifo_fill_level == 0) {
-		return false;
-	}
-
-	uint8_t data[fifo_fill_level];
-
-	data[0] = static_cast<uint8_t>(Register::FIFO_DATA);
-
-	if (transfer(&data[0], 1, &data[0], fifo_fill_level) != PX4_OK) {
-		return false;
-	}
-
-	const uint8_t *p = &data[0];
-
-	while (fifo_fill_level >= 7) {
-		uint8_t frame_len = 2;
-
-		switch (p[0] & 0xFC) {
-		case 0x84: {
-				// accel frame
-				frame_len = 7;
-				const uint8_t *d = p + 1;
-				int16_t xyz[3] {
-					int16_t(uint16_t(d[0] | (d[1] << 8))),
-					int16_t(uint16_t(d[2] | (d[3] << 8))),
-					int16_t(uint16_t(d[4] | (d[5] << 8)))
-				};
-
-
-				const int16_t tX[3] = {1, 0, 0};
-				const int16_t tY[3] = {0, -1, 0};
-				const int16_t tZ[3] = {0, 0, -1};
-
-				float x = 0;
-				float y = 0;
-				float z = 0;
-
-				x = xyz[0] * tX[0] + xyz[1] * tX[1] + xyz[2] * tX[2];
-				y = xyz[0] * tY[0] + xyz[1] * tY[1] + xyz[2] * tY[2];
-				z = xyz[0] * tZ[0] + xyz[1] * tZ[1] + xyz[2] * tZ[2];
-
-				accel.x[accel.samples] = x;
-				accel.y[accel.samples] = y;
-				accel.z[accel.samples] = z;
-				accel.samples++;
-
-				break;
-			}
-
-		case 0x40:
-			// skip frame
-			frame_len = 2;
-			break;
-
-		case 0x44:
-			// sensortime frame
-			frame_len = 4;
-			break;
-
-		case 0x48:
-			// fifo config frame
-			frame_len = 2;
-			break;
-
-		case 0x50:
-			// sample drop frame
-			frame_len = 2;
-			break;
-		}
-
-		p += frame_len;
-		fifo_fill_level -= frame_len;
-	}
-
-	_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) {
-		//PX4_WARN("accel.samples: %d", accel.samples);
-		_px4_accel.updateFIFO(accel);
-		return true;
-	}
-
-	return true;
-}
-
 void BMI088_Accelerometer::FIFOReset()
 {
 	perf_count(_fifo_reset_perf);
@@ -590,17 +571,17 @@ void BMI088_Accelerometer::FIFOReset()
 void BMI088_Accelerometer::UpdateTemperature()
 {
 	// stored in an 11-bit value in 2’s complement format
-	uint8_t temperature_buf[4] {};
-	temperature_buf[0] = static_cast<uint8_t>(Register::TEMP_MSB) | ACC_I2C_ADDR_PRIMARY;
+	uint8_t temperature_buf[2] {};
+	uint8_t cmd = static_cast<uint8_t>(Register::TEMP_MSB);
 	// temperature_buf[1] dummy byte
 
-	if (transfer(&temperature_buf[0], 1, &temperature_buf[0], sizeof(temperature_buf)) != PX4_OK) {
+	if (transfer(&cmd, 1, &temperature_buf[0], sizeof(temperature_buf)) != PX4_OK) {
 		perf_count(_bad_transfer_perf);
 		return;
 	}
 
-	const uint8_t TEMP_MSB = temperature_buf[2];
-	const uint8_t TEMP_LSB = temperature_buf[3];
+	const uint8_t TEMP_MSB = temperature_buf[0];
+	const uint8_t TEMP_LSB = temperature_buf[1];
 
 	// Datasheet 5.3.7: Register 0x22 – 0x23: Temperature sensor data
 	uint16_t Temp_uint11 = (TEMP_MSB * 8) + (TEMP_LSB / 32);
@@ -616,308 +597,11 @@ void BMI088_Accelerometer::UpdateTemperature()
 	float temperature = (Temp_int11 * 0.125f) + 23.f; // Temp_int11 * 0.125°C/LSB + 23°C
 
 	if (PX4_ISFINITE(temperature)) {
-		_px4_accel.set_temperature(temperature);
+		_temperature = temperature;
 
 	} else {
 		perf_count(_bad_transfer_perf);
 	}
 }
 
-bool BMI088_Accelerometer::SelfTest()
-{
-	PX4_WARN("Running self-test with datasheet recomended steps(page 17)");
-	// Reset
-	PX4_WARN("Reseting the sensor");
-
-	if (RegisterWrite(Register::ACC_SOFTRESET, 0xB6) == PX4_OK) {
-		PX4_WARN("Reset success");
-	}
-
-	usleep(100000);
-	PX4_WARN("Accel on");
-
-	if (RegisterWrite(Register::ACC_PWR_CTRL, 0x04) == PX4_OK) {
-		PX4_WARN("Accel on success");
-	}
-
-	usleep(100000);
-	PX4_WARN("Sensor ErrReg: 0x%02x", CheckSensorErrReg());
-	Configure();
-	usleep(1000000);
-	PX4_WARN("Sensor ErrReg: 0x%02x", CheckSensorErrReg());
-	const uint8_t ACC_CHIP_ID = RegisterRead(Register::ACC_CHIP_ID);
-	PX4_WARN("ACC_CHIP_ID: 0x%02x", ACC_CHIP_ID);
-	usleep(30000);
-	PX4_WARN("Sensor ErrReg: 0x%02x", CheckSensorErrReg());
-
-	if (RegisterWrite(Register::ACC_PWR_CONF, 0) == PX4_OK) {
-		PX4_WARN("Start sensor success");
-		PX4_WARN("ACC_PWR_CONF(0): 0x%02x", RegisterRead(Register::ACC_PWR_CONF));
-	}
-
-	usleep(2000000);
-	PX4_WARN("Sensor ErrReg: 0x%02x", CheckSensorErrReg());
-
-	if (RegisterWrite(Register::ACC_RANGE, 0x03) == PX4_OK) {
-		PX4_WARN("Range set success");
-		PX4_WARN("ACC_RANGE(0x03): 0x%02x", RegisterRead(Register::ACC_RANGE));
-	}
-
-	usleep(100000);
-	PX4_WARN("Sensor ErrReg: 0x%02x", CheckSensorErrReg());
-
-	if (RegisterWrite(Register::ACC_CONF, 0xA7) == PX4_OK) {
-		PX4_WARN("Conf set success");
-		PX4_WARN("ACC_CONF(0xA7): 0x%02x", RegisterRead(Register::ACC_CONF));
-	}
-
-	usleep(100000);
-	PX4_WARN("Sensor ErrReg: 0x%02x", CheckSensorErrReg());
-
-	// Positive sel-test polarity
-	if (RegisterWrite(Register::ACC_SELF_TEST, 0x0D) == PX4_OK) {
-		PX4_WARN("Self-test positive mode set success");
-		PX4_WARN("ACC_SELF_TEST(0x0D): 0x%02x", RegisterRead(Register::ACC_SELF_TEST));
-	}
-
-	usleep(100000);
-	PX4_WARN("Sensor ErrReg: 0x%02x", CheckSensorErrReg());
-
-	float *accel_mss = ReadAccelDataFIFO();
-	PX4_WARN("Positive value");
-	PX4_WARN("X %f", (double)accel_mss[0]);
-	PX4_WARN("Y %f", (double)accel_mss[1]);
-	PX4_WARN("Z %f", (double)accel_mss[2]);
-
-	// Negative sel-test polarity
-	if (RegisterWrite(Register::ACC_SELF_TEST, 0x09) == PX4_OK) {
-		PX4_WARN("Self-test negative mode set success");
-		PX4_WARN("ACC_SELF_TEST(0x09): 0x%02x", RegisterRead(Register::ACC_SELF_TEST));
-	}
-
-	usleep(600000);
-	PX4_WARN("Sensor ErrReg: 0x%02x", CheckSensorErrReg());
-	float *accel_mss2 = ReadAccelDataFIFO();
-	PX4_WARN("Negative value");
-	PX4_WARN("X %f", (double)accel_mss2[0]);
-	PX4_WARN("Y %f", (double)accel_mss2[1]);
-	PX4_WARN("Z %f", (double)accel_mss2[2]);
-
-	// Calculate difference between positive and negative sef-test response
-	float diff_x = accel_mss[0] - accel_mss2[0];
-	float diff_y = accel_mss[1] - accel_mss2[1];
-	float diff_z = accel_mss[2] - accel_mss2[2];
-
-	PX4_WARN("Diff value");
-	PX4_WARN("diff_x %f", (double)diff_x);
-	PX4_WARN("diff_y %f", (double)diff_y);
-	PX4_WARN("diff_z %f", (double)diff_z);
-
-
-	if (diff_x >= 1000) {
-		PX4_WARN("X Axis self-test success");
-	}
-
-	if (diff_y >= 1000) {
-		PX4_WARN("Y Axis self-test success");
-	}
-
-	if (diff_z >= 500) {
-		PX4_WARN("Z Axis self-test success");
-	}
-
-
-	// Disable self-test
-	RegisterWrite(Register::ACC_SELF_TEST, 0x00);
-	usleep(60000);
-
-	PX4_WARN("Sensor ErrReg: 0x%02x", CheckSensorErrReg());
-	// Reset
-	//PX4_WARN("Reseting the sensor again");
-	//RegisterWrite(Register::ACC_SOFTRESET, 0xB6);
-	//usleep(100000);
-	return true;
-}
-
-float *BMI088_Accelerometer::ReadAccelData()
-{
-	uint8_t cmd[1] = {0x12};
-
-	uint8_t buf[6] = {0, 0, 0, 0, 0, 0};
-	uint8_t *buffer = buf;
-
-	int16_t accel[3];
-
-	if (transfer(&cmd[0], 1, buffer, sizeof(buf)) == PX4_OK) {
-		PX4_WARN("ReadAccelData transfer success");
-	}
-
-	for (uint8_t i = 0; i < sizeof(buf); i++) {
-		PX4_WARN("buf[%d]: %f", i, (double)buf[i]);
-	}
-
-	accel[0] = (buf[1] << 8) | buf[0];
-	accel[1] = (buf[3] << 8) | buf[2];
-	accel[2] = (buf[5] << 8) | buf[4];
-
-	float *accel_mss = new float[3];
-
-	accel_mss[0] = (float) accel[0] / 32768.0f * 1000.0f * powf(2.0f, 24.0f + 1.0f) * 1.50f;
-	accel_mss[1] = (float) accel[1] / 32768.0f * 1000.0f * powf(2.0f, 24.0f + 1.0f) * 1.50f;
-	accel_mss[2] = (float) accel[2] / 32768.0f * 1000.0f * powf(2.0f, 24.0f + 1.0f) * 1.50f;
-
-	return accel_mss;
-}
-
-float *BMI088_Accelerometer::ReadAccelDataFIFO()
-{
-	float *accel_mg = new float[3];
-	struct FIFO::bmi08x_sensor_data bmi08x_accel;
-	uint8_t buffer[50] = {0};
-
-	PX4_WARN("FIFO mode is stop-at-full");
-	/* Desired FIFO mode is stop-at-full: set bit #0 to 1 in 0x48. Bit #1 must always be one! */
-	buffer[0] = 0x00 | 0x02;
-	RegisterWrite(Register::FIFO_CONFIG_0, buffer[0]);
-	PX4_WARN("FIFO_CONFIG_0(0x%02x): 0x%02x", buffer[0], RegisterRead(Register::FIFO_CONFIG_0));
-
-	PX4_WARN("Downsampling factor 2**4 = 16");
-	/* Downsampling factor 2**4 = 16: write 4 into bit #4-6 of reg. 0x45. Bit #7 must always be one! */
-	buffer[0] = 0x10 | 0x80;
-	RegisterWrite(Register::FIFO_DOWN_SAMPLING, buffer[0]);
-	PX4_WARN("FIFO_DOWN_SAMPLING(0x%02x): 0x%02x", buffer[0], RegisterRead(Register::FIFO_DOWN_SAMPLING));
-
-	/* Set water mark to 42 bytes (aka 6 frames, each 7 bytes: 1 byte header + 6 bytes accel data) */
-	// uint16_t wml = 42;
-	// buffer[0] = (uint8_t) wml & 0xff;
-	// buffer[1] = (uint8_t) (wml >> 8) & 0xff;
-	// uint8_t add = static_cast<uint8_t>(Register::FIFO_WTM_0);
-	// uint8_t cmd[3] = { add, buffer[0], buffer[1]};
-	// transfer(cmd, sizeof(cmd), nullptr, 0);
-	// PX4_WARN("FIFO_WTM_0(0x%02x): 0x%02x",cmd[0], RegisterRead(Register::FIFO_WTM_0));
-
-	/* Enable the actual FIFO functionality: write 0x50 to 0x49. Bit #4 must always be one! */
-	buffer[0] = 0x10 | 0x40;
-	RegisterWrite(Register::FIFO_CONFIG_1, buffer[0]);
-	PX4_WARN("FIFO_CONFIG_1(0x%02x): 0x%02x", buffer[0], RegisterRead(Register::FIFO_CONFIG_1));
-	usleep(1000000);
-
-	int fifo_fill_level = 0;
-
-	uint8_t data_o[2] = { 0, 0 };
-	uint8_t data_i[1] = {static_cast<uint8_t>(Register::FIFO_LENGTH_0)};
-	data_i[0] = static_cast<uint8_t>(Register::FIFO_LENGTH_0);
-
-	transfer(&data_i[0], 1, &data_o[0], 2);
-	fifo_fill_level = data_o[0] + 256 * data_o[1];
-	PX4_WARN("fifo_fill_level %d", fifo_fill_level);
-
-	// while(fifo_fill_level < wml)
-	// {
-	// 	transfer(&data_i[0], 1, &data_o[0], 2);
-	// 	fifo_fill_level = data_o[0] + 256 * data_o[1];
-	// 	PX4_WARN("fifo_fill_level %d", fifo_fill_level);
-	// }
-
-	uint8_t custom_size = 42;
-	uint8_t buffer_data[custom_size] = {0};
-	buffer[0] = static_cast<uint8_t>(Register::FIFO_DATA);
-	bmi08x_accel.x = 10;
-	PX4_WARN("bmi08x_accel %d", bmi08x_accel.x);
-	transfer(&buffer[0], 1, &buffer_data[0], custom_size);
-
-	/* This is a super-simple FIFO parsing loop, hoping it will only find valid accel data packets */
-	for (int i = 1; i < custom_size;) {
-		/* Header of acceleration sensor data frame: 100001xxb, where x is INT1/INT2 tag, ignored here */
-		if (buffer_data[i] == (0x84 & 0x8c)) {
-			UnpackSensorData(&bmi08x_accel, &buffer_data[i + 1]);
-			PX4_WARN("Frame: %03d ax:%f ay:%f az:%f", i / 6, (double)bmi08x_accel.x, (double)bmi08x_accel.y,
-				 (double)bmi08x_accel.z);
-			accel_mg[0] = bmi08x_accel.x;
-			accel_mg[1] = bmi08x_accel.y;
-			accel_mg[2] = bmi08x_accel.z;
-			float *data_in_mg = SensorDataTomg(accel_mg);
-			PX4_WARN("Frame mg: %03d ax:%f ay:%f az:%f", i / 6, (double)data_in_mg[0], (double)data_in_mg[1],
-				 (double)data_in_mg[2]);
-			i += 7;
-
-		} else {
-			i++;
-		}
-	}
-
-	return accel_mg;
-}
-
-uint8_t  BMI088_Accelerometer::CheckSensorErrReg()
-{
-	return RegisterRead(Register::ACC_ERR_REG);
-}
-
-void BMI088_Accelerometer::UnpackSensorData(struct FIFO::bmi08x_sensor_data *sens_data, uint8_t *buffer)
-{
-	uint16_t data_lsb;
-	uint16_t data_msb;
-	uint16_t start_idx = 0;
-	/* Gyro raw x data */
-	data_lsb = buffer[start_idx++];
-	data_msb = buffer[start_idx++];
-	sens_data->x = (int16_t)((data_msb << 8) | data_lsb);
-	/* Gyro raw y data */
-	data_lsb = buffer[start_idx++];
-	data_msb = buffer[start_idx++];
-	sens_data->y = (int16_t)((data_msb << 8) | data_lsb);
-	/* Gyro raw z data */
-	data_lsb = buffer[start_idx++];
-	data_msb = buffer[start_idx++];
-	sens_data->z = (int16_t)((data_msb << 8) | data_lsb);
-}
-
-float *BMI088_Accelerometer::SensorDataTomg(float *data)
-{
-	data[0] = (float) data[0] / 32768.0f * 1000.0f * powf(2.0f, 24.0f + 1.0f) * 1.50f;
-	data[1] = (float) data[1] / 32768.0f * 1000.0f * powf(2.0f, 24.0f + 1.0f) * 1.50f;
-	data[2] = (float) data[2] / 32768.0f * 1000.0f * powf(2.0f, 24.0f + 1.0f) * 1.50f;
-	return data;
-}
-
-bool BMI088_Accelerometer::NormalRead(const hrt_abstime &timestamp_sample)
-{
-	const int16_t tX[3] = {1, 0, 0};
-	const int16_t tY[3] = {0, -1, 0};
-	const int16_t tZ[3] = {0, 0, -1};
-
-	float x = 0;
-	float y = 0;
-	float z = 0;
-	uint8_t buffer[6] = {0};
-	uint8_t cmd[1] = {static_cast<uint8_t>(Register::ACC_READ)};
-
-	transfer(&cmd[0], 1, &buffer[0], 6);
-
-	uint8_t RATE_X_LSB = buffer[0];
-	uint8_t RATE_X_MSB = buffer[1];
-
-	uint8_t RATE_Y_LSB = buffer[2];
-	uint8_t RATE_Y_MSB = buffer[3];
-
-	uint8_t RATE_Z_LSB = buffer[4];
-	uint8_t RATE_Z_MSB = buffer[5];
-
-	const int16_t accel_x = combine(RATE_X_MSB, RATE_X_LSB);
-	const int16_t accel_y = combine(RATE_Y_MSB, RATE_Y_LSB);
-	const int16_t accel_z = combine(RATE_Z_MSB, RATE_Z_LSB);
-
-	// 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)
-	x = accel_x * tX[0] + accel_y * tX[1] + accel_z * tX[2];
-	y = accel_x * tY[0] + accel_y * tY[1] + accel_z * tY[2];
-	z = accel_x * tZ[0] + accel_y * tZ[1] + accel_z * tZ[2];
-
-	//PX4_WARN("x: %f | y: %f | z: %f", (double)x, (double)y ,(double)z);
-	_px4_accel.update(timestamp_sample, x, y, z);
-
-	return true;
-}
-
 } // namespace Bosch::BMI088::Accelerometer
diff --git a/src/drivers/imu/bosch/bmi088_i2c/BMI088_Accelerometer.hpp b/src/drivers/imu/bosch/bmi088_i2c/BMI088_Accelerometer.hpp
index 189ffe9a75..7987a0df32 100644
--- a/src/drivers/imu/bosch/bmi088_i2c/BMI088_Accelerometer.hpp
+++ b/src/drivers/imu/bosch/bmi088_i2c/BMI088_Accelerometer.hpp
@@ -1,6 +1,6 @@
 /****************************************************************************
  *
- *   Copyright (c) 2020-2021 PX4 Development Team. All rights reserved.
+ *   Copyright (c) 2020-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
@@ -35,10 +35,11 @@
 
 #include "BMI088.hpp"
 
-#include <lib/drivers/accelerometer/PX4Accelerometer.hpp>
-
 #include "Bosch_BMI088_Accelerometer_Registers.hpp"
 
+#include <uORB/PublicationMulti.hpp>
+#include <uORB/topics/sensor_accel.h>
+
 namespace Bosch::BMI088::Accelerometer
 {
 
@@ -55,11 +56,10 @@ private:
 	void exit_and_cleanup() override;
 
 	// Sensor Configuration
-	// static constexpr uint32_t RATE{1600}; // 1600 Hz
 	static constexpr uint32_t RATE{1600}; // 1600 Hz
 	static constexpr float FIFO_SAMPLE_DT{1e6f / RATE};
 
-	static constexpr int32_t FIFO_MAX_SAMPLES{math::min(FIFO::SIZE / sizeof(FIFO::DATA), sizeof(sensor_accel_fifo_s::x) / sizeof(sensor_accel_fifo_s::x[0]))};
+	static constexpr int32_t FIFO_MAX_SAMPLES{FIFO::SIZE / sizeof(FIFO::DATA)};
 
 	// Transfer data
 	struct FIFOTransferBuffer {
@@ -69,10 +69,6 @@ private:
 		uint8_t FIFO_LENGTH_1{0};
 		FIFO::DATA f[FIFO_MAX_SAMPLES] {};
 	};
-	// Transfer data without length
-	struct FIFOTransferBufferWithoutLength {
-		FIFO::DATA f[FIFO_MAX_SAMPLES] {};
-	};
 	// ensure no struct padding
 	static_assert(sizeof(FIFOTransferBuffer) == (4 + FIFO_MAX_SAMPLES *sizeof(FIFO::DATA)));
 
@@ -97,7 +93,7 @@ private:
 	bool RegisterCheck(const register_config_t &reg_cfg);
 
 	uint8_t RegisterRead(Register reg);
-	uint8_t RegisterWrite(Register reg, uint8_t value);
+	void RegisterWrite(Register reg, uint8_t value);
 	void RegisterSetAndClearBits(Register reg, uint8_t setbits, uint8_t clearbits);
 
 	uint16_t FIFOReadCount();
@@ -105,16 +101,14 @@ private:
 	void FIFOReset();
 
 	void UpdateTemperature();
-	void UnpackSensorData(struct FIFO::bmi08x_sensor_data *sens_data, uint8_t *buffer);
-	bool SelfTest();
-	float *ReadAccelData();
-	float *ReadAccelDataFIFO();
-	float *SensorDataTomg(float *data);
-	uint8_t CheckSensorErrReg();
-	bool SimpleFIFORead(const hrt_abstime &timestamp_sample);
-	bool NormalRead(const hrt_abstime &timestamp_sample);
 
-	PX4Accelerometer _px4_accel;
+	uORB::PublicationMulti<sensor_accel_s> _sensor_accel_pub{ORB_ID(sensor_accel)};
+
+	const enum Rotation _rotation;
+	float _accel_scale{0.f};
+	float _accel_range{0.f};
+
+	float _temperature{NAN};
 
 	perf_counter_t _bad_register_perf{perf_alloc(PC_COUNT, MODULE_NAME"_accel: bad register")};
 	perf_counter_t _bad_transfer_perf{perf_alloc(PC_COUNT, MODULE_NAME"_accel: bad transfer")};
@@ -129,7 +123,7 @@ private:
 	static constexpr uint8_t size_register_cfg{10};
 	register_config_t _register_cfg[size_register_cfg] {
 		// Register                        | Set bits, Clear bits
-		{ Register::ACC_PWR_CONF,          0, ACC_PWR_CONF_BIT::acc_pwr_save }, //
+		{ Register::ACC_PWR_CONF,          0, ACC_PWR_CONF_BIT::acc_pwr_save },
 		{ Register::ACC_PWR_CTRL,          ACC_PWR_CTRL_BIT::acc_enable, 0 },
 		{ Register::ACC_CONF,              ACC_CONF_BIT::acc_bwp_Normal | ACC_CONF_BIT::acc_odr_1600, Bit1 | Bit0 },
 		{ Register::ACC_RANGE,             ACC_RANGE_BIT::acc_range_24g, 0 },
diff --git a/src/drivers/imu/bosch/bmi088_i2c/BMI088_Gyroscope.cpp b/src/drivers/imu/bosch/bmi088_i2c/BMI088_Gyroscope.cpp
index ad177c68e5..1dd331db8d 100644
--- a/src/drivers/imu/bosch/bmi088_i2c/BMI088_Gyroscope.cpp
+++ b/src/drivers/imu/bosch/bmi088_i2c/BMI088_Gyroscope.cpp
@@ -1,6 +1,6 @@
 /****************************************************************************
  *
- *   Copyright (c) 2020-2021 PX4 Development Team. All rights reserved.
+ *   Copyright (c) 2020-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
@@ -33,8 +33,6 @@
 
 #include "BMI088_Gyroscope.hpp"
 
-#include <px4_platform/board_dma_alloc.h>
-
 using namespace time_literals;
 
 namespace Bosch::BMI088::Gyroscope
@@ -42,13 +40,13 @@ namespace Bosch::BMI088::Gyroscope
 
 BMI088_Gyroscope::BMI088_Gyroscope(const I2CSPIDriverConfig &config) :
 	BMI088(config),
-	_px4_gyro(get_device_id(), config.rotation)
+	_rotation(config.rotation)
 {
 	if (config.drdy_gpio != 0) {
 		_drdy_missed_perf = perf_alloc(PC_COUNT, MODULE_NAME"_gyro: DRDY missed");
 	}
 
-	ConfigureSampleRate(2000);
+	ConfigureSampleRate(RATE);
 }
 
 BMI088_Gyroscope::~BMI088_Gyroscope()
@@ -98,13 +96,6 @@ void BMI088_Gyroscope::RunImpl()
 	const hrt_abstime now = hrt_absolute_time();
 
 	switch (_state) {
-
-	case STATE::SELFTEST:
-		//SelfTest();
-		_state = STATE::RESET;
-		ScheduleDelayed(1_ms);
-		break;
-
 	case STATE::RESET:
 		// GYRO_SOFTRESET: Writing a value of 0xB6 to this register resets the sensor.
 		// Following a delay of 30 ms, all configuration settings are overwritten with their reset value.
@@ -170,8 +161,90 @@ void BMI088_Gyroscope::RunImpl()
 		break;
 
 	case STATE::FIFO_READ: {
-			SimpleFIFORead(now);
+			hrt_abstime timestamp_sample = 0;
+
+			if (_data_ready_interrupt_enabled) {
+				// scheduled from interrupt if _drdy_timestamp_sample was set as expected
+				const hrt_abstime drdy_timestamp_sample = _drdy_timestamp_sample.fetch_and(0);
+
+				if ((now - drdy_timestamp_sample) < _fifo_empty_interval_us) {
+					timestamp_sample = drdy_timestamp_sample;
+
+				} else {
+					perf_count(_drdy_missed_perf);
+				}
+
+				// push backup schedule back
+				ScheduleDelayed(_fifo_empty_interval_us * 2);
+			}
+
+			// always check current FIFO status/count
+			bool success = false;
+			const uint8_t FIFO_STATUS = RegisterRead(Register::FIFO_STATUS);
+
+			if (FIFO_STATUS & FIFO_STATUS_BIT::Fifo_overrun) {
+				FIFOReset();
+				perf_count(_fifo_overflow_perf);
+
+			} else {
+				const uint8_t fifo_frame_counter = FIFO_STATUS & FIFO_STATUS_BIT::Fifo_frame_counter;
+
+				if (fifo_frame_counter > FIFO_MAX_SAMPLES) {
+					// not technically an overflow, but more samples than we expected or can publish
+					FIFOReset();
+					perf_count(_fifo_overflow_perf);
+
+				} else if (fifo_frame_counter == 0) {
+					perf_count(_fifo_empty_perf);
+
+				} else if (fifo_frame_counter >= 1) {
+
+					uint8_t samples = fifo_frame_counter;
+
+					// tolerate minor jitter, leave sample to next iteration if behind by only 1
+					if (samples == _fifo_samples + 1) {
+						// sample timestamp set from data ready already corresponds to _fifo_samples
+						if (timestamp_sample == 0) {
+							timestamp_sample = now - static_cast<int>(FIFO_SAMPLE_DT);
+						}
+
+						samples--;
+					}
+
+					if (FIFORead((timestamp_sample == 0) ? now : timestamp_sample, 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;
+				}
+			}
+
+			if (!success || hrt_elapsed_time(&_last_config_check_timestamp) > 100_ms) {
+				// check configuration registers periodically or immediately following any failure
+				if (RegisterCheck(_register_cfg[_checked_register])) {
+					_last_config_check_timestamp = now;
+					_checked_register = (_checked_register + 1) % size_register_cfg;
+
+				} else {
+					// register check failed, force reset
+					perf_count(_bad_register_perf);
+					Reset();
+				}
+			}
 		}
+
 		break;
 	}
 }
@@ -182,28 +255,28 @@ void BMI088_Gyroscope::ConfigureGyro()
 
 	switch (GYRO_RANGE) {
 	case gyro_range_2000_dps:
-		_px4_gyro.set_scale(math::radians(1.f / 16.384f));
-		_px4_gyro.set_range(math::radians(2000.f));
+		_gyro_scale = math::radians(1.f / 16.384f);
+		_gyro_range = math::radians(2000.f);
 		break;
 
 	case gyro_range_1000_dps:
-		_px4_gyro.set_scale(math::radians(1.f / 32.768f));
-		_px4_gyro.set_range(math::radians(1000.f));
+		_gyro_scale = math::radians(1.f / 32.768f);
+		_gyro_range = math::radians(1000.f);
 		break;
 
 	case gyro_range_500_dps:
-		_px4_gyro.set_scale(math::radians(1.f / 65.536f));
-		_px4_gyro.set_range(math::radians(500.f));
+		_gyro_scale = math::radians(1.f / 65.536f);
+		_gyro_range = math::radians(500.f);
 		break;
 
 	case gyro_range_250_dps:
-		_px4_gyro.set_scale(math::radians(1.f / 131.072f));
-		_px4_gyro.set_range(math::radians(250.f));
+		_gyro_scale = math::radians(1.f / 131.072f);
+		_gyro_range = math::radians(250.f);
 		break;
 
 	case gyro_range_125_dps:
-		_px4_gyro.set_scale(math::radians(1.f / 262.144f));
-		_px4_gyro.set_range(math::radians(125.f));
+		_gyro_scale = math::radians(1.f / 262.144f);
+		_gyro_range = math::radians(125.f);
 		break;
 	}
 }
@@ -306,16 +379,15 @@ bool BMI088_Gyroscope::RegisterCheck(const register_config_t &reg_cfg)
 
 uint8_t BMI088_Gyroscope::RegisterRead(Register reg)
 {
-	uint8_t add = static_cast<uint8_t>(reg);
-	uint8_t cmd[2] = {add, 0};
-	transfer(&cmd[0], 1, &cmd[1], 1);
-	return cmd[1];
+	uint8_t cmd = static_cast<uint8_t>(reg);
+	uint8_t value = 0;
+	transfer(&cmd, 1, &value, 1);
+	return value;
 }
 
 void BMI088_Gyroscope::RegisterWrite(Register reg, uint8_t value)
 {
-	uint8_t add = static_cast<uint8_t>(reg);
-	uint8_t cmd[2] = {add, value};
+	uint8_t cmd[2] { (uint8_t)reg, value };
 	transfer(cmd, sizeof(cmd), nullptr, 0);
 }
 
@@ -333,37 +405,50 @@ void BMI088_Gyroscope::RegisterSetAndClearBits(Register reg, uint8_t setbits, ui
 bool BMI088_Gyroscope::FIFORead(const hrt_abstime &timestamp_sample, uint8_t samples)
 {
 	FIFOTransferBuffer buffer{};
-	const size_t transfer_size = math::min(samples * sizeof(FIFO::DATA) + 1, FIFO::SIZE);
+	const size_t transfer_size = math::min(samples * sizeof(FIFO::DATA), FIFO::SIZE);
 
-	//PX4_WARN("Estimated transfer size: %d", transfer_size);
-	if (transfer((uint8_t *)&buffer, 1, (uint8_t *)&buffer, transfer_size) != PX4_OK) {
+	if (transfer((uint8_t *)&buffer.cmd, 1, (uint8_t *)&buffer.f, transfer_size) != PX4_OK) {
 		perf_count(_bad_transfer_perf);
 		return false;
 	}
 
-	sensor_gyro_fifo_s gyro{};
-	gyro.timestamp_sample = timestamp_sample;
-	gyro.samples = samples;
-	gyro.dt = FIFO_SAMPLE_DT;
+	sensor_gyro_s sensor_gyro{};
+	sensor_gyro.timestamp_sample = timestamp_sample;
+	sensor_gyro.device_id = get_device_id();
+
+	float gyro_sum[3] {};
 
 	for (int i = 0; i < samples; i++) {
 		const FIFO::DATA &fifo_sample = buffer.f[i];
 
-		const int16_t gyro_x = combine(fifo_sample.RATE_X_MSB, fifo_sample.RATE_X_LSB);
-		const int16_t gyro_y = combine(fifo_sample.RATE_Y_MSB, fifo_sample.RATE_Y_LSB);
-		const int16_t gyro_z = combine(fifo_sample.RATE_Z_MSB, fifo_sample.RATE_Z_LSB);
+		int16_t gyro_x = combine(fifo_sample.RATE_X_MSB, fifo_sample.RATE_X_LSB);
+		int16_t gyro_y = combine(fifo_sample.RATE_Y_MSB, fifo_sample.RATE_Y_LSB);
+		int16_t gyro_z = combine(fifo_sample.RATE_Z_MSB, fifo_sample.RATE_Z_LSB);
 
 		// 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;
-		gyro.y[i] = (gyro_y == INT16_MIN) ? INT16_MAX : -gyro_y;
-		gyro.z[i] = (gyro_z == INT16_MIN) ? INT16_MAX : -gyro_z;
+		//gyro.x[i] = gyro_x;
+		gyro_y = math::negate(gyro_y);
+		gyro_z = math::negate(gyro_z);
+
+		rotate_3i(_rotation, gyro_x, gyro_y, gyro_z);
+
+		gyro_sum[0] += gyro_x * _gyro_scale;
+		gyro_sum[1] += gyro_y * _gyro_scale;
+		gyro_sum[2] += gyro_z * _gyro_scale;
 	}
 
-	_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));
+	sensor_gyro.x = gyro_sum[0] / samples;
+	sensor_gyro.y = gyro_sum[1] / samples;
+	sensor_gyro.z = gyro_sum[2] / samples;
 
-	_px4_gyro.updateFIFO(gyro);
+	sensor_gyro.range = _gyro_range;
+	sensor_gyro.temperature = NAN;
+	sensor_gyro.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);
+
+	sensor_gyro.timestamp = hrt_absolute_time();
+	_sensor_gyro_pub.publish(sensor_gyro);
 
 	return true;
 }
@@ -390,125 +475,4 @@ void BMI088_Gyroscope::FIFOReset()
 	}
 }
 
-bool BMI088_Gyroscope::SelfTest()
-{
-	//Datasheet page 17 self test
-
-	//Set bit0 to enable built in self test
-	RegisterWrite(Register::SELF_TEST, 0x01);
-	usleep(10000);
-	uint8_t res = 0;
-	uint8_t test_res = false;
-
-	while (true) {
-		res = RegisterRead(Register::SELF_TEST);
-
-		if ((res & 0x02) == 0x02) {
-			if ((res & 0x04) == 0x00) {
-				PX4_WARN("Gyro Self-test success");
-				test_res = true;
-
-			} else {
-				PX4_WARN("Gyro Self-test error");
-			}
-
-			break;
-		}
-	}
-
-	RegisterWrite(Register::SELF_TEST, 0x00);
-	return test_res;
-}
-
-bool BMI088_Gyroscope::NormalRead(const hrt_abstime &timestamp_sample)
-{
-	float x = 0;
-	float y = 0;
-	float z = 0;
-	uint8_t buffer[6] = {0};
-	uint8_t cmd[1] = {static_cast<uint8_t>(Register::READ_GYRO)};
-
-	transfer(&cmd[0], 1, &buffer[0], 6);
-
-	uint8_t RATE_X_LSB = buffer[0];
-	uint8_t RATE_X_MSB = buffer[1];
-	uint8_t RATE_Y_LSB = buffer[2];
-	uint8_t RATE_Y_MSB = buffer[3];
-	uint8_t RATE_Z_LSB = buffer[4];
-	uint8_t RATE_Z_MSB = buffer[5];
-
-	const int16_t gyro_x = combine(RATE_X_MSB, RATE_X_LSB);
-	const int16_t gyro_y = combine(RATE_Y_MSB, RATE_Y_LSB);
-	const int16_t gyro_z = combine(RATE_Z_MSB, RATE_Z_LSB);
-
-	// 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)
-	x = gyro_x;
-	y = (gyro_y == INT16_MIN) ? INT16_MAX : -gyro_y;
-	z = (gyro_z == INT16_MIN) ? INT16_MAX : -gyro_z;
-
-	_px4_gyro.update(timestamp_sample, x, y, z);
-
-	return true;
-}
-
-bool BMI088_Gyroscope::SimpleFIFORead(const hrt_abstime &timestamp_sample)
-{
-	uint8_t n_frames;
-	sensor_gyro_fifo_s gyro{};
-	gyro.timestamp_sample = timestamp_sample;
-	gyro.samples = 0;
-	gyro.dt = FIFO_SAMPLE_DT;
-
-	uint8_t data_i[1] = {static_cast<uint8_t>(Register::FIFO_STATUS)};
-
-	transfer(&data_i[0], 1, &n_frames, 1);
-
-	n_frames &= 0x7F;
-
-	int n_frames_to_read = 6;
-
-	// don't read more than 8 frames at a time
-	if (n_frames > n_frames_to_read) {
-		n_frames = n_frames_to_read;
-	}
-
-	if (n_frames == 0) {
-		return false;
-	}
-
-	uint8_t data[6 * n_frames];
-	data[0] = static_cast<uint8_t>(Register::FIFO_DATA);
-
-	if (transfer(&data[0], 1, &data[0], 6 * n_frames) != PX4_OK) {
-		//PX4_WARN("transfer(&data[0], 1, &data[0], fifo_fill_level) != PX4_OK");
-		return false;
-	}
-
-	for (uint8_t i = 0; i < n_frames; i++) {
-		const uint8_t *d = &data[i * 6];
-		int16_t xyz[3] {
-			int16_t(uint16_t(d[0] | d[1] << 8)),
-			int16_t(uint16_t(d[2] | d[3] << 8)),
-			int16_t(uint16_t(d[4] | d[5] << 8))
-		};
-
-		gyro.x[i] = xyz[0];
-		gyro.y[i] = (xyz[1] == INT16_MIN) ? INT16_MAX : -xyz[1];
-		gyro.z[i] = (xyz[2] == INT16_MIN) ? INT16_MAX : -xyz[2];
-		gyro.samples++;
-	}
-
-	_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) {
-		//PX4_WARN("accel.samples: %d", accel.samples);
-		_px4_gyro.updateFIFO(gyro);
-		return true;
-	}
-
-	return true;
-
-}
 } // namespace Bosch::BMI088::Gyroscope
diff --git a/src/drivers/imu/bosch/bmi088_i2c/BMI088_Gyroscope.hpp b/src/drivers/imu/bosch/bmi088_i2c/BMI088_Gyroscope.hpp
index 33d95b3b14..879caec6df 100644
--- a/src/drivers/imu/bosch/bmi088_i2c/BMI088_Gyroscope.hpp
+++ b/src/drivers/imu/bosch/bmi088_i2c/BMI088_Gyroscope.hpp
@@ -1,6 +1,6 @@
 /****************************************************************************
  *
- *   Copyright (c) 2020-2021 PX4 Development Team. All rights reserved.
+ *   Copyright (c) 2020-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
@@ -35,10 +35,11 @@
 
 #include "BMI088.hpp"
 
-#include <lib/drivers/gyroscope/PX4Gyroscope.hpp>
-
 #include "Bosch_BMI088_Gyroscope_Registers.hpp"
 
+#include <uORB/PublicationMulti.hpp>
+#include <uORB/topics/sensor_gyro.h>
+
 namespace Bosch::BMI088::Gyroscope
 {
 
@@ -55,10 +56,10 @@ private:
 	void exit_and_cleanup() override;
 
 	// Sensor Configuration
-	static constexpr uint32_t RATE{400}; // 2000 Hz
+	static constexpr uint32_t RATE{2000}; // 2000 Hz
 	static constexpr float FIFO_SAMPLE_DT{1e6f / RATE};
 
-	static constexpr int32_t FIFO_MAX_SAMPLES{math::min(FIFO::SIZE / sizeof(FIFO::DATA), sizeof(sensor_gyro_fifo_s::x) / sizeof(sensor_gyro_fifo_s::x[0]))};
+	static constexpr int32_t FIFO_MAX_SAMPLES{FIFO::SIZE / sizeof(FIFO::DATA)};
 
 	// Transfer data
 	struct FIFOTransferBuffer {
@@ -95,10 +96,11 @@ private:
 	bool FIFORead(const hrt_abstime &timestamp_sample, uint8_t samples);
 	void FIFOReset();
 
-	bool SelfTest();
-	bool NormalRead(const hrt_abstime &timestamp_sample);
-	bool SimpleFIFORead(const hrt_abstime &timestamp_sample);
-	PX4Gyroscope _px4_gyro;
+	uORB::PublicationMulti<sensor_gyro_s> _sensor_gyro_pub{ORB_ID(sensor_gyro)};
+
+	const enum Rotation _rotation;
+	float _gyro_scale{0.f};
+	float _gyro_range{0.f};
 
 	perf_counter_t _bad_register_perf{perf_alloc(PC_COUNT, MODULE_NAME"_gyro: bad register")};
 	perf_counter_t _bad_transfer_perf{perf_alloc(PC_COUNT, MODULE_NAME"_gyro: bad transfer")};
@@ -114,13 +116,13 @@ private:
 	register_config_t _register_cfg[size_register_cfg] {
 		// Register                         | Set bits, Clear bits
 		{ Register::GYRO_RANGE,             GYRO_RANGE_BIT::gyro_range_2000_dps, 0 },
-		{ Register::GYRO_BANDWIDTH,         0, GYRO_BANDWIDTH_BIT::gyro_bw_2000_Hz},
+		{ Register::GYRO_BANDWIDTH,         0, GYRO_BANDWIDTH_BIT::gyro_bw_532_Hz },
 		{ Register::GYRO_INT_CTRL,          GYRO_INT_CTRL_BIT::fifo_en, 0 },
 		{ Register::INT3_INT4_IO_CONF,      0, INT3_INT4_IO_CONF_BIT::Int3_od | INT3_INT4_IO_CONF_BIT::Int3_lvl },
 		{ Register::INT3_INT4_IO_MAP,       INT3_INT4_IO_MAP_BIT::Int3_fifo, 0 },
 		{ Register::FIFO_WM_ENABLE,         FIFO_WM_ENABLE_BIT::fifo_wm_enable, 0 },
 		{ Register::FIFO_CONFIG_0,          0, 0 }, // fifo_water_mark_level_trigger_retain<6:0>
-		{ Register::FIFO_CONFIG_1,          FIFO_CONFIG_1_BIT::FIFO_MODE_STREAM, 0 },
+		{ Register::FIFO_CONFIG_1,          FIFO_CONFIG_1_BIT::FIFO_MODE, 0 },
 	};
 };
 
diff --git a/src/drivers/imu/bosch/bmi088_i2c/Bosch_BMI088_Accelerometer_Registers.hpp b/src/drivers/imu/bosch/bmi088_i2c/Bosch_BMI088_Accelerometer_Registers.hpp
index 02a9ea56c3..7652f5ba0d 100644
--- a/src/drivers/imu/bosch/bmi088_i2c/Bosch_BMI088_Accelerometer_Registers.hpp
+++ b/src/drivers/imu/bosch/bmi088_i2c/Bosch_BMI088_Accelerometer_Registers.hpp
@@ -1,6 +1,6 @@
 /****************************************************************************
  *
- *   Copyright (c) 2019 PX4 Development Team. All rights reserved.
+ *   Copyright (c) 2020-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
@@ -48,15 +48,14 @@ static constexpr uint8_t Bit7 = (1 << 7);
 
 static constexpr uint32_t SPI_SPEED = 10 * 1000 * 1000; // 10MHz SPI serial interface
 
-static constexpr uint32_t I2C_400_SPEED = 400 * 1000; // 400kHz I2C interface
-static constexpr uint32_t I2C_200_SPEED = 200 * 1000; // 200kHz I2C interface
-static constexpr uint32_t I2C_100_SPEED = 100 * 1000; // 100kHz I2C interface
+static constexpr uint32_t I2C_SPEED = 400 * 1000; // 400kHz I2C interface
 
 static constexpr uint8_t DIR_READ = 0x80;
 
-static constexpr uint8_t ID = 0x1E;
+static constexpr uint8_t ID_088 = 0x1E;
+static constexpr uint8_t ID_090L = 0x1A;
 
-static constexpr uint8_t ACC_I2C_ADDR_PRIMARY = 0x18;
+static constexpr uint8_t ACC_I2C_ADDR_PRIMARY   = 0x18;
 static constexpr uint8_t ACC_I2C_ADDR_SECONDARY = 0x19;
 
 enum class Register : uint8_t {
@@ -74,7 +73,7 @@ enum class Register : uint8_t {
 	ACC_CONF           = 0x40,
 	ACC_RANGE          = 0x41,
 
-	FIFO_DOWN_SAMPLING = 0x45,
+	FIFO_DOWNS         = 0x45,
 	FIFO_WTM_0         = 0x46,
 	FIFO_WTM_1         = 0x47,
 	FIFO_CONFIG_0      = 0x48,
@@ -87,24 +86,15 @@ enum class Register : uint8_t {
 	ACC_PWR_CONF       = 0x7C,
 	ACC_PWR_CTRL       = 0x7D,
 	ACC_SOFTRESET      = 0x7E,
-	ACC_SELF_TEST      = 0x6D,
-	ACC_I2C_ADDR_PRIMARY_REG = 0x6D,
-	ACC_I2C_ADDR_SECONDARY_REG = 0x19,
-	ACC_READ = 0x12
 };
 
 // ACC_CONF
 enum ACC_CONF_BIT : uint8_t {
 	// [7:4] acc_bwp
 	acc_bwp_Normal = Bit7 | Bit5,        // Filter setting normal
-	// [7:4] acc_bwp
-	acc_bwp_osr_4 = Bit7,        // OSR4
+
 	// [3:0] acc_odr
 	acc_odr_1600   = Bit3 | Bit2,        // ODR 1600 Hz
-	// [3:0] acc_odr
-	acc_odr_12_5   = Bit2 | Bit0,        // ODR 12.5 Hz
-	// [3:0] acc_odr
-	acc_odr_100   = Bit3,        	     // ODR 100 Hz
 };
 
 // ACC_RANGE
@@ -158,7 +148,7 @@ enum ACC_PWR_CTRL_BIT : uint8_t {
 
 namespace FIFO
 {
-
+static constexpr size_t SIZE = 1024;
 
 // 1. Acceleration sensor data frame   - Frame length: 7 bytes (1 byte header + 6 bytes payload)
 // Payload: the next bytes contain the sensor data in the same order as defined in the register map (addresses 0x12 – 0x17).
@@ -176,8 +166,6 @@ struct DATA {
 	uint8_t ACC_Z_LSB;
 	uint8_t ACC_Z_MSB;
 };
-static constexpr size_t SIZE = 1024;
-//static constexpr size_t SIZE = sizeof(DATA) * 10;
 static_assert(sizeof(DATA) == 7);
 
 enum header : uint8_t {
@@ -187,15 +175,6 @@ enum header : uint8_t {
 	FIFO_input_config_frame = 0b01001000,
 	sample_drop_frame       = 0b01010000,
 };
-struct bmi08x_sensor_data {
-	/*! X-axis sensor data */
-	int16_t x;
 
-	/*! Y-axis sensor data */
-	int16_t y;
-
-	/*! Z-axis sensor data */
-	int16_t z;
-};
 } // namespace FIFO
 } // namespace Bosch::BMI088::Accelerometer
diff --git a/src/drivers/imu/bosch/bmi088_i2c/Bosch_BMI088_Gyroscope_Registers.hpp b/src/drivers/imu/bosch/bmi088_i2c/Bosch_BMI088_Gyroscope_Registers.hpp
index a3b4530c1c..647e4d2f3c 100644
--- a/src/drivers/imu/bosch/bmi088_i2c/Bosch_BMI088_Gyroscope_Registers.hpp
+++ b/src/drivers/imu/bosch/bmi088_i2c/Bosch_BMI088_Gyroscope_Registers.hpp
@@ -1,6 +1,6 @@
 /****************************************************************************
  *
- *   Copyright (c) 2020 PX4 Development Team. All rights reserved.
+ *   Copyright (c) 2020-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
@@ -48,9 +48,7 @@ static constexpr uint8_t Bit7 = (1 << 7);
 
 static constexpr uint32_t SPI_SPEED = 10 * 1000 * 1000; // 10MHz SPI serial interface
 
-static constexpr uint32_t I2C_400_SPEED = 400 * 1000; // 400kHz I2C interface
-static constexpr uint32_t I2C_200_SPEED = 200 * 1000; // 200kHz I2C interface
-static constexpr uint32_t I2C_100_SPEED = 100 * 1000; // 100kHz I2C interface
+static constexpr uint32_t I2C_SPEED = 400 * 1000; // 400kHz I2C interface
 
 static constexpr uint8_t DIR_READ = 0x80;
 
@@ -76,8 +74,6 @@ enum class Register : uint8_t {
 	FIFO_CONFIG_0     = 0x3D,
 	FIFO_CONFIG_1     = 0x3E,
 	FIFO_DATA         = 0x3F,
-	SELF_TEST         = 0x3C,
-	READ_GYRO         = 0x02,
 };
 
 // FIFO_STATUS
@@ -97,9 +93,7 @@ enum GYRO_RANGE_BIT : uint8_t {
 
 // GYRO_BANDWIDTH
 enum GYRO_BANDWIDTH_BIT : uint8_t {
-	gyro_bw_100_Hz = Bit2 | Bit1 | Bit0,
-	gyro_bw_200_Hz = Bit4,
-	gyro_bw_2000_Hz = 0x00,
+	gyro_bw_532_Hz = Bit2 | Bit1 | Bit0
 };
 
 // GYRO_INT_CTRL
@@ -131,7 +125,6 @@ enum FIFO_WM_ENABLE_BIT : uint8_t {
 // FIFO_CONFIG_1
 enum FIFO_CONFIG_1_BIT : uint8_t {
 	FIFO_MODE = Bit6,
-	FIFO_MODE_STREAM = Bit7,
 };
 
 
diff --git a/src/drivers/imu/bosch/bmi088_i2c/CMakeLists.txt b/src/drivers/imu/bosch/bmi088_i2c/CMakeLists.txt
index a86ef7637e..59d74055c0 100644
--- a/src/drivers/imu/bosch/bmi088_i2c/CMakeLists.txt
+++ b/src/drivers/imu/bosch/bmi088_i2c/CMakeLists.txt
@@ -1,6 +1,6 @@
 ############################################################################
 #
-#   Copyright (c) 2019-2020 PX4 Development Team. All rights reserved.
+#   Copyright (c) 2019-2022 PX4 Development Team. All rights reserved.
 #
 # Redistribution and use in source and binary forms, with or without
 # modification, are permitted provided that the following conditions
@@ -48,7 +48,5 @@ px4_add_module(
 
 		bmi088_i2c_main.cpp
 	DEPENDS
-		drivers_accelerometer
-		drivers_gyroscope
 		px4_work_queue
 	)
diff --git a/src/drivers/imu/bosch/bmi088_i2c/bmi088_i2c_main.cpp b/src/drivers/imu/bosch/bmi088_i2c/bmi088_i2c_main.cpp
index 34f5c46f9c..7184bef4d9 100644
--- a/src/drivers/imu/bosch/bmi088_i2c/bmi088_i2c_main.cpp
+++ b/src/drivers/imu/bosch/bmi088_i2c/bmi088_i2c_main.cpp
@@ -1,6 +1,6 @@
 /****************************************************************************
  *
- *   Copyright (c) 2020, 2021 PX4 Development Team. All rights reserved.
+ *   Copyright (c) 2020-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
@@ -60,19 +60,20 @@ extern "C" int bmi088_i2c_main(int argc, char *argv[])
 	uint16_t type = 0;
 	const char *name = MODULE_NAME;
 
-
 	while ((ch = cli.getOpt(argc, argv, "AGR:")) != EOF) {
 		switch (ch) {
 		case 'A':
 			type = DRV_ACC_DEVTYPE_BMI088;
 			name = MODULE_NAME "_accel";
 			cli.i2c_address = 0x18;
+			cli.default_i2c_frequency = 400 * 1000;
 			break;
 
 		case 'G':
 			type = DRV_GYR_DEVTYPE_BMI088;
 			name = MODULE_NAME "_gyro";
 			cli.i2c_address = 0x69;
+			cli.default_i2c_frequency = 400 * 1000;
 			break;
 
 		case 'R':
@@ -92,17 +93,14 @@ extern "C" int bmi088_i2c_main(int argc, char *argv[])
 
 	if (!strcmp(verb, "start")) {
 		return ThisDriver::module_start(cli, iterator);
-	}
 
-	if (!strcmp(verb, "stop")) {
+	} else if (!strcmp(verb, "stop")) {
 		return ThisDriver::module_stop(iterator);
-	}
 
-	if (!strcmp(verb, "status")) {
+	} else if (!strcmp(verb, "status")) {
 		return ThisDriver::module_status(iterator);
 	}
 
-	PX4_WARN("print_usage1");
 	ThisDriver::print_usage();
 	return -1;
 }
diff --git a/src/drivers/imu/fxas21002c/FXAS21002C.cpp b/src/drivers/imu/fxas21002c/FXAS21002C.cpp
index 9bc41e903b..2be1d96df5 100644
--- a/src/drivers/imu/fxas21002c/FXAS21002C.cpp
+++ b/src/drivers/imu/fxas21002c/FXAS21002C.cpp
@@ -68,7 +68,7 @@ using namespace time_literals;
 FXAS21002C::FXAS21002C(device::Device *interface, const I2CSPIDriverConfig &config) :
 	I2CSPIDriver(config),
 	_interface(interface),
-	_px4_gyro(_interface->get_device_id(), config.rotation),
+	_rotation(config.rotation),
 	_sample_perf(perf_alloc(PC_ELAPSED, MODULE_NAME": read")),
 	_errors(perf_alloc(PC_COUNT, MODULE_NAME": err")),
 	_bad_registers(perf_alloc(PC_COUNT, MODULE_NAME": bad register")),
@@ -186,7 +186,7 @@ int FXAS21002C::set_range(unsigned max_dps)
 
 	set_standby(_current_rate, true);
 
-	_px4_gyro.set_scale(new_range_scale_dps_digit / 180.0f * M_PI_F);
+	_gyro_scale = new_range_scale_dps_digit / 180.0f * M_PI_F);
 
 	modify_reg(FXAS21002C_CTRL_REG0, CTRL_REG0_FS_MASK, bits);
 	set_standby(_current_rate, false);
@@ -378,9 +378,6 @@ void FXAS21002C::RunImpl()
 	}
 
 	// report the error count as the number of bad register reads. This allows the higher level
-	_px4_gyro.set_error_count(perf_event_count(_bad_registers));
-	_px4_gyro.update(timestamp_sample, x_raw, y_raw, z_raw);
-
 	if (hrt_elapsed_time(&_last_temperature_update) > 100_ms) {
 		/*
 		 * The TEMP register contains an 8-bit 2's complement temperature value with a range
@@ -389,10 +386,23 @@ void FXAS21002C::RunImpl()
 		 * mode and actively measuring the angular rate.
 		 */
 		const float temperature = read_reg(FXAS21002C_TEMP) * 1.0f;
-		_px4_gyro.set_temperature(temperature);
+		_last_temperature = temperature;
 		_last_temperature_update = timestamp_sample;
 	}
 
+	// sensor_gyro
+	sensor_gyro_s sensor_gyro{};
+	sensor_gyro.timestamp_sample = time_now_us;
+	sensor_gyro.device_id = 1;
+	sensor_gyro.x = gyro(0);
+	sensor_gyro.y = gyro(1);
+	sensor_gyro.z = gyro(2);
+	sensor_gyro.range = math::radians(2000.f);
+	sensor_gyro.temperature = NAN;
+	sensor_gyro.error_count = perf_event_count(_bad_registers);
+	sensor_gyro.timestamp = hrt_absolute_time();
+	_sensor_gyro_pub.publish(sensor_gyro);
+
 	/* stop the perf counter */
 	perf_end(_sample_perf);
 }
diff --git a/src/drivers/imu/fxas21002c/FXAS21002C.hpp b/src/drivers/imu/fxas21002c/FXAS21002C.hpp
index 0c21eb70fd..571c2ac0c9 100644
--- a/src/drivers/imu/fxas21002c/FXAS21002C.hpp
+++ b/src/drivers/imu/fxas21002c/FXAS21002C.hpp
@@ -40,7 +40,6 @@
 #pragma once
 
 #include <drivers/device/Device.hpp>
-#include <lib/drivers/gyroscope/PX4Gyroscope.hpp>
 #include <perf/perf_counter.h>
 #include <px4_platform_common/getopt.h>
 #include <px4_platform_common/i2c_spi_buses.h>
@@ -211,7 +210,6 @@ protected:
 
 private:
 	device::Device *_interface;
-	PX4Gyroscope _px4_gyro;
 
 	unsigned _current_rate{800};
 
diff --git a/src/drivers/imu/fxos8701cq/FXOS8701CQ.cpp b/src/drivers/imu/fxos8701cq/FXOS8701CQ.cpp
index fc5a22d214..52a0c8d9ac 100644
--- a/src/drivers/imu/fxos8701cq/FXOS8701CQ.cpp
+++ b/src/drivers/imu/fxos8701cq/FXOS8701CQ.cpp
@@ -56,18 +56,13 @@ const uint8_t FXOS8701CQ::_checked_registers[FXOS8701C_NUM_CHECKED_REGISTERS] =
 FXOS8701CQ::FXOS8701CQ(device::Device *interface, const I2CSPIDriverConfig &config) :
 	I2CSPIDriver(config),
 	_interface(interface),
-	_px4_accel(interface->get_device_id(), config.rotation),
 #if !defined(BOARD_HAS_NOISY_FXOS8700_MAG)
-	_px4_mag(interface->get_device_id(), config.rotation),
 	_mag_sample_perf(perf_alloc(PC_ELAPSED, MODULE_NAME": mag read")),
 #endif
 	_accel_sample_perf(perf_alloc(PC_ELAPSED, MODULE_NAME": acc read")),
 	_bad_registers(perf_alloc(PC_COUNT, MODULE_NAME": bad reg")),
 	_accel_duplicates(perf_alloc(PC_COUNT, MODULE_NAME": acc dupe"))
 {
-#if !defined(BOARD_HAS_NOISY_FXOS8700_MAG)
-	_px4_mag.set_scale(0.001f);
-#endif
 }
 
 FXOS8701CQ::~FXOS8701CQ()
@@ -195,7 +190,7 @@ FXOS8701CQ::accel_set_range(unsigned max_g)
 
 	modify_reg(FXOS8701CQ_XYZ_DATA_CFG, XYZ_DATA_CFG_FS_MASK, setbits);
 
-	_px4_accel.set_scale(accel_range_scale);
+	_accel_scale = accel_range_scale;
 
 	return OK;
 }
@@ -335,9 +330,6 @@ void FXOS8701CQ::RunImpl()
 	}
 
 	// report the error count as the sum of the number of bad register reads and bad values.
-	_px4_accel.set_error_count(perf_event_count(_bad_registers));
-	_px4_accel.update(timestamp_sample, x, y, z);
-
 	if (hrt_elapsed_time(&_last_temperature_update) > 100_ms) {
 		/*
 		 * Eight-bit 2’s complement sensor temperature value with 0.96 °C/LSB sensitivity.
@@ -349,9 +341,21 @@ void FXOS8701CQ::RunImpl()
 		_last_temperature_update = timestamp_sample;
 		float temperature = (read_reg(FXOS8701CQ_TEMP)) * 0.96f;
 
-		_px4_accel.set_temperature(temperature);
+		_last_temperature = temperature;
 	}
 
+	sensor_accel_s sensor_accel{};
+	sensor_accel.timestamp_sample = timestamp_sample;
+	sensor_accel.device_id = get_device_id();
+	sensor_accel.x = x;
+	sensor_accel.y = y;
+	sensor_accel.z = z;
+	sensor_accel.range = 16.f * CONSTANTS_ONE_G;
+	sensor_accel.temperature = _last_temperature;
+	sensor_accel.error_count = perf_event_count(_bad_registers);
+	sensor_accel.timestamp = hrt_absolute_time();
+	_sensor_accel_pub.publish(sensor_accel);
+
 
 #if !defined(BOARD_HAS_NOISY_FXOS8700_MAG)
 
diff --git a/src/drivers/imu/fxos8701cq/FXOS8701CQ.hpp b/src/drivers/imu/fxos8701cq/FXOS8701CQ.hpp
index ac3954a8e2..c54322b662 100644
--- a/src/drivers/imu/fxos8701cq/FXOS8701CQ.hpp
+++ b/src/drivers/imu/fxos8701cq/FXOS8701CQ.hpp
@@ -41,7 +41,6 @@
 
 #include <drivers/device/i2c.h>
 #include <drivers/device/spi.h>
-#include <lib/drivers/accelerometer/PX4Accelerometer.hpp>
 #include <lib/geo/geo.h>
 #include <lib/perf/perf_counter.h>
 #include <px4_platform_common/i2c_spi_buses.h>
@@ -231,9 +230,6 @@ private:
 	 */
 	int			accel_set_samplerate(unsigned frequency);
 
-
-	PX4Accelerometer	_px4_accel;
-
 #if !defined(BOARD_HAS_NOISY_FXOS8700_MAG)
 	PX4Magnetometer		_px4_mag;
 	hrt_abstime		_mag_last_measure{0};
diff --git a/src/drivers/imu/invensense/icm20602/CMakeLists.txt b/src/drivers/imu/invensense/icm20602/CMakeLists.txt
index 05ed93d307..785f27ff6c 100644
--- a/src/drivers/imu/invensense/icm20602/CMakeLists.txt
+++ b/src/drivers/imu/invensense/icm20602/CMakeLists.txt
@@ -1,6 +1,6 @@
 ############################################################################
 #
-#   Copyright (c) 2019-2021 PX4 Development Team. All rights reserved.
+#   Copyright (c) 2019-2022 PX4 Development Team. All rights reserved.
 #
 # Redistribution and use in source and binary forms, with or without
 # modification, are permitted provided that the following conditions
@@ -35,13 +35,12 @@ px4_add_module(
 	MODULE drivers__imu__invensense__icm20602
 	MAIN icm20602
 	COMPILE_FLAGS
+		${MAX_CUSTOM_OPT_LEVEL}
 	SRCS
+		icm20602_main.cpp
 		ICM20602.cpp
 		ICM20602.hpp
-		icm20602_main.cpp
 		InvenSense_ICM20602_registers.hpp
 	DEPENDS
-		drivers_accelerometer
-		drivers_gyroscope
 		px4_work_queue
 	)
diff --git a/src/drivers/imu/invensense/icm20602/ICM20602.cpp b/src/drivers/imu/invensense/icm20602/ICM20602.cpp
index c13ab0b800..6352151c22 100644
--- a/src/drivers/imu/invensense/icm20602/ICM20602.cpp
+++ b/src/drivers/imu/invensense/icm20602/ICM20602.cpp
@@ -1,6 +1,6 @@
 /****************************************************************************
  *
- *   Copyright (c) 2019-2021 PX4 Development Team. All rights reserved.
+ *   Copyright (c) 2019-2022 PX4 Development Team. All rights reserved.
  *
  * Redistribution and use in source and binary forms, with or without
  * modification, are permitted provided that the following conditions
@@ -33,6 +33,8 @@
 
 #include "ICM20602.hpp"
 
+#include <lib/parameters/param.h>
+
 using namespace time_literals;
 
 static constexpr int16_t combine(uint8_t msb, uint8_t lsb)
@@ -44,14 +46,16 @@ ICM20602::ICM20602(const I2CSPIDriverConfig &config) :
 	SPI(config),
 	I2CSPIDriver(config),
 	_drdy_gpio(config.drdy_gpio),
-	_px4_accel(get_device_id(), config.rotation),
-	_px4_gyro(get_device_id(), config.rotation)
+	_rotation(config.rotation)
 {
 	if (_drdy_gpio != 0) {
 		_drdy_missed_perf = perf_alloc(PC_COUNT, MODULE_NAME": DRDY missed");
 	}
 
-	ConfigureSampleRate(_px4_gyro.get_max_rate_hz());
+	int32_t imu_gyro_rate_max = 400;
+	param_get(param_find("IMU_GYRO_RATEMAX"), &imu_gyro_rate_max);
+
+	ConfigureSampleRate(imu_gyro_rate_max);
 }
 
 ICM20602::~ICM20602()
@@ -131,14 +135,19 @@ bool ICM20602::StoreCheckedRegisterValue(Register reg)
 
 int ICM20602::probe()
 {
-	const uint8_t whoami = RegisterRead(Register::WHO_AM_I);
+	// 3 retries
+	for (int retry = 0; retry < 3; retry++) {
+		const uint8_t whoami = RegisterRead(Register::WHO_AM_I);
 
-	if (whoami != WHOAMI) {
-		DEVICE_DEBUG("unexpected WHO_AM_I 0x%02x", whoami);
-		return PX4_ERROR;
+		if (whoami == WHOAMI) {
+			return PX4_OK;
+
+		} else {
+			DEVICE_DEBUG("unexpected WHO_AM_I 0x%02x", whoami);
+		}
 	}
 
-	return PX4_OK;
+	return PX4_ERROR;
 }
 
 void ICM20602::RunImpl()
@@ -196,8 +205,8 @@ void ICM20602::RunImpl()
 				ScheduleDelayed(100_ms);
 
 			} else {
-				PX4_DEBUG("Reset not complete, check again in 10 ms");
-				ScheduleDelayed(10_ms);
+				PX4_DEBUG("Reset not complete, check again in 100 ms");
+				ScheduleDelayed(100_ms);
 			}
 		}
 
@@ -244,7 +253,7 @@ void ICM20602::RunImpl()
 				// scheduled from interrupt if _drdy_timestamp_sample was set as expected
 				const hrt_abstime drdy_timestamp_sample = _drdy_timestamp_sample.fetch_and(0);
 
-				if ((now - drdy_timestamp_sample) < _fifo_empty_interval_us) {
+				if (now < drdy_timestamp_sample + _fifo_empty_interval_us) {
 					timestamp_sample = drdy_timestamp_sample;
 					samples = _fifo_gyro_samples;
 
@@ -310,13 +319,14 @@ void ICM20602::RunImpl()
 
 				// full reset if things are failing consistently
 				if (_failure_count > 10) {
+					PX4_DEBUG("Full reset because things are failing consistently");
 					Reset();
 					return;
 				}
 			}
 
+			// check configuration registers periodically or immediately following any failure
 			if (!success || hrt_elapsed_time(&_last_config_check_timestamp) > 100_ms) {
-				// check configuration registers periodically or immediately following any failure
 				if (RegisterCheck(_register_cfg[_checked_register])) {
 					_last_config_check_timestamp = now;
 					_checked_register = (_checked_register + 1) % size_register_cfg;
@@ -324,6 +334,7 @@ void ICM20602::RunImpl()
 				} else {
 					// register check failed, force reset
 					perf_count(_bad_register_perf);
+					PX4_DEBUG("Force reset because register 0x%02hhX check failed ", (uint8_t)_register_cfg[_checked_register].reg);
 					Reset();
 				}
 			}
@@ -333,65 +344,10 @@ void ICM20602::RunImpl()
 	}
 }
 
-void ICM20602::ConfigureAccel()
-{
-	const uint8_t ACCEL_FS_SEL = RegisterRead(Register::ACCEL_CONFIG) & (Bit4 | Bit3); // [4:3] ACCEL_FS_SEL[1:0]
-
-	switch (ACCEL_FS_SEL) {
-	case ACCEL_FS_SEL_2G:
-		_px4_accel.set_scale(CONSTANTS_ONE_G / 16384.f);
-		_px4_accel.set_range(2.f * CONSTANTS_ONE_G);
-		break;
-
-	case ACCEL_FS_SEL_4G:
-		_px4_accel.set_scale(CONSTANTS_ONE_G / 8192.f);
-		_px4_accel.set_range(4.f * CONSTANTS_ONE_G);
-		break;
-
-	case ACCEL_FS_SEL_8G:
-		_px4_accel.set_scale(CONSTANTS_ONE_G / 4096.f);
-		_px4_accel.set_range(8.f * CONSTANTS_ONE_G);
-		break;
-
-	case ACCEL_FS_SEL_16G:
-		_px4_accel.set_scale(CONSTANTS_ONE_G / 2048.f);
-		_px4_accel.set_range(16.f * CONSTANTS_ONE_G);
-		break;
-	}
-}
-
-void ICM20602::ConfigureGyro()
-{
-	const uint8_t FS_SEL = RegisterRead(Register::GYRO_CONFIG) & (Bit4 | Bit3); // [4:3] FS_SEL[1:0]
-
-	float range_dps = 0.f;
-
-	switch (FS_SEL) {
-	case FS_SEL_250_DPS:
-		range_dps = 250.f;
-		break;
-
-	case FS_SEL_500_DPS:
-		range_dps = 500.f;
-		break;
-
-	case FS_SEL_1000_DPS:
-		range_dps = 1000.f;
-		break;
-
-	case FS_SEL_2000_DPS:
-		range_dps = 2000.f;
-		break;
-	}
-
-	_px4_gyro.set_scale(math::radians(range_dps / 32768.f));
-	_px4_gyro.set_range(math::radians(range_dps));
-}
-
 void ICM20602::ConfigureSampleRate(int sample_rate)
 {
-	// round down to nearest FIFO sample dt * SAMPLES_PER_TRANSFER
-	const float min_interval = FIFO_SAMPLE_DT * SAMPLES_PER_TRANSFER;
+	// 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));
@@ -438,9 +394,6 @@ bool ICM20602::Configure()
 		}
 	}
 
-	ConfigureAccel();
-	ConfigureGyro();
-
 	return success;
 }
 
@@ -463,7 +416,7 @@ bool ICM20602::DataReadyInterruptConfigure()
 	}
 
 	// Setup data ready on falling edge
-	return px4_arch_gpiosetevent(_drdy_gpio, false, true, true, &DataReadyInterruptCallback, this) == 0;
+	return (px4_arch_gpiosetevent(_drdy_gpio, false, true, false, &DataReadyInterruptCallback, this) == 0);
 }
 
 bool ICM20602::DataReadyInterruptDisable()
@@ -472,7 +425,7 @@ bool ICM20602::DataReadyInterruptDisable()
 		return false;
 	}
 
-	return px4_arch_gpiosetevent(_drdy_gpio, false, false, false, nullptr, nullptr) == 0;
+	return (px4_arch_gpiosetevent(_drdy_gpio, false, false, false, nullptr, nullptr) == 0);
 }
 
 bool ICM20602::RegisterCheck(const register_config_t &reg_cfg)
@@ -521,32 +474,46 @@ void ICM20602::RegisterSetAndClearBits(Register reg, uint8_t setbits, uint8_t cl
 
 uint16_t ICM20602::FIFOReadCount()
 {
-	// read FIFO count
-	uint8_t fifo_count_buf[3] {};
-	fifo_count_buf[0] = static_cast<uint8_t>(Register::FIFO_COUNTH) | DIR_READ;
+	// transfer buffer
+	struct TransferBuffer {
+		uint8_t cmd{static_cast<uint8_t>(Register::FIFO_COUNTH) | DIR_READ};
+		uint8_t FIFO_COUNTH{0};
+		uint8_t FIFO_COUNTL{0};
+	} buffer{};
 
-	if (transfer(fifo_count_buf, fifo_count_buf, sizeof(fifo_count_buf)) != PX4_OK) {
+	// read FIFO count
+	if (transfer((uint8_t *)&buffer, (uint8_t *)&buffer, sizeof(buffer)) != PX4_OK) {
 		perf_count(_bad_transfer_perf);
 		return 0;
 	}
 
-	return combine(fifo_count_buf[1], fifo_count_buf[2]);
+	return (buffer.FIFO_COUNTH << 8) + buffer.FIFO_COUNTL;
 }
 
 bool ICM20602::FIFORead(const hrt_abstime &timestamp_sample, uint8_t samples)
 {
-	FIFOTransferBuffer buffer{};
-	const size_t transfer_size = math::min(samples * sizeof(FIFO::DATA) + 3, FIFO::SIZE);
+	// FIFO transfer buffer
+	struct TransferBuffer {
+		uint8_t cmd{static_cast<uint8_t>(Register::FIFO_COUNTH) | DIR_READ};
+		uint8_t FIFO_COUNTH{0};
+		uint8_t FIFO_COUNTL{0};
+		FIFO::DATA f[FIFO_MAX_SAMPLES] {};
+	} buffer{};
+
+	// cmd + FIFO_COUNTH + FIFO_COUNTL + samples (FIFO::DATA)
+	const size_t transfer_size = 3 + math::min(samples * sizeof(FIFO::DATA), FIFO::SIZE);
 
 	if (transfer((uint8_t *)&buffer, (uint8_t *)&buffer, transfer_size) != PX4_OK) {
 		perf_count(_bad_transfer_perf);
 		return false;
 	}
 
-	const uint16_t fifo_count_bytes = combine(buffer.FIFO_COUNTH, buffer.FIFO_COUNTL);
+	const uint16_t fifo_count_bytes = (buffer.FIFO_COUNTH << 8) + buffer.FIFO_COUNTL;
 	const uint8_t fifo_count_samples = fifo_count_bytes / sizeof(FIFO::DATA);
 
-	if ((fifo_count_bytes >= FIFO::SIZE) || (fifo_count_samples > FIFO_MAX_SAMPLES)) {
+	const uint8_t valid_samples = math::min(samples, fifo_count_samples);
+
+	if ((fifo_count_bytes >= FIFO::SIZE) || (valid_samples > FIFO_MAX_SAMPLES)) {
 		perf_count(_fifo_overflow_perf);
 		FIFOReset();
 		return false;
@@ -556,17 +523,45 @@ bool ICM20602::FIFORead(const hrt_abstime &timestamp_sample, uint8_t samples)
 		return false;
 	}
 
-	const uint8_t valid_samples = math::min(samples, fifo_count_samples);
-
 	if (valid_samples > 0) {
-		// use raw temperature to first validate FIFO transfer
-		if (ProcessTemperature(buffer.f, valid_samples)) {
-			ProcessGyro(timestamp_sample, buffer.f, valid_samples);
+		sensor_imu_fifo_s sensor_imu_fifo{};
+		sensor_imu_fifo.timestamp_sample = timestamp_sample;
+		sensor_imu_fifo.device_id = get_device_id();
+		sensor_imu_fifo.dt = FIFO_SAMPLE_DT;
+		sensor_imu_fifo.samples = valid_samples;
+		sensor_imu_fifo.accel_scale = ACCEL_SCALE;
+		sensor_imu_fifo.gyro_scale = GYRO_SCALE;
 
-			if (ProcessAccel(timestamp_sample, buffer.f, valid_samples)) {
-				return true;
-			}
+		float temperature_sum{0};
+
+		for (int i = 0; i < valid_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)
+			sensor_imu_fifo.accel_x[i] =              combine(buffer.f[i].ACCEL_XOUT_H, buffer.f[i].ACCEL_XOUT_L);
+			sensor_imu_fifo.accel_y[i] = math::negate(combine(buffer.f[i].ACCEL_YOUT_H, buffer.f[i].ACCEL_YOUT_L));
+			sensor_imu_fifo.accel_z[i] = math::negate(combine(buffer.f[i].ACCEL_ZOUT_H, buffer.f[i].ACCEL_ZOUT_L));
+			rotate_3i(_rotation, sensor_imu_fifo.accel_x[i], sensor_imu_fifo.accel_y[i], sensor_imu_fifo.accel_z[i]);
+
+			sensor_imu_fifo.gyro_x[i] =              combine(buffer.f[i].GYRO_XOUT_H, buffer.f[i].GYRO_XOUT_L);
+			sensor_imu_fifo.gyro_y[i] = math::negate(combine(buffer.f[i].GYRO_YOUT_H, buffer.f[i].GYRO_YOUT_L));
+			sensor_imu_fifo.gyro_z[i] = math::negate(combine(buffer.f[i].GYRO_ZOUT_H, buffer.f[i].GYRO_ZOUT_L));
+			rotate_3i(_rotation, sensor_imu_fifo.gyro_x[i], sensor_imu_fifo.gyro_y[i], sensor_imu_fifo.gyro_z[i]);
+
+			temperature_sum += combine(buffer.f[i].TEMP_OUT_H, buffer.f[i].TEMP_OUT_L);
 		}
+
+		// use average temperature reading
+		const float temperature_avg = temperature_sum / valid_samples;
+		const float temperature_C = (temperature_avg / TEMPERATURE_SENSITIVITY) + TEMPERATURE_OFFSET;
+		sensor_imu_fifo.temperature = temperature_C;
+
+		sensor_imu_fifo.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);
+
+		sensor_imu_fifo.timestamp = hrt_absolute_time();
+		_sensor_imu_fifo_pub.publish(sensor_imu_fifo);
+
+		return true;
 	}
 
 	return false;
@@ -594,124 +589,29 @@ void ICM20602::FIFOReset()
 	}
 }
 
-static bool fifo_accel_equal(const FIFO::DATA &f0, const FIFO::DATA &f1)
+float ICM20602::ReadTemperature()
 {
-	return (memcmp(&f0.ACCEL_XOUT_H, &f1.ACCEL_XOUT_H, 6) == 0);
-}
+	// transfer buffer
+	struct TransferBuffer {
+		uint8_t cmd{static_cast<uint8_t>(Register::TEMP_OUT_H) | DIR_READ};
+		uint8_t TEMP_OUT_H{0};
+		uint8_t TEMP_OUT_L{0};
+	} buffer{};
 
-bool ICM20602::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 * SAMPLES_PER_TRANSFER;
-
-	bool bad_data = false;
-
-	// accel data is doubled in FIFO, but might be shifted
-	int accel_first_sample = 1;
-
-	if (samples >= 4) {
-		if (fifo_accel_equal(fifo[0], fifo[1]) && fifo_accel_equal(fifo[2], fifo[3])) {
-			// [A0, A1, A2, A3]
-			//  A0==A1, A2==A3
-			accel_first_sample = 1;
-
-		} else if (fifo_accel_equal(fifo[1], fifo[2])) {
-			// [A0, A1, A2, A3]
-			//  A0, A1==A2, A3
-			accel_first_sample = 0;
-
-		} else {
-			// no matching accel samples is an error
-			bad_data = true;
-			perf_count(_bad_transfer_perf);
-		}
-	}
-
-	for (int i = accel_first_sample; i < samples; i = i + SAMPLES_PER_TRANSFER) {
-		int16_t accel_x = combine(fifo[i].ACCEL_XOUT_H, fifo[i].ACCEL_XOUT_L);
-		int16_t accel_y = combine(fifo[i].ACCEL_YOUT_H, fifo[i].ACCEL_YOUT_L);
-		int16_t accel_z = combine(fifo[i].ACCEL_ZOUT_H, fifo[i].ACCEL_ZOUT_L);
-
-		// 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[accel.samples] = accel_x;
-		accel.y[accel.samples] = (accel_y == INT16_MIN) ? INT16_MAX : -accel_y;
-		accel.z[accel.samples] = (accel_z == INT16_MIN) ? INT16_MAX : -accel_z;
-		accel.samples++;
-	}
-
-	_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) {
-		_px4_accel.updateFIFO(accel);
-	}
-
-	return !bad_data;
-}
-
-void ICM20602::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 = samples;
-	gyro.dt = FIFO_SAMPLE_DT;
-
-	for (int i = 0; i < samples; i++) {
-		const int16_t gyro_x = combine(fifo[i].GYRO_XOUT_H, fifo[i].GYRO_XOUT_L);
-		const int16_t gyro_y = combine(fifo[i].GYRO_YOUT_H, fifo[i].GYRO_YOUT_L);
-		const int16_t gyro_z = combine(fifo[i].GYRO_ZOUT_H, fifo[i].GYRO_ZOUT_L);
-
-		// 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;
-		gyro.y[i] = (gyro_y == INT16_MIN) ? INT16_MAX : -gyro_y;
-		gyro.z[i] = (gyro_z == INT16_MIN) ? INT16_MAX : -gyro_z;
-	}
-
-	_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));
-
-	_px4_gyro.updateFIFO(gyro);
-}
-
-bool ICM20602::ProcessTemperature(const FIFO::DATA fifo[], const uint8_t samples)
-{
-	int16_t temperature[FIFO_MAX_SAMPLES];
-	float temperature_sum{0};
-
-	for (int i = 0; i < samples; i++) {
-		const int16_t t = combine(fifo[i].TEMP_OUT_H, fifo[i].TEMP_OUT_L);
-		temperature_sum += t;
-		temperature[i] = t;
-	}
-
-	const float temperature_avg = temperature_sum / samples;
-
-	for (int i = 0; i < 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 temperature_C = (temperature_avg / TEMPERATURE_SENSITIVITY) + TEMPERATURE_OFFSET;
-
-	if (PX4_ISFINITE(temperature_C)
-	    && (temperature_C >= TEMPERATURE_SENSOR_MIN)
-	    && (temperature_C <= TEMPERATURE_SENSOR_MAX)) {
-
-		_px4_accel.set_temperature(temperature_C);
-		_px4_gyro.set_temperature(temperature_C);
-		return true;
-
-	} else {
+	if (transfer((uint8_t *)&buffer, (uint8_t *)&buffer, sizeof(buffer)) != PX4_OK) {
 		perf_count(_bad_transfer_perf);
+		return NAN;;
 	}
 
-	return false;
+	const int16_t TEMP_OUT = combine(buffer.TEMP_OUT_H, buffer.TEMP_OUT_L);
+	const float TEMP_degC = (TEMP_OUT / TEMPERATURE_SENSITIVITY) + TEMPERATURE_OFFSET;
+
+	if (PX4_ISFINITE(TEMP_degC)
+	    && (TEMP_degC >= TEMPERATURE_SENSOR_MIN)
+	    && (TEMP_degC <= TEMPERATURE_SENSOR_MAX)) {
+
+		return TEMP_degC;
+	}
+
+	return NAN;
 }
diff --git a/src/drivers/imu/invensense/icm20602/ICM20602.hpp b/src/drivers/imu/invensense/icm20602/ICM20602.hpp
index 3855ce12ea..71001ec2b9 100644
--- a/src/drivers/imu/invensense/icm20602/ICM20602.hpp
+++ b/src/drivers/imu/invensense/icm20602/ICM20602.hpp
@@ -1,6 +1,6 @@
 /****************************************************************************
  *
- *   Copyright (c) 2019-2021 PX4 Development Team. All rights reserved.
+ *   Copyright (c) 2019-2022 PX4 Development Team. All rights reserved.
  *
  * Redistribution and use in source and binary forms, with or without
  * modification, are permitted provided that the following conditions
@@ -43,14 +43,15 @@
 #include "InvenSense_ICM20602_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/PublicationMulti.hpp>
+#include <uORB/topics/sensor_imu_fifo.h>
+
 using namespace InvenSense_ICM20602;
 
 class ICM20602 : public device::SPI, public I2CSPIDriver<ICM20602>
@@ -71,22 +72,13 @@ private:
 
 	// Sensor Configuration
 	static constexpr float FIFO_SAMPLE_DT{1e6f / 8000.f};
-	static constexpr int32_t SAMPLES_PER_TRANSFER{2};                    // ensure at least 1 new accel sample per transfer
 	static constexpr float GYRO_RATE{1e6f / FIFO_SAMPLE_DT};             // 8000 Hz gyro
-	static constexpr float ACCEL_RATE{GYRO_RATE / SAMPLES_PER_TRANSFER}; // 4000 Hz accel
 
 	// maximum FIFO samples per transfer is limited to the size of sensor_accel_fifo/sensor_gyro_fifo
-	static constexpr int32_t FIFO_MAX_SAMPLES{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 int32_t FIFO_MAX_SAMPLES{math::min(FIFO::SIZE / sizeof(FIFO::DATA), (size_t)sensor_imu_fifo_s::FIFO_SIZE)};
 
-	// Transfer data
-	struct FIFOTransferBuffer {
-		uint8_t cmd{static_cast<uint8_t>(Register::FIFO_COUNTH) | DIR_READ};
-		uint8_t FIFO_COUNTH{0};
-		uint8_t FIFO_COUNTL{0};
-		FIFO::DATA f[FIFO_MAX_SAMPLES] {};
-	};
-	// ensure no struct padding
-	static_assert(sizeof(FIFOTransferBuffer) == (3 + FIFO_MAX_SAMPLES *sizeof(FIFO::DATA)));
+	static constexpr float ACCEL_SCALE{CONSTANTS_ONE_G / 2048.f}; // ACCEL_FS_SEL_16G:
+	static constexpr float GYRO_SCALE{math::radians(2000.f / 32768.f)}; // FS_SEL_2000_DPS:
 
 	struct register_config_t {
 		Register reg;
@@ -99,8 +91,6 @@ private:
 	bool Reset();
 
 	bool Configure();
-	void ConfigureAccel();
-	void ConfigureGyro();
 	void ConfigureSampleRate(int sample_rate);
 	void ConfigureFIFOWatermark(uint8_t samples);
 
@@ -120,14 +110,13 @@ private:
 	bool FIFORead(const hrt_abstime &timestamp_sample, uint8_t samples);
 	void FIFOReset();
 
-	bool 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);
+	float ReadTemperature();
 
 	const spi_drdy_gpio_t _drdy_gpio;
 
-	PX4Accelerometer _px4_accel;
-	PX4Gyroscope _px4_gyro;
+	uORB::PublicationMulti<sensor_imu_fifo_s> _sensor_imu_fifo_pub{ORB_ID(sensor_imu_fifo)};
+
+	const enum Rotation _rotation;
 
 	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")};
diff --git a/src/drivers/imu/invensense/icm20608g/CMakeLists.txt b/src/drivers/imu/invensense/icm20608g/CMakeLists.txt
index 307ca6c216..59bbd77845 100644
--- a/src/drivers/imu/invensense/icm20608g/CMakeLists.txt
+++ b/src/drivers/imu/invensense/icm20608g/CMakeLists.txt
@@ -41,7 +41,5 @@ px4_add_module(
 		icm20608g_main.cpp
 		InvenSense_ICM20608G_registers.hpp
 	DEPENDS
-		drivers_accelerometer
-		drivers_gyroscope
 		px4_work_queue
 	)
diff --git a/src/drivers/imu/invensense/icm20608g/ICM20608G.cpp b/src/drivers/imu/invensense/icm20608g/ICM20608G.cpp
index 06fe4b048e..a1e10b352f 100644
--- a/src/drivers/imu/invensense/icm20608g/ICM20608G.cpp
+++ b/src/drivers/imu/invensense/icm20608g/ICM20608G.cpp
@@ -1,6 +1,6 @@
 /****************************************************************************
  *
- *   Copyright (c) 2019-2021 PX4 Development Team. All rights reserved.
+ *   Copyright (c) 2019-2022 PX4 Development Team. All rights reserved.
  *
  * Redistribution and use in source and binary forms, with or without
  * modification, are permitted provided that the following conditions
@@ -33,6 +33,8 @@
 
 #include "ICM20608G.hpp"
 
+#include <lib/parameters/param.h>
+
 using namespace time_literals;
 
 static constexpr int16_t combine(uint8_t msb, uint8_t lsb)
@@ -44,14 +46,16 @@ ICM20608G::ICM20608G(const I2CSPIDriverConfig &config) :
 	SPI(config),
 	I2CSPIDriver(config),
 	_drdy_gpio(config.drdy_gpio),
-	_px4_accel(get_device_id(), config.rotation),
-	_px4_gyro(get_device_id(), config.rotation)
+	_rotation(config.rotation)
 {
 	if (_drdy_gpio != 0) {
 		_drdy_missed_perf = perf_alloc(PC_COUNT, MODULE_NAME": DRDY missed");
 	}
 
-	ConfigureSampleRate(_px4_gyro.get_max_rate_hz());
+	int32_t imu_gyro_rate_max = 400;
+	param_get(param_find("IMU_GYRO_RATEMAX"), &imu_gyro_rate_max);
+
+	ConfigureSampleRate(imu_gyro_rate_max);
 }
 
 ICM20608G::~ICM20608G()
@@ -108,7 +112,7 @@ void ICM20608G::print_status()
 bool ICM20608G::StoreCheckedRegisterValue(Register reg)
 {
 	// 3 retries
-	for (int i = 0; i < 3; i++) {
+	for (int retry = 0; retry < 3; retry++) {
 		uint8_t read1 = RegisterRead(reg);
 		uint8_t read2 = RegisterRead(reg);
 
@@ -131,14 +135,19 @@ bool ICM20608G::StoreCheckedRegisterValue(Register reg)
 
 int ICM20608G::probe()
 {
-	const uint8_t whoami = RegisterRead(Register::WHO_AM_I);
+	// 3 retries
+	for (int retry = 0; retry < 3; retry++) {
+		const uint8_t whoami = RegisterRead(Register::WHO_AM_I);
 
-	if (whoami != WHOAMI) {
-		DEVICE_DEBUG("unexpected WHO_AM_I 0x%02x", whoami);
-		return PX4_ERROR;
+		if (whoami == WHOAMI) {
+			return PX4_OK;
+
+		} else {
+			DEVICE_DEBUG("unexpected WHO_AM_I 0x%02x", whoami);
+		}
 	}
 
-	return PX4_OK;
+	return PX4_ERROR;
 }
 
 void ICM20608G::RunImpl()
@@ -187,8 +196,8 @@ void ICM20608G::RunImpl()
 				ScheduleDelayed(100_ms);
 
 			} else {
-				PX4_DEBUG("Reset not complete, check again in 10 ms");
-				ScheduleDelayed(10_ms);
+				PX4_DEBUG("Reset not complete, check again in 100 ms");
+				ScheduleDelayed(100_ms);
 			}
 		}
 
@@ -196,6 +205,8 @@ void ICM20608G::RunImpl()
 
 	case STATE::CONFIGURE:
 		if (Configure()) {
+			_temperature = ReadTemperature();
+
 			// if configure succeeded then start reading from FIFO
 			_state = STATE::FIFO_READ;
 
@@ -234,7 +245,7 @@ void ICM20608G::RunImpl()
 				// scheduled from interrupt if _drdy_timestamp_sample was set as expected
 				const hrt_abstime drdy_timestamp_sample = _drdy_timestamp_sample.fetch_and(0);
 
-				if ((now - drdy_timestamp_sample) < _fifo_empty_interval_us) {
+				if (now < drdy_timestamp_sample + _fifo_empty_interval_us) {
 					timestamp_sample = drdy_timestamp_sample;
 
 				} else {
@@ -271,7 +282,7 @@ void ICM20608G::RunImpl()
 					FIFOReset();
 					perf_count(_fifo_overflow_perf);
 
-				} else if (samples >= SAMPLES_PER_TRANSFER) {
+				} else if (samples >= 1) {
 					if (FIFORead(timestamp_sample, samples)) {
 						success = true;
 
@@ -287,6 +298,7 @@ void ICM20608G::RunImpl()
 
 				// full reset if things are failing consistently
 				if (_failure_count > 10) {
+					PX4_DEBUG("Full reset because things are failing consistently");
 					Reset();
 					return;
 				}
@@ -301,13 +313,14 @@ void ICM20608G::RunImpl()
 				} else {
 					// register check failed, force reset
 					perf_count(_bad_register_perf);
+					PX4_DEBUG("Force reset because register 0x%02hhX check failed ", (uint8_t)_register_cfg[_checked_register].reg);
 					Reset();
 				}
 
 			} else {
 				// periodically update temperature (~1 Hz)
 				if (hrt_elapsed_time(&_temperature_update_timestamp) >= 1_s) {
-					UpdateTemperature();
+					_temperature = ReadTemperature();
 					_temperature_update_timestamp = now;
 				}
 			}
@@ -317,65 +330,10 @@ void ICM20608G::RunImpl()
 	}
 }
 
-void ICM20608G::ConfigureAccel()
-{
-	const uint8_t ACCEL_FS_SEL = RegisterRead(Register::ACCEL_CONFIG) & (Bit4 | Bit3); // [4:3] ACCEL_FS_SEL[1:0]
-
-	switch (ACCEL_FS_SEL) {
-	case ACCEL_FS_SEL_2G:
-		_px4_accel.set_scale(CONSTANTS_ONE_G / 16384.f);
-		_px4_accel.set_range(2.f * CONSTANTS_ONE_G);
-		break;
-
-	case ACCEL_FS_SEL_4G:
-		_px4_accel.set_scale(CONSTANTS_ONE_G / 8192.f);
-		_px4_accel.set_range(4.f * CONSTANTS_ONE_G);
-		break;
-
-	case ACCEL_FS_SEL_8G:
-		_px4_accel.set_scale(CONSTANTS_ONE_G / 4096.f);
-		_px4_accel.set_range(8.f * CONSTANTS_ONE_G);
-		break;
-
-	case ACCEL_FS_SEL_16G:
-		_px4_accel.set_scale(CONSTANTS_ONE_G / 2048.f);
-		_px4_accel.set_range(16.f * CONSTANTS_ONE_G);
-		break;
-	}
-}
-
-void ICM20608G::ConfigureGyro()
-{
-	const uint8_t FS_SEL = RegisterRead(Register::GYRO_CONFIG) & (Bit4 | Bit3); // [4:3] FS_SEL[1:0]
-
-	float range_dps = 0.f;
-
-	switch (FS_SEL) {
-	case FS_SEL_250_DPS:
-		range_dps = 250.f;
-		break;
-
-	case FS_SEL_500_DPS:
-		range_dps = 500.f;
-		break;
-
-	case FS_SEL_1000_DPS:
-		range_dps = 1000.f;
-		break;
-
-	case FS_SEL_2000_DPS:
-		range_dps = 2000.f;
-		break;
-	}
-
-	_px4_gyro.set_scale(math::radians(range_dps / 32768.f));
-	_px4_gyro.set_range(math::radians(range_dps));
-}
-
 void ICM20608G::ConfigureSampleRate(int sample_rate)
 {
-	// round down to nearest FIFO sample dt * SAMPLES_PER_TRANSFER
-	const float min_interval = FIFO_SAMPLE_DT * SAMPLES_PER_TRANSFER;
+	// 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));
@@ -400,9 +358,6 @@ bool ICM20608G::Configure()
 		}
 	}
 
-	ConfigureAccel();
-	ConfigureGyro();
-
 	return success;
 }
 
@@ -429,7 +384,7 @@ bool ICM20608G::DataReadyInterruptConfigure()
 	}
 
 	// Setup data ready on falling edge
-	return px4_arch_gpiosetevent(_drdy_gpio, false, true, true, &DataReadyInterruptCallback, this) == 0;
+	return (px4_arch_gpiosetevent(_drdy_gpio, false, true, false, &DataReadyInterruptCallback, this) == 0);
 }
 
 bool ICM20608G::DataReadyInterruptDisable()
@@ -438,7 +393,7 @@ bool ICM20608G::DataReadyInterruptDisable()
 		return false;
 	}
 
-	return px4_arch_gpiosetevent(_drdy_gpio, false, false, false, nullptr, nullptr) == 0;
+	return (px4_arch_gpiosetevent(_drdy_gpio, false, false, false, nullptr, nullptr) == 0);
 }
 
 bool ICM20608G::RegisterCheck(const register_config_t &reg_cfg)
@@ -487,31 +442,75 @@ void ICM20608G::RegisterSetAndClearBits(Register reg, uint8_t setbits, uint8_t c
 
 uint16_t ICM20608G::FIFOReadCount()
 {
-	// read FIFO count
-	uint8_t fifo_count_buf[3] {};
-	fifo_count_buf[0] = static_cast<uint8_t>(Register::FIFO_COUNTH) | DIR_READ;
+	// transfer buffer
+	struct TransferBuffer {
+		uint8_t cmd{static_cast<uint8_t>(Register::FIFO_COUNTH) | DIR_READ};
+		uint8_t FIFO_COUNTH{0};
+		uint8_t FIFO_COUNTL{0};
+	} buffer{};
 
-	if (transfer(fifo_count_buf, fifo_count_buf, sizeof(fifo_count_buf)) != PX4_OK) {
+	// read FIFO count
+	if (transfer((uint8_t *)&buffer, (uint8_t *)&buffer, sizeof(buffer)) != PX4_OK) {
 		perf_count(_bad_transfer_perf);
 		return 0;
 	}
 
-	return combine(fifo_count_buf[1], fifo_count_buf[2]);
+	return (buffer.FIFO_COUNTH << 8) + buffer.FIFO_COUNTL;
 }
 
 bool ICM20608G::FIFORead(const hrt_abstime &timestamp_sample, uint8_t samples)
 {
-	FIFOTransferBuffer buffer{};
-	const size_t transfer_size = math::min(samples * sizeof(FIFO::DATA) + 1, FIFO::SIZE);
+	// FIFO transfer buffer
+	struct FIFOTransferBuffer {
+		uint8_t cmd{static_cast<uint8_t>(Register::FIFO_R_W) | DIR_READ};
+		FIFO::DATA f[FIFO_MAX_SAMPLES] {};
+	} buffer{};
+
+	// cmd + samples (FIFO::DATA)
+	const size_t transfer_size = 1 + math::min(samples * sizeof(FIFO::DATA), FIFO::SIZE);
 
 	if (transfer((uint8_t *)&buffer, (uint8_t *)&buffer, transfer_size) != PX4_OK) {
 		perf_count(_bad_transfer_perf);
 		return false;
 	}
 
+	const uint8_t valid_samples = samples;
 
-	ProcessGyro(timestamp_sample, buffer.f, samples);
-	return ProcessAccel(timestamp_sample, buffer.f, samples);
+	if (valid_samples > 0) {
+		sensor_imu_fifo_s sensor_imu_fifo{};
+		sensor_imu_fifo.timestamp_sample = timestamp_sample;
+		sensor_imu_fifo.device_id = get_device_id();
+		sensor_imu_fifo.dt = FIFO_SAMPLE_DT;
+		sensor_imu_fifo.samples = valid_samples;
+		sensor_imu_fifo.accel_scale = ACCEL_SCALE;
+		sensor_imu_fifo.gyro_scale = GYRO_SCALE;
+
+		for (int i = 0; i < valid_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)
+			sensor_imu_fifo.accel_x[i] =              combine(buffer.f[i].ACCEL_XOUT_H, buffer.f[i].ACCEL_XOUT_L);
+			sensor_imu_fifo.accel_y[i] = math::negate(combine(buffer.f[i].ACCEL_YOUT_H, buffer.f[i].ACCEL_YOUT_L));
+			sensor_imu_fifo.accel_z[i] = math::negate(combine(buffer.f[i].ACCEL_ZOUT_H, buffer.f[i].ACCEL_ZOUT_L));
+			rotate_3i(_rotation, sensor_imu_fifo.accel_x[i], sensor_imu_fifo.accel_y[i], sensor_imu_fifo.accel_z[i]);
+
+			sensor_imu_fifo.gyro_x[i] =              combine(buffer.f[i].GYRO_XOUT_H, buffer.f[i].GYRO_XOUT_L);
+			sensor_imu_fifo.gyro_y[i] = math::negate(combine(buffer.f[i].GYRO_YOUT_H, buffer.f[i].GYRO_YOUT_L));
+			sensor_imu_fifo.gyro_z[i] = math::negate(combine(buffer.f[i].GYRO_ZOUT_H, buffer.f[i].GYRO_ZOUT_L));
+			rotate_3i(_rotation, sensor_imu_fifo.gyro_x[i], sensor_imu_fifo.gyro_y[i], sensor_imu_fifo.gyro_z[i]);
+		}
+
+		sensor_imu_fifo.temperature = (hrt_elapsed_time(&_temperature_update_timestamp) < 5_s) ? _temperature : NAN;
+
+		sensor_imu_fifo.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);
+
+		sensor_imu_fifo.timestamp = hrt_absolute_time();
+		_sensor_imu_fifo_pub.publish(sensor_imu_fifo);
+
+		return true;
+	}
+
+	return false;
 }
 
 void ICM20608G::FIFOReset()
@@ -537,105 +536,30 @@ void ICM20608G::FIFOReset()
 	}
 }
 
-static bool fifo_accel_equal(const FIFO::DATA &f0, const FIFO::DATA &f1)
+float ICM20608G::ReadTemperature()
 {
-	return (memcmp(&f0.ACCEL_XOUT_H, &f1.ACCEL_XOUT_H, 6) == 0);
-}
+	// transfer buffer
+	struct TransferBuffer {
+		uint8_t cmd{static_cast<uint8_t>(Register::TEMP_OUT_H) | DIR_READ};
+		uint8_t TEMP_OUT_H{0};
+		uint8_t TEMP_OUT_L{0};
+	} buffer{};
 
-bool ICM20608G::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 * SAMPLES_PER_TRANSFER;
-
-	bool bad_data = false;
-
-	// accel data is doubled in FIFO, but might be shifted
-	int accel_first_sample = 1;
-
-	if (samples >= 4) {
-		if (fifo_accel_equal(fifo[0], fifo[1]) && fifo_accel_equal(fifo[2], fifo[3])) {
-			// [A0, A1, A2, A3]
-			//  A0==A1, A2==A3
-			accel_first_sample = 1;
-
-		} else if (fifo_accel_equal(fifo[1], fifo[2])) {
-			// [A0, A1, A2, A3]
-			//  A0, A1==A2, A3
-			accel_first_sample = 0;
-
-		} else {
-			// no matching accel samples is an error
-			bad_data = true;
-			perf_count(_bad_transfer_perf);
-		}
-	}
-
-	for (int i = accel_first_sample; i < samples; i = i + SAMPLES_PER_TRANSFER) {
-		int16_t accel_x = combine(fifo[i].ACCEL_XOUT_H, fifo[i].ACCEL_XOUT_L);
-		int16_t accel_y = combine(fifo[i].ACCEL_YOUT_H, fifo[i].ACCEL_YOUT_L);
-		int16_t accel_z = combine(fifo[i].ACCEL_ZOUT_H, fifo[i].ACCEL_ZOUT_L);
-
-		// 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[accel.samples] = accel_x;
-		accel.y[accel.samples] = (accel_y == INT16_MIN) ? INT16_MAX : -accel_y;
-		accel.z[accel.samples] = (accel_z == INT16_MIN) ? INT16_MAX : -accel_z;
-		accel.samples++;
-	}
-
-	_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) {
-		_px4_accel.updateFIFO(accel);
-	}
-
-	return !bad_data;
-}
-
-void ICM20608G::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 = samples;
-	gyro.dt = FIFO_SAMPLE_DT;
-
-	for (int i = 0; i < samples; i++) {
-		const int16_t gyro_x = combine(fifo[i].GYRO_XOUT_H, fifo[i].GYRO_XOUT_L);
-		const int16_t gyro_y = combine(fifo[i].GYRO_YOUT_H, fifo[i].GYRO_YOUT_L);
-		const int16_t gyro_z = combine(fifo[i].GYRO_ZOUT_H, fifo[i].GYRO_ZOUT_L);
-
-		// 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;
-		gyro.y[i] = (gyro_y == INT16_MIN) ? INT16_MAX : -gyro_y;
-		gyro.z[i] = (gyro_z == INT16_MIN) ? INT16_MAX : -gyro_z;
-	}
-
-	_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));
-
-	_px4_gyro.updateFIFO(gyro);
-}
-
-void ICM20608G::UpdateTemperature()
-{
 	// read current temperature
-	uint8_t temperature_buf[3] {};
-	temperature_buf[0] = static_cast<uint8_t>(Register::TEMP_OUT_H) | DIR_READ;
-
-	if (transfer(temperature_buf, temperature_buf, sizeof(temperature_buf)) != PX4_OK) {
+	if (transfer((uint8_t *)&buffer, (uint8_t *)&buffer, sizeof(buffer)) != PX4_OK) {
 		perf_count(_bad_transfer_perf);
-		return;
+		return NAN;
 	}
 
-	const int16_t TEMP_OUT = combine(temperature_buf[1], temperature_buf[2]);
+	const int16_t TEMP_OUT = combine(buffer.TEMP_OUT_H, buffer.TEMP_OUT_L);
 	const float TEMP_degC = (TEMP_OUT / TEMPERATURE_SENSITIVITY) + TEMPERATURE_OFFSET;
 
-	if (PX4_ISFINITE(TEMP_degC)) {
-		_px4_accel.set_temperature(TEMP_degC);
-		_px4_gyro.set_temperature(TEMP_degC);
+	if (PX4_ISFINITE(TEMP_degC)
+	    && (TEMP_degC >= TEMPERATURE_SENSOR_MIN)
+	    && (TEMP_degC <= TEMPERATURE_SENSOR_MAX)) {
+
+		return TEMP_degC;
 	}
+
+	return NAN;
 }
diff --git a/src/drivers/imu/invensense/icm20608g/ICM20608G.hpp b/src/drivers/imu/invensense/icm20608g/ICM20608G.hpp
index 9608eeb816..2eb3f3d2eb 100644
--- a/src/drivers/imu/invensense/icm20608g/ICM20608G.hpp
+++ b/src/drivers/imu/invensense/icm20608g/ICM20608G.hpp
@@ -1,6 +1,6 @@
 /****************************************************************************
  *
- *   Copyright (c) 2019-2021 PX4 Development Team. All rights reserved.
+ *   Copyright (c) 2019-2022 PX4 Development Team. All rights reserved.
  *
  * Redistribution and use in source and binary forms, with or without
  * modification, are permitted provided that the following conditions
@@ -43,14 +43,15 @@
 #include "InvenSense_ICM20608G_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/PublicationMulti.hpp>
+#include <uORB/topics/sensor_imu_fifo.h>
+
 using namespace InvenSense_ICM20608G;
 
 class ICM20608G : public device::SPI, public I2CSPIDriver<ICM20608G>
@@ -71,20 +72,13 @@ private:
 
 	// Sensor Configuration
 	static constexpr float FIFO_SAMPLE_DT{1e6f / 8000.f};
-	static constexpr int32_t SAMPLES_PER_TRANSFER{2};                    // ensure at least 1 new accel sample per transfer
 	static constexpr float GYRO_RATE{1e6f / FIFO_SAMPLE_DT};             // 8000 Hz gyro
-	static constexpr float ACCEL_RATE{GYRO_RATE / SAMPLES_PER_TRANSFER}; // 4000 Hz accel
 
 	// maximum FIFO samples per transfer is limited to the size of sensor_accel_fifo/sensor_gyro_fifo
-	static constexpr int32_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 int32_t FIFO_MAX_SAMPLES{math::min(FIFO::SIZE / sizeof(FIFO::DATA), (size_t)sensor_imu_fifo_s::FIFO_SIZE)};
 
-	// Transfer data
-	struct FIFOTransferBuffer {
-		uint8_t cmd{static_cast<uint8_t>(Register::FIFO_R_W) | DIR_READ};
-		FIFO::DATA f[FIFO_MAX_SAMPLES] {};
-	};
-	// ensure no struct padding
-	static_assert(sizeof(FIFOTransferBuffer) == (1 + FIFO_MAX_SAMPLES *sizeof(FIFO::DATA)));
+	static constexpr float ACCEL_SCALE{CONSTANTS_ONE_G / 2048.f}; // ACCEL_FS_SEL_16G
+	static constexpr float GYRO_SCALE{math::radians(2000.f / 32768.f)}; // FS_SEL
 
 	struct register_config_t {
 		Register reg;
@@ -97,8 +91,6 @@ private:
 	bool Reset();
 
 	bool Configure();
-	void ConfigureAccel();
-	void ConfigureGyro();
 	void ConfigureSampleRate(int sample_rate);
 
 	static int DataReadyInterruptCallback(int irq, void *context, void *arg);
@@ -117,14 +109,15 @@ private:
 	bool FIFORead(const hrt_abstime &timestamp_sample, uint8_t samples);
 	void FIFOReset();
 
-	bool 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);
-	void UpdateTemperature();
+	float ReadTemperature();
 
 	const spi_drdy_gpio_t _drdy_gpio;
 
-	PX4Accelerometer _px4_accel;
-	PX4Gyroscope _px4_gyro;
+	uORB::PublicationMulti<sensor_imu_fifo_s> _sensor_imu_fifo_pub{ORB_ID(sensor_imu_fifo)};
+
+	const enum Rotation _rotation;
+
+	float _temperature{NAN};
 
 	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")};
diff --git a/src/drivers/imu/invensense/icm20608g/InvenSense_ICM20608G_registers.hpp b/src/drivers/imu/invensense/icm20608g/InvenSense_ICM20608G_registers.hpp
index 20d901bce9..96f8db56e9 100644
--- a/src/drivers/imu/invensense/icm20608g/InvenSense_ICM20608G_registers.hpp
+++ b/src/drivers/imu/invensense/icm20608g/InvenSense_ICM20608G_registers.hpp
@@ -1,6 +1,6 @@
 /****************************************************************************
  *
- *   Copyright (c) 2019-2021 PX4 Development Team. All rights reserved.
+ *   Copyright (c) 2019-2022 PX4 Development Team. All rights reserved.
  *
  * Redistribution and use in source and binary forms, with or without
  * modification, are permitted provided that the following conditions
@@ -42,6 +42,8 @@
 
 #include <cstdint>
 
+namespace InvenSense_ICM20608G
+{
 // TODO: move to a central header
 static constexpr uint8_t Bit0 = (1 << 0);
 static constexpr uint8_t Bit1 = (1 << 1);
@@ -52,8 +54,6 @@ static constexpr uint8_t Bit5 = (1 << 5);
 static constexpr uint8_t Bit6 = (1 << 6);
 static constexpr uint8_t Bit7 = (1 << 7);
 
-namespace InvenSense_ICM20608G
-{
 static constexpr uint32_t SPI_SPEED = 8 * 1000 * 1000; // 8MHz SPI serial interface
 static constexpr uint8_t DIR_READ = 0x80;
 
@@ -61,8 +61,11 @@ static constexpr uint8_t WHOAMI = 0xAF;
 
 static constexpr float TEMPERATURE_SENSITIVITY = 326.8f; // LSB/C
 static constexpr float TEMPERATURE_OFFSET = 25.f; // C
+static constexpr float TEMPERATURE_SENSOR_MIN = -40.f; // °C
+static constexpr float TEMPERATURE_SENSOR_MAX = 85.f; // °C
 
 enum class Register : uint8_t {
+
 	CONFIG            = 0x1A,
 	GYRO_CONFIG       = 0x1B,
 	ACCEL_CONFIG      = 0x1C,
diff --git a/src/drivers/imu/invensense/icm20649/CMakeLists.txt b/src/drivers/imu/invensense/icm20649/CMakeLists.txt
index 905bfe227a..d513f8acd4 100644
--- a/src/drivers/imu/invensense/icm20649/CMakeLists.txt
+++ b/src/drivers/imu/invensense/icm20649/CMakeLists.txt
@@ -34,6 +34,7 @@ px4_add_module(
 	MODULE drivers__imu__invensense__icm20649
 	MAIN icm20649
 	COMPILE_FLAGS
+		${MAX_CUSTOM_OPT_LEVEL}
 	SRCS
 		ICM20649.cpp
 		ICM20649.hpp
@@ -41,6 +42,4 @@ px4_add_module(
 		InvenSense_ICM20649_registers.hpp
 	DEPENDS
 		px4_work_queue
-		drivers_accelerometer
-		drivers_gyroscope
 	)
diff --git a/src/drivers/imu/invensense/icm20649/ICM20649.cpp b/src/drivers/imu/invensense/icm20649/ICM20649.cpp
index 77c4012c41..55225172cd 100644
--- a/src/drivers/imu/invensense/icm20649/ICM20649.cpp
+++ b/src/drivers/imu/invensense/icm20649/ICM20649.cpp
@@ -1,6 +1,6 @@
 /****************************************************************************
  *
- *   Copyright (c) 2020-2021 PX4 Development Team. All rights reserved.
+ *   Copyright (c) 2020-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
@@ -33,6 +33,8 @@
 
 #include "ICM20649.hpp"
 
+#include <lib/parameters/param.h>
+
 using namespace time_literals;
 
 static constexpr int16_t combine(uint8_t msb, uint8_t lsb)
@@ -43,15 +45,12 @@ static constexpr int16_t combine(uint8_t msb, uint8_t lsb)
 ICM20649::ICM20649(const I2CSPIDriverConfig &config) :
 	SPI(config),
 	I2CSPIDriver(config),
-	_drdy_gpio(config.drdy_gpio),
-	_px4_accel(get_device_id(), config.rotation),
-	_px4_gyro(get_device_id(), config.rotation)
+	_rotation(config.rotation)
 {
-	if (_drdy_gpio != 0) {
-		_drdy_missed_perf = perf_alloc(PC_COUNT, MODULE_NAME": DRDY missed");
-	}
+	int32_t imu_gyro_rate_max = 400;
+	param_get(param_find("IMU_GYRO_RATEMAX"), &imu_gyro_rate_max);
 
-	ConfigureSampleRate(_px4_gyro.get_max_rate_hz());
+	ConfigureSampleRate(imu_gyro_rate_max);
 }
 
 ICM20649::~ICM20649()
@@ -61,7 +60,6 @@ ICM20649::~ICM20649()
 	perf_free(_fifo_empty_perf);
 	perf_free(_fifo_overflow_perf);
 	perf_free(_fifo_reset_perf);
-	perf_free(_drdy_missed_perf);
 }
 
 int ICM20649::init()
@@ -79,18 +77,11 @@ int ICM20649::init()
 bool ICM20649::Reset()
 {
 	_state = STATE::RESET;
-	DataReadyInterruptDisable();
 	ScheduleClear();
 	ScheduleNow();
 	return true;
 }
 
-void ICM20649::exit_and_cleanup()
-{
-	DataReadyInterruptDisable();
-	I2CSPIDriverBase::exit_and_cleanup();
-}
-
 void ICM20649::print_status()
 {
 	I2CSPIDriverBase::print_status();
@@ -102,13 +93,13 @@ void ICM20649::print_status()
 	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 ICM20649::probe()
 {
-	for (int i = 0; i < 3; i++) {
-		uint8_t whoami = RegisterRead(Register::BANK_0::WHO_AM_I);
+	// 3 retries
+	for (int retry = 0; retry < 3; retry++) {
+		const uint8_t whoami = RegisterRead(Register::BANK_0::WHO_AM_I);
 
 		if (whoami == WHOAMI) {
 			return PX4_OK;
@@ -175,19 +166,12 @@ void ICM20649::RunImpl()
 
 	case STATE::CONFIGURE:
 		if (Configure()) {
+			_temperature = ReadTemperature();
+
 			// 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 {
-				_data_ready_interrupt_enabled = false;
-				ScheduleOnInterval(_fifo_empty_interval_us, _fifo_empty_interval_us);
-			}
+			ScheduleOnInterval(_fifo_empty_interval_us, _fifo_empty_interval_us);
 
 			FIFOReset();
 
@@ -207,23 +191,6 @@ void ICM20649::RunImpl()
 		break;
 
 	case STATE::FIFO_READ: {
-			hrt_abstime timestamp_sample = now;
-
-			if (_data_ready_interrupt_enabled) {
-				// scheduled from interrupt if _drdy_timestamp_sample was set as expected
-				const hrt_abstime drdy_timestamp_sample = _drdy_timestamp_sample.fetch_and(0);
-
-				if ((now - drdy_timestamp_sample) < _fifo_empty_interval_us) {
-					timestamp_sample = drdy_timestamp_sample;
-
-				} else {
-					perf_count(_drdy_missed_perf);
-				}
-
-				// push backup schedule back
-				ScheduleDelayed(_fifo_empty_interval_us * 2);
-			}
-
 			// always check current FIFO count
 			bool success = false;
 			const uint16_t fifo_count = FIFOReadCount();
@@ -241,7 +208,7 @@ void ICM20649::RunImpl()
 
 				if (samples > _fifo_gyro_samples) {
 					// grab desired number of samples, but reschedule next cycle sooner
-					int extra_samples = samples - _fifo_gyro_samples;
+					const int extra_samples = samples - _fifo_gyro_samples;
 					samples = _fifo_gyro_samples;
 
 					if (_fifo_gyro_samples > extra_samples) {
@@ -260,7 +227,7 @@ void ICM20649::RunImpl()
 				}
 
 				if (samples == _fifo_gyro_samples) {
-					if (FIFORead(timestamp_sample, samples)) {
+					if (FIFORead(now, samples)) {
 						success = true;
 
 						if (_failure_count > 0) {
@@ -275,13 +242,14 @@ void ICM20649::RunImpl()
 
 				// full reset if things are failing consistently
 				if (_failure_count > 10) {
+					PX4_DEBUG("Full reset because things are failing consistently");
 					Reset();
 					return;
 				}
 			}
 
+			// check configuration registers periodically or immediately following any failure
 			if (!success || hrt_elapsed_time(&_last_config_check_timestamp) > 100_ms) {
-				// check configuration registers periodically or immediately following any failure
 				if (RegisterCheck(_register_bank0_cfg[_checked_register_bank0])
 				    && RegisterCheck(_register_bank2_cfg[_checked_register_bank2])
 				   ) {
@@ -298,7 +266,7 @@ void ICM20649::RunImpl()
 			} else {
 				// periodically update temperature (~1 Hz)
 				if (hrt_elapsed_time(&_temperature_update_timestamp) >= 1_s) {
-					UpdateTemperature();
+					_temperature = ReadTemperature();
 					_temperature_update_timestamp = now;
 				}
 			}
@@ -308,65 +276,10 @@ void ICM20649::RunImpl()
 	}
 }
 
-void ICM20649::ConfigureAccel()
-{
-	const uint8_t ACCEL_FS_SEL = RegisterRead(Register::BANK_2::ACCEL_CONFIG) & (Bit2 | Bit1); // 2:1 ACCEL_FS_SEL[1:0]
-
-	switch (ACCEL_FS_SEL) {
-	case ACCEL_FS_SEL_4G:
-		_px4_accel.set_scale(CONSTANTS_ONE_G / 8192.f);
-		_px4_accel.set_range(4.f * CONSTANTS_ONE_G);
-		break;
-
-	case ACCEL_FS_SEL_8G:
-		_px4_accel.set_scale(CONSTANTS_ONE_G / 4096.f);
-		_px4_accel.set_range(8.f * CONSTANTS_ONE_G);
-		break;
-
-	case ACCEL_FS_SEL_16G:
-		_px4_accel.set_scale(CONSTANTS_ONE_G / 2048.f);
-		_px4_accel.set_range(16.f * CONSTANTS_ONE_G);
-		break;
-
-	case ACCEL_FS_SEL_30G:
-		_px4_accel.set_scale(CONSTANTS_ONE_G / 1024.f);
-		_px4_accel.set_range(30.f * CONSTANTS_ONE_G);
-		break;
-	}
-}
-
-void ICM20649::ConfigureGyro()
-{
-	const uint8_t GYRO_FS_SEL = RegisterRead(Register::BANK_2::GYRO_CONFIG_1) & (Bit2 | Bit1); // 2:1 GYRO_FS_SEL[1:0]
-
-	float range_dps = 0.f;
-
-	switch (GYRO_FS_SEL) {
-	case GYRO_FS_SEL_500_DPS:
-		range_dps = 500.f;
-		break;
-
-	case GYRO_FS_SEL_1000_DPS:
-		range_dps = 1000.f;
-		break;
-
-	case GYRO_FS_SEL_2000_DPS:
-		range_dps = 2000.f;
-		break;
-
-	case GYRO_FS_SEL_4000_DPS:
-		range_dps = 4000.f;
-		break;
-	}
-
-	_px4_gyro.set_scale(math::radians(range_dps / 32768.f));
-	_px4_gyro.set_range(math::radians(range_dps));
-}
-
 void ICM20649::ConfigureSampleRate(int sample_rate)
 {
-	// round down to nearest FIFO sample dt * SAMPLES_PER_TRANSFER
-	const float min_interval = FIFO_SAMPLE_DT * SAMPLES_PER_TRANSFER;
+	// 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));
@@ -415,57 +328,9 @@ bool ICM20649::Configure()
 		}
 	}
 
-	ConfigureAccel();
-	ConfigureGyro();
-
 	return success;
 }
 
-int ICM20649::DataReadyInterruptCallback(int irq, void *context, void *arg)
-{
-	static_cast<ICM20649 *>(arg)->DataReady();
-	return 0;
-}
-
-void ICM20649::DataReady()
-{
-	// at least the required number of samples in the FIFO
-	if (++_drdy_count >= _fifo_gyro_samples) {
-		_drdy_timestamp_sample.store(hrt_absolute_time());
-		_drdy_count -= _fifo_gyro_samples;
-		ScheduleNow();
-	}
-}
-
-bool ICM20649::DataReadyInterruptConfigure()
-{
-	// TODO: enable data ready interrupt
-	return false;
-#if 0
-
-	if (_drdy_gpio == 0) {
-		return false;
-	}
-
-	// Setup data ready on falling edge
-	return px4_arch_gpiosetevent(_drdy_gpio, false, true, true, &DataReadyInterruptCallback, this) == 0;
-#endif
-}
-
-bool ICM20649::DataReadyInterruptDisable()
-{
-	// TODO: enable data ready interrupt
-	return false;
-#if 0
-
-	if (_drdy_gpio == 0) {
-		return false;
-	}
-
-	return px4_arch_gpiosetevent(_drdy_gpio, false, false, false, nullptr, nullptr) == 0;
-#endif
-}
-
 template <typename T>
 bool ICM20649::RegisterCheck(const T &reg_cfg)
 {
@@ -520,54 +385,89 @@ uint16_t ICM20649::FIFOReadCount()
 {
 	SelectRegisterBank(REG_BANK_SEL_BIT::USER_BANK_0);
 
-	// read FIFO count
-	uint8_t fifo_count_buf[3] {};
-	fifo_count_buf[0] = static_cast<uint8_t>(Register::BANK_0::FIFO_COUNTH) | DIR_READ;
+	// transfer buffer
+	struct TransferBuffer {
+		uint8_t cmd{static_cast<uint8_t>(Register::BANK_0::FIFO_COUNTH) | DIR_READ};
+		uint8_t FIFO_COUNTH{0};
+		uint8_t FIFO_COUNTL{0};
+	} buffer{};
 
-	if (transfer(fifo_count_buf, fifo_count_buf, sizeof(fifo_count_buf)) != PX4_OK) {
+	// read FIFO count
+	if (transfer((uint8_t *)&buffer, (uint8_t *)&buffer, sizeof(buffer)) != PX4_OK) {
 		perf_count(_bad_transfer_perf);
 		return 0;
 	}
 
-	return combine(fifo_count_buf[1], fifo_count_buf[2]);
+	return (buffer.FIFO_COUNTH << 8) + buffer.FIFO_COUNTL;
 }
 
 bool ICM20649::FIFORead(const hrt_abstime &timestamp_sample, uint8_t samples)
 {
 	SelectRegisterBank(REG_BANK_SEL_BIT::USER_BANK_0);
 
-	FIFOTransferBuffer buffer{};
-	const size_t transfer_size = math::min(samples * sizeof(FIFO::DATA) + 3, FIFO::SIZE);
+	// FIFO transfer buffer
+	struct TransferBuffer {
+		uint8_t cmd{static_cast<uint8_t>(Register::BANK_0::FIFO_COUNTH) | DIR_READ};
+		uint8_t FIFO_COUNTH{0};
+		uint8_t FIFO_COUNTL{0};
+		FIFO::DATA f[FIFO_MAX_SAMPLES] {};
+	} buffer{};
+
+	// cmd + FIFO_COUNTH + FIFO_COUNTL + samples (FIFO::DATA)
+	const size_t transfer_size = 3 + math::min(samples * sizeof(FIFO::DATA), FIFO::SIZE);
 
 	if (transfer((uint8_t *)&buffer, (uint8_t *)&buffer, transfer_size) != PX4_OK) {
 		perf_count(_bad_transfer_perf);
 		return false;
 	}
 
+	const uint16_t fifo_count_bytes = (buffer.FIFO_COUNTH << 8) + buffer.FIFO_COUNTL;
+	const uint8_t fifo_count_samples = fifo_count_bytes / sizeof(FIFO::DATA);
 
-	const uint16_t fifo_count_bytes = combine(buffer.FIFO_COUNTH, buffer.FIFO_COUNTL);
+	const uint8_t valid_samples = math::min(samples, fifo_count_samples);
 
-	if (fifo_count_bytes >= FIFO::SIZE) {
+	if ((fifo_count_bytes >= FIFO::SIZE) || (valid_samples > FIFO_MAX_SAMPLES)) {
 		perf_count(_fifo_overflow_perf);
 		FIFOReset();
 		return false;
-	}
 
-	const uint8_t fifo_count_samples = fifo_count_bytes / sizeof(FIFO::DATA);
-
-	if (fifo_count_samples == 0) {
+	} else if (fifo_count_samples == 0) {
 		perf_count(_fifo_empty_perf);
 		return false;
 	}
 
-	const uint16_t valid_samples = math::min(samples, fifo_count_samples);
-
 	if (valid_samples > 0) {
-		ProcessGyro(timestamp_sample, buffer.f, valid_samples);
+		sensor_imu_fifo_s sensor_imu_fifo{};
+		sensor_imu_fifo.timestamp_sample = timestamp_sample;
+		sensor_imu_fifo.device_id = get_device_id();
+		sensor_imu_fifo.dt = FIFO_SAMPLE_DT;
+		sensor_imu_fifo.samples = valid_samples;
+		sensor_imu_fifo.accel_scale = ACCEL_SCALE;
+		sensor_imu_fifo.gyro_scale = GYRO_SCALE;
 
-		if (ProcessAccel(timestamp_sample, buffer.f, valid_samples)) {
-			return true;
+		for (int i = 0; i < valid_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)
+			sensor_imu_fifo.accel_x[i] =              combine(buffer.f[i].ACCEL_XOUT_H, buffer.f[i].ACCEL_XOUT_L);
+			sensor_imu_fifo.accel_y[i] = math::negate(combine(buffer.f[i].ACCEL_YOUT_H, buffer.f[i].ACCEL_YOUT_L));
+			sensor_imu_fifo.accel_z[i] = math::negate(combine(buffer.f[i].ACCEL_ZOUT_H, buffer.f[i].ACCEL_ZOUT_L));
+			rotate_3i(_rotation, sensor_imu_fifo.accel_x[i], sensor_imu_fifo.accel_y[i], sensor_imu_fifo.accel_z[i]);
+
+			sensor_imu_fifo.gyro_x[i] =              combine(buffer.f[i].GYRO_XOUT_H, buffer.f[i].GYRO_XOUT_L);
+			sensor_imu_fifo.gyro_y[i] = math::negate(combine(buffer.f[i].GYRO_YOUT_H, buffer.f[i].GYRO_YOUT_L));
+			sensor_imu_fifo.gyro_z[i] = math::negate(combine(buffer.f[i].GYRO_ZOUT_H, buffer.f[i].GYRO_ZOUT_L));
+			rotate_3i(_rotation, sensor_imu_fifo.gyro_x[i], sensor_imu_fifo.gyro_y[i], sensor_imu_fifo.gyro_z[i]);
 		}
+
+		sensor_imu_fifo.temperature = (hrt_elapsed_time(&_temperature_update_timestamp) < 5_s) ? _temperature : NAN;
+
+		sensor_imu_fifo.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);
+
+		sensor_imu_fifo.timestamp = hrt_absolute_time();
+		_sensor_imu_fifo_pub.publish(sensor_imu_fifo);
+
+		return true;
 	}
 
 	return false;
@@ -580,112 +480,34 @@ void ICM20649::FIFOReset()
 	// FIFO_RST: reset FIFO
 	RegisterSetBits(Register::BANK_0::FIFO_RST, FIFO_RST_BIT::FIFO_RESET);
 	RegisterClearBits(Register::BANK_0::FIFO_RST, FIFO_RST_BIT::FIFO_RESET);
-
-	// reset while FIFO is disabled
-	_drdy_count = 0;
-	_drdy_timestamp_sample.store(0);
 }
 
-static bool fifo_accel_equal(const FIFO::DATA &f0, const FIFO::DATA &f1)
+float ICM20649::ReadTemperature()
 {
-	return (memcmp(&f0.ACCEL_XOUT_H, &f1.ACCEL_XOUT_H, 6) == 0);
-}
+	// transfer buffer
+	struct TransferBuffer {
+		uint8_t cmd{static_cast<uint8_t>(Register::BANK_0::TEMP_OUT_H) | DIR_READ};
+		uint8_t TEMP_OUT_H{0};
+		uint8_t TEMP_OUT_L{0};
+	} buffer{};
 
-bool ICM20649::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 * SAMPLES_PER_TRANSFER;
-
-	bool bad_data = false;
-
-	// accel data is doubled in FIFO, but might be shifted
-	int accel_first_sample = 1;
-
-	if (samples >= 4) {
-		if (fifo_accel_equal(fifo[0], fifo[1]) && fifo_accel_equal(fifo[2], fifo[3])) {
-			// [A0, A1, A2, A3]
-			//  A0==A1, A2==A3
-			accel_first_sample = 1;
-
-		} else if (fifo_accel_equal(fifo[1], fifo[2])) {
-			// [A0, A1, A2, A3]
-			//  A0, A1==A2, A3
-			accel_first_sample = 0;
-
-		} else {
-			// no matching accel samples is an error
-			bad_data = true;
-			perf_count(_bad_transfer_perf);
-		}
-	}
-
-	for (int i = accel_first_sample; i < samples; i = i + SAMPLES_PER_TRANSFER) {
-		int16_t accel_x = combine(fifo[i].ACCEL_XOUT_H, fifo[i].ACCEL_XOUT_L);
-		int16_t accel_y = combine(fifo[i].ACCEL_YOUT_H, fifo[i].ACCEL_YOUT_L);
-		int16_t accel_z = combine(fifo[i].ACCEL_ZOUT_H, fifo[i].ACCEL_ZOUT_L);
-
-		// 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[accel.samples] = accel_x;
-		accel.y[accel.samples] = (accel_y == INT16_MIN) ? INT16_MAX : -accel_y;
-		accel.z[accel.samples] = (accel_z == INT16_MIN) ? INT16_MAX : -accel_z;
-		accel.samples++;
-	}
-
-	_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) {
-		_px4_accel.updateFIFO(accel);
-	}
-
-	return !bad_data;
-}
-
-void ICM20649::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 = samples;
-	gyro.dt = FIFO_SAMPLE_DT;
-
-	for (int i = 0; i < samples; i++) {
-		const int16_t gyro_x = combine(fifo[i].GYRO_XOUT_H, fifo[i].GYRO_XOUT_L);
-		const int16_t gyro_y = combine(fifo[i].GYRO_YOUT_H, fifo[i].GYRO_YOUT_L);
-		const int16_t gyro_z = combine(fifo[i].GYRO_ZOUT_H, fifo[i].GYRO_ZOUT_L);
-
-		// 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;
-		gyro.y[i] = (gyro_y == INT16_MIN) ? INT16_MAX : -gyro_y;
-		gyro.z[i] = (gyro_z == INT16_MIN) ? INT16_MAX : -gyro_z;
-	}
-
-	_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));
-
-	_px4_gyro.updateFIFO(gyro);
-}
-
-void ICM20649::UpdateTemperature()
-{
 	// read current temperature
-	uint8_t temperature_buf[3] {};
-	temperature_buf[0] = static_cast<uint8_t>(Register::BANK_0::TEMP_OUT_H) | DIR_READ;
 	SelectRegisterBank(REG_BANK_SEL_BIT::USER_BANK_0);
 
-	if (transfer(temperature_buf, temperature_buf, sizeof(temperature_buf)) != PX4_OK) {
+	if (transfer((uint8_t *)&buffer, (uint8_t *)&buffer, sizeof(buffer)) != PX4_OK) {
 		perf_count(_bad_transfer_perf);
-		return;
+		return NAN;
 	}
 
-	const int16_t TEMP_OUT = combine(temperature_buf[1], temperature_buf[2]);
+	const int16_t TEMP_OUT = combine(buffer.TEMP_OUT_H, buffer.TEMP_OUT_L);
 	const float TEMP_degC = (TEMP_OUT / TEMPERATURE_SENSITIVITY) + TEMPERATURE_OFFSET;
 
-	if (PX4_ISFINITE(TEMP_degC)) {
-		_px4_accel.set_temperature(TEMP_degC);
-		_px4_gyro.set_temperature(TEMP_degC);
+	if (PX4_ISFINITE(TEMP_degC)
+	    && (TEMP_degC >= TEMPERATURE_SENSOR_MIN)
+	    && (TEMP_degC <= TEMPERATURE_SENSOR_MAX)) {
+
+		return TEMP_degC;
 	}
+
+	return NAN;
 }
diff --git a/src/drivers/imu/invensense/icm20649/ICM20649.hpp b/src/drivers/imu/invensense/icm20649/ICM20649.hpp
index 531f0be988..8f36537026 100644
--- a/src/drivers/imu/invensense/icm20649/ICM20649.hpp
+++ b/src/drivers/imu/invensense/icm20649/ICM20649.hpp
@@ -1,6 +1,6 @@
 /****************************************************************************
  *
- *   Copyright (c) 2020-2021 PX4 Development Team. All rights reserved.
+ *   Copyright (c) 2020-2022 PX4 Development Team. All rights reserved.
  *
  * Redistribution and use in source and binary forms, with or without
  * modification, are permitted provided that the following conditions
@@ -43,14 +43,15 @@
 #include "InvenSense_ICM20649_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/PublicationMulti.hpp>
+#include <uORB/topics/sensor_imu_fifo.h>
+
 using namespace InvenSense_ICM20649;
 
 class ICM20649 : public device::SPI, public I2CSPIDriver<ICM20649>
@@ -67,26 +68,15 @@ public:
 	void print_status() override;
 
 private:
-	void exit_and_cleanup() override;
-
 	// Sensor Configuration
 	static constexpr float FIFO_SAMPLE_DT{1e6f / 9000.f};
-	static constexpr int32_t SAMPLES_PER_TRANSFER{2};                    // ensure at least 1 new accel sample per transfer
 	static constexpr float GYRO_RATE{1e6f / FIFO_SAMPLE_DT};             // 9000 Hz gyro
-	static constexpr float ACCEL_RATE{GYRO_RATE / SAMPLES_PER_TRANSFER}; // 4500 Hz accel
 
 	// maximum FIFO samples per transfer is limited to the size of sensor_accel_fifo/sensor_gyro_fifo
-	static constexpr int32_t FIFO_MAX_SAMPLES{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 int32_t FIFO_MAX_SAMPLES{math::min(FIFO::SIZE / sizeof(FIFO::DATA), (size_t)sensor_imu_fifo_s::FIFO_SIZE)};
 
-	// Transfer data
-	struct FIFOTransferBuffer {
-		uint8_t cmd{static_cast<uint8_t>(Register::BANK_0::FIFO_COUNTH) | DIR_READ};
-		uint8_t FIFO_COUNTH{0};
-		uint8_t FIFO_COUNTL{0};
-		FIFO::DATA f[FIFO_MAX_SAMPLES] {};
-	};
-	// ensure no struct padding
-	static_assert(sizeof(FIFOTransferBuffer) == (3 + FIFO_MAX_SAMPLES *sizeof(FIFO::DATA)));
+	static constexpr float ACCEL_SCALE{CONSTANTS_ONE_G / 1024.f}; // ACCEL_FS_SEL_30G:
+	static constexpr float GYRO_SCALE{math::radians(4000.f / 32768.f)}; // GYRO_FS_SEL_4000_DPS:
 
 	struct register_bank0_config_t {
 		Register::BANK_0 reg;
@@ -105,19 +95,12 @@ private:
 	bool Reset();
 
 	bool Configure();
-	void ConfigureAccel();
-	void ConfigureGyro();
 	void ConfigureSampleRate(int sample_rate);
 
 	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_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);
@@ -129,21 +112,19 @@ private:
 	bool FIFORead(const hrt_abstime &timestamp_sample, uint8_t samples);
 	void FIFOReset();
 
-	bool 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);
-	void UpdateTemperature();
+	float ReadTemperature();
 
-	const spi_drdy_gpio_t _drdy_gpio;
+	uORB::PublicationMulti<sensor_imu_fifo_s> _sensor_imu_fifo_pub{ORB_ID(sensor_imu_fifo)};
 
-	PX4Accelerometer _px4_accel;
-	PX4Gyroscope _px4_gyro;
+	const enum Rotation _rotation;
+
+	float _temperature{NAN};
 
 	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};
@@ -152,10 +133,6 @@ private:
 
 	enum REG_BANK_SEL_BIT _last_register_bank {REG_BANK_SEL_BIT::USER_BANK_0};
 
-	px4::atomic<hrt_abstime> _drdy_timestamp_sample{0};
-	int32_t _drdy_count{0};
-	bool _data_ready_interrupt_enabled{false};
-
 	enum class STATE : uint8_t {
 		RESET,
 		WAIT_FOR_RESET,
diff --git a/src/drivers/imu/invensense/icm20649/InvenSense_ICM20649_registers.hpp b/src/drivers/imu/invensense/icm20649/InvenSense_ICM20649_registers.hpp
index 7ea181f339..e1d4206c0e 100644
--- a/src/drivers/imu/invensense/icm20649/InvenSense_ICM20649_registers.hpp
+++ b/src/drivers/imu/invensense/icm20649/InvenSense_ICM20649_registers.hpp
@@ -1,6 +1,6 @@
 /****************************************************************************
  *
- *   Copyright (c) 2020 PX4 Development Team. All rights reserved.
+ *   Copyright (c) 2020-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
@@ -61,6 +61,8 @@ static constexpr uint8_t WHOAMI = 0xE1;
 
 static constexpr float TEMPERATURE_SENSITIVITY = 333.87f; // LSB/C
 static constexpr float TEMPERATURE_OFFSET = 21.f; // C
+static constexpr float TEMPERATURE_SENSOR_MIN = -40.f; // °C
+static constexpr float TEMPERATURE_SENSOR_MAX = 85.f; // °C
 
 namespace Register
 {
diff --git a/src/drivers/imu/invensense/icm20689/CMakeLists.txt b/src/drivers/imu/invensense/icm20689/CMakeLists.txt
index 34ba239bc0..e7bde33419 100644
--- a/src/drivers/imu/invensense/icm20689/CMakeLists.txt
+++ b/src/drivers/imu/invensense/icm20689/CMakeLists.txt
@@ -41,7 +41,5 @@ px4_add_module(
 		icm20689_main.cpp
 		InvenSense_ICM20689_registers.hpp
 	DEPENDS
-		drivers_accelerometer
-		drivers_gyroscope
 		px4_work_queue
 	)
diff --git a/src/drivers/imu/invensense/icm20689/ICM20689.cpp b/src/drivers/imu/invensense/icm20689/ICM20689.cpp
index 8337ba2cc4..5eb29d5668 100644
--- a/src/drivers/imu/invensense/icm20689/ICM20689.cpp
+++ b/src/drivers/imu/invensense/icm20689/ICM20689.cpp
@@ -1,6 +1,6 @@
 /****************************************************************************
  *
- *   Copyright (c) 2020-2021 PX4 Development Team. All rights reserved.
+ *   Copyright (c) 2020-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
@@ -33,6 +33,8 @@
 
 #include "ICM20689.hpp"
 
+#include <lib/parameters/param.h>
+
 using namespace time_literals;
 
 static constexpr int16_t combine(uint8_t msb, uint8_t lsb)
@@ -44,14 +46,16 @@ ICM20689::ICM20689(const I2CSPIDriverConfig &config) :
 	SPI(config),
 	I2CSPIDriver(config),
 	_drdy_gpio(config.drdy_gpio),
-	_px4_accel(get_device_id(), config.rotation),
-	_px4_gyro(get_device_id(), config.rotation)
+	_rotation(config.rotation)
 {
 	if (_drdy_gpio != 0) {
 		_drdy_missed_perf = perf_alloc(PC_COUNT, MODULE_NAME": DRDY missed");
 	}
 
-	ConfigureSampleRate(_px4_gyro.get_max_rate_hz());
+	int32_t imu_gyro_rate_max = 400;
+	param_get(param_find("IMU_GYRO_RATEMAX"), &imu_gyro_rate_max);
+
+	ConfigureSampleRate(imu_gyro_rate_max);
 }
 
 ICM20689::~ICM20689()
@@ -108,7 +112,7 @@ void ICM20689::print_status()
 bool ICM20689::StoreCheckedRegisterValue(Register reg)
 {
 	// 3 retries
-	for (int i = 0; i < 3; i++) {
+	for (int retry = 0; retry < 3; retry++) {
 		uint8_t read1 = RegisterRead(reg);
 		uint8_t read2 = RegisterRead(reg);
 
@@ -131,14 +135,19 @@ bool ICM20689::StoreCheckedRegisterValue(Register reg)
 
 int ICM20689::probe()
 {
-	const uint8_t whoami = RegisterRead(Register::WHO_AM_I);
+	// 3 retries
+	for (int retry = 0; retry < 3; retry++) {
+		const uint8_t whoami = RegisterRead(Register::WHO_AM_I);
 
-	if (whoami != WHOAMI) {
-		DEVICE_DEBUG("unexpected WHO_AM_I 0x%02x", whoami);
-		return PX4_ERROR;
+		if (whoami == WHOAMI) {
+			return PX4_OK;
+
+		} else {
+			DEVICE_DEBUG("unexpected WHO_AM_I 0x%02x", whoami);
+		}
 	}
 
-	return PX4_OK;
+	return PX4_ERROR;
 }
 
 void ICM20689::RunImpl()
@@ -187,8 +196,8 @@ void ICM20689::RunImpl()
 				ScheduleDelayed(100_ms);
 
 			} else {
-				PX4_DEBUG("Reset not complete, check again in 10 ms");
-				ScheduleDelayed(10_ms);
+				PX4_DEBUG("Reset not complete, check again in 100 ms");
+				ScheduleDelayed(100_ms);
 			}
 		}
 
@@ -196,6 +205,8 @@ void ICM20689::RunImpl()
 
 	case STATE::CONFIGURE:
 		if (Configure()) {
+			_temperature = ReadTemperature();
+
 			// if configure succeeded then start reading from FIFO
 			_state = STATE::FIFO_READ;
 
@@ -234,7 +245,7 @@ void ICM20689::RunImpl()
 				// scheduled from interrupt if _drdy_timestamp_sample was set as expected
 				const hrt_abstime drdy_timestamp_sample = _drdy_timestamp_sample.fetch_and(0);
 
-				if ((now - drdy_timestamp_sample) < _fifo_empty_interval_us) {
+				if (now < drdy_timestamp_sample + _fifo_empty_interval_us) {
 					timestamp_sample = drdy_timestamp_sample;
 
 				} else {
@@ -271,7 +282,7 @@ void ICM20689::RunImpl()
 					FIFOReset();
 					perf_count(_fifo_overflow_perf);
 
-				} else if (samples >= SAMPLES_PER_TRANSFER) {
+				} else if (samples >= 1) {
 					if (FIFORead(timestamp_sample, samples)) {
 						success = true;
 
@@ -287,6 +298,7 @@ void ICM20689::RunImpl()
 
 				// full reset if things are failing consistently
 				if (_failure_count > 10) {
+					PX4_DEBUG("Full reset because things are failing consistently");
 					Reset();
 					return;
 				}
@@ -301,13 +313,14 @@ void ICM20689::RunImpl()
 				} else {
 					// register check failed, force reset
 					perf_count(_bad_register_perf);
+					PX4_DEBUG("Force reset because register 0x%02hhX check failed ", (uint8_t)_register_cfg[_checked_register].reg);
 					Reset();
 				}
 
 			} else {
 				// periodically update temperature (~1 Hz)
 				if (hrt_elapsed_time(&_temperature_update_timestamp) >= 1_s) {
-					UpdateTemperature();
+					_temperature = ReadTemperature();
 					_temperature_update_timestamp = now;
 				}
 			}
@@ -317,65 +330,10 @@ void ICM20689::RunImpl()
 	}
 }
 
-void ICM20689::ConfigureAccel()
-{
-	const uint8_t ACCEL_FS_SEL = RegisterRead(Register::ACCEL_CONFIG) & (Bit4 | Bit3); // [4:3] ACCEL_FS_SEL[1:0]
-
-	switch (ACCEL_FS_SEL) {
-	case ACCEL_FS_SEL_2G:
-		_px4_accel.set_scale(CONSTANTS_ONE_G / 16384.f);
-		_px4_accel.set_range(2.f * CONSTANTS_ONE_G);
-		break;
-
-	case ACCEL_FS_SEL_4G:
-		_px4_accel.set_scale(CONSTANTS_ONE_G / 8192.f);
-		_px4_accel.set_range(4.f * CONSTANTS_ONE_G);
-		break;
-
-	case ACCEL_FS_SEL_8G:
-		_px4_accel.set_scale(CONSTANTS_ONE_G / 4096.f);
-		_px4_accel.set_range(8.f * CONSTANTS_ONE_G);
-		break;
-
-	case ACCEL_FS_SEL_16G:
-		_px4_accel.set_scale(CONSTANTS_ONE_G / 2048.f);
-		_px4_accel.set_range(16.f * CONSTANTS_ONE_G);
-		break;
-	}
-}
-
-void ICM20689::ConfigureGyro()
-{
-	const uint8_t FS_SEL = RegisterRead(Register::GYRO_CONFIG) & (Bit4 | Bit3); // [4:3] FS_SEL[1:0]
-
-	float range_dps = 0.f;
-
-	switch (FS_SEL) {
-	case FS_SEL_250_DPS:
-		range_dps = 250.f;
-		break;
-
-	case FS_SEL_500_DPS:
-		range_dps = 500.f;
-		break;
-
-	case FS_SEL_1000_DPS:
-		range_dps = 1000.f;
-		break;
-
-	case FS_SEL_2000_DPS:
-		range_dps = 2000.f;
-		break;
-	}
-
-	_px4_gyro.set_scale(math::radians(range_dps / 32768.f));
-	_px4_gyro.set_range(math::radians(range_dps));
-}
-
 void ICM20689::ConfigureSampleRate(int sample_rate)
 {
-	// round down to nearest FIFO sample dt * SAMPLES_PER_TRANSFER
-	const float min_interval = FIFO_SAMPLE_DT * SAMPLES_PER_TRANSFER;
+	// 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));
@@ -400,9 +358,6 @@ bool ICM20689::Configure()
 		}
 	}
 
-	ConfigureAccel();
-	ConfigureGyro();
-
 	return success;
 }
 
@@ -429,7 +384,7 @@ bool ICM20689::DataReadyInterruptConfigure()
 	}
 
 	// Setup data ready on falling edge
-	return px4_arch_gpiosetevent(_drdy_gpio, false, true, true, &DataReadyInterruptCallback, this) == 0;
+	return (px4_arch_gpiosetevent(_drdy_gpio, false, true, false, &DataReadyInterruptCallback, this) == 0);
 }
 
 bool ICM20689::DataReadyInterruptDisable()
@@ -438,7 +393,7 @@ bool ICM20689::DataReadyInterruptDisable()
 		return false;
 	}
 
-	return px4_arch_gpiosetevent(_drdy_gpio, false, false, false, nullptr, nullptr) == 0;
+	return (px4_arch_gpiosetevent(_drdy_gpio, false, false, false, nullptr, nullptr) == 0);
 }
 
 bool ICM20689::RegisterCheck(const register_config_t &reg_cfg)
@@ -487,31 +442,75 @@ void ICM20689::RegisterSetAndClearBits(Register reg, uint8_t setbits, uint8_t cl
 
 uint16_t ICM20689::FIFOReadCount()
 {
-	// read FIFO count
-	uint8_t fifo_count_buf[3] {};
-	fifo_count_buf[0] = static_cast<uint8_t>(Register::FIFO_COUNTH) | DIR_READ;
+	// transfer buffer
+	struct TransferBuffer {
+		uint8_t cmd{static_cast<uint8_t>(Register::FIFO_COUNTH) | DIR_READ};
+		uint8_t FIFO_COUNTH{0};
+		uint8_t FIFO_COUNTL{0};
+	} buffer{};
 
-	if (transfer(fifo_count_buf, fifo_count_buf, sizeof(fifo_count_buf)) != PX4_OK) {
+	// read FIFO count
+	if (transfer((uint8_t *)&buffer, (uint8_t *)&buffer, sizeof(buffer)) != PX4_OK) {
 		perf_count(_bad_transfer_perf);
 		return 0;
 	}
 
-	return combine(fifo_count_buf[1], fifo_count_buf[2]);
+	return (buffer.FIFO_COUNTH << 8) + buffer.FIFO_COUNTL;
 }
 
 bool ICM20689::FIFORead(const hrt_abstime &timestamp_sample, uint8_t samples)
 {
-	FIFOTransferBuffer buffer{};
-	const size_t transfer_size = math::min(samples * sizeof(FIFO::DATA) + 1, FIFO::SIZE);
+	// FIFO transfer buffer
+	struct FIFOTransferBuffer {
+		uint8_t cmd{static_cast<uint8_t>(Register::FIFO_R_W) | DIR_READ};
+		FIFO::DATA f[FIFO_MAX_SAMPLES] {};
+	} buffer{};
+
+	// cmd + samples (FIFO::DATA)
+	const size_t transfer_size = 1 + math::min(samples * sizeof(FIFO::DATA), FIFO::SIZE);
 
 	if (transfer((uint8_t *)&buffer, (uint8_t *)&buffer, transfer_size) != PX4_OK) {
 		perf_count(_bad_transfer_perf);
 		return false;
 	}
 
+	const uint8_t valid_samples = samples;
 
-	ProcessGyro(timestamp_sample, buffer.f, samples);
-	return ProcessAccel(timestamp_sample, buffer.f, samples);
+	if (valid_samples > 0) {
+		sensor_imu_fifo_s sensor_imu_fifo{};
+		sensor_imu_fifo.timestamp_sample = timestamp_sample;
+		sensor_imu_fifo.device_id = get_device_id();
+		sensor_imu_fifo.dt = FIFO_SAMPLE_DT;
+		sensor_imu_fifo.samples = valid_samples;
+		sensor_imu_fifo.accel_scale = ACCEL_SCALE;
+		sensor_imu_fifo.gyro_scale = GYRO_SCALE;
+
+		for (int i = 0; i < valid_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)
+			sensor_imu_fifo.accel_x[i] =              combine(buffer.f[i].ACCEL_XOUT_H, buffer.f[i].ACCEL_XOUT_L);
+			sensor_imu_fifo.accel_y[i] = math::negate(combine(buffer.f[i].ACCEL_YOUT_H, buffer.f[i].ACCEL_YOUT_L));
+			sensor_imu_fifo.accel_z[i] = math::negate(combine(buffer.f[i].ACCEL_ZOUT_H, buffer.f[i].ACCEL_ZOUT_L));
+			rotate_3i(_rotation, sensor_imu_fifo.accel_x[i], sensor_imu_fifo.accel_y[i], sensor_imu_fifo.accel_z[i]);
+
+			sensor_imu_fifo.gyro_x[i] =              combine(buffer.f[i].GYRO_XOUT_H, buffer.f[i].GYRO_XOUT_L);
+			sensor_imu_fifo.gyro_y[i] = math::negate(combine(buffer.f[i].GYRO_YOUT_H, buffer.f[i].GYRO_YOUT_L));
+			sensor_imu_fifo.gyro_z[i] = math::negate(combine(buffer.f[i].GYRO_ZOUT_H, buffer.f[i].GYRO_ZOUT_L));
+			rotate_3i(_rotation, sensor_imu_fifo.gyro_x[i], sensor_imu_fifo.gyro_y[i], sensor_imu_fifo.gyro_z[i]);
+		}
+
+		sensor_imu_fifo.temperature = (hrt_elapsed_time(&_temperature_update_timestamp) < 5_s) ? _temperature : NAN;
+
+		sensor_imu_fifo.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);
+
+		sensor_imu_fifo.timestamp = hrt_absolute_time();
+		_sensor_imu_fifo_pub.publish(sensor_imu_fifo);
+
+		return true;
+	}
+
+	return false;
 }
 
 void ICM20689::FIFOReset()
@@ -537,105 +536,30 @@ void ICM20689::FIFOReset()
 	}
 }
 
-static bool fifo_accel_equal(const FIFO::DATA &f0, const FIFO::DATA &f1)
+float ICM20689::ReadTemperature()
 {
-	return (memcmp(&f0.ACCEL_XOUT_H, &f1.ACCEL_XOUT_H, 6) == 0);
-}
+	// transfer buffer
+	struct TransferBuffer {
+		uint8_t cmd{static_cast<uint8_t>(Register::TEMP_OUT_H) | DIR_READ};
+		uint8_t TEMP_OUT_H{0};
+		uint8_t TEMP_OUT_L{0};
+	} buffer{};
 
-bool ICM20689::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 * SAMPLES_PER_TRANSFER;
-
-	bool bad_data = false;
-
-	// accel data is doubled in FIFO, but might be shifted
-	int accel_first_sample = 1;
-
-	if (samples >= 4) {
-		if (fifo_accel_equal(fifo[0], fifo[1]) && fifo_accel_equal(fifo[2], fifo[3])) {
-			// [A0, A1, A2, A3]
-			//  A0==A1, A2==A3
-			accel_first_sample = 1;
-
-		} else if (fifo_accel_equal(fifo[1], fifo[2])) {
-			// [A0, A1, A2, A3]
-			//  A0, A1==A2, A3
-			accel_first_sample = 0;
-
-		} else {
-			// no matching accel samples is an error
-			bad_data = true;
-			perf_count(_bad_transfer_perf);
-		}
-	}
-
-	for (int i = accel_first_sample; i < samples; i = i + SAMPLES_PER_TRANSFER) {
-		int16_t accel_x = combine(fifo[i].ACCEL_XOUT_H, fifo[i].ACCEL_XOUT_L);
-		int16_t accel_y = combine(fifo[i].ACCEL_YOUT_H, fifo[i].ACCEL_YOUT_L);
-		int16_t accel_z = combine(fifo[i].ACCEL_ZOUT_H, fifo[i].ACCEL_ZOUT_L);
-
-		// 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[accel.samples] = accel_x;
-		accel.y[accel.samples] = (accel_y == INT16_MIN) ? INT16_MAX : -accel_y;
-		accel.z[accel.samples] = (accel_z == INT16_MIN) ? INT16_MAX : -accel_z;
-		accel.samples++;
-	}
-
-	_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) {
-		_px4_accel.updateFIFO(accel);
-	}
-
-	return !bad_data;
-}
-
-void ICM20689::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 = samples;
-	gyro.dt = FIFO_SAMPLE_DT;
-
-	for (int i = 0; i < samples; i++) {
-		const int16_t gyro_x = combine(fifo[i].GYRO_XOUT_H, fifo[i].GYRO_XOUT_L);
-		const int16_t gyro_y = combine(fifo[i].GYRO_YOUT_H, fifo[i].GYRO_YOUT_L);
-		const int16_t gyro_z = combine(fifo[i].GYRO_ZOUT_H, fifo[i].GYRO_ZOUT_L);
-
-		// 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;
-		gyro.y[i] = (gyro_y == INT16_MIN) ? INT16_MAX : -gyro_y;
-		gyro.z[i] = (gyro_z == INT16_MIN) ? INT16_MAX : -gyro_z;
-	}
-
-	_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));
-
-	_px4_gyro.updateFIFO(gyro);
-}
-
-void ICM20689::UpdateTemperature()
-{
 	// read current temperature
-	uint8_t temperature_buf[3] {};
-	temperature_buf[0] = static_cast<uint8_t>(Register::TEMP_OUT_H) | DIR_READ;
-
-	if (transfer(temperature_buf, temperature_buf, sizeof(temperature_buf)) != PX4_OK) {
+	if (transfer((uint8_t *)&buffer, (uint8_t *)&buffer, sizeof(buffer)) != PX4_OK) {
 		perf_count(_bad_transfer_perf);
-		return;
+		return NAN;
 	}
 
-	const int16_t TEMP_OUT = combine(temperature_buf[1], temperature_buf[2]);
+	const int16_t TEMP_OUT = combine(buffer.TEMP_OUT_H, buffer.TEMP_OUT_L);
 	const float TEMP_degC = (TEMP_OUT / TEMPERATURE_SENSITIVITY) + TEMPERATURE_OFFSET;
 
-	if (PX4_ISFINITE(TEMP_degC)) {
-		_px4_accel.set_temperature(TEMP_degC);
-		_px4_gyro.set_temperature(TEMP_degC);
+	if (PX4_ISFINITE(TEMP_degC)
+	    && (TEMP_degC >= TEMPERATURE_SENSOR_MIN)
+	    && (TEMP_degC <= TEMPERATURE_SENSOR_MAX)) {
+
+		return TEMP_degC;
 	}
+
+	return NAN;
 }
diff --git a/src/drivers/imu/invensense/icm20689/ICM20689.hpp b/src/drivers/imu/invensense/icm20689/ICM20689.hpp
index df3b8a734e..cf2813f880 100644
--- a/src/drivers/imu/invensense/icm20689/ICM20689.hpp
+++ b/src/drivers/imu/invensense/icm20689/ICM20689.hpp
@@ -1,6 +1,6 @@
 /****************************************************************************
  *
- *   Copyright (c) 2020-2021 PX4 Development Team. All rights reserved.
+ *   Copyright (c) 2020-2022 PX4 Development Team. All rights reserved.
  *
  * Redistribution and use in source and binary forms, with or without
  * modification, are permitted provided that the following conditions
@@ -43,14 +43,15 @@
 #include "InvenSense_ICM20689_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/PublicationMulti.hpp>
+#include <uORB/topics/sensor_imu_fifo.h>
+
 using namespace InvenSense_ICM20689;
 
 class ICM20689 : public device::SPI, public I2CSPIDriver<ICM20689>
@@ -71,20 +72,13 @@ private:
 
 	// Sensor Configuration
 	static constexpr float FIFO_SAMPLE_DT{1e6f / 8000.f};
-	static constexpr int32_t SAMPLES_PER_TRANSFER{2};                    // ensure at least 1 new accel sample per transfer
 	static constexpr float GYRO_RATE{1e6f / FIFO_SAMPLE_DT};             // 8000 Hz gyro
-	static constexpr float ACCEL_RATE{GYRO_RATE / SAMPLES_PER_TRANSFER}; // 4000 Hz accel
 
 	// maximum FIFO samples per transfer is limited to the size of sensor_accel_fifo/sensor_gyro_fifo
-	static constexpr int32_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 int32_t FIFO_MAX_SAMPLES{math::min(FIFO::SIZE / sizeof(FIFO::DATA), (size_t)sensor_imu_fifo_s::FIFO_SIZE)};
 
-	// Transfer data
-	struct FIFOTransferBuffer {
-		uint8_t cmd{static_cast<uint8_t>(Register::FIFO_R_W) | DIR_READ};
-		FIFO::DATA f[FIFO_MAX_SAMPLES] {};
-	};
-	// ensure no struct padding
-	static_assert(sizeof(FIFOTransferBuffer) == (1 + FIFO_MAX_SAMPLES *sizeof(FIFO::DATA)));
+	static constexpr float ACCEL_SCALE{CONSTANTS_ONE_G / 2048.f}; // ACCEL_FS_SEL
+	static constexpr float GYRO_SCALE{math::radians(2000.f / 32768.f)}; // FS_SEL
 
 	struct register_config_t {
 		Register reg;
@@ -97,8 +91,6 @@ private:
 	bool Reset();
 
 	bool Configure();
-	void ConfigureAccel();
-	void ConfigureGyro();
 	void ConfigureSampleRate(int sample_rate);
 
 	static int DataReadyInterruptCallback(int irq, void *context, void *arg);
@@ -117,14 +109,15 @@ private:
 	bool FIFORead(const hrt_abstime &timestamp_sample, uint8_t samples);
 	void FIFOReset();
 
-	bool 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);
-	void UpdateTemperature();
+	float ReadTemperature();
 
 	const spi_drdy_gpio_t _drdy_gpio;
 
-	PX4Accelerometer _px4_accel;
-	PX4Gyroscope _px4_gyro;
+	uORB::PublicationMulti<sensor_imu_fifo_s> _sensor_imu_fifo_pub{ORB_ID(sensor_imu_fifo)};
+
+	const enum Rotation _rotation;
+
+	float _temperature{NAN};
 
 	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")};
diff --git a/src/drivers/imu/invensense/icm20689/InvenSense_ICM20689_registers.hpp b/src/drivers/imu/invensense/icm20689/InvenSense_ICM20689_registers.hpp
index 754812d3ea..c83aff323b 100644
--- a/src/drivers/imu/invensense/icm20689/InvenSense_ICM20689_registers.hpp
+++ b/src/drivers/imu/invensense/icm20689/InvenSense_ICM20689_registers.hpp
@@ -1,6 +1,6 @@
 /****************************************************************************
  *
- *   Copyright (c) 2020-2021 PX4 Development Team. All rights reserved.
+ *   Copyright (c) 2020-2022 PX4 Development Team. All rights reserved.
  *
  * Redistribution and use in source and binary forms, with or without
  * modification, are permitted provided that the following conditions
@@ -43,6 +43,8 @@
 #include <cstdint>
 #include <cstddef>
 
+namespace InvenSense_ICM20689
+{
 // TODO: move to a central header
 static constexpr uint8_t Bit0 = (1 << 0);
 static constexpr uint8_t Bit1 = (1 << 1);
@@ -53,8 +55,6 @@ static constexpr uint8_t Bit5 = (1 << 5);
 static constexpr uint8_t Bit6 = (1 << 6);
 static constexpr uint8_t Bit7 = (1 << 7);
 
-namespace InvenSense_ICM20689
-{
 static constexpr uint32_t SPI_SPEED = 8 * 1000 * 1000; // 8MHz SPI serial interface
 static constexpr uint8_t DIR_READ = 0x80;
 
@@ -62,8 +62,11 @@ static constexpr uint8_t WHOAMI = 0x98;
 
 static constexpr float TEMPERATURE_SENSITIVITY = 326.8f; // LSB/C
 static constexpr float TEMPERATURE_OFFSET = 25.f; // C
+static constexpr float TEMPERATURE_SENSOR_MIN = -40.f; // °C
+static constexpr float TEMPERATURE_SENSOR_MAX = 85.f; // °C
 
 enum class Register : uint8_t {
+
 	CONFIG            = 0x1A,
 	GYRO_CONFIG       = 0x1B,
 	ACCEL_CONFIG      = 0x1C,
diff --git a/src/drivers/imu/invensense/icm20948/CMakeLists.txt b/src/drivers/imu/invensense/icm20948/CMakeLists.txt
index b9ea57f5c0..e003a9691a 100644
--- a/src/drivers/imu/invensense/icm20948/CMakeLists.txt
+++ b/src/drivers/imu/invensense/icm20948/CMakeLists.txt
@@ -1,6 +1,6 @@
 ############################################################################
 #
-#   Copyright (c) 2020 PX4 Development Team. All rights reserved.
+#   Copyright (c) 2020-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
@@ -34,6 +34,7 @@ px4_add_module(
 	MODULE drivers__imu__invensense__icm20948
 	MAIN icm20948
 	COMPILE_FLAGS
+		-Wno-error
 	SRCS
 		AKM_AK09916_registers.hpp
 		ICM20948.cpp
@@ -44,8 +45,6 @@ px4_add_module(
 		InvenSense_ICM20948_registers.hpp
 	DEPENDS
 		px4_work_queue
-		drivers_accelerometer
-		drivers_gyroscope
 		drivers_magnetometer
 	)
 
diff --git a/src/drivers/imu/invensense/icm20948/ICM20948.cpp b/src/drivers/imu/invensense/icm20948/ICM20948.cpp
index 2031e62b89..f0e4931aac 100644
--- a/src/drivers/imu/invensense/icm20948/ICM20948.cpp
+++ b/src/drivers/imu/invensense/icm20948/ICM20948.cpp
@@ -1,6 +1,6 @@
 /****************************************************************************
  *
- *   Copyright (c) 2020-2021 PX4 Development Team. All rights reserved.
+ *   Copyright (c) 2020-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
@@ -33,6 +33,8 @@
 
 #include "ICM20948.hpp"
 
+#include <lib/parameters/param.h>
+
 #include "AKM_AK09916_registers.hpp"
 
 using namespace time_literals;
@@ -46,14 +48,16 @@ ICM20948::ICM20948(const I2CSPIDriverConfig &config) :
 	SPI(config),
 	I2CSPIDriver(config),
 	_drdy_gpio(config.drdy_gpio),
-	_px4_accel(get_device_id(), config.rotation),
-	_px4_gyro(get_device_id(), config.rotation)
+	_rotation(config.rotation)
 {
 	if (_drdy_gpio != 0) {
 		_drdy_missed_perf = perf_alloc(PC_COUNT, MODULE_NAME": DRDY missed");
 	}
 
-	ConfigureSampleRate(_px4_gyro.get_max_rate_hz());
+	int32_t imu_gyro_rate_max = 400;
+	param_get(param_find("IMU_GYRO_RATEMAX"), &imu_gyro_rate_max);
+
+	ConfigureSampleRate(imu_gyro_rate_max);
 
 	bool enable_magnetometer = config.custom1 == 1;
 
@@ -136,8 +140,9 @@ void ICM20948::print_status()
 
 int ICM20948::probe()
 {
-	for (int i = 0; i < 3; i++) {
-		uint8_t whoami = RegisterRead(Register::BANK_0::WHO_AM_I);
+	// 3 retries
+	for (int retry = 0; retry < 3; retry++) {
+		const uint8_t whoami = RegisterRead(Register::BANK_0::WHO_AM_I);
 
 		if (whoami == WHOAMI) {
 			return PX4_OK;
@@ -249,7 +254,7 @@ void ICM20948::RunImpl()
 				// scheduled from interrupt if _drdy_timestamp_sample was set as expected
 				const hrt_abstime drdy_timestamp_sample = _drdy_timestamp_sample.fetch_and(0);
 
-				if ((now - drdy_timestamp_sample) < _fifo_empty_interval_us) {
+				if (now < drdy_timestamp_sample + _fifo_empty_interval_us) {
 					timestamp_sample = drdy_timestamp_sample;
 
 				} else {
@@ -311,6 +316,7 @@ void ICM20948::RunImpl()
 
 				// full reset if things are failing consistently
 				if (_failure_count > 10) {
+					PX4_DEBUG("Full reset because things are failing consistently");
 					Reset();
 					return;
 				}
@@ -346,65 +352,10 @@ void ICM20948::RunImpl()
 	}
 }
 
-void ICM20948::ConfigureAccel()
-{
-	const uint8_t ACCEL_FS_SEL = RegisterRead(Register::BANK_2::ACCEL_CONFIG) & (Bit2 | Bit1); // 2:1 ACCEL_FS_SEL[1:0]
-
-	switch (ACCEL_FS_SEL) {
-	case ACCEL_FS_SEL_2G:
-		_px4_accel.set_scale(CONSTANTS_ONE_G / 16384.f);
-		_px4_accel.set_range(2.f * CONSTANTS_ONE_G);
-		break;
-
-	case ACCEL_FS_SEL_4G:
-		_px4_accel.set_scale(CONSTANTS_ONE_G / 8192.f);
-		_px4_accel.set_range(4.f * CONSTANTS_ONE_G);
-		break;
-
-	case ACCEL_FS_SEL_8G:
-		_px4_accel.set_scale(CONSTANTS_ONE_G / 4096.f);
-		_px4_accel.set_range(8.f * CONSTANTS_ONE_G);
-		break;
-
-	case ACCEL_FS_SEL_16G:
-		_px4_accel.set_scale(CONSTANTS_ONE_G / 2048.f);
-		_px4_accel.set_range(16.f * CONSTANTS_ONE_G);
-		break;
-	}
-}
-
-void ICM20948::ConfigureGyro()
-{
-	const uint8_t GYRO_FS_SEL = RegisterRead(Register::BANK_2::GYRO_CONFIG_1) & (Bit2 | Bit1); // 2:1 GYRO_FS_SEL[1:0]
-
-	float range_dps = 0.f;
-
-	switch (GYRO_FS_SEL) {
-	case GYRO_FS_SEL_250_DPS:
-		range_dps = 250.f;
-		break;
-
-	case GYRO_FS_SEL_500_DPS:
-		range_dps = 500.f;
-		break;
-
-	case GYRO_FS_SEL_1000_DPS:
-		range_dps = 1000.f;
-		break;
-
-	case GYRO_FS_SEL_2000_DPS:
-		range_dps = 2000.f;
-		break;
-	}
-
-	_px4_gyro.set_scale(math::radians(range_dps / 32768.f));
-	_px4_gyro.set_range(math::radians(range_dps));
-}
-
 void ICM20948::ConfigureSampleRate(int sample_rate)
 {
-	// round down to nearest FIFO sample dt * SAMPLES_PER_TRANSFER
-	const float min_interval = FIFO_SAMPLE_DT * SAMPLES_PER_TRANSFER;
+	// 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));
@@ -462,9 +413,6 @@ bool ICM20948::Configure()
 		}
 	}
 
-	ConfigureAccel();
-	ConfigureGyro();
-
 	return success;
 }
 
@@ -567,54 +515,89 @@ uint16_t ICM20948::FIFOReadCount()
 {
 	SelectRegisterBank(REG_BANK_SEL_BIT::USER_BANK_0);
 
-	// read FIFO count
-	uint8_t fifo_count_buf[3] {};
-	fifo_count_buf[0] = static_cast<uint8_t>(Register::BANK_0::FIFO_COUNTH) | DIR_READ;
+	// transfer buffer
+	struct TransferBuffer {
+		uint8_t cmd{static_cast<uint8_t>(Register::BANK_0::FIFO_COUNTH) | DIR_READ};
+		uint8_t FIFO_COUNTH{0};
+		uint8_t FIFO_COUNTL{0};
+	} buffer{};
 
-	if (transfer(fifo_count_buf, fifo_count_buf, sizeof(fifo_count_buf)) != PX4_OK) {
+	// read FIFO count
+	if (transfer((uint8_t *)&buffer, (uint8_t *)&buffer, sizeof(buffer)) != PX4_OK) {
 		perf_count(_bad_transfer_perf);
 		return 0;
 	}
 
-	return combine(fifo_count_buf[1], fifo_count_buf[2]);
+	return (buffer.FIFO_COUNTH << 8) + buffer.FIFO_COUNTL;
 }
 
 bool ICM20948::FIFORead(const hrt_abstime &timestamp_sample, uint8_t samples)
 {
 	SelectRegisterBank(REG_BANK_SEL_BIT::USER_BANK_0);
 
-	FIFOTransferBuffer buffer{};
-	const size_t transfer_size = math::min(samples * sizeof(FIFO::DATA) + 3, FIFO::SIZE);
+	// FIFO transfer buffer
+	struct TransferBuffer {
+		uint8_t cmd{static_cast<uint8_t>(Register::BANK_0::FIFO_COUNTH) | DIR_READ};
+		uint8_t FIFO_COUNTH{0};
+		uint8_t FIFO_COUNTL{0};
+		FIFO::DATA f[FIFO_MAX_SAMPLES] {};
+	} buffer{};
+
+	// cmd + FIFO_COUNTH + FIFO_COUNTL + samples (FIFO::DATA)
+	const size_t transfer_size = 3 + math::min(samples * sizeof(FIFO::DATA), FIFO::SIZE);
 
 	if (transfer((uint8_t *)&buffer, (uint8_t *)&buffer, transfer_size) != PX4_OK) {
 		perf_count(_bad_transfer_perf);
 		return false;
 	}
 
+	const uint16_t fifo_count_bytes = (buffer.FIFO_COUNTH << 8) + buffer.FIFO_COUNTL;
+	const uint8_t fifo_count_samples = fifo_count_bytes / sizeof(FIFO::DATA);
 
-	const uint16_t fifo_count_bytes = combine(buffer.FIFO_COUNTH, buffer.FIFO_COUNTL);
+	const uint8_t valid_samples = math::min(samples, fifo_count_samples);
 
-	if (fifo_count_bytes >= FIFO::SIZE) {
+	if ((fifo_count_bytes >= FIFO::SIZE) || (valid_samples > FIFO_MAX_SAMPLES)) {
 		perf_count(_fifo_overflow_perf);
 		FIFOReset();
 		return false;
-	}
 
-	const uint8_t fifo_count_samples = fifo_count_bytes / sizeof(FIFO::DATA);
-
-	if (fifo_count_samples == 0) {
+	} else if (fifo_count_samples == 0) {
 		perf_count(_fifo_empty_perf);
 		return false;
 	}
 
-	const uint16_t valid_samples = math::min(samples, fifo_count_samples);
-
 	if (valid_samples > 0) {
-		ProcessGyro(timestamp_sample, buffer.f, valid_samples);
+		sensor_imu_fifo_s sensor_imu_fifo{};
+		sensor_imu_fifo.timestamp_sample = timestamp_sample;
+		sensor_imu_fifo.device_id = get_device_id();
+		sensor_imu_fifo.dt = FIFO_SAMPLE_DT;
+		sensor_imu_fifo.samples = valid_samples;
+		sensor_imu_fifo.accel_scale = ACCEL_SCALE;
+		sensor_imu_fifo.gyro_scale = GYRO_SCALE;
 
-		if (ProcessAccel(timestamp_sample, buffer.f, valid_samples)) {
-			return true;
+		for (int i = 0; i < valid_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)
+			sensor_imu_fifo.accel_x[i] =              combine(buffer.f[i].ACCEL_XOUT_H, buffer.f[i].ACCEL_XOUT_L);
+			sensor_imu_fifo.accel_y[i] = math::negate(combine(buffer.f[i].ACCEL_YOUT_H, buffer.f[i].ACCEL_YOUT_L));
+			sensor_imu_fifo.accel_z[i] = math::negate(combine(buffer.f[i].ACCEL_ZOUT_H, buffer.f[i].ACCEL_ZOUT_L));
+			rotate_3i(_rotation, sensor_imu_fifo.accel_x[i], sensor_imu_fifo.accel_y[i], sensor_imu_fifo.accel_z[i]);
+
+			sensor_imu_fifo.gyro_x[i] =              combine(buffer.f[i].GYRO_XOUT_H, buffer.f[i].GYRO_XOUT_L);
+			sensor_imu_fifo.gyro_y[i] = math::negate(combine(buffer.f[i].GYRO_YOUT_H, buffer.f[i].GYRO_YOUT_L));
+			sensor_imu_fifo.gyro_z[i] = math::negate(combine(buffer.f[i].GYRO_ZOUT_H, buffer.f[i].GYRO_ZOUT_L));
+			rotate_3i(_rotation, sensor_imu_fifo.gyro_x[i], sensor_imu_fifo.gyro_y[i], sensor_imu_fifo.gyro_z[i]);
 		}
+
+		sensor_imu_fifo.temperature = (hrt_elapsed_time(&_temperature_update_timestamp) < 5_s) ? _temperature : NAN;
+
+		sensor_imu_fifo.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);
+
+		sensor_imu_fifo.timestamp = hrt_absolute_time();
+		_sensor_imu_fifo_pub.publish(sensor_imu_fifo);
+
+		return true;
 	}
 
 	return false;
@@ -633,89 +616,6 @@ void ICM20948::FIFOReset()
 	_drdy_timestamp_sample.store(0);
 }
 
-static bool fifo_accel_equal(const FIFO::DATA &f0, const FIFO::DATA &f1)
-{
-	return (memcmp(&f0.ACCEL_XOUT_H, &f1.ACCEL_XOUT_H, 6) == 0);
-}
-
-bool ICM20948::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 * SAMPLES_PER_TRANSFER;
-
-	bool bad_data = false;
-
-	// accel data is doubled in FIFO, but might be shifted
-	int accel_first_sample = 1;
-
-	if (samples >= 4) {
-		if (fifo_accel_equal(fifo[0], fifo[1]) && fifo_accel_equal(fifo[2], fifo[3])) {
-			// [A0, A1, A2, A3]
-			//  A0==A1, A2==A3
-			accel_first_sample = 1;
-
-		} else if (fifo_accel_equal(fifo[1], fifo[2])) {
-			// [A0, A1, A2, A3]
-			//  A0, A1==A2, A3
-			accel_first_sample = 0;
-
-		} else {
-			// no matching accel samples is an error
-			bad_data = true;
-			perf_count(_bad_transfer_perf);
-		}
-	}
-
-	for (int i = accel_first_sample; i < samples; i = i + SAMPLES_PER_TRANSFER) {
-		int16_t accel_x = combine(fifo[i].ACCEL_XOUT_H, fifo[i].ACCEL_XOUT_L);
-		int16_t accel_y = combine(fifo[i].ACCEL_YOUT_H, fifo[i].ACCEL_YOUT_L);
-		int16_t accel_z = combine(fifo[i].ACCEL_ZOUT_H, fifo[i].ACCEL_ZOUT_L);
-
-		// 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[accel.samples] = accel_x;
-		accel.y[accel.samples] = (accel_y == INT16_MIN) ? INT16_MAX : -accel_y;
-		accel.z[accel.samples] = (accel_z == INT16_MIN) ? INT16_MAX : -accel_z;
-		accel.samples++;
-	}
-
-	_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) {
-		_px4_accel.updateFIFO(accel);
-	}
-
-	return !bad_data;
-}
-
-void ICM20948::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 = samples;
-	gyro.dt = FIFO_SAMPLE_DT;
-
-	for (int i = 0; i < samples; i++) {
-		const int16_t gyro_x = combine(fifo[i].GYRO_XOUT_H, fifo[i].GYRO_XOUT_L);
-		const int16_t gyro_y = combine(fifo[i].GYRO_YOUT_H, fifo[i].GYRO_YOUT_L);
-		const int16_t gyro_z = combine(fifo[i].GYRO_ZOUT_H, fifo[i].GYRO_ZOUT_L);
-
-		// 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;
-		gyro.y[i] = (gyro_y == INT16_MIN) ? INT16_MAX : -gyro_y;
-		gyro.z[i] = (gyro_z == INT16_MIN) ? INT16_MAX : -gyro_z;
-	}
-
-	_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));
-
-	_px4_gyro.updateFIFO(gyro);
-}
-
 void ICM20948::UpdateTemperature()
 {
 	// read current temperature
@@ -732,8 +632,7 @@ void ICM20948::UpdateTemperature()
 	const float TEMP_degC = (TEMP_OUT / TEMPERATURE_SENSITIVITY) + TEMPERATURE_OFFSET;
 
 	if (PX4_ISFINITE(TEMP_degC)) {
-		_px4_accel.set_temperature(TEMP_degC);
-		_px4_gyro.set_temperature(TEMP_degC);
+		_temperature = TEMP_degC;
 	}
 }
 
diff --git a/src/drivers/imu/invensense/icm20948/ICM20948.hpp b/src/drivers/imu/invensense/icm20948/ICM20948.hpp
index 103a03fe57..d08103ff4f 100644
--- a/src/drivers/imu/invensense/icm20948/ICM20948.hpp
+++ b/src/drivers/imu/invensense/icm20948/ICM20948.hpp
@@ -1,6 +1,6 @@
 /****************************************************************************
  *
- *   Copyright (c) 2020-2021 PX4 Development Team. All rights reserved.
+ *   Copyright (c) 2020-2022 PX4 Development Team. All rights reserved.
  *
  * Redistribution and use in source and binary forms, with or without
  * modification, are permitted provided that the following conditions
@@ -43,14 +43,15 @@
 #include "InvenSense_ICM20948_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/PublicationMulti.hpp>
+#include <uORB/topics/sensor_imu_fifo.h>
+
 #include "ICM20948_AK09916.hpp"
 
 using namespace InvenSense_ICM20948;
@@ -73,22 +74,13 @@ private:
 
 	// Sensor Configuration
 	static constexpr float FIFO_SAMPLE_DT{1e6f / 9000.f};
-	static constexpr int32_t SAMPLES_PER_TRANSFER{2};                    // ensure at least 1 new accel sample per transfer
 	static constexpr float GYRO_RATE{1e6f / FIFO_SAMPLE_DT};             // 9000 Hz gyro
-	static constexpr float ACCEL_RATE{GYRO_RATE / SAMPLES_PER_TRANSFER}; // 4500 Hz accel
 
 	// maximum FIFO samples per transfer is limited to the size of sensor_accel_fifo/sensor_gyro_fifo
-	static constexpr int32_t FIFO_MAX_SAMPLES{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 int32_t FIFO_MAX_SAMPLES{math::min(FIFO::SIZE / sizeof(FIFO::DATA), (size_t)sensor_imu_fifo_s::FIFO_SIZE)};
 
-	// Transfer data
-	struct FIFOTransferBuffer {
-		uint8_t cmd{static_cast<uint8_t>(Register::BANK_0::FIFO_COUNTH) | DIR_READ};
-		uint8_t FIFO_COUNTH{0};
-		uint8_t FIFO_COUNTL{0};
-		FIFO::DATA f[FIFO_MAX_SAMPLES] {};
-	};
-	// ensure no struct padding
-	static_assert(sizeof(FIFOTransferBuffer) == (3 + FIFO_MAX_SAMPLES *sizeof(FIFO::DATA)));
+	static constexpr float ACCEL_SCALE{CONSTANTS_ONE_G / 2048.f}; // ACCEL_FS_SEL_16G:
+	static constexpr float GYRO_SCALE{math::radians(2000.f / 32768.f)}; // GYRO_FS_SEL_2000_DPS:
 
 	struct register_bank0_config_t {
 		Register::BANK_0 reg;
@@ -113,8 +105,6 @@ private:
 	bool Reset();
 
 	bool Configure();
-	void ConfigureAccel();
-	void ConfigureGyro();
 	void ConfigureSampleRate(int sample_rate);
 
 	void SelectRegisterBank(enum REG_BANK_SEL_BIT bank, bool force = false);
@@ -138,12 +128,16 @@ private:
 	bool FIFORead(const hrt_abstime &timestamp_sample, uint8_t samples);
 	void FIFOReset();
 
-	bool 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);
 	void UpdateTemperature();
 
 	const spi_drdy_gpio_t _drdy_gpio;
 
+	uORB::PublicationMulti<sensor_imu_fifo_s> _sensor_imu_fifo_pub{ORB_ID(sensor_imu_fifo)};
+
+	const enum Rotation _rotation;
+
+	float _temperature{NAN};
+
 	// I2C AUX interface (slave 1 - 4)
 	AKM_AK09916::ICM20948_AK09916 *_slave_ak09916_magnetometer{nullptr};
 	friend class AKM_AK09916::ICM20948_AK09916;
@@ -152,9 +146,6 @@ private:
 	void I2CSlaveExternalSensorDataEnable(uint8_t slave_i2c_addr, uint8_t reg, uint8_t size);
 	bool I2CSlaveExternalSensorDataRead(uint8_t *buffer, uint8_t length);
 
-	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")};
diff --git a/src/drivers/imu/invensense/icm40609d/CMakeLists.txt b/src/drivers/imu/invensense/icm40609d/CMakeLists.txt
index 16ee7e8019..9f707cb674 100644
--- a/src/drivers/imu/invensense/icm40609d/CMakeLists.txt
+++ b/src/drivers/imu/invensense/icm40609d/CMakeLists.txt
@@ -41,6 +41,4 @@ px4_add_module(
 		InvenSense_ICM40609D_registers.hpp
 	DEPENDS
 		px4_work_queue
-		drivers_accelerometer
-		drivers_gyroscope
 	)
diff --git a/src/drivers/imu/invensense/icm40609d/ICM40609D.cpp b/src/drivers/imu/invensense/icm40609d/ICM40609D.cpp
index 8dc0f4b795..35eceb9a3c 100644
--- a/src/drivers/imu/invensense/icm40609d/ICM40609D.cpp
+++ b/src/drivers/imu/invensense/icm40609d/ICM40609D.cpp
@@ -1,6 +1,6 @@
 /****************************************************************************
  *
- *   Copyright (c) 2020-2021 PX4 Development Team. All rights reserved.
+ *   Copyright (c) 2020-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
@@ -33,6 +33,8 @@
 
 #include "ICM40609D.hpp"
 
+#include <lib/parameters/param.h>
+
 using namespace time_literals;
 
 static constexpr int16_t combine(uint8_t msb, uint8_t lsb)
@@ -44,14 +46,16 @@ ICM40609D::ICM40609D(const I2CSPIDriverConfig &config) :
 	SPI(config),
 	I2CSPIDriver(config),
 	_drdy_gpio(config.drdy_gpio),
-	_px4_accel(get_device_id(), config.rotation),
-	_px4_gyro(get_device_id(), config.rotation)
+	_rotation(config.rotation)
 {
-	if (config.drdy_gpio != 0) {
+	if (_drdy_gpio != 0) {
 		_drdy_missed_perf = perf_alloc(PC_COUNT, MODULE_NAME": DRDY missed");
 	}
 
-	ConfigureSampleRate(_px4_gyro.get_max_rate_hz());
+	int32_t imu_gyro_rate_max = 400;
+	param_get(param_find("IMU_GYRO_RATEMAX"), &imu_gyro_rate_max);
+
+	ConfigureSampleRate(imu_gyro_rate_max);
 }
 
 ICM40609D::~ICM40609D()
@@ -61,6 +65,7 @@ ICM40609D::~ICM40609D()
 	perf_free(_fifo_empty_perf);
 	perf_free(_fifo_overflow_perf);
 	perf_free(_fifo_reset_perf);
+	perf_free(_fifo_timestamp_error_perf);
 	perf_free(_drdy_missed_perf);
 }
 
@@ -102,12 +107,14 @@ void ICM40609D::print_status()
 	perf_print_counter(_fifo_empty_perf);
 	perf_print_counter(_fifo_overflow_perf);
 	perf_print_counter(_fifo_reset_perf);
+	perf_print_counter(_fifo_timestamp_error_perf);
 	perf_print_counter(_drdy_missed_perf);
 }
 
 int ICM40609D::probe()
 {
-	for (int i = 0; i < 3; i++) {
+	// 3 retries
+	for (int retry = 0; retry < 3; retry++) {
 		uint8_t whoami = RegisterRead(Register::BANK_0::WHO_AM_I);
 
 		if (whoami == WHOAMI) {
@@ -161,8 +168,8 @@ void ICM40609D::RunImpl()
 				ScheduleDelayed(100_ms);
 
 			} else {
-				PX4_DEBUG("Reset not complete, check again in 10 ms");
-				ScheduleDelayed(10_ms);
+				PX4_DEBUG("Reset not complete, check again in 100 ms");
+				ScheduleDelayed(100_ms);
 			}
 		}
 
@@ -170,21 +177,9 @@ void ICM40609D::RunImpl()
 
 	case STATE::CONFIGURE:
 		if (Configure()) {
-			// 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 {
-				_data_ready_interrupt_enabled = false;
-				ScheduleOnInterval(_fifo_empty_interval_us, _fifo_empty_interval_us);
-			}
-
-			FIFOReset();
+			// if configure succeeded then reset the FIFO
+			_state = STATE::FIFO_RESET;
+			ScheduleDelayed(1_ms);
 
 		} else {
 			// CONFIGURE not complete
@@ -201,6 +196,24 @@ void ICM40609D::RunImpl()
 
 		break;
 
+	case STATE::FIFO_RESET:
+
+		_state = STATE::FIFO_READ;
+		FIFOReset();
+
+		if (DataReadyInterruptConfigure()) {
+			_data_ready_interrupt_enabled = true;
+
+			// backup schedule as a watchdog timeout
+			ScheduleDelayed(100_ms);
+
+		} else {
+			_data_ready_interrupt_enabled = false;
+			ScheduleOnInterval(_fifo_empty_interval_us, _fifo_empty_interval_us);
+		}
+
+		break;
+
 	case STATE::FIFO_READ: {
 			hrt_abstime timestamp_sample = now;
 			uint8_t samples = 0;
@@ -209,7 +222,7 @@ void ICM40609D::RunImpl()
 				// scheduled from interrupt if _drdy_timestamp_sample was set as expected
 				const hrt_abstime drdy_timestamp_sample = _drdy_timestamp_sample.fetch_and(0);
 
-				if ((now - drdy_timestamp_sample) < _fifo_empty_interval_us) {
+				if (now < drdy_timestamp_sample + _fifo_empty_interval_us) {
 					timestamp_sample = drdy_timestamp_sample;
 					samples = _fifo_gyro_samples;
 
@@ -221,6 +234,8 @@ void ICM40609D::RunImpl()
 				ScheduleDelayed(_fifo_empty_interval_us * 2);
 			}
 
+			bool success = false;
+
 			if (samples == 0) {
 				// check current FIFO count
 				const uint16_t fifo_count = FIFOReadCount();
@@ -233,27 +248,42 @@ void ICM40609D::RunImpl()
 					perf_count(_fifo_empty_perf);
 
 				} else {
-					// FIFO count (size in bytes)
-					samples = (fifo_count / sizeof(FIFO::DATA));
+					// FIFO count (size in bytes) should be a multiple of the FIFO::DATA structure
+					samples = fifo_count / sizeof(FIFO::DATA);
 
-					// tolerate minor jitter, leave sample to next iteration if behind by only 1
-					if (samples == _fifo_gyro_samples + 1) {
-						timestamp_sample -= static_cast<int>(FIFO_SAMPLE_DT);
-						samples--;
+					if (samples > _fifo_gyro_samples) {
+						// grab desired number of samples, but reschedule next cycle sooner
+						const int extra_samples = samples - _fifo_gyro_samples;
+						samples = _fifo_gyro_samples;
+
+						if (_fifo_gyro_samples > extra_samples) {
+							// reschedule to run when a total of _fifo_gyro_samples should be available in the FIFO
+							const uint32_t reschedule_delay_us = (_fifo_gyro_samples - extra_samples) * static_cast<int>(FIFO_SAMPLE_DT);
+							ScheduleOnInterval(_fifo_empty_interval_us, reschedule_delay_us);
+
+						} else {
+							// otherwise reschedule to run immediately
+							ScheduleOnInterval(_fifo_empty_interval_us);
+						}
+
+					} else if (samples < _fifo_gyro_samples) {
+						// reschedule next cycle to catch the desired number of samples
+						ScheduleOnInterval(_fifo_empty_interval_us, (_fifo_gyro_samples - samples) * static_cast<int>(FIFO_SAMPLE_DT));
 					}
 
-					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;
+					if (samples == _fifo_gyro_samples) {
+						if (FIFORead(now, samples)) {
+							success = true;
+
+							if (_failure_count > 0) {
+								_failure_count--;
+							}
+						}
 					}
 				}
 			}
 
-			bool success = false;
-
-			if (samples >= 1) {
+			if (samples == _fifo_gyro_samples) {
 				if (FIFORead(timestamp_sample, samples)) {
 					success = true;
 
@@ -268,13 +298,14 @@ void ICM40609D::RunImpl()
 
 				// full reset if things are failing consistently
 				if (_failure_count > 10) {
+					PX4_DEBUG("Full reset because things are failing consistently");
 					Reset();
 					return;
 				}
 			}
 
+			// check configuration registers periodically or immediately following any failure
 			if (!success || hrt_elapsed_time(&_last_config_check_timestamp) > 100_ms) {
-				// check configuration registers periodically or immediately following any failure
 				if (RegisterCheck(_register_bank0_cfg[_checked_register_bank0])
 				   ) {
 					_last_config_check_timestamp = now;
@@ -285,13 +316,6 @@ void ICM40609D::RunImpl()
 					perf_count(_bad_register_perf);
 					Reset();
 				}
-
-			} else {
-				// periodically update temperature (~1 Hz)
-				if (hrt_elapsed_time(&_temperature_update_timestamp) >= 1_s) {
-					UpdateTemperature();
-					_temperature_update_timestamp = now;
-				}
 			}
 		}
 
@@ -299,75 +323,16 @@ void ICM40609D::RunImpl()
 	}
 }
 
-void ICM40609D::ConfigureAccel()
-{
-	const uint8_t ACCEL_FS_SEL = RegisterRead(Register::BANK_0::ACCEL_CONFIG0) & (Bit7 | Bit6 | Bit5); // 7:5 ACCEL_FS_SEL
-
-	switch (ACCEL_FS_SEL) {
-	case ACCEL_FS_SEL_4G:
-		_px4_accel.set_scale(CONSTANTS_ONE_G / 8192.f);
-		_px4_accel.set_range(4.f * CONSTANTS_ONE_G);
-		break;
-
-	case ACCEL_FS_SEL_8G:
-		_px4_accel.set_scale(CONSTANTS_ONE_G / 4096.f);
-		_px4_accel.set_range(8.f * CONSTANTS_ONE_G);
-		break;
-
-	case ACCEL_FS_SEL_16G:
-		_px4_accel.set_scale(CONSTANTS_ONE_G / 2048.f);
-		_px4_accel.set_range(16.f * CONSTANTS_ONE_G);
-		break;
-
-	case ACCEL_FS_SEL_32G:
-		_px4_accel.set_scale(CONSTANTS_ONE_G / 1024.f);
-		_px4_accel.set_range(32.f * CONSTANTS_ONE_G);
-		break;
-	}
-}
-
-void ICM40609D::ConfigureGyro()
-{
-	const uint8_t GYRO_FS_SEL = RegisterRead(Register::BANK_0::GYRO_CONFIG0) & (Bit7 | Bit6 | Bit5); // 7:5 GYRO_FS_SEL
-
-	float range_dps = 0.f;
-
-	switch (GYRO_FS_SEL) {
-	case GYRO_FS_SEL_125_DPS:
-		range_dps = 125.f;
-		break;
-
-	case GYRO_FS_SEL_250_DPS:
-		range_dps = 250.f;
-		break;
-
-	case GYRO_FS_SEL_500_DPS:
-		range_dps = 500.f;
-		break;
-
-	case GYRO_FS_SEL_1000_DPS:
-		range_dps = 1000.f;
-		break;
-
-	case GYRO_FS_SEL_2000_DPS:
-		range_dps = 2000.f;
-		break;
-	}
-
-	_px4_gyro.set_scale(math::radians(range_dps / 32768.f));
-	_px4_gyro.set_range(math::radians(range_dps));
-}
-
 void ICM40609D::ConfigureSampleRate(int sample_rate)
 {
 	// 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));
+	_fifo_gyro_samples = roundf(math::min((float)_fifo_empty_interval_us / (1e6f / 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);
+	_fifo_empty_interval_us = _fifo_gyro_samples * (1e6f / RATE);
 
 	ConfigureFIFOWatermark(_fifo_gyro_samples);
 }
@@ -418,9 +383,6 @@ bool ICM40609D::Configure()
 		}
 	}
 
-	ConfigureAccel();
-	ConfigureGyro();
-
 	return success;
 }
 
@@ -443,7 +405,7 @@ bool ICM40609D::DataReadyInterruptConfigure()
 	}
 
 	// Setup data ready on falling edge
-	return px4_arch_gpiosetevent(_drdy_gpio, false, true, true, &DataReadyInterruptCallback, this) == 0;
+	return (px4_arch_gpiosetevent(_drdy_gpio, false, true, false, &DataReadyInterruptCallback, this) == 0);
 }
 
 bool ICM40609D::DataReadyInterruptDisable()
@@ -452,7 +414,7 @@ bool ICM40609D::DataReadyInterruptDisable()
 		return false;
 	}
 
-	return px4_arch_gpiosetevent(_drdy_gpio, false, false, false, nullptr, nullptr) == 0;
+	return (px4_arch_gpiosetevent(_drdy_gpio, false, false, false, nullptr, nullptr) == 0);
 }
 
 template <typename T>
@@ -507,23 +469,36 @@ void ICM40609D::RegisterSetAndClearBits(T reg, uint8_t setbits, uint8_t clearbit
 
 uint16_t ICM40609D::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);
+	// transfer buffer
+	struct TransferBuffer {
+		uint8_t cmd{static_cast<uint8_t>(Register::BANK_0::FIFO_COUNTH) | DIR_READ};
+		uint8_t FIFO_COUNTH{0};
+		uint8_t FIFO_COUNTL{0};
+	} buffer{};
 
-	if (transfer(fifo_count_buf, fifo_count_buf, sizeof(fifo_count_buf)) != PX4_OK) {
+	// read FIFO count
+	if (transfer((uint8_t *)&buffer, (uint8_t *)&buffer, sizeof(buffer)) != PX4_OK) {
 		perf_count(_bad_transfer_perf);
 		return 0;
 	}
 
-	return combine(fifo_count_buf[1], fifo_count_buf[2]);
+	return (buffer.FIFO_COUNTH << 8) + buffer.FIFO_COUNTL;
 }
 
 bool ICM40609D::FIFORead(const hrt_abstime &timestamp_sample, uint8_t samples)
 {
-	FIFOTransferBuffer buffer{};
-	const size_t transfer_size = math::min(samples * sizeof(FIFO::DATA) + 4, FIFO::SIZE);
+	// FIFO transfer buffer
+	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] {};
+	} buffer{};
+
+	// cmd + INT_STATUS + FIFO_COUNTH + FIFO_COUNTL + samples (FIFO::DATA)
+	const size_t transfer_size = 4 + math::min(samples * sizeof(FIFO::DATA), FIFO::SIZE);
+
 	SelectRegisterBank(REG_BANK_SEL_BIT::USER_BANK_0);
 
 	if (transfer((uint8_t *)&buffer, (uint8_t *)&buffer, transfer_size) != PX4_OK) {
@@ -537,7 +512,7 @@ bool ICM40609D::FIFORead(const hrt_abstime &timestamp_sample, uint8_t samples)
 		return false;
 	}
 
-	const uint16_t fifo_count_bytes = combine(buffer.FIFO_COUNTH, buffer.FIFO_COUNTL);
+	const uint16_t fifo_count_bytes = (buffer.FIFO_COUNTH << 8) + buffer.FIFO_COUNTL;
 
 	if (fifo_count_bytes >= FIFO::SIZE) {
 		perf_count(_fifo_overflow_perf);
@@ -552,40 +527,151 @@ bool ICM40609D::FIFORead(const hrt_abstime &timestamp_sample, uint8_t samples)
 		return false;
 	}
 
+	sensor_imu_fifo_s sensor_imu_fifo{};
+
+	sensor_imu_fifo.timestamp_sample = timestamp_sample;
+	sensor_imu_fifo.device_id = get_device_id();
+	sensor_imu_fifo.dt = FIFO_SAMPLE_DT;
+	sensor_imu_fifo.samples = 0;
+	sensor_imu_fifo.accel_scale = CONSTANTS_ONE_G / 8192.f; // highres accel data 8192 LSB/g
+	sensor_imu_fifo.gyro_scale = math::radians(1.f / 131.f); // highres gyro data 131 LSB/dps
+
 	// check FIFO header in every sample
 	uint8_t valid_samples = 0;
 
+	float temperature_sum = 0;
+
+	float timestamp_interval_sum = 0;
+	int timestamp_interval_sum_count = 0;
+
+	bool accel_scale_16bit = false; // 18-bits of accelerometer data
+	bool gyro_scale_16bit = false; // 20-bits of gyroscope data
+
 	for (int i = 0; i < math::min(samples, fifo_count_samples); i++) {
-		bool valid = true;
+		const FIFO::DATA &fifo = buffer.f[i];
 
 		// 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;
+		const uint8_t FIFO_HEADER = fifo.FIFO_Header;
 
-		if (FIFO_HEADER & FIFO::FIFO_HEADER_BIT::HEADER_MSG) {
-			// FIFO sample empty if HEADER_MSG set
-			valid = false;
+		const bool HEADER_MSG       = FIFO_HEADER & FIFO::FIFO_HEADER_BIT::HEADER_MSG; // FIFO is empty
+		const bool HEADER_ACCEL     = FIFO_HEADER & FIFO::FIFO_HEADER_BIT::HEADER_ACCEL;
+		const bool HEADER_GYRO      = FIFO_HEADER & FIFO::FIFO_HEADER_BIT::HEADER_GYRO;
+		// 3:2 HEADER_TIMESTAMP_FSYNC
+		const bool HEADER_ODR_ACCEL = FIFO_HEADER & FIFO::FIFO_HEADER_BIT::HEADER_ODR_ACCEL; // ODR for accel is different
+		const bool HEADER_ODR_GYRO  = FIFO_HEADER & FIFO::FIFO_HEADER_BIT::HEADER_ODR_GYRO;  // ODR for gyro is different
 
-		} else if (!(FIFO_HEADER & FIFO::FIFO_HEADER_BIT::HEADER_ACCEL)) {
-			// accel bit not set
-			valid = false;
+		if (!HEADER_MSG && HEADER_ACCEL && HEADER_GYRO && HEADER_20 && !HEADER_ODR_ACCEL && !HEADER_ODR_GYRO) {
 
-		} else if (!(FIFO_HEADER & FIFO::FIFO_HEADER_BIT::HEADER_GYRO)) {
-			// gyro bit not set
-			valid = false;
-		}
+			// 20 bit data scaled to 16 bit
+			sensor_imu_fifo.accel_scale = CONSTANTS_ONE_G / 2048.f;
+
+			sensor_imu_fifo.accel_x[valid_samples] = combine(fifo.ACCEL_DATA_X1, fifo.ACCEL_DATA_X0);
+			sensor_imu_fifo.accel_y[valid_samples] = combine(fifo.ACCEL_DATA_Y1, fifo.ACCEL_DATA_Y0);
+			sensor_imu_fifo.accel_z[valid_samples] = combine(fifo.ACCEL_DATA_Z1, fifo.ACCEL_DATA_Z0);
+
+			sensor_imu_fifo.gyro_x[i] = combine(fifo.GYRO_DATA_X1, fifo.GYRO_DATA_X0);
+			sensor_imu_fifo.gyro_y[i] = combine(fifo.GYRO_DATA_Y1, fifo.GYRO_DATA_Y0);
+			sensor_imu_fifo.gyro_z[i] = combine(fifo.GYRO_DATA_Z1, fifo.GYRO_DATA_Z0);
+
+			// temperature
+			const int16_t TEMP_DATA = combine(fifo.TEMP_DATA1, fifo.TEMP_DATA0);
+
+			// sample invalid if -32768
+			if (TEMP_DATA != -32768) {
+				temperature_sum += TEMP_DATA;
+
+			} else {
+				break;
+			}
+
+
+			// HEADER_TIMESTAMP_FSYNC - 0b10: Packet contains ODR Timestamp
+			if (FIFO_HEADER & Bit3) {
+				const uint16_t timestamp = (fifo.TimeStamp_h << 8) + fifo.TimeStamp_l;
+
+				if (_timestamp_prev != 0) {
+					// If TMST_RES = 0 (corresponding to timestamp resolution of 1µs), timestamp interval reported in FIFO requires scaling by a factor of 32/30.
+					// Document Number: DS-000347 Revision: 1.5 Page 59 of 110
+					static constexpr float FIFO_DT_SCALE = 32.f / 30.f;
+
+					float dt = 0;
+
+					if (timestamp > _timestamp_prev) {
+						dt = static_cast<float>(timestamp - _timestamp_prev) * FIFO_DT_SCALE;
+
+					} else if (timestamp < _timestamp_prev) {
+						// uint16_t rollover
+						uint32_t timestamp_new = UINT16_MAX + timestamp;
+						dt = static_cast<float>(timestamp_new - _timestamp_prev) * FIFO_DT_SCALE;
+					}
+
+					timestamp_interval_sum += dt;
+					timestamp_interval_sum_count++;
+
+					// check dt is within +=2% of expected value
+					if ((dt < (FIFO_SAMPLE_DT * 0.98f)) || (dt > (FIFO_SAMPLE_DT * 1.02f))) {
+						perf_count(_fifo_timestamp_error_perf);
+					}
+				}
+
+				_timestamp_prev = timestamp;
+			}
 
-		if (valid) {
 			valid_samples++;
-
-		} else {
-			perf_count(_bad_transfer_perf);
-			break;
 		}
 	}
 
 	if (valid_samples > 0) {
-		ProcessGyro(timestamp_sample, buffer.f, valid_samples);
-		ProcessAccel(timestamp_sample, buffer.f, valid_samples);
+
+		sensor_imu_fifo.samples = valid_samples;
+
+		for (int i = 0; i < sensor_imu_fifo.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)
+
+			// sensor_imu_fifo.accel_x[i]
+			sensor_imu_fifo.accel_y[i] = math::negate(sensor_imu_fifo.accel_y[i]);
+			sensor_imu_fifo.accel_z[i] = math::negate(sensor_imu_fifo.accel_z[i]);
+			rotate_3i(_rotation, sensor_imu_fifo.accel_x[i], sensor_imu_fifo.accel_y[i], sensor_imu_fifo.accel_z[i]);
+
+			// sensor_imu_fifo.gyro_x[i]
+			sensor_imu_fifo.gyro_y[i] = math::negate(sensor_imu_fifo.gyro_y[i]);
+			sensor_imu_fifo.gyro_z[i] = math::negate(sensor_imu_fifo.gyro_z[i]);
+			rotate_3i(_rotation, sensor_imu_fifo.gyro_x[i], sensor_imu_fifo.gyro_y[i], sensor_imu_fifo.gyro_z[i]);
+		}
+
+		const float temperature_avg = temperature_sum / valid_samples;
+
+		// use average temperature reading
+		const float TEMP_degC = (temperature_avg / TEMPERATURE_SENSITIVITY) + TEMPERATURE_OFFSET;
+
+		if (PX4_ISFINITE(TEMP_degC)) {
+			sensor_imu_fifo.temperature = TEMP_degC;
+
+		} else {
+			perf_count(_bad_transfer_perf);
+			return false;
+		}
+
+		if (timestamp_interval_sum > 0) {
+			const float dt_avg = (timestamp_interval_sum / timestamp_interval_sum_count);
+
+			// check dt is within +=1% of expected value
+			if ((dt_avg < (FIFO_SAMPLE_DT * 0.99f)) || (dt_avg > (FIFO_SAMPLE_DT * 1.01f))) {
+				PX4_ERR("DT error %.6f", (double)dt_avg);
+				perf_count(_fifo_timestamp_error_perf);
+
+			} else {
+				sensor_imu_fifo.dt = dt_avg;
+			}
+		}
+
+		sensor_imu_fifo.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);
+
+		sensor_imu_fifo.timestamp = hrt_absolute_time();
+		_sensor_imu_fifo_pub.publish(sensor_imu_fifo);
+
 		return true;
 	}
 
@@ -601,80 +687,6 @@ void ICM40609D::FIFOReset()
 
 	// reset while FIFO is disabled
 	_drdy_timestamp_sample.store(0);
-}
-
-void ICM40609D::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;
-
-	for (int i = 0; i < samples; i++) {
-		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);
-
-		// 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[accel.samples] = accel_x;
-		accel.y[accel.samples] = (accel_y == INT16_MIN) ? INT16_MAX : -accel_y;
-		accel.z[accel.samples] = (accel_z == INT16_MIN) ? INT16_MAX : -accel_z;
-		accel.samples++;
-	}
-
-	_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) {
-		_px4_accel.updateFIFO(accel);
-	}
-}
-
-void ICM40609D::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 = samples;
-	gyro.dt = FIFO_SAMPLE_DT;
-
-	for (int i = 0; i < samples; i++) {
-		const int16_t gyro_x = combine(fifo[i].GYRO_DATA_X1, fifo[i].GYRO_DATA_X0);
-		const int16_t gyro_y = combine(fifo[i].GYRO_DATA_Y1, fifo[i].GYRO_DATA_Y0);
-		const int16_t gyro_z = combine(fifo[i].GYRO_DATA_Z1, fifo[i].GYRO_DATA_Z0);
-
-		// 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;
-		gyro.y[i] = (gyro_y == INT16_MIN) ? INT16_MAX : -gyro_y;
-		gyro.z[i] = (gyro_z == INT16_MIN) ? INT16_MAX : -gyro_z;
-	}
-
-	_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));
-
-	_px4_gyro.updateFIFO(gyro);
-}
-
-void ICM40609D::UpdateTemperature()
-{
-	// read current temperature
-	uint8_t temperature_buf[3] {};
-	temperature_buf[0] = static_cast<uint8_t>(Register::BANK_0::TEMP_DATA1) | DIR_READ;
-	SelectRegisterBank(REG_BANK_SEL_BIT::USER_BANK_0);
-
-	if (transfer(temperature_buf, temperature_buf, sizeof(temperature_buf)) != PX4_OK) {
-		perf_count(_bad_transfer_perf);
-		return;
-	}
-
-	const int16_t TEMP_DATA = combine(temperature_buf[1], temperature_buf[2]);
-
-	// Temperature in Degrees Centigrade
-	const float TEMP_degC = (TEMP_DATA / TEMPERATURE_SENSITIVITY) + TEMPERATURE_OFFSET;
-
-	if (PX4_ISFINITE(TEMP_degC)) {
-		_px4_accel.set_temperature(TEMP_degC);
-		_px4_gyro.set_temperature(TEMP_degC);
-	}
+
+	_timestamp_prev = 0;
 }
diff --git a/src/drivers/imu/invensense/icm40609d/ICM40609D.hpp b/src/drivers/imu/invensense/icm40609d/ICM40609D.hpp
index ab6fcfa1f2..337a1a0749 100644
--- a/src/drivers/imu/invensense/icm40609d/ICM40609D.hpp
+++ b/src/drivers/imu/invensense/icm40609d/ICM40609D.hpp
@@ -1,6 +1,6 @@
 /****************************************************************************
  *
- *   Copyright (c) 2020-2021 PX4 Development Team. All rights reserved.
+ *   Copyright (c) 2020-2022 PX4 Development Team. All rights reserved.
  *
  * Redistribution and use in source and binary forms, with or without
  * modification, are permitted provided that the following conditions
@@ -43,14 +43,15 @@
 #include "InvenSense_ICM40609D_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/PublicationMulti.hpp>
+#include <uORB/topics/sensor_imu_fifo.h>
+
 using namespace InvenSense_ICM40609D;
 
 class ICM40609D : public device::SPI, public I2CSPIDriver<ICM40609D>
@@ -71,22 +72,10 @@ private:
 
 	// Sensor Configuration
 	static constexpr float FIFO_SAMPLE_DT{1e6f / 8000.f};     // 8000 Hz accel & gyro ODR configured
-	static constexpr float GYRO_RATE{1e6f / FIFO_SAMPLE_DT};
-	static constexpr float ACCEL_RATE{1e6f / FIFO_SAMPLE_DT};
+	static constexpr float RATE{1e6f / FIFO_SAMPLE_DT};
 
 	// maximum FIFO samples per transfer is limited to the size of sensor_accel_fifo/sensor_gyro_fifo
-	static constexpr int32_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))};
-
-	// 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)));
+	static constexpr int32_t FIFO_MAX_SAMPLES{math::min(FIFO::SIZE / sizeof(FIFO::DATA), (size_t)sensor_imu_fifo_s::FIFO_SIZE)};
 
 	struct register_bank0_config_t {
 		Register::BANK_0 reg;
@@ -99,8 +88,6 @@ private:
 	bool Reset();
 
 	bool Configure();
-	void ConfigureAccel();
-	void ConfigureGyro();
 	void ConfigureSampleRate(int sample_rate);
 	void ConfigureFIFOWatermark(uint8_t samples);
 
@@ -123,20 +110,18 @@ private:
 	bool FIFORead(const hrt_abstime &timestamp_sample, uint8_t samples);
 	void FIFOReset();
 
-	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);
-	void UpdateTemperature();
-
 	const spi_drdy_gpio_t _drdy_gpio;
 
-	PX4Accelerometer _px4_accel;
-	PX4Gyroscope _px4_gyro;
+	uORB::PublicationMulti<sensor_imu_fifo_s> _sensor_imu_fifo_pub{ORB_ID(sensor_imu_fifo)};
+
+	const enum Rotation _rotation;
 
 	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 _fifo_timestamp_error_perf{perf_alloc(PC_COUNT, MODULE_NAME": FIFO timestamp error")};
 	perf_counter_t _drdy_missed_perf{nullptr};
 
 	hrt_abstime _reset_timestamp{0};
@@ -144,6 +129,8 @@ private:
 	hrt_abstime _temperature_update_timestamp{0};
 	int _failure_count{0};
 
+	uint16_t _timestamp_prev{0};
+
 	enum REG_BANK_SEL_BIT _last_register_bank {REG_BANK_SEL_BIT::USER_BANK_0};
 
 	px4::atomic<hrt_abstime> _drdy_timestamp_sample{0};
@@ -153,11 +140,12 @@ private:
 		RESET,
 		WAIT_FOR_RESET,
 		CONFIGURE,
+		FIFO_RESET,
 		FIFO_READ,
 	} _state{STATE::RESET};
 
 	uint16_t _fifo_empty_interval_us{1250}; // default 1250 us / 800 Hz transfer interval
-	int32_t _fifo_gyro_samples{static_cast<int32_t>(_fifo_empty_interval_us / (1000000 / GYRO_RATE))};
+	int32_t _fifo_gyro_samples{static_cast<int32_t>(_fifo_empty_interval_us / (1000000 / RATE))};
 
 	uint8_t _checked_register_bank0{0};
 	static constexpr uint8_t size_register_bank0_cfg{10};
diff --git a/src/drivers/imu/invensense/icm42605/CMakeLists.txt b/src/drivers/imu/invensense/icm42605/CMakeLists.txt
index fdb1ac7c0c..4fb554acc9 100644
--- a/src/drivers/imu/invensense/icm42605/CMakeLists.txt
+++ b/src/drivers/imu/invensense/icm42605/CMakeLists.txt
@@ -41,6 +41,4 @@ px4_add_module(
 		InvenSense_ICM42605_registers.hpp
 	DEPENDS
 		px4_work_queue
-		drivers_accelerometer
-		drivers_gyroscope
 	)
diff --git a/src/drivers/imu/invensense/icm42605/ICM42605.cpp b/src/drivers/imu/invensense/icm42605/ICM42605.cpp
index e805382194..395fb3b37d 100644
--- a/src/drivers/imu/invensense/icm42605/ICM42605.cpp
+++ b/src/drivers/imu/invensense/icm42605/ICM42605.cpp
@@ -1,6 +1,6 @@
 /****************************************************************************
  *
- *   Copyright (c) 2020-2021 PX4 Development Team. All rights reserved.
+ *   Copyright (c) 2020-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
@@ -33,6 +33,8 @@
 
 #include "ICM42605.hpp"
 
+#include <lib/parameters/param.h>
+
 using namespace time_literals;
 
 static constexpr int16_t combine(uint8_t msb, uint8_t lsb)
@@ -44,14 +46,16 @@ ICM42605::ICM42605(const I2CSPIDriverConfig &config) :
 	SPI(config),
 	I2CSPIDriver(config),
 	_drdy_gpio(config.drdy_gpio),
-	_px4_accel(get_device_id(), config.rotation),
-	_px4_gyro(get_device_id(), config.rotation)
+	_rotation(config.rotation)
 {
-	if (config.drdy_gpio != 0) {
+	if (_drdy_gpio != 0) {
 		_drdy_missed_perf = perf_alloc(PC_COUNT, MODULE_NAME": DRDY missed");
 	}
 
-	ConfigureSampleRate(_px4_gyro.get_max_rate_hz());
+	int32_t imu_gyro_rate_max = 400;
+	param_get(param_find("IMU_GYRO_RATEMAX"), &imu_gyro_rate_max);
+
+	ConfigureSampleRate(imu_gyro_rate_max);
 }
 
 ICM42605::~ICM42605()
@@ -61,6 +65,7 @@ ICM42605::~ICM42605()
 	perf_free(_fifo_empty_perf);
 	perf_free(_fifo_overflow_perf);
 	perf_free(_fifo_reset_perf);
+	perf_free(_fifo_timestamp_error_perf);
 	perf_free(_drdy_missed_perf);
 }
 
@@ -102,12 +107,14 @@ void ICM42605::print_status()
 	perf_print_counter(_fifo_empty_perf);
 	perf_print_counter(_fifo_overflow_perf);
 	perf_print_counter(_fifo_reset_perf);
+	perf_print_counter(_fifo_timestamp_error_perf);
 	perf_print_counter(_drdy_missed_perf);
 }
 
 int ICM42605::probe()
 {
-	for (int i = 0; i < 3; i++) {
+	// 3 retries
+	for (int retry = 0; retry < 3; retry++) {
 		uint8_t whoami = RegisterRead(Register::BANK_0::WHO_AM_I);
 
 		if (whoami == WHOAMI) {
@@ -162,8 +169,8 @@ void ICM42605::RunImpl()
 				ScheduleDelayed(100_ms);
 
 			} else {
-				PX4_DEBUG("Reset not complete, check again in 10 ms");
-				ScheduleDelayed(10_ms);
+				PX4_DEBUG("Reset not complete, check again in 100 ms");
+				ScheduleDelayed(100_ms);
 			}
 		}
 
@@ -171,21 +178,9 @@ void ICM42605::RunImpl()
 
 	case STATE::CONFIGURE:
 		if (Configure()) {
-			// 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 {
-				_data_ready_interrupt_enabled = false;
-				ScheduleOnInterval(_fifo_empty_interval_us, _fifo_empty_interval_us);
-			}
-
-			FIFOReset();
+			// if configure succeeded then reset the FIFO
+			_state = STATE::FIFO_RESET;
+			ScheduleDelayed(1_ms);
 
 		} else {
 			// CONFIGURE not complete
@@ -202,6 +197,24 @@ void ICM42605::RunImpl()
 
 		break;
 
+	case STATE::FIFO_RESET:
+
+		_state = STATE::FIFO_READ;
+		FIFOReset();
+
+		if (DataReadyInterruptConfigure()) {
+			_data_ready_interrupt_enabled = true;
+
+			// backup schedule as a watchdog timeout
+			ScheduleDelayed(100_ms);
+
+		} else {
+			_data_ready_interrupt_enabled = false;
+			ScheduleOnInterval(_fifo_empty_interval_us, _fifo_empty_interval_us);
+		}
+
+		break;
+
 	case STATE::FIFO_READ: {
 			hrt_abstime timestamp_sample = now;
 			uint8_t samples = 0;
@@ -210,7 +223,7 @@ void ICM42605::RunImpl()
 				// scheduled from interrupt if _drdy_timestamp_sample was set as expected
 				const hrt_abstime drdy_timestamp_sample = _drdy_timestamp_sample.fetch_and(0);
 
-				if ((now - drdy_timestamp_sample) < _fifo_empty_interval_us) {
+				if (now < drdy_timestamp_sample + _fifo_empty_interval_us) {
 					timestamp_sample = drdy_timestamp_sample;
 					samples = _fifo_gyro_samples;
 
@@ -222,6 +235,8 @@ void ICM42605::RunImpl()
 				ScheduleDelayed(_fifo_empty_interval_us * 2);
 			}
 
+			bool success = false;
+
 			if (samples == 0) {
 				// check current FIFO count
 				const uint16_t fifo_count = FIFOReadCount();
@@ -234,27 +249,42 @@ void ICM42605::RunImpl()
 					perf_count(_fifo_empty_perf);
 
 				} else {
-					// FIFO count (size in bytes)
-					samples = (fifo_count / sizeof(FIFO::DATA));
+					// FIFO count (size in bytes) should be a multiple of the FIFO::DATA structure
+					samples = fifo_count / sizeof(FIFO::DATA);
 
-					// tolerate minor jitter, leave sample to next iteration if behind by only 1
-					if (samples == _fifo_gyro_samples + 1) {
-						timestamp_sample -= static_cast<int>(FIFO_SAMPLE_DT);
-						samples--;
+					if (samples > _fifo_gyro_samples) {
+						// grab desired number of samples, but reschedule next cycle sooner
+						const int extra_samples = samples - _fifo_gyro_samples;
+						samples = _fifo_gyro_samples;
+
+						if (_fifo_gyro_samples > extra_samples) {
+							// reschedule to run when a total of _fifo_gyro_samples should be available in the FIFO
+							const uint32_t reschedule_delay_us = (_fifo_gyro_samples - extra_samples) * static_cast<int>(FIFO_SAMPLE_DT);
+							ScheduleOnInterval(_fifo_empty_interval_us, reschedule_delay_us);
+
+						} else {
+							// otherwise reschedule to run immediately
+							ScheduleOnInterval(_fifo_empty_interval_us);
+						}
+
+					} else if (samples < _fifo_gyro_samples) {
+						// reschedule next cycle to catch the desired number of samples
+						ScheduleOnInterval(_fifo_empty_interval_us, (_fifo_gyro_samples - samples) * static_cast<int>(FIFO_SAMPLE_DT));
 					}
 
-					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;
+					if (samples == _fifo_gyro_samples) {
+						if (FIFORead(now, samples)) {
+							success = true;
+
+							if (_failure_count > 0) {
+								_failure_count--;
+							}
+						}
 					}
 				}
 			}
 
-			bool success = false;
-
-			if (samples >= 1) {
+			if (samples == _fifo_gyro_samples) {
 				if (FIFORead(timestamp_sample, samples)) {
 					success = true;
 
@@ -269,13 +299,14 @@ void ICM42605::RunImpl()
 
 				// full reset if things are failing consistently
 				if (_failure_count > 10) {
+					PX4_DEBUG("Full reset because things are failing consistently");
 					Reset();
 					return;
 				}
 			}
 
+			// check configuration registers periodically or immediately following any failure
 			if (!success || hrt_elapsed_time(&_last_config_check_timestamp) > 100_ms) {
-				// check configuration registers periodically or immediately following any failure
 				if (RegisterCheck(_register_bank0_cfg[_checked_register_bank0])
 				   ) {
 					_last_config_check_timestamp = now;
@@ -286,13 +317,6 @@ void ICM42605::RunImpl()
 					perf_count(_bad_register_perf);
 					Reset();
 				}
-
-			} else {
-				// periodically update temperature (~1 Hz)
-				if (hrt_elapsed_time(&_temperature_update_timestamp) >= 1_s) {
-					UpdateTemperature();
-					_temperature_update_timestamp = now;
-				}
 			}
 		}
 
@@ -300,75 +324,16 @@ void ICM42605::RunImpl()
 	}
 }
 
-void ICM42605::ConfigureAccel()
-{
-	const uint8_t ACCEL_FS_SEL = RegisterRead(Register::BANK_0::ACCEL_CONFIG0) & (Bit7 | Bit6 | Bit5); // 7:5 ACCEL_FS_SEL
-
-	switch (ACCEL_FS_SEL) {
-	case ACCEL_FS_SEL_2G:
-		_px4_accel.set_scale(CONSTANTS_ONE_G / 16384.f);
-		_px4_accel.set_range(2.f * CONSTANTS_ONE_G);
-		break;
-
-	case ACCEL_FS_SEL_4G:
-		_px4_accel.set_scale(CONSTANTS_ONE_G / 8192.f);
-		_px4_accel.set_range(4.f * CONSTANTS_ONE_G);
-		break;
-
-	case ACCEL_FS_SEL_8G:
-		_px4_accel.set_scale(CONSTANTS_ONE_G / 4096.f);
-		_px4_accel.set_range(8.f * CONSTANTS_ONE_G);
-		break;
-
-	case ACCEL_FS_SEL_16G:
-		_px4_accel.set_scale(CONSTANTS_ONE_G / 2048.f);
-		_px4_accel.set_range(16.f * CONSTANTS_ONE_G);
-		break;
-	}
-}
-
-void ICM42605::ConfigureGyro()
-{
-	const uint8_t GYRO_FS_SEL = RegisterRead(Register::BANK_0::GYRO_CONFIG0) & (Bit7 | Bit6 | Bit5); // 7:5 GYRO_FS_SEL
-
-	float range_dps = 0.f;
-
-	switch (GYRO_FS_SEL) {
-	case GYRO_FS_SEL_125_DPS:
-		range_dps = 125.f;
-		break;
-
-	case GYRO_FS_SEL_250_DPS:
-		range_dps = 250.f;
-		break;
-
-	case GYRO_FS_SEL_500_DPS:
-		range_dps = 500.f;
-		break;
-
-	case GYRO_FS_SEL_1000_DPS:
-		range_dps = 1000.f;
-		break;
-
-	case GYRO_FS_SEL_2000_DPS:
-		range_dps = 2000.f;
-		break;
-	}
-
-	_px4_gyro.set_scale(math::radians(range_dps / 32768.f));
-	_px4_gyro.set_range(math::radians(range_dps));
-}
-
 void ICM42605::ConfigureSampleRate(int sample_rate)
 {
 	// 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));
+	_fifo_gyro_samples = roundf(math::min((float)_fifo_empty_interval_us / (1e6f / 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);
+	_fifo_empty_interval_us = _fifo_gyro_samples * (1e6f / RATE);
 
 	ConfigureFIFOWatermark(_fifo_gyro_samples);
 }
@@ -419,9 +384,6 @@ bool ICM42605::Configure()
 		}
 	}
 
-	ConfigureAccel();
-	ConfigureGyro();
-
 	return success;
 }
 
@@ -444,7 +406,7 @@ bool ICM42605::DataReadyInterruptConfigure()
 	}
 
 	// Setup data ready on falling edge
-	return px4_arch_gpiosetevent(_drdy_gpio, false, true, true, &DataReadyInterruptCallback, this) == 0;
+	return (px4_arch_gpiosetevent(_drdy_gpio, false, true, false, &DataReadyInterruptCallback, this) == 0);
 }
 
 bool ICM42605::DataReadyInterruptDisable()
@@ -453,7 +415,7 @@ bool ICM42605::DataReadyInterruptDisable()
 		return false;
 	}
 
-	return px4_arch_gpiosetevent(_drdy_gpio, false, false, false, nullptr, nullptr) == 0;
+	return (px4_arch_gpiosetevent(_drdy_gpio, false, false, false, nullptr, nullptr) == 0);
 }
 
 template <typename T>
@@ -508,23 +470,36 @@ void ICM42605::RegisterSetAndClearBits(T reg, uint8_t setbits, uint8_t clearbits
 
 uint16_t ICM42605::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);
+	// transfer buffer
+	struct TransferBuffer {
+		uint8_t cmd{static_cast<uint8_t>(Register::BANK_0::FIFO_COUNTH) | DIR_READ};
+		uint8_t FIFO_COUNTH{0};
+		uint8_t FIFO_COUNTL{0};
+	} buffer{};
 
-	if (transfer(fifo_count_buf, fifo_count_buf, sizeof(fifo_count_buf)) != PX4_OK) {
+	// read FIFO count
+	if (transfer((uint8_t *)&buffer, (uint8_t *)&buffer, sizeof(buffer)) != PX4_OK) {
 		perf_count(_bad_transfer_perf);
 		return 0;
 	}
 
-	return combine(fifo_count_buf[1], fifo_count_buf[2]);
+	return (buffer.FIFO_COUNTH << 8) + buffer.FIFO_COUNTL;
 }
 
 bool ICM42605::FIFORead(const hrt_abstime &timestamp_sample, uint8_t samples)
 {
-	FIFOTransferBuffer buffer{};
-	const size_t transfer_size = math::min(samples * sizeof(FIFO::DATA) + 4, FIFO::SIZE);
+	// FIFO transfer buffer
+	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] {};
+	} buffer{};
+
+	// cmd + INT_STATUS + FIFO_COUNTH + FIFO_COUNTL + samples (FIFO::DATA)
+	const size_t transfer_size = 4 + math::min(samples * sizeof(FIFO::DATA), FIFO::SIZE);
+
 	SelectRegisterBank(REG_BANK_SEL_BIT::USER_BANK_0);
 
 	if (transfer((uint8_t *)&buffer, (uint8_t *)&buffer, transfer_size) != PX4_OK) {
@@ -538,7 +513,7 @@ bool ICM42605::FIFORead(const hrt_abstime &timestamp_sample, uint8_t samples)
 		return false;
 	}
 
-	const uint16_t fifo_count_bytes = combine(buffer.FIFO_COUNTH, buffer.FIFO_COUNTL);
+	const uint16_t fifo_count_bytes = (buffer.FIFO_COUNTH << 8) + buffer.FIFO_COUNTL;
 
 	if (fifo_count_bytes >= FIFO::SIZE) {
 		perf_count(_fifo_overflow_perf);
@@ -553,40 +528,151 @@ bool ICM42605::FIFORead(const hrt_abstime &timestamp_sample, uint8_t samples)
 		return false;
 	}
 
+	sensor_imu_fifo_s sensor_imu_fifo{};
+
+	sensor_imu_fifo.timestamp_sample = timestamp_sample;
+	sensor_imu_fifo.device_id = get_device_id();
+	sensor_imu_fifo.dt = FIFO_SAMPLE_DT;
+	sensor_imu_fifo.samples = 0;
+	sensor_imu_fifo.accel_scale = CONSTANTS_ONE_G / 8192.f; // highres accel data 8192 LSB/g
+	sensor_imu_fifo.gyro_scale = math::radians(1.f / 131.f); // highres gyro data 131 LSB/dps
+
 	// check FIFO header in every sample
 	uint8_t valid_samples = 0;
 
+	float temperature_sum = 0;
+
+	float timestamp_interval_sum = 0;
+	int timestamp_interval_sum_count = 0;
+
+	bool accel_scale_16bit = false; // 18-bits of accelerometer data
+	bool gyro_scale_16bit = false; // 20-bits of gyroscope data
+
 	for (int i = 0; i < math::min(samples, fifo_count_samples); i++) {
-		bool valid = true;
+		const FIFO::DATA &fifo = buffer.f[i];
 
 		// 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;
+		const uint8_t FIFO_HEADER = fifo.FIFO_Header;
 
-		if (FIFO_HEADER & FIFO::FIFO_HEADER_BIT::HEADER_MSG) {
-			// FIFO sample empty if HEADER_MSG set
-			valid = false;
+		const bool HEADER_MSG       = FIFO_HEADER & FIFO::FIFO_HEADER_BIT::HEADER_MSG; // FIFO is empty
+		const bool HEADER_ACCEL     = FIFO_HEADER & FIFO::FIFO_HEADER_BIT::HEADER_ACCEL;
+		const bool HEADER_GYRO      = FIFO_HEADER & FIFO::FIFO_HEADER_BIT::HEADER_GYRO;
+		// 3:2 HEADER_TIMESTAMP_FSYNC
+		const bool HEADER_ODR_ACCEL = FIFO_HEADER & FIFO::FIFO_HEADER_BIT::HEADER_ODR_ACCEL; // ODR for accel is different
+		const bool HEADER_ODR_GYRO  = FIFO_HEADER & FIFO::FIFO_HEADER_BIT::HEADER_ODR_GYRO;  // ODR for gyro is different
 
-		} else if (!(FIFO_HEADER & FIFO::FIFO_HEADER_BIT::HEADER_ACCEL)) {
-			// accel bit not set
-			valid = false;
+		if (!HEADER_MSG && HEADER_ACCEL && HEADER_GYRO && HEADER_20 && !HEADER_ODR_ACCEL && !HEADER_ODR_GYRO) {
 
-		} else if (!(FIFO_HEADER & FIFO::FIFO_HEADER_BIT::HEADER_GYRO)) {
-			// gyro bit not set
-			valid = false;
-		}
+			// 20 bit data scaled to 16 bit
+			sensor_imu_fifo.accel_scale = CONSTANTS_ONE_G / 2048.f;
+
+			sensor_imu_fifo.accel_x[valid_samples] = combine(fifo.ACCEL_DATA_X1, fifo.ACCEL_DATA_X0);
+			sensor_imu_fifo.accel_y[valid_samples] = combine(fifo.ACCEL_DATA_Y1, fifo.ACCEL_DATA_Y0);
+			sensor_imu_fifo.accel_z[valid_samples] = combine(fifo.ACCEL_DATA_Z1, fifo.ACCEL_DATA_Z0);
+
+			sensor_imu_fifo.gyro_x[i] = combine(fifo.GYRO_DATA_X1, fifo.GYRO_DATA_X0);
+			sensor_imu_fifo.gyro_y[i] = combine(fifo.GYRO_DATA_Y1, fifo.GYRO_DATA_Y0);
+			sensor_imu_fifo.gyro_z[i] = combine(fifo.GYRO_DATA_Z1, fifo.GYRO_DATA_Z0);
+
+			// temperature
+			const int16_t TEMP_DATA = combine(fifo.TEMP_DATA1, fifo.TEMP_DATA0);
+
+			// sample invalid if -32768
+			if (TEMP_DATA != -32768) {
+				temperature_sum += TEMP_DATA;
+
+			} else {
+				break;
+			}
+
+
+			// HEADER_TIMESTAMP_FSYNC - 0b10: Packet contains ODR Timestamp
+			if (FIFO_HEADER & Bit3) {
+				const uint16_t timestamp = (fifo.TimeStamp_h << 8) + fifo.TimeStamp_l;
+
+				if (_timestamp_prev != 0) {
+					// If TMST_RES = 0 (corresponding to timestamp resolution of 1µs), timestamp interval reported in FIFO requires scaling by a factor of 32/30.
+					// Document Number: DS-000347 Revision: 1.5 Page 59 of 110
+					static constexpr float FIFO_DT_SCALE = 32.f / 30.f;
+
+					float dt = 0;
+
+					if (timestamp > _timestamp_prev) {
+						dt = static_cast<float>(timestamp - _timestamp_prev) * FIFO_DT_SCALE;
+
+					} else if (timestamp < _timestamp_prev) {
+						// uint16_t rollover
+						uint32_t timestamp_new = UINT16_MAX + timestamp;
+						dt = static_cast<float>(timestamp_new - _timestamp_prev) * FIFO_DT_SCALE;
+					}
+
+					timestamp_interval_sum += dt;
+					timestamp_interval_sum_count++;
+
+					// check dt is within +=2% of expected value
+					if ((dt < (FIFO_SAMPLE_DT * 0.98f)) || (dt > (FIFO_SAMPLE_DT * 1.02f))) {
+						perf_count(_fifo_timestamp_error_perf);
+					}
+				}
+
+				_timestamp_prev = timestamp;
+			}
 
-		if (valid) {
 			valid_samples++;
-
-		} else {
-			perf_count(_bad_transfer_perf);
-			break;
 		}
 	}
 
 	if (valid_samples > 0) {
-		ProcessGyro(timestamp_sample, buffer.f, valid_samples);
-		ProcessAccel(timestamp_sample, buffer.f, valid_samples);
+
+		sensor_imu_fifo.samples = valid_samples;
+
+		for (int i = 0; i < sensor_imu_fifo.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)
+
+			// sensor_imu_fifo.accel_x[i]
+			sensor_imu_fifo.accel_y[i] = math::negate(sensor_imu_fifo.accel_y[i]);
+			sensor_imu_fifo.accel_z[i] = math::negate(sensor_imu_fifo.accel_z[i]);
+			rotate_3i(_rotation, sensor_imu_fifo.accel_x[i], sensor_imu_fifo.accel_y[i], sensor_imu_fifo.accel_z[i]);
+
+			// sensor_imu_fifo.gyro_x[i]
+			sensor_imu_fifo.gyro_y[i] = math::negate(sensor_imu_fifo.gyro_y[i]);
+			sensor_imu_fifo.gyro_z[i] = math::negate(sensor_imu_fifo.gyro_z[i]);
+			rotate_3i(_rotation, sensor_imu_fifo.gyro_x[i], sensor_imu_fifo.gyro_y[i], sensor_imu_fifo.gyro_z[i]);
+		}
+
+		const float temperature_avg = temperature_sum / valid_samples;
+
+		// use average temperature reading
+		const float TEMP_degC = (temperature_avg / TEMPERATURE_SENSITIVITY) + TEMPERATURE_OFFSET;
+
+		if (PX4_ISFINITE(TEMP_degC)) {
+			sensor_imu_fifo.temperature = TEMP_degC;
+
+		} else {
+			perf_count(_bad_transfer_perf);
+			return false;
+		}
+
+		if (timestamp_interval_sum > 0) {
+			const float dt_avg = (timestamp_interval_sum / timestamp_interval_sum_count);
+
+			// check dt is within +=1% of expected value
+			if ((dt_avg < (FIFO_SAMPLE_DT * 0.99f)) || (dt_avg > (FIFO_SAMPLE_DT * 1.01f))) {
+				PX4_ERR("DT error %.6f", (double)dt_avg);
+				perf_count(_fifo_timestamp_error_perf);
+
+			} else {
+				sensor_imu_fifo.dt = dt_avg;
+			}
+		}
+
+		sensor_imu_fifo.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);
+
+		sensor_imu_fifo.timestamp = hrt_absolute_time();
+		_sensor_imu_fifo_pub.publish(sensor_imu_fifo);
+
 		return true;
 	}
 
@@ -602,80 +688,6 @@ void ICM42605::FIFOReset()
 
 	// reset while FIFO is disabled
 	_drdy_timestamp_sample.store(0);
-}
-
-void ICM42605::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;
-
-	for (int i = 0; i < samples; i++) {
-		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);
-
-		// 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[accel.samples] = accel_x;
-		accel.y[accel.samples] = (accel_y == INT16_MIN) ? INT16_MAX : -accel_y;
-		accel.z[accel.samples] = (accel_z == INT16_MIN) ? INT16_MAX : -accel_z;
-		accel.samples++;
-	}
-
-	_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) {
-		_px4_accel.updateFIFO(accel);
-	}
-}
-
-void ICM42605::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 = samples;
-	gyro.dt = FIFO_SAMPLE_DT;
-
-	for (int i = 0; i < samples; i++) {
-		const int16_t gyro_x = combine(fifo[i].GYRO_DATA_X1, fifo[i].GYRO_DATA_X0);
-		const int16_t gyro_y = combine(fifo[i].GYRO_DATA_Y1, fifo[i].GYRO_DATA_Y0);
-		const int16_t gyro_z = combine(fifo[i].GYRO_DATA_Z1, fifo[i].GYRO_DATA_Z0);
-
-		// 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;
-		gyro.y[i] = (gyro_y == INT16_MIN) ? INT16_MAX : -gyro_y;
-		gyro.z[i] = (gyro_z == INT16_MIN) ? INT16_MAX : -gyro_z;
-	}
-
-	_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));
-
-	_px4_gyro.updateFIFO(gyro);
-}
-
-void ICM42605::UpdateTemperature()
-{
-	// read current temperature
-	uint8_t temperature_buf[3] {};
-	temperature_buf[0] = static_cast<uint8_t>(Register::BANK_0::TEMP_DATA1) | DIR_READ;
-	SelectRegisterBank(REG_BANK_SEL_BIT::USER_BANK_0);
-
-	if (transfer(temperature_buf, temperature_buf, sizeof(temperature_buf)) != PX4_OK) {
-		perf_count(_bad_transfer_perf);
-		return;
-	}
-
-	const int16_t TEMP_DATA = combine(temperature_buf[1], temperature_buf[2]);
-
-	// Temperature in Degrees Centigrade
-	const float TEMP_degC = (TEMP_DATA / TEMPERATURE_SENSITIVITY) + TEMPERATURE_OFFSET;
-
-	if (PX4_ISFINITE(TEMP_degC)) {
-		_px4_accel.set_temperature(TEMP_degC);
-		_px4_gyro.set_temperature(TEMP_degC);
-	}
+
+	_timestamp_prev = 0;
 }
diff --git a/src/drivers/imu/invensense/icm42605/ICM42605.hpp b/src/drivers/imu/invensense/icm42605/ICM42605.hpp
index 98ad916d90..2424b081bd 100644
--- a/src/drivers/imu/invensense/icm42605/ICM42605.hpp
+++ b/src/drivers/imu/invensense/icm42605/ICM42605.hpp
@@ -1,6 +1,6 @@
 /****************************************************************************
  *
- *   Copyright (c) 2020-2021 PX4 Development Team. All rights reserved.
+ *   Copyright (c) 2020-2022 PX4 Development Team. All rights reserved.
  *
  * Redistribution and use in source and binary forms, with or without
  * modification, are permitted provided that the following conditions
@@ -43,14 +43,15 @@
 #include "InvenSense_ICM42605_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/PublicationMulti.hpp>
+#include <uORB/topics/sensor_imu_fifo.h>
+
 using namespace InvenSense_ICM42605;
 
 class ICM42605 : public device::SPI, public I2CSPIDriver<ICM42605>
@@ -71,22 +72,10 @@ private:
 
 	// Sensor Configuration
 	static constexpr float FIFO_SAMPLE_DT{1e6f / 8000.f};     // 8000 Hz accel & gyro ODR configured
-	static constexpr float GYRO_RATE{1e6f / FIFO_SAMPLE_DT};
-	static constexpr float ACCEL_RATE{1e6f / FIFO_SAMPLE_DT};
+	static constexpr float RATE{1e6f / FIFO_SAMPLE_DT};
 
 	// maximum FIFO samples per transfer is limited to the size of sensor_accel_fifo/sensor_gyro_fifo
-	static constexpr int32_t FIFO_MAX_SAMPLES{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))};
-
-	// 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)));
+	static constexpr int32_t FIFO_MAX_SAMPLES{math::min(FIFO::SIZE / sizeof(FIFO::DATA), (size_t)sensor_imu_fifo_s::FIFO_SIZE)};
 
 	struct register_bank0_config_t {
 		Register::BANK_0 reg;
@@ -99,8 +88,6 @@ private:
 	bool Reset();
 
 	bool Configure();
-	void ConfigureAccel();
-	void ConfigureGyro();
 	void ConfigureSampleRate(int sample_rate);
 	void ConfigureFIFOWatermark(uint8_t samples);
 
@@ -123,20 +110,18 @@ private:
 	bool FIFORead(const hrt_abstime &timestamp_sample, uint8_t samples);
 	void FIFOReset();
 
-	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);
-	void UpdateTemperature();
-
 	const spi_drdy_gpio_t _drdy_gpio;
 
-	PX4Accelerometer _px4_accel;
-	PX4Gyroscope _px4_gyro;
+	uORB::PublicationMulti<sensor_imu_fifo_s> _sensor_imu_fifo_pub{ORB_ID(sensor_imu_fifo)};
+
+	const enum Rotation _rotation;
 
 	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 _fifo_timestamp_error_perf{perf_alloc(PC_COUNT, MODULE_NAME": FIFO timestamp error")};
 	perf_counter_t _drdy_missed_perf{nullptr};
 
 	hrt_abstime _reset_timestamp{0};
@@ -144,6 +129,8 @@ private:
 	hrt_abstime _temperature_update_timestamp{0};
 	int _failure_count{0};
 
+	uint16_t _timestamp_prev{0};
+
 	enum REG_BANK_SEL_BIT _last_register_bank {REG_BANK_SEL_BIT::USER_BANK_0};
 
 	px4::atomic<hrt_abstime> _drdy_timestamp_sample{0};
@@ -153,11 +140,12 @@ private:
 		RESET,
 		WAIT_FOR_RESET,
 		CONFIGURE,
+		FIFO_RESET,
 		FIFO_READ,
 	} _state{STATE::RESET};
 
 	uint16_t _fifo_empty_interval_us{1250}; // default 1250 us / 800 Hz transfer interval
-	int32_t _fifo_gyro_samples{static_cast<int32_t>(_fifo_empty_interval_us / (1000000 / GYRO_RATE))};
+	int32_t _fifo_gyro_samples{static_cast<int32_t>(_fifo_empty_interval_us / (1000000 / RATE))};
 
 	uint8_t _checked_register_bank0{0};
 	static constexpr uint8_t size_register_bank0_cfg{10};
diff --git a/src/drivers/imu/invensense/icm42605/InvenSense_ICM42605_registers.hpp b/src/drivers/imu/invensense/icm42605/InvenSense_ICM42605_registers.hpp
index 4a4080feda..01d20b2686 100644
--- a/src/drivers/imu/invensense/icm42605/InvenSense_ICM42605_registers.hpp
+++ b/src/drivers/imu/invensense/icm42605/InvenSense_ICM42605_registers.hpp
@@ -1,6 +1,6 @@
 /****************************************************************************
  *
- *   Copyright (c) 2020 PX4 Development Team. All rights reserved.
+ *   Copyright (c) 2020-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
@@ -242,7 +242,7 @@ enum FIFO_HEADER_BIT : uint8_t {
 	HEADER_MSG             = Bit7, // 1: FIFO is empty
 	HEADER_ACCEL           = Bit6,
 	HEADER_GYRO            = Bit5,
-	HEADER_20              = Bit4,
+
 	HEADER_TIMESTAMP_FSYNC = Bit3 | Bit2,
 	HEADER_ODR_ACCEL       = Bit1,
 	HEADER_ODR_GYRO        = Bit0,
diff --git a/src/drivers/imu/invensense/icm42670p/CMakeLists.txt b/src/drivers/imu/invensense/icm42670p/CMakeLists.txt
index b426216c30..c2f9fba63d 100644
--- a/src/drivers/imu/invensense/icm42670p/CMakeLists.txt
+++ b/src/drivers/imu/invensense/icm42670p/CMakeLists.txt
@@ -41,6 +41,4 @@ px4_add_module(
 		InvenSense_ICM42670P_registers.hpp
 	DEPENDS
 		px4_work_queue
-		drivers_accelerometer
-		drivers_gyroscope
 	)
diff --git a/src/drivers/imu/invensense/icm42670p/ICM42670P.cpp b/src/drivers/imu/invensense/icm42670p/ICM42670P.cpp
index a7d1244a43..b02da7b78d 100644
--- a/src/drivers/imu/invensense/icm42670p/ICM42670P.cpp
+++ b/src/drivers/imu/invensense/icm42670p/ICM42670P.cpp
@@ -33,6 +33,8 @@
 
 #include "ICM42670P.hpp"
 
+#include <lib/parameters/param.h>
+
 using namespace time_literals;
 
 static constexpr int16_t combine(uint8_t msb, uint8_t lsb)
@@ -44,14 +46,16 @@ ICM42670P::ICM42670P(const I2CSPIDriverConfig &config):
 	SPI(config),
 	I2CSPIDriver(config),
 	_drdy_gpio(config.drdy_gpio),
-	_px4_accel(get_device_id(), config.rotation),
-	_px4_gyro(get_device_id(), config.rotation)
+	_rotation(config.rotation)
 {
-	if (config.drdy_gpio != 0) {
+	if (_drdy_gpio != 0) {
 		_drdy_missed_perf = perf_alloc(PC_COUNT, MODULE_NAME": DRDY missed");
 	}
 
-	ConfigureSampleRate(_px4_gyro.get_max_rate_hz());
+	int32_t imu_gyro_rate_max = 400;
+	param_get(param_find("IMU_GYRO_RATEMAX"), &imu_gyro_rate_max);
+
+	ConfigureSampleRate(imu_gyro_rate_max);
 }
 
 ICM42670P::~ICM42670P()
@@ -61,6 +65,7 @@ ICM42670P::~ICM42670P()
 	perf_free(_fifo_empty_perf);
 	perf_free(_fifo_overflow_perf);
 	perf_free(_fifo_reset_perf);
+	perf_free(_fifo_timestamp_error_perf);
 	perf_free(_drdy_missed_perf);
 }
 
@@ -102,19 +107,25 @@ void ICM42670P::print_status()
 	perf_print_counter(_fifo_empty_perf);
 	perf_print_counter(_fifo_overflow_perf);
 	perf_print_counter(_fifo_reset_perf);
+	perf_print_counter(_fifo_timestamp_error_perf);
 	perf_print_counter(_drdy_missed_perf);
 }
 
 int ICM42670P::probe()
 {
-	const uint8_t whoami = RegisterRead(Register::BANK_0::WHO_AM_I);
+	// 3 retries
+	for (int retry = 0; retry < 3; retry++) {
+		uint8_t whoami = RegisterRead(Register::BANK_0::WHO_AM_I);
 
-	if (whoami != WHOAMI) {
-		DEVICE_DEBUG("unexpected WHO_AM_I 0x%02x", whoami);
-		return PX4_ERROR;
+		if (whoami == WHOAMI) {
+			return PX4_OK;
+
+		} else {
+			DEVICE_DEBUG("unexpected WHO_AM_I 0x%02x", whoami);
+		}
 	}
 
-	return PX4_OK;
+	return PX4_ERROR;
 }
 
 void ICM42670P::RunImpl()
@@ -149,8 +160,8 @@ void ICM42670P::RunImpl()
 				ScheduleDelayed(100_ms);
 
 			} else {
-				PX4_DEBUG("Reset not complete, check again in 10 ms");
-				ScheduleDelayed(10_ms);
+				PX4_DEBUG("Reset not complete, check again in 100 ms");
+				ScheduleDelayed(100_ms);
 			}
 		}
 
@@ -158,21 +169,9 @@ void ICM42670P::RunImpl()
 
 	case STATE::CONFIGURE:
 		if (Configure()) {
-			// 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 {
-				_data_ready_interrupt_enabled = false;
-				ScheduleOnInterval(_fifo_empty_interval_us, _fifo_empty_interval_us);
-			}
-
-			FIFOReset();
+			// if configure succeeded then reset the FIFO
+			_state = STATE::FIFO_RESET;
+			ScheduleDelayed(1_ms);
 
 		} else {
 			// CONFIGURE not complete
@@ -189,6 +188,24 @@ void ICM42670P::RunImpl()
 
 		break;
 
+	case STATE::FIFO_RESET:
+
+		_state = STATE::FIFO_READ;
+		FIFOReset();
+
+		if (DataReadyInterruptConfigure()) {
+			_data_ready_interrupt_enabled = true;
+
+			// backup schedule as a watchdog timeout
+			ScheduleDelayed(100_ms);
+
+		} else {
+			_data_ready_interrupt_enabled = false;
+			ScheduleOnInterval(_fifo_empty_interval_us, _fifo_empty_interval_us);
+		}
+
+		break;
+
 	case STATE::FIFO_READ: {
 			hrt_abstime timestamp_sample = now;
 			uint8_t samples = 0;
@@ -197,7 +214,7 @@ void ICM42670P::RunImpl()
 				// scheduled from interrupt if _drdy_timestamp_sample was set as expected
 				const hrt_abstime drdy_timestamp_sample = _drdy_timestamp_sample.fetch_and(0);
 
-				if ((now - drdy_timestamp_sample) < _fifo_empty_interval_us) {
+				if (now < drdy_timestamp_sample + _fifo_empty_interval_us) {
 					timestamp_sample = drdy_timestamp_sample;
 					samples = _fifo_gyro_samples;
 
@@ -209,6 +226,8 @@ void ICM42670P::RunImpl()
 				ScheduleDelayed(_fifo_empty_interval_us * 2);
 			}
 
+			bool success = false;
+
 			if (samples == 0) {
 				// check current FIFO count
 				const uint16_t fifo_count = FIFOReadCount();
@@ -221,27 +240,42 @@ void ICM42670P::RunImpl()
 					perf_count(_fifo_empty_perf);
 
 				} else {
-					// FIFO count (size in bytes)
-					samples = (fifo_count / sizeof(FIFO::DATA));
+					// FIFO count (size in bytes) should be a multiple of the FIFO::DATA structure
+					samples = fifo_count / sizeof(FIFO::DATA);
 
-					// tolerate minor jitter, leave sample to next iteration if behind by only 1
-					if (samples == _fifo_gyro_samples + 1) {
-						timestamp_sample -= static_cast<int>(FIFO_SAMPLE_DT);
-						samples--;
+					if (samples > _fifo_gyro_samples) {
+						// grab desired number of samples, but reschedule next cycle sooner
+						const int extra_samples = samples - _fifo_gyro_samples;
+						samples = _fifo_gyro_samples;
+
+						if (_fifo_gyro_samples > extra_samples) {
+							// reschedule to run when a total of _fifo_gyro_samples should be available in the FIFO
+							const uint32_t reschedule_delay_us = (_fifo_gyro_samples - extra_samples) * static_cast<int>(FIFO_SAMPLE_DT);
+							ScheduleOnInterval(_fifo_empty_interval_us, reschedule_delay_us);
+
+						} else {
+							// otherwise reschedule to run immediately
+							ScheduleOnInterval(_fifo_empty_interval_us);
+						}
+
+					} else if (samples < _fifo_gyro_samples) {
+						// reschedule next cycle to catch the desired number of samples
+						ScheduleOnInterval(_fifo_empty_interval_us, (_fifo_gyro_samples - samples) * static_cast<int>(FIFO_SAMPLE_DT));
 					}
 
-					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;
+					if (samples == _fifo_gyro_samples) {
+						if (FIFORead(now, samples)) {
+							success = true;
+
+							if (_failure_count > 0) {
+								_failure_count--;
+							}
+						}
 					}
 				}
 			}
 
-			bool success = false;
-
-			if (samples >= 1) {
+			if (samples == _fifo_gyro_samples) {
 				if (FIFORead(timestamp_sample, samples)) {
 					success = true;
 
@@ -256,13 +290,14 @@ void ICM42670P::RunImpl()
 
 				// full reset if things are failing consistently
 				if (_failure_count > 10) {
+					PX4_DEBUG("Full reset because things are failing consistently");
 					Reset();
 					return;
 				}
 			}
 
+			// check configuration registers periodically or immediately following any failure
 			if (!success || hrt_elapsed_time(&_last_config_check_timestamp) > 100_ms) {
-				// check configuration registers periodically or immediately following any failure
 				if (RegisterCheck(_register_bank0_cfg[_checked_register_bank0])
 				    && RegisterCheck(_register_mreg1_cfg[_checked_register_mreg1])
 				   ) {
@@ -275,13 +310,6 @@ void ICM42670P::RunImpl()
 					perf_count(_bad_register_perf);
 					Reset();
 				}
-
-			} else {
-				// periodically update temperature (~1 Hz)
-				if (hrt_elapsed_time(&_temperature_update_timestamp) >= 1_s) {
-					UpdateTemperature();
-					_temperature_update_timestamp = now;
-				}
 			}
 		}
 
@@ -295,10 +323,10 @@ void ICM42670P::ConfigureSampleRate(int sample_rate)
 	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));
+	_fifo_gyro_samples = roundf(math::min((float)_fifo_empty_interval_us / (1e6f / 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);
+	_fifo_empty_interval_us = _fifo_gyro_samples * (1e6f / RATE);
 
 	ConfigureFIFOWatermark(_fifo_gyro_samples);
 }
@@ -346,12 +374,6 @@ bool ICM42670P::Configure()
 		}
 	}
 
-	_px4_accel.set_scale(CONSTANTS_ONE_G / 2048.f);
-	_px4_accel.set_range(16.f * CONSTANTS_ONE_G);
-
-	_px4_gyro.set_scale(math::radians(2000.f / 32768.f));
-	_px4_gyro.set_range(math::radians(2000.f));
-
 	return success;
 }
 
@@ -374,7 +396,7 @@ bool ICM42670P::DataReadyInterruptConfigure()
 	}
 
 	// Setup data ready on falling edge
-	return px4_arch_gpiosetevent(_drdy_gpio, false, true, true, &DataReadyInterruptCallback, this) == 0;
+	return (px4_arch_gpiosetevent(_drdy_gpio, false, true, false, &DataReadyInterruptCallback, this) == 0);
 }
 
 bool ICM42670P::DataReadyInterruptDisable()
@@ -383,7 +405,7 @@ bool ICM42670P::DataReadyInterruptDisable()
 		return false;
 	}
 
-	return px4_arch_gpiosetevent(_drdy_gpio, false, false, false, nullptr, nullptr) == 0;
+	return (px4_arch_gpiosetevent(_drdy_gpio, false, false, false, nullptr, nullptr) == 0);
 }
 
 template <typename T>
@@ -457,7 +479,6 @@ void ICM42670P::RegisterWrite(Register::MREG1 reg, uint8_t value)
 	px4_udelay(10);
 }
 
-
 template <typename T>
 void ICM42670P::RegisterSetAndClearBits(T reg, uint8_t setbits, uint8_t clearbits)
 {
@@ -472,22 +493,35 @@ void ICM42670P::RegisterSetAndClearBits(T reg, uint8_t setbits, uint8_t clearbit
 
 uint16_t ICM42670P::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;
+	// transfer buffer
+	struct TransferBuffer {
+		uint8_t cmd{static_cast<uint8_t>(Register::BANK_0::FIFO_COUNTH) | DIR_READ};
+		uint8_t FIFO_COUNTH{0};
+		uint8_t FIFO_COUNTL{0};
+	} buffer{};
 
-	if (transfer(fifo_count_buf, fifo_count_buf, sizeof(fifo_count_buf)) != PX4_OK) {
+	// read FIFO count
+	if (transfer((uint8_t *)&buffer, (uint8_t *)&buffer, sizeof(buffer)) != PX4_OK) {
 		perf_count(_bad_transfer_perf);
 		return 0;
 	}
 
-	return combine(fifo_count_buf[1], fifo_count_buf[2]);
+	return (buffer.FIFO_COUNTH << 8) + buffer.FIFO_COUNTL;
 }
 
 bool ICM42670P::FIFORead(const hrt_abstime &timestamp_sample, uint8_t samples)
 {
-	FIFOTransferBuffer buffer{};
-	const size_t transfer_size = math::min(samples * sizeof(FIFO::DATA) + 6, FIFO::SIZE);
+	// FIFO transfer buffer
+	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] {};
+	} buffer{};
+
+	// cmd + INT_STATUS + FIFO_COUNTH + FIFO_COUNTL + samples (FIFO::DATA)
+	const size_t transfer_size = 4 + math::min(samples * sizeof(FIFO::DATA), FIFO::SIZE);
 
 	if (transfer((uint8_t *)&buffer, (uint8_t *)&buffer, transfer_size) != PX4_OK) {
 		perf_count(_bad_transfer_perf);
@@ -500,7 +534,7 @@ bool ICM42670P::FIFORead(const hrt_abstime &timestamp_sample, uint8_t samples)
 		return false;
 	}
 
-	const uint16_t fifo_count_bytes = combine(buffer.FIFO_COUNTH, buffer.FIFO_COUNTL);
+	const uint16_t fifo_count_bytes = (buffer.FIFO_COUNTH << 8) + buffer.FIFO_COUNTL;
 
 	if (fifo_count_bytes >= FIFO::SIZE) {
 		perf_count(_fifo_overflow_perf);
@@ -515,52 +549,151 @@ bool ICM42670P::FIFORead(const hrt_abstime &timestamp_sample, uint8_t samples)
 		return false;
 	}
 
+	sensor_imu_fifo_s sensor_imu_fifo{};
+
+	sensor_imu_fifo.timestamp_sample = timestamp_sample;
+	sensor_imu_fifo.device_id = get_device_id();
+	sensor_imu_fifo.dt = FIFO_SAMPLE_DT;
+	sensor_imu_fifo.samples = 0;
+	sensor_imu_fifo.accel_scale = CONSTANTS_ONE_G / 8192.f; // highres accel data 8192 LSB/g
+	sensor_imu_fifo.gyro_scale = math::radians(1.f / 131.f); // highres gyro data 131 LSB/dps
+
 	// check FIFO header in every sample
 	uint8_t valid_samples = 0;
 
+	float temperature_sum = 0;
+
+	float timestamp_interval_sum = 0;
+	int timestamp_interval_sum_count = 0;
+
+	bool accel_scale_16bit = false; // 18-bits of accelerometer data
+	bool gyro_scale_16bit = false; // 20-bits of gyroscope data
+
 	for (int i = 0; i < math::min(samples, fifo_count_samples); i++) {
-		bool valid = true;
+		const FIFO::DATA &fifo = buffer.f[i];
 
 		// 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;
+		const uint8_t FIFO_HEADER = fifo.FIFO_Header;
 
-		if (FIFO_HEADER & FIFO::FIFO_HEADER_BIT::HEADER_MSG) {
-			// FIFO sample empty if HEADER_MSG set
-			valid = false;
+		const bool HEADER_MSG       = FIFO_HEADER & FIFO::FIFO_HEADER_BIT::HEADER_MSG; // FIFO is empty
+		const bool HEADER_ACCEL     = FIFO_HEADER & FIFO::FIFO_HEADER_BIT::HEADER_ACCEL;
+		const bool HEADER_GYRO      = FIFO_HEADER & FIFO::FIFO_HEADER_BIT::HEADER_GYRO;
+		// 3:2 HEADER_TIMESTAMP_FSYNC
+		const bool HEADER_ODR_ACCEL = FIFO_HEADER & FIFO::FIFO_HEADER_BIT::HEADER_ODR_ACCEL; // ODR for accel is different
+		const bool HEADER_ODR_GYRO  = FIFO_HEADER & FIFO::FIFO_HEADER_BIT::HEADER_ODR_GYRO;  // ODR for gyro is different
 
-		} else if (!(FIFO_HEADER & FIFO::FIFO_HEADER_BIT::HEADER_ACCEL)) {
-			// accel bit not set
-			valid = false;
+		if (!HEADER_MSG && HEADER_ACCEL && HEADER_GYRO && !HEADER_ODR_ACCEL && !HEADER_ODR_GYRO) {
 
-		} else if (!(FIFO_HEADER & FIFO::FIFO_HEADER_BIT::HEADER_GYRO)) {
-			// gyro bit not set
-			valid = false;
+			// 20 bit data scaled to 16 bit
+			sensor_imu_fifo.accel_scale = CONSTANTS_ONE_G / 2048.f;
 
-		} else if (FIFO_HEADER & FIFO::FIFO_HEADER_BIT::HEADER_20) {
-			// Packet does not contain a new and valid extended 20-bit data
-			valid = false;
+			sensor_imu_fifo.accel_x[valid_samples] = combine(fifo.ACCEL_DATA_X1, fifo.ACCEL_DATA_X0);
+			sensor_imu_fifo.accel_y[valid_samples] = combine(fifo.ACCEL_DATA_Y1, fifo.ACCEL_DATA_Y0);
+			sensor_imu_fifo.accel_z[valid_samples] = combine(fifo.ACCEL_DATA_Z1, fifo.ACCEL_DATA_Z0);
 
-		} else if (FIFO_HEADER & FIFO::FIFO_HEADER_BIT::HEADER_ODR_ACCEL) {
-			// accel ODR changed
-			valid = false;
+			sensor_imu_fifo.gyro_x[i] = combine(fifo.GYRO_DATA_X1, fifo.GYRO_DATA_X0);
+			sensor_imu_fifo.gyro_y[i] = combine(fifo.GYRO_DATA_Y1, fifo.GYRO_DATA_Y0);
+			sensor_imu_fifo.gyro_z[i] = combine(fifo.GYRO_DATA_Z1, fifo.GYRO_DATA_Z0);
 
-		} else if (FIFO_HEADER & FIFO::FIFO_HEADER_BIT::HEADER_ODR_GYRO) {
-			// gyro ODR changed
-			valid = false;
-		}
+			// temperature
+			const int16_t TEMP_DATA = combine(fifo.TEMP_DATA1, fifo.TEMP_DATA0);
+
+			// sample invalid if -32768
+			if (TEMP_DATA != -32768) {
+				temperature_sum += TEMP_DATA;
+
+			} else {
+				break;
+			}
+
+
+			// HEADER_TIMESTAMP_FSYNC - 0b10: Packet contains ODR Timestamp
+			if (FIFO_HEADER & Bit3) {
+				const uint16_t timestamp = (fifo.TimeStamp_h << 8) + fifo.TimeStamp_l;
+
+				if (_timestamp_prev != 0) {
+					// If TMST_RES = 0 (corresponding to timestamp resolution of 1µs), timestamp interval reported in FIFO requires scaling by a factor of 32/30.
+					// Document Number: DS-000347 Revision: 1.5 Page 59 of 110
+					static constexpr float FIFO_DT_SCALE = 32.f / 30.f;
+
+					float dt = 0;
+
+					if (timestamp > _timestamp_prev) {
+						dt = static_cast<float>(timestamp - _timestamp_prev) * FIFO_DT_SCALE;
+
+					} else if (timestamp < _timestamp_prev) {
+						// uint16_t rollover
+						uint32_t timestamp_new = UINT16_MAX + timestamp;
+						dt = static_cast<float>(timestamp_new - _timestamp_prev) * FIFO_DT_SCALE;
+					}
+
+					timestamp_interval_sum += dt;
+					timestamp_interval_sum_count++;
+
+					// check dt is within +=2% of expected value
+					if ((dt < (FIFO_SAMPLE_DT * 0.98f)) || (dt > (FIFO_SAMPLE_DT * 1.02f))) {
+						perf_count(_fifo_timestamp_error_perf);
+					}
+				}
+
+				_timestamp_prev = timestamp;
+			}
 
-		if (valid) {
 			valid_samples++;
-
-		} else {
-			perf_count(_bad_transfer_perf);
-			break;
 		}
 	}
 
 	if (valid_samples > 0) {
-		ProcessGyro(timestamp_sample, buffer.f, valid_samples);
-		ProcessAccel(timestamp_sample, buffer.f, valid_samples);
+
+		sensor_imu_fifo.samples = valid_samples;
+
+		for (int i = 0; i < sensor_imu_fifo.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)
+
+			// sensor_imu_fifo.accel_x[i]
+			sensor_imu_fifo.accel_y[i] = math::negate(sensor_imu_fifo.accel_y[i]);
+			sensor_imu_fifo.accel_z[i] = math::negate(sensor_imu_fifo.accel_z[i]);
+			rotate_3i(_rotation, sensor_imu_fifo.accel_x[i], sensor_imu_fifo.accel_y[i], sensor_imu_fifo.accel_z[i]);
+
+			// sensor_imu_fifo.gyro_x[i]
+			sensor_imu_fifo.gyro_y[i] = math::negate(sensor_imu_fifo.gyro_y[i]);
+			sensor_imu_fifo.gyro_z[i] = math::negate(sensor_imu_fifo.gyro_z[i]);
+			rotate_3i(_rotation, sensor_imu_fifo.gyro_x[i], sensor_imu_fifo.gyro_y[i], sensor_imu_fifo.gyro_z[i]);
+		}
+
+		const float temperature_avg = temperature_sum / valid_samples;
+
+		// use average temperature reading
+		const float TEMP_degC = (temperature_avg / TEMPERATURE_SENSITIVITY) + TEMPERATURE_OFFSET;
+
+		if (PX4_ISFINITE(TEMP_degC)) {
+			sensor_imu_fifo.temperature = TEMP_degC;
+
+		} else {
+			perf_count(_bad_transfer_perf);
+			return false;
+		}
+
+		if (timestamp_interval_sum > 0) {
+			const float dt_avg = (timestamp_interval_sum / timestamp_interval_sum_count);
+
+			// check dt is within +=1% of expected value
+			if ((dt_avg < (FIFO_SAMPLE_DT * 0.99f)) || (dt_avg > (FIFO_SAMPLE_DT * 1.01f))) {
+				PX4_ERR("DT error %.6f", (double)dt_avg);
+				perf_count(_fifo_timestamp_error_perf);
+
+			} else {
+				sensor_imu_fifo.dt = dt_avg;
+			}
+		}
+
+		sensor_imu_fifo.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);
+
+		sensor_imu_fifo.timestamp = hrt_absolute_time();
+		_sensor_imu_fifo_pub.publish(sensor_imu_fifo);
+
 		return true;
 	}
 
@@ -571,93 +704,11 @@ void ICM42670P::FIFOReset()
 {
 	perf_count(_fifo_reset_perf);
 
-	// FIFO flush requires the following programming sequence:
-	//  Write FIFO_FLUSH = 1
-	//  Wait for 1.5 µs
-	//  Read FIFO_FLUSH, it should now be 0
-
 	// SIGNAL_PATH_RESET: FIFO flush
 	RegisterSetBits(Register::BANK_0::SIGNAL_PATH_RESET, SIGNAL_PATH_RESET_BIT::FIFO_FLUSH);
-	px4_udelay(2); // Wait for 1.5 µs
-
-	const uint8_t SIGNAL_PATH_RESET = RegisterRead(Register::BANK_0::SIGNAL_PATH_RESET);
-
-	if ((SIGNAL_PATH_RESET & SIGNAL_PATH_RESET_BIT::FIFO_FLUSH) != 0) {
-		PX4_DEBUG("SIGNAL_PATH_RESET FIFO_FLUSH failed");
-	}
 
 	// reset while FIFO is disabled
 	_drdy_timestamp_sample.store(0);
-}
-
-void ICM42670P::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 = samples;
-	accel.dt = FIFO_SAMPLE_DT;
-
-	for (int i = 0; i < samples; i++) {
-		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);
-
-		// 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;
-		accel.y[i] = math::negate(accel_y);
-		accel.z[i] = math::negate(accel_z);
-	}
-
-	_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));
-
-	_px4_accel.updateFIFO(accel);
-}
-
-void ICM42670P::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 = samples;
-	gyro.dt = FIFO_SAMPLE_DT;
-
-	for (int i = 0; i < samples; i++) {
-		const int16_t gyro_x = combine(fifo[i].GYRO_DATA_X1, fifo[i].GYRO_DATA_X0);
-		const int16_t gyro_y = combine(fifo[i].GYRO_DATA_Y1, fifo[i].GYRO_DATA_Y0);
-		const int16_t gyro_z = combine(fifo[i].GYRO_DATA_Z1, fifo[i].GYRO_DATA_Z0);
-
-		// 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;
-		gyro.y[i] = math::negate(gyro_y);
-		gyro.z[i] = math::negate(gyro_z);
-	}
-
-	_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));
-
-	_px4_gyro.updateFIFO(gyro);
-}
-
-void ICM42670P::UpdateTemperature()
-{
-	// read current temperature
-	uint8_t temperature_buf[3] {};
-	temperature_buf[0] = static_cast<uint8_t>(Register::BANK_0::TEMP_DATA1) | DIR_READ;
-
-	if (transfer(temperature_buf, temperature_buf, sizeof(temperature_buf)) != PX4_OK) {
-		perf_count(_bad_transfer_perf);
-		return;
-	}
-
-	const int16_t TEMP_DATA = combine(temperature_buf[1], temperature_buf[2]);
-
-	// Temperature in Degrees Centigrade
-	const float TEMP_degC = (TEMP_DATA / TEMPERATURE_SENSITIVITY) + TEMPERATURE_OFFSET;
-
-	if (PX4_ISFINITE(TEMP_degC)) {
-		_px4_accel.set_temperature(TEMP_degC);
-		_px4_gyro.set_temperature(TEMP_degC);
-	}
+
+	_timestamp_prev = 0;
 }
diff --git a/src/drivers/imu/invensense/icm42670p/ICM42670P.hpp b/src/drivers/imu/invensense/icm42670p/ICM42670P.hpp
index 362418f7ae..201b9b2b24 100644
--- a/src/drivers/imu/invensense/icm42670p/ICM42670P.hpp
+++ b/src/drivers/imu/invensense/icm42670p/ICM42670P.hpp
@@ -43,13 +43,15 @@
 #include "InvenSense_ICM42670P_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/PublicationMulti.hpp>
+#include <uORB/topics/sensor_imu_fifo.h>
+
 using namespace InvenSense_ICM42670P;
 
 class ICM42670P : public device::SPI, public I2CSPIDriver<ICM42670P>
@@ -70,24 +72,10 @@ private:
 
 	// Sensor Configuration
 	static constexpr float FIFO_SAMPLE_DT{1e6f / 1600.f};     // 1600 Hz accel & gyro ODR configured
-	static constexpr float GYRO_RATE{1e6f / FIFO_SAMPLE_DT};
-	static constexpr float ACCEL_RATE{1e6f / FIFO_SAMPLE_DT};
+	static constexpr float RATE{1e6f / FIFO_SAMPLE_DT};
 
 	// maximum FIFO samples per transfer is limited to the size of sensor_accel_fifo/sensor_gyro_fifo
-	static constexpr int32_t FIFO_MAX_SAMPLES{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))};
-
-	// 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 INT_STATUS2{0};
-		uint8_t INT_STATUS3{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) == (6 + FIFO_MAX_SAMPLES *sizeof(FIFO::DATA)));
+	static constexpr int32_t FIFO_MAX_SAMPLES{math::min(FIFO::SIZE / sizeof(FIFO::DATA), (size_t)sensor_imu_fifo_s::FIFO_SIZE)};
 
 	struct register_bank0_config_t {
 		Register::BANK_0 reg;
@@ -129,20 +117,18 @@ private:
 	bool FIFORead(const hrt_abstime &timestamp_sample, uint8_t samples);
 	void FIFOReset();
 
-	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);
-	void UpdateTemperature();
-
 	const spi_drdy_gpio_t _drdy_gpio;
 
-	PX4Accelerometer _px4_accel;
-	PX4Gyroscope _px4_gyro;
+	uORB::PublicationMulti<sensor_imu_fifo_s> _sensor_imu_fifo_pub{ORB_ID(sensor_imu_fifo)};
+
+	const enum Rotation _rotation;
 
 	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 _fifo_timestamp_error_perf{perf_alloc(PC_COUNT, MODULE_NAME": FIFO timestamp error")};
 	perf_counter_t _drdy_missed_perf{nullptr};
 
 	hrt_abstime _reset_timestamp{0};
@@ -150,6 +136,8 @@ private:
 	hrt_abstime _temperature_update_timestamp{0};
 	int _failure_count{0};
 
+	uint16_t _timestamp_prev{0};
+
 	px4::atomic<hrt_abstime> _drdy_timestamp_sample{0};
 	bool _data_ready_interrupt_enabled{false};
 
@@ -157,11 +145,12 @@ private:
 		RESET,
 		WAIT_FOR_RESET,
 		CONFIGURE,
+		FIFO_RESET,
 		FIFO_READ,
 	} _state{STATE::RESET};
 
 	uint16_t _fifo_empty_interval_us{1250}; // default 1250 us / 800 Hz transfer interval
-	int32_t _fifo_gyro_samples{static_cast<int32_t>(_fifo_empty_interval_us / (1000000 / GYRO_RATE))};
+	int32_t _fifo_gyro_samples{static_cast<int32_t>(_fifo_empty_interval_us / (1000000 / RATE))};
 
 	uint8_t _checked_register_bank0{0};
 	static constexpr uint8_t size_register_bank0_cfg{10};
diff --git a/src/drivers/imu/invensense/icm42670p/InvenSense_ICM42670P_registers.hpp b/src/drivers/imu/invensense/icm42670p/InvenSense_ICM42670P_registers.hpp
index 5e9d8259c7..3daab23177 100644
--- a/src/drivers/imu/invensense/icm42670p/InvenSense_ICM42670P_registers.hpp
+++ b/src/drivers/imu/invensense/icm42670p/InvenSense_ICM42670P_registers.hpp
@@ -231,7 +231,7 @@ 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
diff --git a/src/drivers/imu/invensense/icm42688p/CMakeLists.txt b/src/drivers/imu/invensense/icm42688p/CMakeLists.txt
index 8ffd2f7328..e94dcc4314 100644
--- a/src/drivers/imu/invensense/icm42688p/CMakeLists.txt
+++ b/src/drivers/imu/invensense/icm42688p/CMakeLists.txt
@@ -1,6 +1,6 @@
 ############################################################################
 #
-#   Copyright (c) 2020 PX4 Development Team. All rights reserved.
+#   Copyright (c) 2020-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
@@ -42,6 +42,4 @@ px4_add_module(
 		InvenSense_ICM42688P_registers.hpp
 	DEPENDS
 		px4_work_queue
-		drivers_accelerometer
-		drivers_gyroscope
 	)
diff --git a/src/drivers/imu/invensense/icm42688p/ICM42688P.cpp b/src/drivers/imu/invensense/icm42688p/ICM42688P.cpp
index dea93d9524..7d003c0fc3 100644
--- a/src/drivers/imu/invensense/icm42688p/ICM42688P.cpp
+++ b/src/drivers/imu/invensense/icm42688p/ICM42688P.cpp
@@ -33,6 +33,8 @@
 
 #include "ICM42688P.hpp"
 
+#include <lib/parameters/param.h>
+
 using namespace time_literals;
 
 static constexpr int16_t combine(uint8_t msb, uint8_t lsb)
@@ -44,14 +46,16 @@ ICM42688P::ICM42688P(const I2CSPIDriverConfig &config) :
 	SPI(config),
 	I2CSPIDriver(config),
 	_drdy_gpio(config.drdy_gpio),
-	_px4_accel(get_device_id(), config.rotation),
-	_px4_gyro(get_device_id(), config.rotation)
+	_rotation(config.rotation)
 {
-	if (config.drdy_gpio != 0) {
+	if (_drdy_gpio != 0) {
 		_drdy_missed_perf = perf_alloc(PC_COUNT, MODULE_NAME": DRDY missed");
 	}
 
-	ConfigureSampleRate(_px4_gyro.get_max_rate_hz());
+	int32_t imu_gyro_rate_max = 400;
+	param_get(param_find("IMU_GYRO_RATEMAX"), &imu_gyro_rate_max);
+
+	ConfigureSampleRate(imu_gyro_rate_max);
 }
 
 ICM42688P::~ICM42688P()
@@ -61,6 +65,7 @@ ICM42688P::~ICM42688P()
 	perf_free(_fifo_empty_perf);
 	perf_free(_fifo_overflow_perf);
 	perf_free(_fifo_reset_perf);
+	perf_free(_fifo_timestamp_error_perf);
 	perf_free(_drdy_missed_perf);
 }
 
@@ -102,12 +107,14 @@ void ICM42688P::print_status()
 	perf_print_counter(_fifo_empty_perf);
 	perf_print_counter(_fifo_overflow_perf);
 	perf_print_counter(_fifo_reset_perf);
+	perf_print_counter(_fifo_timestamp_error_perf);
 	perf_print_counter(_drdy_missed_perf);
 }
 
 int ICM42688P::probe()
 {
-	for (int i = 0; i < 3; i++) {
+	// 3 retries
+	for (int retry = 0; retry < 3; retry++) {
 		uint8_t whoami = RegisterRead(Register::BANK_0::WHO_AM_I);
 
 		if (whoami == WHOAMI) {
@@ -162,8 +169,8 @@ void ICM42688P::RunImpl()
 				ScheduleDelayed(100_ms);
 
 			} else {
-				PX4_DEBUG("Reset not complete, check again in 10 ms");
-				ScheduleDelayed(10_ms);
+				PX4_DEBUG("Reset not complete, check again in 100 ms");
+				ScheduleDelayed(100_ms);
 			}
 		}
 
@@ -216,7 +223,7 @@ void ICM42688P::RunImpl()
 				// scheduled from interrupt if _drdy_timestamp_sample was set as expected
 				const hrt_abstime drdy_timestamp_sample = _drdy_timestamp_sample.fetch_and(0);
 
-				if ((now - drdy_timestamp_sample) < _fifo_empty_interval_us) {
+				if (now < drdy_timestamp_sample + _fifo_empty_interval_us) {
 					timestamp_sample = drdy_timestamp_sample;
 					samples = _fifo_gyro_samples;
 
@@ -228,6 +235,8 @@ void ICM42688P::RunImpl()
 				ScheduleDelayed(_fifo_empty_interval_us * 2);
 			}
 
+			bool success = false;
+
 			if (samples == 0) {
 				// check current FIFO count
 				const uint16_t fifo_count = FIFOReadCount();
@@ -240,27 +249,42 @@ void ICM42688P::RunImpl()
 					perf_count(_fifo_empty_perf);
 
 				} else {
-					// FIFO count (size in bytes)
-					samples = (fifo_count / sizeof(FIFO::DATA));
+					// FIFO count (size in bytes) should be a multiple of the FIFO::DATA structure
+					samples = fifo_count / sizeof(FIFO::DATA);
 
-					// tolerate minor jitter, leave sample to next iteration if behind by only 1
-					if (samples == _fifo_gyro_samples + 1) {
-						timestamp_sample -= static_cast<int>(FIFO_SAMPLE_DT);
-						samples--;
+					if (samples > _fifo_gyro_samples) {
+						// grab desired number of samples, but reschedule next cycle sooner
+						const int extra_samples = samples - _fifo_gyro_samples;
+						samples = _fifo_gyro_samples;
+
+						if (_fifo_gyro_samples > extra_samples) {
+							// reschedule to run when a total of _fifo_gyro_samples should be available in the FIFO
+							const uint32_t reschedule_delay_us = (_fifo_gyro_samples - extra_samples) * static_cast<int>(FIFO_SAMPLE_DT);
+							ScheduleOnInterval(_fifo_empty_interval_us, reschedule_delay_us);
+
+						} else {
+							// otherwise reschedule to run immediately
+							ScheduleOnInterval(_fifo_empty_interval_us);
+						}
+
+					} else if (samples < _fifo_gyro_samples) {
+						// reschedule next cycle to catch the desired number of samples
+						ScheduleOnInterval(_fifo_empty_interval_us, (_fifo_gyro_samples - samples) * static_cast<int>(FIFO_SAMPLE_DT));
 					}
 
-					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;
+					if (samples == _fifo_gyro_samples) {
+						if (FIFORead(now, samples)) {
+							success = true;
+
+							if (_failure_count > 0) {
+								_failure_count--;
+							}
+						}
 					}
 				}
 			}
 
-			bool success = false;
-
-			if (samples >= 1) {
+			if (samples == _fifo_gyro_samples) {
 				if (FIFORead(timestamp_sample, samples)) {
 					success = true;
 
@@ -275,13 +299,14 @@ void ICM42688P::RunImpl()
 
 				// full reset if things are failing consistently
 				if (_failure_count > 10) {
+					PX4_DEBUG("Full reset because things are failing consistently");
 					Reset();
 					return;
 				}
 			}
 
+			// check configuration registers periodically or immediately following any failure
 			if (!success || hrt_elapsed_time(&_last_config_check_timestamp) > 100_ms) {
-				// 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])
@@ -309,10 +334,10 @@ void ICM42688P::ConfigureSampleRate(int sample_rate)
 	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));
+	_fifo_gyro_samples = roundf(math::min((float)_fifo_empty_interval_us / (1e6f / 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);
+	_fifo_empty_interval_us = _fifo_gyro_samples * (1e6f / RATE);
 
 	ConfigureFIFOWatermark(_fifo_gyro_samples);
 }
@@ -362,6 +387,7 @@ bool ICM42688P::Configure()
 		RegisterSetAndClearBits(reg_cfg.reg, reg_cfg.set_bits, reg_cfg.clear_bits);
 	}
 
+
 	// now check that all are configured
 	bool success = true;
 
@@ -383,11 +409,6 @@ bool ICM42688P::Configure()
 		}
 	}
 
-	// 20-bits data format used
-	//  the only FSR settings that are operational are ±2000dps for gyroscope and ±16g for accelerometer
-	_px4_accel.set_range(16.f * CONSTANTS_ONE_G);
-	_px4_gyro.set_range(math::radians(2000.f));
-
 	return success;
 }
 
@@ -410,7 +431,7 @@ bool ICM42688P::DataReadyInterruptConfigure()
 	}
 
 	// Setup data ready on falling edge
-	return px4_arch_gpiosetevent(_drdy_gpio, false, true, true, &DataReadyInterruptCallback, this) == 0;
+	return (px4_arch_gpiosetevent(_drdy_gpio, false, true, false, &DataReadyInterruptCallback, this) == 0);
 }
 
 bool ICM42688P::DataReadyInterruptDisable()
@@ -419,7 +440,7 @@ bool ICM42688P::DataReadyInterruptDisable()
 		return false;
 	}
 
-	return px4_arch_gpiosetevent(_drdy_gpio, false, false, false, nullptr, nullptr) == 0;
+	return (px4_arch_gpiosetevent(_drdy_gpio, false, false, false, nullptr, nullptr) == 0);
 }
 
 template <typename T>
@@ -474,114 +495,20 @@ void ICM42688P::RegisterSetAndClearBits(T reg, uint8_t setbits, uint8_t clearbit
 
 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);
+	// transfer buffer
+	struct TransferBuffer {
+		uint8_t cmd{static_cast<uint8_t>(Register::BANK_0::FIFO_COUNTH) | DIR_READ};
+		uint8_t FIFO_COUNTH{0};
+		uint8_t FIFO_COUNTL{0};
+	} buffer{};
 
-	if (transfer(fifo_count_buf, fifo_count_buf, sizeof(fifo_count_buf)) != PX4_OK) {
+	// read FIFO count
+	if (transfer((uint8_t *)&buffer, (uint8_t *)&buffer, sizeof(buffer)) != PX4_OK) {
 		perf_count(_bad_transfer_perf);
 		return 0;
 	}
 
-	return combine(fifo_count_buf[1], fifo_count_buf[2]);
-}
-
-bool ICM42688P::FIFORead(const hrt_abstime &timestamp_sample, uint8_t samples)
-{
-	FIFOTransferBuffer buffer{};
-	const size_t transfer_size = math::min(samples * sizeof(FIFO::DATA) + 4, FIFO::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 uint8_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
-	uint8_t valid_samples = 0;
-
-	for (int i = 0; i < math::min(samples, fifo_count_samples); 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 (valid_samples > 0) {
-		if (ProcessTemperature(buffer.f, valid_samples)) {
-			ProcessGyro(timestamp_sample, buffer.f, valid_samples);
-			ProcessAccel(timestamp_sample, buffer.f, valid_samples);
-			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_timestamp_sample.store(0);
+	return (buffer.FIFO_COUNTH << 8) + buffer.FIFO_COUNTL;
 }
 
 static constexpr int32_t reassemble_20bit(const uint32_t a, const uint32_t b, const uint32_t c)
@@ -601,206 +528,288 @@ static constexpr int32_t reassemble_20bit(const uint32_t a, const uint32_t b, co
 	return static_cast<int32_t>(x);
 }
 
-void ICM42688P::ProcessAccel(const hrt_abstime &timestamp_sample, const FIFO::DATA fifo[], const uint8_t samples)
+bool ICM42688P::FIFORead(const hrt_abstime &timestamp_sample, uint8_t samples)
 {
-	sensor_accel_fifo_s accel{};
-	accel.timestamp_sample = timestamp_sample;
-	accel.samples = 0;
-	accel.dt = FIFO_SAMPLE_DT;
+	// FIFO transfer buffer
+	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] {};
+	} buffer{};
 
-	// 18-bits of accelerometer data
-	bool scale_20bit = false;
+	// cmd + INT_STATUS + FIFO_COUNTH + FIFO_COUNTL + samples (FIFO::DATA)
+	const size_t transfer_size = 4 + math::min(samples * sizeof(FIFO::DATA), FIFO::SIZE);
 
-	// 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);
+	SelectRegisterBank(REG_BANK_SEL_BIT::USER_BANK_0);
 
-		// 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 (transfer((uint8_t *)&buffer, (uint8_t *)&buffer, transfer_size) != PX4_OK) {
+		perf_count(_bad_transfer_perf);
+		return false;
+	}
 
-			if (accel_x >= max_accel || accel_x <= min_accel) {
-				scale_20bit = true;
+	if (buffer.INT_STATUS & INT_STATUS_BIT::FIFO_FULL_INT) {
+		perf_count(_fifo_overflow_perf);
+		FIFOReset();
+		return false;
+	}
+
+	const uint16_t fifo_count_bytes = (buffer.FIFO_COUNTH << 8) + buffer.FIFO_COUNTL;
+
+	if (fifo_count_bytes >= FIFO::SIZE) {
+		perf_count(_fifo_overflow_perf);
+		FIFOReset();
+		return false;
+	}
+
+	const uint8_t fifo_count_samples = fifo_count_bytes / sizeof(FIFO::DATA);
+
+	if (fifo_count_samples == 0) {
+		perf_count(_fifo_empty_perf);
+		return false;
+	}
+
+	sensor_imu_fifo_s sensor_imu_fifo{};
+
+	sensor_imu_fifo.timestamp_sample = timestamp_sample;
+	sensor_imu_fifo.device_id = get_device_id();
+	sensor_imu_fifo.dt = FIFO_SAMPLE_DT;
+	sensor_imu_fifo.samples = 0;
+	sensor_imu_fifo.accel_scale = CONSTANTS_ONE_G / 8192.f; // highres accel data 8192 LSB/g
+	sensor_imu_fifo.gyro_scale = math::radians(1.f / 131.f); // highres gyro data 131 LSB/dps
+
+	// check FIFO header in every sample
+	uint8_t valid_samples = 0;
+
+	float temperature_sum = 0;
+
+	float timestamp_interval_sum = 0;
+	int timestamp_interval_sum_count = 0;
+
+	bool accel_scale_16bit = false; // 18-bits of accelerometer data
+	bool gyro_scale_16bit = false; // 20-bits of gyroscope data
+
+	for (int i = 0; i < math::min(samples, fifo_count_samples); i++) {
+		const FIFO::DATA &fifo = buffer.f[i];
+
+		// With FIFO_ACCEL_EN and FIFO_GYRO_EN header should be 8’b_0110_10xx
+		const uint8_t FIFO_HEADER = fifo.FIFO_Header;
+
+		const bool HEADER_MSG       = FIFO_HEADER & FIFO::FIFO_HEADER_BIT::HEADER_MSG; // FIFO is empty
+		const bool HEADER_ACCEL     = FIFO_HEADER & FIFO::FIFO_HEADER_BIT::HEADER_ACCEL;
+		const bool HEADER_GYRO      = FIFO_HEADER & FIFO::FIFO_HEADER_BIT::HEADER_GYRO;
+		const bool HEADER_20        = FIFO_HEADER & FIFO::FIFO_HEADER_BIT::HEADER_20; // valid sample of extended 20-bit data
+		// 3:2 HEADER_TIMESTAMP_FSYNC
+		const bool HEADER_ODR_ACCEL = FIFO_HEADER & FIFO::FIFO_HEADER_BIT::HEADER_ODR_ACCEL; // ODR for accel is different
+		const bool HEADER_ODR_GYRO  = FIFO_HEADER & FIFO::FIFO_HEADER_BIT::HEADER_ODR_GYRO;  // ODR for gyro is different
+
+		if (!HEADER_MSG && HEADER_ACCEL && HEADER_GYRO && HEADER_20 && !HEADER_ODR_ACCEL && !HEADER_ODR_GYRO) {
+
+			if (!accel_scale_16bit) {
+				// Accel: 20 bit hires mode, Accel data is 18 bit
+				// Sign extension + Accel [19:12] + Accel [11:4] + Accel [3:2] (20 bit extension byte)
+				int32_t accel_x = reassemble_20bit(fifo.ACCEL_DATA_X1, fifo.ACCEL_DATA_X0, fifo.Ext_Accel_X_Gyro_X & 0xF0 >> 4);
+				int32_t accel_y = reassemble_20bit(fifo.ACCEL_DATA_Y1, fifo.ACCEL_DATA_Y0, fifo.Ext_Accel_Y_Gyro_Y & 0xF0 >> 4);
+				int32_t accel_z = reassemble_20bit(fifo.ACCEL_DATA_Z1, fifo.ACCEL_DATA_Z0, fifo.Ext_Accel_Z_Gyro_Z & 0xF0 >> 4);
+
+				// sample invalid if -524288
+				if (accel_x == -524288 || accel_y == -524288 || accel_z == -524288) {
+					break;
+				}
+
+				// shift by 2 (2 least significant bits are always 0)
+				accel_x = accel_x / 4;
+				accel_y = accel_y / 4;
+				accel_z = accel_z / 4;
+
+				// check if any values are going to exceed int16 limits
+				if ((accel_x >= INT16_MAX || accel_x <= INT16_MIN)
+				    || (accel_y >= INT16_MAX || accel_y <= INT16_MIN)
+				    || (accel_z >= INT16_MAX || accel_z <= INT16_MIN)) {
+
+					accel_scale_16bit = true;
+
+					// 20 bit data scaled to 16 bit
+					sensor_imu_fifo.accel_scale = CONSTANTS_ONE_G / 2048.f;
+
+					// rescale any existing data
+					for (int j = 0; j < valid_samples + 1; j++) {
+						sensor_imu_fifo.accel_x[valid_samples] = combine(buffer.f[j].ACCEL_DATA_X1, buffer.f[j].ACCEL_DATA_X0);
+						sensor_imu_fifo.accel_y[valid_samples] = combine(buffer.f[j].ACCEL_DATA_Y1, buffer.f[j].ACCEL_DATA_Y0);
+						sensor_imu_fifo.accel_z[valid_samples] = combine(buffer.f[j].ACCEL_DATA_Z1, buffer.f[j].ACCEL_DATA_Z0);
+					}
+
+				} else {
+					sensor_imu_fifo.accel_x[valid_samples] = accel_x;
+					sensor_imu_fifo.accel_y[valid_samples] = accel_y;
+					sensor_imu_fifo.accel_z[valid_samples] = accel_z;
+				}
+
+			} else {
+				sensor_imu_fifo.accel_x[valid_samples] = combine(fifo.ACCEL_DATA_X1, fifo.ACCEL_DATA_X0);
+				sensor_imu_fifo.accel_y[valid_samples] = combine(fifo.ACCEL_DATA_Y1, fifo.ACCEL_DATA_Y0);
+				sensor_imu_fifo.accel_z[valid_samples] = combine(fifo.ACCEL_DATA_Z1, fifo.ACCEL_DATA_Z0);
 			}
 
-			if (accel_y >= max_accel || accel_y <= min_accel) {
-				scale_20bit = true;
+
+			if (!gyro_scale_16bit) {
+				// Gyro: 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.GYRO_DATA_X1, fifo.GYRO_DATA_X0, fifo.Ext_Accel_X_Gyro_X & 0x0F);
+				int32_t gyro_y = reassemble_20bit(fifo.GYRO_DATA_Y1, fifo.GYRO_DATA_Y0, fifo.Ext_Accel_Y_Gyro_Y & 0x0F);
+				int32_t gyro_z = reassemble_20bit(fifo.GYRO_DATA_Z1, fifo.GYRO_DATA_Z0, fifo.Ext_Accel_Z_Gyro_Z & 0x0F);
+
+				// shift by 1 (least significant bit is always 0)
+				gyro_x = gyro_x / 2;
+				gyro_y = gyro_y / 2;
+				gyro_z = gyro_z / 2;
+
+				// check if any gyro values are going to exceed int16 limits
+				if ((gyro_x >= INT16_MAX || gyro_x <= INT16_MIN)
+				    || (gyro_y >= INT16_MAX || gyro_y <= INT16_MIN)
+				    || (gyro_z >= INT16_MAX || gyro_z <= INT16_MIN)) {
+
+
+					gyro_scale_16bit = true;
+
+					// 20 bit data scaled to 16 bit
+					sensor_imu_fifo.gyro_scale = math::radians(2000.f / 32768.f);
+
+					// rescale any existing data
+					for (int j = 0; j < valid_samples + 1; j++) {
+						sensor_imu_fifo.gyro_x[j] = combine(buffer.f[j].GYRO_DATA_X1, buffer.f[j].GYRO_DATA_X0);
+						sensor_imu_fifo.gyro_y[j] = combine(buffer.f[j].GYRO_DATA_Y1, buffer.f[j].GYRO_DATA_Y0);
+						sensor_imu_fifo.gyro_z[j] = combine(buffer.f[j].GYRO_DATA_Z1, buffer.f[j].GYRO_DATA_Z0);
+					}
+
+				} else {
+					sensor_imu_fifo.gyro_x[valid_samples] = gyro_x;
+					sensor_imu_fifo.gyro_y[valid_samples] = gyro_y;
+					sensor_imu_fifo.gyro_z[valid_samples] = gyro_z;
+				}
+
+			} else {
+				sensor_imu_fifo.gyro_x[i] = combine(fifo.GYRO_DATA_X1, fifo.GYRO_DATA_X0);
+				sensor_imu_fifo.gyro_y[i] = combine(fifo.GYRO_DATA_Y1, fifo.GYRO_DATA_Y0);
+				sensor_imu_fifo.gyro_z[i] = combine(fifo.GYRO_DATA_Z1, fifo.GYRO_DATA_Z0);
 			}
 
-			if (accel_z >= max_accel || accel_z <= min_accel) {
-				scale_20bit = true;
+
+			// temperature
+			const int16_t TEMP_DATA = combine(fifo.TEMP_DATA1, fifo.TEMP_DATA0);
+
+			// sample invalid if -32768
+			if (TEMP_DATA != -32768) {
+				temperature_sum += TEMP_DATA;
+
+			} else {
+				break;
 			}
 
-			// 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
-		_px4_accel.set_scale(CONSTANTS_ONE_G / 8192.f);
+			// HEADER_TIMESTAMP_FSYNC - 0b10: Packet contains ODR Timestamp
+			if (FIFO_HEADER & Bit3) {
+				const uint16_t timestamp = (fifo.TimeStamp_h << 8) + fifo.TimeStamp_l;
 
-	} 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);
+				if (_timestamp_prev != 0) {
+					// If TMST_RES = 0 (corresponding to timestamp resolution of 1µs), timestamp interval reported in FIFO requires scaling by a factor of 32/30.
+					// Document Number: DS-000347 Revision: 1.5 Page 59 of 110
+					static constexpr float FIFO_DT_SCALE = 32.f / 30.f;
 
-			accel.x[i] = accel_x;
-			accel.y[i] = accel_y;
-			accel.z[i] = accel_z;
-		}
+					float dt = 0;
 
-		_px4_accel.set_scale(CONSTANTS_ONE_G / 2048.f);
-	}
+					if (timestamp > _timestamp_prev) {
+						dt = static_cast<float>(timestamp - _timestamp_prev) * FIFO_DT_SCALE;
 
-	// 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];
-	}
+					} else if (timestamp < _timestamp_prev) {
+						// uint16_t rollover
+						uint32_t timestamp_new = UINT16_MAX + timestamp;
+						dt = static_cast<float>(timestamp_new - _timestamp_prev) * FIFO_DT_SCALE;
+					}
 
-	_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));
+					timestamp_interval_sum += dt;
+					timestamp_interval_sum_count++;
 
-	if (accel.samples > 0) {
-		_px4_accel.updateFIFO(accel);
-	}
-}
+					// check dt is within +=2% of expected value
+					if ((dt < (FIFO_SAMPLE_DT * 0.98f)) || (dt > (FIFO_SAMPLE_DT * 1.02f))) {
+						perf_count(_fifo_timestamp_error_perf);
+					}
+				}
 
-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;
+				_timestamp_prev = timestamp;
+			}
 
-	// 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
-		_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);
-		}
-
-		_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];
-	}
-
-	_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) {
-		_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;
-			}
+		sensor_imu_fifo.samples = valid_samples;
+
+		for (int i = 0; i < sensor_imu_fifo.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)
+
+			// sensor_imu_fifo.accel_x[i]
+			sensor_imu_fifo.accel_y[i] = math::negate(sensor_imu_fifo.accel_y[i]);
+			sensor_imu_fifo.accel_z[i] = math::negate(sensor_imu_fifo.accel_z[i]);
+			rotate_3i(_rotation, sensor_imu_fifo.accel_x[i], sensor_imu_fifo.accel_y[i], sensor_imu_fifo.accel_z[i]);
+
+			// sensor_imu_fifo.gyro_x[i]
+			sensor_imu_fifo.gyro_y[i] = math::negate(sensor_imu_fifo.gyro_y[i]);
+			sensor_imu_fifo.gyro_z[i] = math::negate(sensor_imu_fifo.gyro_z[i]);
+			rotate_3i(_rotation, sensor_imu_fifo.gyro_x[i], sensor_imu_fifo.gyro_y[i], sensor_imu_fifo.gyro_z[i]);
 		}
 
+		const float temperature_avg = temperature_sum / valid_samples;
+
 		// use average temperature reading
 		const float TEMP_degC = (temperature_avg / TEMPERATURE_SENSITIVITY) + TEMPERATURE_OFFSET;
 
 		if (PX4_ISFINITE(TEMP_degC)) {
-			_px4_accel.set_temperature(TEMP_degC);
-			_px4_gyro.set_temperature(TEMP_degC);
-			return true;
+			sensor_imu_fifo.temperature = TEMP_degC;
 
 		} else {
 			perf_count(_bad_transfer_perf);
+			return false;
 		}
+
+		if (timestamp_interval_sum > 0) {
+			const float dt_avg = (timestamp_interval_sum / timestamp_interval_sum_count);
+
+			// check dt is within +=1% of expected value
+			if ((dt_avg < (FIFO_SAMPLE_DT * 0.99f)) || (dt_avg > (FIFO_SAMPLE_DT * 1.01f))) {
+				PX4_ERR("DT error %.6f", (double)dt_avg);
+				perf_count(_fifo_timestamp_error_perf);
+
+			} else {
+				sensor_imu_fifo.dt = dt_avg;
+			}
+		}
+
+		sensor_imu_fifo.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);
+
+		sensor_imu_fifo.timestamp = hrt_absolute_time();
+		_sensor_imu_fifo_pub.publish(sensor_imu_fifo);
+
+		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_timestamp_sample.store(0);
+
+	_timestamp_prev = 0;
+}
diff --git a/src/drivers/imu/invensense/icm42688p/ICM42688P.hpp b/src/drivers/imu/invensense/icm42688p/ICM42688P.hpp
index b118f8c920..a24735f6fa 100644
--- a/src/drivers/imu/invensense/icm42688p/ICM42688P.hpp
+++ b/src/drivers/imu/invensense/icm42688p/ICM42688P.hpp
@@ -1,6 +1,6 @@
 /****************************************************************************
  *
- *   Copyright (c) 2020-2021 PX4 Development Team. All rights reserved.
+ *   Copyright (c) 2020-2022 PX4 Development Team. All rights reserved.
  *
  * Redistribution and use in source and binary forms, with or without
  * modification, are permitted provided that the following conditions
@@ -43,14 +43,15 @@
 #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/PublicationMulti.hpp>
+#include <uORB/topics/sensor_imu_fifo.h>
+
 using namespace InvenSense_ICM42688P;
 
 class ICM42688P : public device::SPI, public I2CSPIDriver<ICM42688P>
@@ -71,22 +72,10 @@ private:
 
 	// Sensor Configuration
 	static constexpr float FIFO_SAMPLE_DT{1e6f / 8000.f};     // 8000 Hz accel & gyro ODR configured
-	static constexpr float GYRO_RATE{1e6f / FIFO_SAMPLE_DT};
-	static constexpr float ACCEL_RATE{1e6f / FIFO_SAMPLE_DT};
+	static constexpr float RATE{1e6f / FIFO_SAMPLE_DT};
 
 	// maximum FIFO samples per transfer is limited to the size of sensor_accel_fifo/sensor_gyro_fifo
-	static constexpr int32_t FIFO_MAX_SAMPLES{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))};
-
-	// 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)));
+	static constexpr int32_t FIFO_MAX_SAMPLES{math::min(FIFO::SIZE / sizeof(FIFO::DATA), (size_t)sensor_imu_fifo_s::FIFO_SIZE)};
 
 	struct register_bank0_config_t {
 		Register::BANK_0 reg;
@@ -135,20 +124,18 @@ private:
 	bool FIFORead(const hrt_abstime &timestamp_sample, uint8_t samples);
 	void FIFOReset();
 
-	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;
 
-	PX4Accelerometer _px4_accel;
-	PX4Gyroscope _px4_gyro;
+	uORB::PublicationMulti<sensor_imu_fifo_s> _sensor_imu_fifo_pub{ORB_ID(sensor_imu_fifo)};
+
+	const enum Rotation _rotation;
 
 	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 _fifo_timestamp_error_perf{perf_alloc(PC_COUNT, MODULE_NAME": FIFO timestamp error")};
 	perf_counter_t _drdy_missed_perf{nullptr};
 
 	hrt_abstime _reset_timestamp{0};
@@ -156,6 +143,8 @@ private:
 	hrt_abstime _temperature_update_timestamp{0};
 	int _failure_count{0};
 
+	uint16_t _timestamp_prev{0};
+
 	enum REG_BANK_SEL_BIT _last_register_bank {REG_BANK_SEL_BIT::USER_BANK_0};
 
 	px4::atomic<hrt_abstime> _drdy_timestamp_sample{0};
@@ -170,25 +159,26 @@ private:
 	} _state{STATE::RESET};
 
 	uint16_t _fifo_empty_interval_us{1250}; // default 1250 us / 800 Hz transfer interval
-	int32_t _fifo_gyro_samples{static_cast<int32_t>(_fifo_empty_interval_us / (1000000 / GYRO_RATE))};
+	int32_t _fifo_gyro_samples{static_cast<int32_t>(_fifo_empty_interval_us / (1000000 / RATE))};
 
 	uint8_t _checked_register_bank0{0};
-	static constexpr uint8_t size_register_bank0_cfg{13};
+	static constexpr uint8_t size_register_bank0_cfg{14};
 	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::PWR_MGMT0,            PWR_MGMT0_BIT::GYRO_MODE_LOW_NOISE | PWR_MGMT0_BIT::ACCEL_MODE_LOW_NOISE, 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_CONFIG0,         GYRO_CONFIG0_BIT::GYRO_ODR_8KHZ_SET, GYRO_CONFIG0_BIT::GYRO_FS_SEL_2000_DPS_CLEAR | GYRO_CONFIG0_BIT::GYRO_ODR_8KHZ_CLEAR },
+		{ Register::BANK_0::ACCEL_CONFIG0,        ACCEL_CONFIG0_BIT::ACCEL_ODR_8KHZ_SET, ACCEL_CONFIG0_BIT::ACCEL_FS_SEL_16G_CLEAR | ACCEL_CONFIG0_BIT::ACCEL_ODR_8KHZ_CLEAR },
+		{ Register::BANK_0::GYRO_CONFIG1,         0, GYRO_CONFIG1_BIT::GYRO_UI_FILT_ORD_1ST_CLEAR },
 		{ 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::ACCEL_CONFIG1,        0, ACCEL_CONFIG1_BIT::ACCEL_UI_FILT_ORD_1ST_CLEAR },
+		{ Register::BANK_0::FIFO_CONFIG1,         FIFO_CONFIG1_BIT::FIFO_RESUME_PARTIAL_RD | 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 },
+		{ Register::BANK_0::INT_CONFIG0,          INT_CONFIG0_BIT::FIFO_THS_INT_CLEAR, 0 },
+		{ Register::BANK_0::INT_CONFIG1,          INT_CONFIG1_BIT::INT_TPULSE_DURATION | INT_CONFIG1_BIT::INT_TDEASSERT_DISABLE, INT_CONFIG1_BIT::INT_ASYNC_RESET },
+		{ Register::BANK_0::INT_SOURCE0,          INT_SOURCE0_BIT::FIFO_THS_INT1_EN, INT_SOURCE0_BIT::RESET_DONE_INT1_EN },
 	};
 
 	uint8_t _checked_register_bank1{0};
diff --git a/src/drivers/imu/invensense/icm42688p/InvenSense_ICM42688P_registers.hpp b/src/drivers/imu/invensense/icm42688p/InvenSense_ICM42688P_registers.hpp
index b8d9260a49..0b39a63141 100644
--- a/src/drivers/imu/invensense/icm42688p/InvenSense_ICM42688P_registers.hpp
+++ b/src/drivers/imu/invensense/icm42688p/InvenSense_ICM42688P_registers.hpp
@@ -1,6 +1,6 @@
 /****************************************************************************
  *
- *   Copyright (c) 2020 PX4 Development Team. All rights reserved.
+ *   Copyright (c) 2020-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
@@ -82,8 +82,7 @@ enum class BANK_0 : uint8_t {
 	FIFO_DATA          = 0x30,
 
 	SIGNAL_PATH_RESET  = 0x4B,
-	INTF_CONFIG0       = 0x4C,
-	INTF_CONFIG1       = 0x4D,
+
 	PWR_MGMT0          = 0x4E,
 	GYRO_CONFIG0       = 0x4F,
 	ACCEL_CONFIG0      = 0x50,
@@ -96,7 +95,7 @@ enum class BANK_0 : uint8_t {
 	FIFO_CONFIG3       = 0x61,
 
 	INT_CONFIG0        = 0x63,
-
+	INT_CONFIG1        = 0x64,
 	INT_SOURCE0        = 0x65,
 
 	SELF_TEST_CONFIG   = 0x70,
@@ -121,7 +120,7 @@ enum class BANK_2 : uint8_t {
 
 // DEVICE_CONFIG
 enum DEVICE_CONFIG_BIT : uint8_t {
-	SOFT_RESET_CONFIG = Bit0, //
+	SOFT_RESET_CONFIG = Bit0,
 };
 
 // INT_CONFIG
@@ -140,15 +139,14 @@ enum FIFO_CONFIG_BIT : uint8_t {
 // 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,
+	FIFO_FLUSH = Bit1,
 };
 
 // PWR_MGMT0
@@ -160,55 +158,42 @@ enum PWR_MGMT0_BIT : uint8_t {
 // 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,
-
+	//  0b000: ±2000dps (default)
+	GYRO_FS_SEL_2000_DPS_CLEAR = Bit7 | Bit6 | Bit5,
 
 	// 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,
+	//  0b0001: 32kHz (maximum)
+	GYRO_ODR_32KHZ_SET         = Bit0,
+	GYRO_ODR_32KHZ_CLEAR       = Bit3 | Bit2 | Bit0,
+	//  0b0011: 8kHz
+	GYRO_ODR_8KHZ_SET          = Bit1 | Bit0,
+	GYRO_ODR_8KHZ_CLEAR        = Bit3 | Bit2,
+	//  0b0110: 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,
-
+	//  0b000: ±16g (default)
+	ACCEL_FS_SEL_16G_CLEAR     = Bit7 | 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,
+	//  0b0001: 32kHz (maximum)
+	ACCEL_ODR_32KHZ_SET        = Bit0,
+	ACCEL_ODR_32KHZ_CLEAR      = Bit3 | Bit2 | Bit0,
+	//  0b0011: 8kHz
+	ACCEL_ODR_8KHZ_SET         = Bit1 | Bit0,
+	ACCEL_ODR_8KHZ_CLEAR       = Bit3 | Bit2,
+	//  0b0110: 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_UI_FILT_ORD_1ST_CLEAR = Bit3 | Bit2, // 00: 1st Order UI filter
 };
 
 // GYRO_ACCEL_CONFIG0
@@ -222,14 +207,15 @@ enum GYRO_ACCEL_CONFIG0_BIT : uint8_t {
 
 // ACCEL_CONFIG1
 enum ACCEL_CONFIG1_BIT : uint8_t {
-	ACCEL_UI_FILT_ORD = Bit4 | Bit3, // 00: 1st Order
+	ACCEL_UI_FILT_ORD_1ST_CLEAR = Bit4 | Bit3, // 00: 1st Order UI filter
 };
 
 // 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,
@@ -238,18 +224,21 @@ enum FIFO_CONFIG1_BIT : uint8_t {
 // INT_CONFIG0
 enum INT_CONFIG0_BIT : uint8_t {
 	// 3:2 FIFO_THS_INT_CLEAR
-	CLEAR_ON_FIFO_READ = Bit3,
+	FIFO_THS_INT_CLEAR = Bit3, // 10: Clear on FIFO data 1Byte Read
+};
+
+// INT_CONFIG1
+enum INT_CONFIG1_BIT : uint8_t {
+	INT_TPULSE_DURATION   = Bit6, // 1: Interrupt pulse duration is 8 µs. Required if ODR ≥ 4kHz, optional for ODR < 4kHz.
+	INT_TDEASSERT_DISABLE = Bit5, // 1: Disables de-assert duration. Required if ODR ≥ 4kHz, optional for ODR < 4kHz.
+	INT_ASYNC_RESET       = Bit4, // User should change setting to 0 from default setting of 1, for proper INT1 and INT2 pin operation
 };
 
 // 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,
+	RESET_DONE_INT1_EN = Bit4, // 1: Reset done interrupt routed to INT1 (enabled by default)
+
 	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
@@ -277,6 +266,7 @@ enum ACCEL_CONFIG_STATIC2_BIT : uint8_t {
 	ACCEL_AAF_DIS = Bit0,
 };
 
+
 namespace FIFO
 {
 static constexpr size_t SIZE = 2048;
diff --git a/src/drivers/imu/invensense/icm42688p/icm42688p_main.cpp b/src/drivers/imu/invensense/icm42688p/icm42688p_main.cpp
index 618d57fea2..ffd7c1ea35 100644
--- a/src/drivers/imu/invensense/icm42688p/icm42688p_main.cpp
+++ b/src/drivers/imu/invensense/icm42688p/icm42688p_main.cpp
@@ -1,6 +1,6 @@
 /****************************************************************************
  *
- *   Copyright (c) 2020, 2021 PX4 Development Team. All rights reserved.
+ *   Copyright (c) 2020-2022 PX4 Development Team. All rights reserved.
  *
  * Redistribution and use in source and binary forms, with or without
  * modification, are permitted provided that the following conditions
@@ -72,13 +72,11 @@ extern "C" int icm42688p_main(int argc, char *argv[])
 
 	if (!strcmp(verb, "start")) {
 		return ThisDriver::module_start(cli, iterator);
-	}
 
-	if (!strcmp(verb, "stop")) {
+	} else if (!strcmp(verb, "stop")) {
 		return ThisDriver::module_stop(iterator);
-	}
 
-	if (!strcmp(verb, "status")) {
+	} else if (!strcmp(verb, "status")) {
 		return ThisDriver::module_status(iterator);
 	}
 
diff --git a/src/drivers/imu/invensense/iim42652/CMakeLists.txt b/src/drivers/imu/invensense/iim42652/CMakeLists.txt
index 1ce153c6f6..9ba4534819 100644
--- a/src/drivers/imu/invensense/iim42652/CMakeLists.txt
+++ b/src/drivers/imu/invensense/iim42652/CMakeLists.txt
@@ -42,6 +42,4 @@ px4_add_module(
 		InvenSense_IIM42652_registers.hpp
 	DEPENDS
 		px4_work_queue
-		drivers_accelerometer
-		drivers_gyroscope
 	)
diff --git a/src/drivers/imu/invensense/iim42652/IIM42652.cpp b/src/drivers/imu/invensense/iim42652/IIM42652.cpp
index dabd688715..7cc397153f 100644
--- a/src/drivers/imu/invensense/iim42652/IIM42652.cpp
+++ b/src/drivers/imu/invensense/iim42652/IIM42652.cpp
@@ -33,6 +33,8 @@
 
 #include "IIM42652.hpp"
 
+#include <lib/parameters/param.h>
+
 using namespace time_literals;
 
 static constexpr int16_t combine(uint8_t msb, uint8_t lsb)
@@ -44,14 +46,16 @@ IIM42652::IIM42652(const I2CSPIDriverConfig &config) :
 	SPI(config),
 	I2CSPIDriver(config),
 	_drdy_gpio(config.drdy_gpio),
-	_px4_accel(get_device_id(), config.rotation),
-	_px4_gyro(get_device_id(), config.rotation)
+	_rotation(config.rotation)
 {
-	if (config.drdy_gpio != 0) {
+	if (_drdy_gpio != 0) {
 		_drdy_missed_perf = perf_alloc(PC_COUNT, MODULE_NAME": DRDY missed");
 	}
 
-	ConfigureSampleRate(_px4_gyro.get_max_rate_hz());
+	int32_t imu_gyro_rate_max = 400;
+	param_get(param_find("IMU_GYRO_RATEMAX"), &imu_gyro_rate_max);
+
+	ConfigureSampleRate(imu_gyro_rate_max);
 }
 
 IIM42652::~IIM42652()
@@ -61,6 +65,7 @@ IIM42652::~IIM42652()
 	perf_free(_fifo_empty_perf);
 	perf_free(_fifo_overflow_perf);
 	perf_free(_fifo_reset_perf);
+	perf_free(_fifo_timestamp_error_perf);
 	perf_free(_drdy_missed_perf);
 }
 
@@ -102,12 +107,14 @@ void IIM42652::print_status()
 	perf_print_counter(_fifo_empty_perf);
 	perf_print_counter(_fifo_overflow_perf);
 	perf_print_counter(_fifo_reset_perf);
+	perf_print_counter(_fifo_timestamp_error_perf);
 	perf_print_counter(_drdy_missed_perf);
 }
 
 int IIM42652::probe()
 {
-	for (int i = 0; i < 3; i++) {
+	// 3 retries
+	for (int retry = 0; retry < 3; retry++) {
 		uint8_t whoami = RegisterRead(Register::BANK_0::WHO_AM_I);
 
 		if (whoami == WHOAMI) {
@@ -162,8 +169,8 @@ void IIM42652::RunImpl()
 				ScheduleDelayed(100_ms);
 
 			} else {
-				PX4_DEBUG("Reset not complete, check again in 10 ms");
-				ScheduleDelayed(10_ms);
+				PX4_DEBUG("Reset not complete, check again in 100 ms");
+				ScheduleDelayed(100_ms);
 			}
 		}
 
@@ -171,21 +178,9 @@ void IIM42652::RunImpl()
 
 	case STATE::CONFIGURE:
 		if (Configure()) {
-			// 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 {
-				_data_ready_interrupt_enabled = false;
-				ScheduleOnInterval(_fifo_empty_interval_us, _fifo_empty_interval_us);
-			}
-
-			FIFOReset();
+			// if configure succeeded then reset the FIFO
+			_state = STATE::FIFO_RESET;
+			ScheduleDelayed(1_ms);
 
 		} else {
 			// CONFIGURE not complete
@@ -202,6 +197,24 @@ void IIM42652::RunImpl()
 
 		break;
 
+	case STATE::FIFO_RESET:
+
+		_state = STATE::FIFO_READ;
+		FIFOReset();
+
+		if (DataReadyInterruptConfigure()) {
+			_data_ready_interrupt_enabled = true;
+
+			// backup schedule as a watchdog timeout
+			ScheduleDelayed(100_ms);
+
+		} else {
+			_data_ready_interrupt_enabled = false;
+			ScheduleOnInterval(_fifo_empty_interval_us, _fifo_empty_interval_us);
+		}
+
+		break;
+
 	case STATE::FIFO_READ: {
 			hrt_abstime timestamp_sample = now;
 			uint8_t samples = 0;
@@ -210,7 +223,7 @@ void IIM42652::RunImpl()
 				// scheduled from interrupt if _drdy_timestamp_sample was set as expected
 				const hrt_abstime drdy_timestamp_sample = _drdy_timestamp_sample.fetch_and(0);
 
-				if ((now - drdy_timestamp_sample) < _fifo_empty_interval_us) {
+				if (now < drdy_timestamp_sample + _fifo_empty_interval_us) {
 					timestamp_sample = drdy_timestamp_sample;
 					samples = _fifo_gyro_samples;
 
@@ -222,6 +235,8 @@ void IIM42652::RunImpl()
 				ScheduleDelayed(_fifo_empty_interval_us * 2);
 			}
 
+			bool success = false;
+
 			if (samples == 0) {
 				// check current FIFO count
 				const uint16_t fifo_count = FIFOReadCount();
@@ -234,27 +249,42 @@ void IIM42652::RunImpl()
 					perf_count(_fifo_empty_perf);
 
 				} else {
-					// FIFO count (size in bytes)
-					samples = (fifo_count / sizeof(FIFO::DATA));
+					// FIFO count (size in bytes) should be a multiple of the FIFO::DATA structure
+					samples = fifo_count / sizeof(FIFO::DATA);
 
-					// tolerate minor jitter, leave sample to next iteration if behind by only 1
-					if (samples == _fifo_gyro_samples + 1) {
-						timestamp_sample -= static_cast<int>(FIFO_SAMPLE_DT);
-						samples--;
+					if (samples > _fifo_gyro_samples) {
+						// grab desired number of samples, but reschedule next cycle sooner
+						const int extra_samples = samples - _fifo_gyro_samples;
+						samples = _fifo_gyro_samples;
+
+						if (_fifo_gyro_samples > extra_samples) {
+							// reschedule to run when a total of _fifo_gyro_samples should be available in the FIFO
+							const uint32_t reschedule_delay_us = (_fifo_gyro_samples - extra_samples) * static_cast<int>(FIFO_SAMPLE_DT);
+							ScheduleOnInterval(_fifo_empty_interval_us, reschedule_delay_us);
+
+						} else {
+							// otherwise reschedule to run immediately
+							ScheduleOnInterval(_fifo_empty_interval_us);
+						}
+
+					} else if (samples < _fifo_gyro_samples) {
+						// reschedule next cycle to catch the desired number of samples
+						ScheduleOnInterval(_fifo_empty_interval_us, (_fifo_gyro_samples - samples) * static_cast<int>(FIFO_SAMPLE_DT));
 					}
 
-					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;
+					if (samples == _fifo_gyro_samples) {
+						if (FIFORead(now, samples)) {
+							success = true;
+
+							if (_failure_count > 0) {
+								_failure_count--;
+							}
+						}
 					}
 				}
 			}
 
-			bool success = false;
-
-			if (samples >= 1) {
+			if (samples == _fifo_gyro_samples) {
 				if (FIFORead(timestamp_sample, samples)) {
 					success = true;
 
@@ -269,13 +299,14 @@ void IIM42652::RunImpl()
 
 				// full reset if things are failing consistently
 				if (_failure_count > 10) {
+					PX4_DEBUG("Full reset because things are failing consistently");
 					Reset();
 					return;
 				}
 			}
 
+			// check configuration registers periodically or immediately following any failure
 			if (!success || hrt_elapsed_time(&_last_config_check_timestamp) > 100_ms) {
-				// 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])
@@ -303,10 +334,10 @@ void IIM42652::ConfigureSampleRate(int sample_rate)
 	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));
+	_fifo_gyro_samples = roundf(math::min((float)_fifo_empty_interval_us / (1e6f / 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);
+	_fifo_empty_interval_us = _fifo_gyro_samples * (1e6f / RATE);
 
 	ConfigureFIFOWatermark(_fifo_gyro_samples);
 }
@@ -356,6 +387,7 @@ bool IIM42652::Configure()
 		RegisterSetAndClearBits(reg_cfg.reg, reg_cfg.set_bits, reg_cfg.clear_bits);
 	}
 
+
 	// now check that all are configured
 	bool success = true;
 
@@ -377,11 +409,6 @@ bool IIM42652::Configure()
 		}
 	}
 
-	// 20-bits data format used
-	//  the only FSR settings that are operational are ±2000dps for gyroscope and ±16g for accelerometer
-	_px4_accel.set_range(16.f * CONSTANTS_ONE_G);
-	_px4_gyro.set_range(math::radians(2000.f));
-
 	return success;
 }
 
@@ -404,7 +431,7 @@ bool IIM42652::DataReadyInterruptConfigure()
 	}
 
 	// Setup data ready on falling edge
-	return px4_arch_gpiosetevent(_drdy_gpio, false, true, true, &DataReadyInterruptCallback, this) == 0;
+	return (px4_arch_gpiosetevent(_drdy_gpio, false, true, false, &DataReadyInterruptCallback, this) == 0);
 }
 
 bool IIM42652::DataReadyInterruptDisable()
@@ -413,7 +440,7 @@ bool IIM42652::DataReadyInterruptDisable()
 		return false;
 	}
 
-	return px4_arch_gpiosetevent(_drdy_gpio, false, false, false, nullptr, nullptr) == 0;
+	return (px4_arch_gpiosetevent(_drdy_gpio, false, false, false, nullptr, nullptr) == 0);
 }
 
 template <typename T>
@@ -468,114 +495,20 @@ void IIM42652::RegisterSetAndClearBits(T reg, uint8_t setbits, uint8_t clearbits
 
 uint16_t IIM42652::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);
+	// transfer buffer
+	struct TransferBuffer {
+		uint8_t cmd{static_cast<uint8_t>(Register::BANK_0::FIFO_COUNTH) | DIR_READ};
+		uint8_t FIFO_COUNTH{0};
+		uint8_t FIFO_COUNTL{0};
+	} buffer{};
 
-	if (transfer(fifo_count_buf, fifo_count_buf, sizeof(fifo_count_buf)) != PX4_OK) {
+	// read FIFO count
+	if (transfer((uint8_t *)&buffer, (uint8_t *)&buffer, sizeof(buffer)) != PX4_OK) {
 		perf_count(_bad_transfer_perf);
 		return 0;
 	}
 
-	return combine(fifo_count_buf[1], fifo_count_buf[2]);
-}
-
-bool IIM42652::FIFORead(const hrt_abstime &timestamp_sample, uint8_t samples)
-{
-	FIFOTransferBuffer buffer{};
-	const size_t transfer_size = math::min(samples * sizeof(FIFO::DATA) + 4, FIFO::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 uint8_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
-	uint8_t valid_samples = 0;
-
-	for (int i = 0; i < math::min(samples, fifo_count_samples); 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 (valid_samples > 0) {
-		if (ProcessTemperature(buffer.f, valid_samples)) {
-			ProcessGyro(timestamp_sample, buffer.f, valid_samples);
-			ProcessAccel(timestamp_sample, buffer.f, valid_samples);
-			return true;
-		}
-	}
-
-	return false;
-}
-
-void IIM42652::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_timestamp_sample.store(0);
+	return (buffer.FIFO_COUNTH << 8) + buffer.FIFO_COUNTL;
 }
 
 static constexpr int32_t reassemble_20bit(const uint32_t a, const uint32_t b, const uint32_t c)
@@ -595,206 +528,288 @@ static constexpr int32_t reassemble_20bit(const uint32_t a, const uint32_t b, co
 	return static_cast<int32_t>(x);
 }
 
-void IIM42652::ProcessAccel(const hrt_abstime &timestamp_sample, const FIFO::DATA fifo[], const uint8_t samples)
+bool IIM42652::FIFORead(const hrt_abstime &timestamp_sample, uint8_t samples)
 {
-	sensor_accel_fifo_s accel{};
-	accel.timestamp_sample = timestamp_sample;
-	accel.samples = 0;
-	accel.dt = FIFO_SAMPLE_DT;
+	// FIFO transfer buffer
+	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] {};
+	} buffer{};
 
-	// 18-bits of accelerometer data
-	bool scale_20bit = false;
+	// cmd + INT_STATUS + FIFO_COUNTH + FIFO_COUNTL + samples (FIFO::DATA)
+	const size_t transfer_size = 4 + math::min(samples * sizeof(FIFO::DATA), FIFO::SIZE);
 
-	// 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);
+	SelectRegisterBank(REG_BANK_SEL_BIT::USER_BANK_0);
 
-		// 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 (transfer((uint8_t *)&buffer, (uint8_t *)&buffer, transfer_size) != PX4_OK) {
+		perf_count(_bad_transfer_perf);
+		return false;
+	}
 
-			if (accel_x >= max_accel || accel_x <= min_accel) {
-				scale_20bit = true;
+	if (buffer.INT_STATUS & INT_STATUS_BIT::FIFO_FULL_INT) {
+		perf_count(_fifo_overflow_perf);
+		FIFOReset();
+		return false;
+	}
+
+	const uint16_t fifo_count_bytes = (buffer.FIFO_COUNTH << 8) + buffer.FIFO_COUNTL;
+
+	if (fifo_count_bytes >= FIFO::SIZE) {
+		perf_count(_fifo_overflow_perf);
+		FIFOReset();
+		return false;
+	}
+
+	const uint8_t fifo_count_samples = fifo_count_bytes / sizeof(FIFO::DATA);
+
+	if (fifo_count_samples == 0) {
+		perf_count(_fifo_empty_perf);
+		return false;
+	}
+
+	sensor_imu_fifo_s sensor_imu_fifo{};
+
+	sensor_imu_fifo.timestamp_sample = timestamp_sample;
+	sensor_imu_fifo.device_id = get_device_id();
+	sensor_imu_fifo.dt = FIFO_SAMPLE_DT;
+	sensor_imu_fifo.samples = 0;
+	sensor_imu_fifo.accel_scale = CONSTANTS_ONE_G / 8192.f; // highres accel data 8192 LSB/g
+	sensor_imu_fifo.gyro_scale = math::radians(1.f / 131.f); // highres gyro data 131 LSB/dps
+
+	// check FIFO header in every sample
+	uint8_t valid_samples = 0;
+
+	float temperature_sum = 0;
+
+	float timestamp_interval_sum = 0;
+	int timestamp_interval_sum_count = 0;
+
+	bool accel_scale_16bit = false; // 18-bits of accelerometer data
+	bool gyro_scale_16bit = false; // 20-bits of gyroscope data
+
+	for (int i = 0; i < math::min(samples, fifo_count_samples); i++) {
+		const FIFO::DATA &fifo = buffer.f[i];
+
+		// With FIFO_ACCEL_EN and FIFO_GYRO_EN header should be 8’b_0110_10xx
+		const uint8_t FIFO_HEADER = fifo.FIFO_Header;
+
+		const bool HEADER_MSG       = FIFO_HEADER & FIFO::FIFO_HEADER_BIT::HEADER_MSG; // FIFO is empty
+		const bool HEADER_ACCEL     = FIFO_HEADER & FIFO::FIFO_HEADER_BIT::HEADER_ACCEL;
+		const bool HEADER_GYRO      = FIFO_HEADER & FIFO::FIFO_HEADER_BIT::HEADER_GYRO;
+		const bool HEADER_20        = FIFO_HEADER & FIFO::FIFO_HEADER_BIT::HEADER_20; // valid sample of extended 20-bit data
+		// 3:2 HEADER_TIMESTAMP_FSYNC
+		const bool HEADER_ODR_ACCEL = FIFO_HEADER & FIFO::FIFO_HEADER_BIT::HEADER_ODR_ACCEL; // ODR for accel is different
+		const bool HEADER_ODR_GYRO  = FIFO_HEADER & FIFO::FIFO_HEADER_BIT::HEADER_ODR_GYRO;  // ODR for gyro is different
+
+		if (!HEADER_MSG && HEADER_ACCEL && HEADER_GYRO && HEADER_20 && !HEADER_ODR_ACCEL && !HEADER_ODR_GYRO) {
+
+			if (!accel_scale_16bit) {
+				// Accel: 20 bit hires mode, Accel data is 18 bit
+				// Sign extension + Accel [19:12] + Accel [11:4] + Accel [3:2] (20 bit extension byte)
+				int32_t accel_x = reassemble_20bit(fifo.ACCEL_DATA_X1, fifo.ACCEL_DATA_X0, fifo.Ext_Accel_X_Gyro_X & 0xF0 >> 4);
+				int32_t accel_y = reassemble_20bit(fifo.ACCEL_DATA_Y1, fifo.ACCEL_DATA_Y0, fifo.Ext_Accel_Y_Gyro_Y & 0xF0 >> 4);
+				int32_t accel_z = reassemble_20bit(fifo.ACCEL_DATA_Z1, fifo.ACCEL_DATA_Z0, fifo.Ext_Accel_Z_Gyro_Z & 0xF0 >> 4);
+
+				// sample invalid if -524288
+				if (accel_x == -524288 || accel_y == -524288 || accel_z == -524288) {
+					break;
+				}
+
+				// shift by 2 (2 least significant bits are always 0)
+				accel_x = accel_x / 4;
+				accel_y = accel_y / 4;
+				accel_z = accel_z / 4;
+
+				// check if any values are going to exceed int16 limits
+				if ((accel_x >= INT16_MAX || accel_x <= INT16_MIN)
+				    || (accel_y >= INT16_MAX || accel_y <= INT16_MIN)
+				    || (accel_z >= INT16_MAX || accel_z <= INT16_MIN)) {
+
+					accel_scale_16bit = true;
+
+					// 20 bit data scaled to 16 bit
+					sensor_imu_fifo.accel_scale = CONSTANTS_ONE_G / 2048.f;
+
+					// rescale any existing data
+					for (int j = 0; j < valid_samples + 1; j++) {
+						sensor_imu_fifo.accel_x[valid_samples] = combine(buffer.f[j].ACCEL_DATA_X1, buffer.f[j].ACCEL_DATA_X0);
+						sensor_imu_fifo.accel_y[valid_samples] = combine(buffer.f[j].ACCEL_DATA_Y1, buffer.f[j].ACCEL_DATA_Y0);
+						sensor_imu_fifo.accel_z[valid_samples] = combine(buffer.f[j].ACCEL_DATA_Z1, buffer.f[j].ACCEL_DATA_Z0);
+					}
+
+				} else {
+					sensor_imu_fifo.accel_x[valid_samples] = accel_x;
+					sensor_imu_fifo.accel_y[valid_samples] = accel_y;
+					sensor_imu_fifo.accel_z[valid_samples] = accel_z;
+				}
+
+			} else {
+				sensor_imu_fifo.accel_x[valid_samples] = combine(fifo.ACCEL_DATA_X1, fifo.ACCEL_DATA_X0);
+				sensor_imu_fifo.accel_y[valid_samples] = combine(fifo.ACCEL_DATA_Y1, fifo.ACCEL_DATA_Y0);
+				sensor_imu_fifo.accel_z[valid_samples] = combine(fifo.ACCEL_DATA_Z1, fifo.ACCEL_DATA_Z0);
 			}
 
-			if (accel_y >= max_accel || accel_y <= min_accel) {
-				scale_20bit = true;
+
+			if (!gyro_scale_16bit) {
+				// Gyro: 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.GYRO_DATA_X1, fifo.GYRO_DATA_X0, fifo.Ext_Accel_X_Gyro_X & 0x0F);
+				int32_t gyro_y = reassemble_20bit(fifo.GYRO_DATA_Y1, fifo.GYRO_DATA_Y0, fifo.Ext_Accel_Y_Gyro_Y & 0x0F);
+				int32_t gyro_z = reassemble_20bit(fifo.GYRO_DATA_Z1, fifo.GYRO_DATA_Z0, fifo.Ext_Accel_Z_Gyro_Z & 0x0F);
+
+				// shift by 1 (least significant bit is always 0)
+				gyro_x = gyro_x / 2;
+				gyro_y = gyro_y / 2;
+				gyro_z = gyro_z / 2;
+
+				// check if any gyro values are going to exceed int16 limits
+				if ((gyro_x >= INT16_MAX || gyro_x <= INT16_MIN)
+				    || (gyro_y >= INT16_MAX || gyro_y <= INT16_MIN)
+				    || (gyro_z >= INT16_MAX || gyro_z <= INT16_MIN)) {
+
+
+					gyro_scale_16bit = true;
+
+					// 20 bit data scaled to 16 bit
+					sensor_imu_fifo.gyro_scale = math::radians(2000.f / 32768.f);
+
+					// rescale any existing data
+					for (int j = 0; j < valid_samples + 1; j++) {
+						sensor_imu_fifo.gyro_x[j] = combine(buffer.f[j].GYRO_DATA_X1, buffer.f[j].GYRO_DATA_X0);
+						sensor_imu_fifo.gyro_y[j] = combine(buffer.f[j].GYRO_DATA_Y1, buffer.f[j].GYRO_DATA_Y0);
+						sensor_imu_fifo.gyro_z[j] = combine(buffer.f[j].GYRO_DATA_Z1, buffer.f[j].GYRO_DATA_Z0);
+					}
+
+				} else {
+					sensor_imu_fifo.gyro_x[valid_samples] = gyro_x;
+					sensor_imu_fifo.gyro_y[valid_samples] = gyro_y;
+					sensor_imu_fifo.gyro_z[valid_samples] = gyro_z;
+				}
+
+			} else {
+				sensor_imu_fifo.gyro_x[i] = combine(fifo.GYRO_DATA_X1, fifo.GYRO_DATA_X0);
+				sensor_imu_fifo.gyro_y[i] = combine(fifo.GYRO_DATA_Y1, fifo.GYRO_DATA_Y0);
+				sensor_imu_fifo.gyro_z[i] = combine(fifo.GYRO_DATA_Z1, fifo.GYRO_DATA_Z0);
 			}
 
-			if (accel_z >= max_accel || accel_z <= min_accel) {
-				scale_20bit = true;
+
+			// temperature
+			const int16_t TEMP_DATA = combine(fifo.TEMP_DATA1, fifo.TEMP_DATA0);
+
+			// sample invalid if -32768
+			if (TEMP_DATA != -32768) {
+				temperature_sum += TEMP_DATA;
+
+			} else {
+				break;
 			}
 
-			// 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
-		_px4_accel.set_scale(CONSTANTS_ONE_G / 8192.f);
+			// HEADER_TIMESTAMP_FSYNC - 0b10: Packet contains ODR Timestamp
+			if (FIFO_HEADER & Bit3) {
+				const uint16_t timestamp = (fifo.TimeStamp_h << 8) + fifo.TimeStamp_l;
 
-	} 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);
+				if (_timestamp_prev != 0) {
+					// If TMST_RES = 0 (corresponding to timestamp resolution of 1µs), timestamp interval reported in FIFO requires scaling by a factor of 32/30.
+					// Document Number: DS-000347 Revision: 1.5 Page 59 of 110
+					static constexpr float FIFO_DT_SCALE = 32.f / 30.f;
 
-			accel.x[i] = accel_x;
-			accel.y[i] = accel_y;
-			accel.z[i] = accel_z;
-		}
+					float dt = 0;
 
-		_px4_accel.set_scale(CONSTANTS_ONE_G / 2048.f);
-	}
+					if (timestamp > _timestamp_prev) {
+						dt = static_cast<float>(timestamp - _timestamp_prev) * FIFO_DT_SCALE;
 
-	// 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];
-	}
+					} else if (timestamp < _timestamp_prev) {
+						// uint16_t rollover
+						uint32_t timestamp_new = UINT16_MAX + timestamp;
+						dt = static_cast<float>(timestamp_new - _timestamp_prev) * FIFO_DT_SCALE;
+					}
 
-	_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));
+					timestamp_interval_sum += dt;
+					timestamp_interval_sum_count++;
 
-	if (accel.samples > 0) {
-		_px4_accel.updateFIFO(accel);
-	}
-}
+					// check dt is within +=2% of expected value
+					if ((dt < (FIFO_SAMPLE_DT * 0.98f)) || (dt > (FIFO_SAMPLE_DT * 1.02f))) {
+						perf_count(_fifo_timestamp_error_perf);
+					}
+				}
 
-void IIM42652::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;
+				_timestamp_prev = timestamp;
+			}
 
-	// 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
-		_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);
-		}
-
-		_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];
-	}
-
-	_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) {
-		_px4_gyro.updateFIFO(gyro);
-	}
-}
-
-bool IIM42652::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;
-			}
+		sensor_imu_fifo.samples = valid_samples;
+
+		for (int i = 0; i < sensor_imu_fifo.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)
+
+			// sensor_imu_fifo.accel_x[i]
+			sensor_imu_fifo.accel_y[i] = math::negate(sensor_imu_fifo.accel_y[i]);
+			sensor_imu_fifo.accel_z[i] = math::negate(sensor_imu_fifo.accel_z[i]);
+			rotate_3i(_rotation, sensor_imu_fifo.accel_x[i], sensor_imu_fifo.accel_y[i], sensor_imu_fifo.accel_z[i]);
+
+			// sensor_imu_fifo.gyro_x[i]
+			sensor_imu_fifo.gyro_y[i] = math::negate(sensor_imu_fifo.gyro_y[i]);
+			sensor_imu_fifo.gyro_z[i] = math::negate(sensor_imu_fifo.gyro_z[i]);
+			rotate_3i(_rotation, sensor_imu_fifo.gyro_x[i], sensor_imu_fifo.gyro_y[i], sensor_imu_fifo.gyro_z[i]);
 		}
 
+		const float temperature_avg = temperature_sum / valid_samples;
+
 		// use average temperature reading
 		const float TEMP_degC = (temperature_avg / TEMPERATURE_SENSITIVITY) + TEMPERATURE_OFFSET;
 
 		if (PX4_ISFINITE(TEMP_degC)) {
-			_px4_accel.set_temperature(TEMP_degC);
-			_px4_gyro.set_temperature(TEMP_degC);
-			return true;
+			sensor_imu_fifo.temperature = TEMP_degC;
 
 		} else {
 			perf_count(_bad_transfer_perf);
+			return false;
 		}
+
+		if (timestamp_interval_sum > 0) {
+			const float dt_avg = (timestamp_interval_sum / timestamp_interval_sum_count);
+
+			// check dt is within +=1% of expected value
+			if ((dt_avg < (FIFO_SAMPLE_DT * 0.99f)) || (dt_avg > (FIFO_SAMPLE_DT * 1.01f))) {
+				PX4_ERR("DT error %.6f", (double)dt_avg);
+				perf_count(_fifo_timestamp_error_perf);
+
+			} else {
+				sensor_imu_fifo.dt = dt_avg;
+			}
+		}
+
+		sensor_imu_fifo.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);
+
+		sensor_imu_fifo.timestamp = hrt_absolute_time();
+		_sensor_imu_fifo_pub.publish(sensor_imu_fifo);
+
+		return true;
 	}
 
 	return false;
 }
+
+void IIM42652::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_timestamp_sample.store(0);
+
+	_timestamp_prev = 0;
+}
diff --git a/src/drivers/imu/invensense/iim42652/IIM42652.hpp b/src/drivers/imu/invensense/iim42652/IIM42652.hpp
index 9ba671d093..5274220431 100644
--- a/src/drivers/imu/invensense/iim42652/IIM42652.hpp
+++ b/src/drivers/imu/invensense/iim42652/IIM42652.hpp
@@ -43,14 +43,15 @@
 #include "InvenSense_IIM42652_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/PublicationMulti.hpp>
+#include <uORB/topics/sensor_imu_fifo.h>
+
 using namespace InvenSense_IIM42652;
 
 class IIM42652 : public device::SPI, public I2CSPIDriver<IIM42652>
@@ -71,22 +72,10 @@ private:
 
 	// Sensor Configuration
 	static constexpr float FIFO_SAMPLE_DT{1e6f / 8000.f};     // 8000 Hz accel & gyro ODR configured
-	static constexpr float GYRO_RATE{1e6f / FIFO_SAMPLE_DT};
-	static constexpr float ACCEL_RATE{1e6f / FIFO_SAMPLE_DT};
+	static constexpr float RATE{1e6f / FIFO_SAMPLE_DT};
 
 	// maximum FIFO samples per transfer is limited to the size of sensor_accel_fifo/sensor_gyro_fifo
-	static constexpr int32_t FIFO_MAX_SAMPLES{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))};
-
-	// 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)));
+	static constexpr int32_t FIFO_MAX_SAMPLES{math::min(FIFO::SIZE / sizeof(FIFO::DATA), (size_t)sensor_imu_fifo_s::FIFO_SIZE)};
 
 	struct register_bank0_config_t {
 		Register::BANK_0 reg;
@@ -135,20 +124,18 @@ private:
 	bool FIFORead(const hrt_abstime &timestamp_sample, uint8_t samples);
 	void FIFOReset();
 
-	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;
 
-	PX4Accelerometer _px4_accel;
-	PX4Gyroscope _px4_gyro;
+	uORB::PublicationMulti<sensor_imu_fifo_s> _sensor_imu_fifo_pub{ORB_ID(sensor_imu_fifo)};
+
+	const enum Rotation _rotation;
 
 	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 _fifo_timestamp_error_perf{perf_alloc(PC_COUNT, MODULE_NAME": FIFO timestamp error")};
 	perf_counter_t _drdy_missed_perf{nullptr};
 
 	hrt_abstime _reset_timestamp{0};
@@ -156,6 +143,9 @@ private:
 	hrt_abstime _temperature_update_timestamp{0};
 	int _failure_count{0};
 
+
+	uint16_t _timestamp_prev{0};
+
 	enum REG_BANK_SEL_BIT _last_register_bank {REG_BANK_SEL_BIT::USER_BANK_0};
 
 	px4::atomic<hrt_abstime> _drdy_timestamp_sample{0};
@@ -165,29 +155,31 @@ private:
 		RESET,
 		WAIT_FOR_RESET,
 		CONFIGURE,
+		FIFO_RESET,
 		FIFO_READ,
 	} _state{STATE::RESET};
 
 	uint16_t _fifo_empty_interval_us{1250}; // default 1250 us / 800 Hz transfer interval
-	int32_t _fifo_gyro_samples{static_cast<int32_t>(_fifo_empty_interval_us / (1000000 / GYRO_RATE))};
+	int32_t _fifo_gyro_samples{static_cast<int32_t>(_fifo_empty_interval_us / (1000000 / RATE))};
 
 	uint8_t _checked_register_bank0{0};
-	static constexpr uint8_t size_register_bank0_cfg{13};
+	static constexpr uint8_t size_register_bank0_cfg{14};
 	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::PWR_MGMT0,            PWR_MGMT0_BIT::GYRO_MODE_LOW_NOISE | PWR_MGMT0_BIT::ACCEL_MODE_LOW_NOISE, 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_CONFIG0,         GYRO_CONFIG0_BIT::GYRO_ODR_8KHZ_SET, GYRO_CONFIG0_BIT::GYRO_FS_SEL_2000_DPS_CLEAR | GYRO_CONFIG0_BIT::GYRO_ODR_8KHZ_CLEAR },
+		{ Register::BANK_0::ACCEL_CONFIG0,        ACCEL_CONFIG0_BIT::ACCEL_ODR_8KHZ_SET, ACCEL_CONFIG0_BIT::ACCEL_FS_SEL_16G_CLEAR | ACCEL_CONFIG0_BIT::ACCEL_ODR_8KHZ_CLEAR },
+		{ Register::BANK_0::GYRO_CONFIG1,         0, GYRO_CONFIG1_BIT::GYRO_UI_FILT_ORD_1ST_CLEAR },
 		{ 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::ACCEL_CONFIG1,        0, ACCEL_CONFIG1_BIT::ACCEL_UI_FILT_ORD_1ST_CLEAR },
+		{ Register::BANK_0::FIFO_CONFIG1,         FIFO_CONFIG1_BIT::FIFO_RESUME_PARTIAL_RD | 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 },
+		{ Register::BANK_0::INT_CONFIG0,          INT_CONFIG0_BIT::FIFO_THS_INT_CLEAR, 0 },
+		{ Register::BANK_0::INT_CONFIG1,          INT_CONFIG1_BIT::INT_TPULSE_DURATION | INT_CONFIG1_BIT::INT_TDEASSERT_DISABLE, INT_CONFIG1_BIT::INT_ASYNC_RESET },
+		{ Register::BANK_0::INT_SOURCE0,          INT_SOURCE0_BIT::FIFO_THS_INT1_EN, INT_SOURCE0_BIT::RESET_DONE_INT1_EN },
 	};
 
 	uint8_t _checked_register_bank1{0};
diff --git a/src/drivers/imu/invensense/iim42652/InvenSense_IIM42652_registers.hpp b/src/drivers/imu/invensense/iim42652/InvenSense_IIM42652_registers.hpp
index 7f59099e2f..389aaab2d2 100644
--- a/src/drivers/imu/invensense/iim42652/InvenSense_IIM42652_registers.hpp
+++ b/src/drivers/imu/invensense/iim42652/InvenSense_IIM42652_registers.hpp
@@ -82,8 +82,7 @@ enum class BANK_0 : uint8_t {
 	FIFO_DATA          = 0x30,
 
 	SIGNAL_PATH_RESET  = 0x4B,
-	INTF_CONFIG0       = 0x4C,
-	INTF_CONFIG1       = 0x4D,
+
 	PWR_MGMT0          = 0x4E,
 	GYRO_CONFIG0       = 0x4F,
 	ACCEL_CONFIG0      = 0x50,
@@ -96,7 +95,7 @@ enum class BANK_0 : uint8_t {
 	FIFO_CONFIG3       = 0x61,
 
 	INT_CONFIG0        = 0x63,
-
+	INT_CONFIG1        = 0x64,
 	INT_SOURCE0        = 0x65,
 
 	SELF_TEST_CONFIG   = 0x70,
@@ -121,7 +120,7 @@ enum class BANK_2 : uint8_t {
 
 // DEVICE_CONFIG
 enum DEVICE_CONFIG_BIT : uint8_t {
-	SOFT_RESET_CONFIG = Bit0, //
+	SOFT_RESET_CONFIG = Bit0,
 };
 
 // INT_CONFIG
@@ -140,15 +139,14 @@ enum FIFO_CONFIG_BIT : uint8_t {
 // 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,
+	FIFO_FLUSH = Bit1,
 };
 
 // PWR_MGMT0
@@ -160,55 +158,42 @@ enum PWR_MGMT0_BIT : uint8_t {
 // 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,
-
+	//  0b000: ±2000dps (default)
+	GYRO_FS_SEL_2000_DPS_CLEAR = Bit7 | Bit6 | Bit5,
 
 	// 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,
+	//  0b0001: 32kHz (maximum)
+	GYRO_ODR_32KHZ_SET         = Bit0,
+	GYRO_ODR_32KHZ_CLEAR       = Bit3 | Bit2 | Bit0,
+	//  0b0011: 8kHz
+	GYRO_ODR_8KHZ_SET          = Bit1 | Bit0,
+	GYRO_ODR_8KHZ_CLEAR        = Bit3 | Bit2,
+	//  0b0110: 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,
-
+	//  0b000: ±16g (default)
+	ACCEL_FS_SEL_16G_CLEAR     = Bit7 | 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,
+	//  0b0001: 32kHz (maximum)
+	ACCEL_ODR_32KHZ_SET        = Bit0,
+	ACCEL_ODR_32KHZ_CLEAR      = Bit3 | Bit2 | Bit0,
+	//  0b0011: 8kHz
+	ACCEL_ODR_8KHZ_SET         = Bit1 | Bit0,
+	ACCEL_ODR_8KHZ_CLEAR       = Bit3 | Bit2,
+	//  0b0110: 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_UI_FILT_ORD_1ST_CLEAR = Bit3 | Bit2, // 00: 1st Order UI filter
 };
 
 // GYRO_ACCEL_CONFIG0
@@ -222,14 +207,15 @@ enum GYRO_ACCEL_CONFIG0_BIT : uint8_t {
 
 // ACCEL_CONFIG1
 enum ACCEL_CONFIG1_BIT : uint8_t {
-	ACCEL_UI_FILT_ORD = Bit4 | Bit3, // 00: 1st Order
+	ACCEL_UI_FILT_ORD_1ST_CLEAR = Bit4 | Bit3, // 00: 1st Order UI filter
 };
 
 // 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,
@@ -238,18 +224,21 @@ enum FIFO_CONFIG1_BIT : uint8_t {
 // INT_CONFIG0
 enum INT_CONFIG0_BIT : uint8_t {
 	// 3:2 FIFO_THS_INT_CLEAR
-	CLEAR_ON_FIFO_READ = Bit3,
+	FIFO_THS_INT_CLEAR = Bit3, // 10: Clear on FIFO data 1Byte Read
+};
+
+// INT_CONFIG1
+enum INT_CONFIG1_BIT : uint8_t {
+	INT_TPULSE_DURATION   = Bit6, // 1: Interrupt pulse duration is 8 µs. Required if ODR ≥ 4kHz, optional for ODR < 4kHz.
+	INT_TDEASSERT_DISABLE = Bit5, // 1: Disables de-assert duration. Required if ODR ≥ 4kHz, optional for ODR < 4kHz.
+	INT_ASYNC_RESET       = Bit4, // User should change setting to 0 from default setting of 1, for proper INT1 and INT2 pin operation
 };
 
 // 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,
+	RESET_DONE_INT1_EN = Bit4, // 1: Reset done interrupt routed to INT1 (enabled by default)
+
 	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
@@ -277,6 +266,7 @@ enum ACCEL_CONFIG_STATIC2_BIT : uint8_t {
 	ACCEL_AAF_DIS = Bit0,
 };
 
+
 namespace FIFO
 {
 static constexpr size_t SIZE = 2048;
diff --git a/src/drivers/imu/invensense/iim42652/iim42652_main.cpp b/src/drivers/imu/invensense/iim42652/iim42652_main.cpp
index ae88e7e709..036ad2fa1a 100644
--- a/src/drivers/imu/invensense/iim42652/iim42652_main.cpp
+++ b/src/drivers/imu/invensense/iim42652/iim42652_main.cpp
@@ -72,13 +72,11 @@ extern "C" int iim42652_main(int argc, char *argv[])
 
 	if (!strcmp(verb, "start")) {
 		return ThisDriver::module_start(cli, iterator);
-	}
 
-	if (!strcmp(verb, "stop")) {
+	} else if (!strcmp(verb, "stop")) {
 		return ThisDriver::module_stop(iterator);
-	}
 
-	if (!strcmp(verb, "status")) {
+	} else if (!strcmp(verb, "status")) {
 		return ThisDriver::module_status(iterator);
 	}
 
diff --git a/src/drivers/imu/invensense/mpu6000/CMakeLists.txt b/src/drivers/imu/invensense/mpu6000/CMakeLists.txt
index 65af48a8e8..10796c0e10 100644
--- a/src/drivers/imu/invensense/mpu6000/CMakeLists.txt
+++ b/src/drivers/imu/invensense/mpu6000/CMakeLists.txt
@@ -41,7 +41,5 @@ px4_add_module(
 		mpu6000_main.cpp
 		InvenSense_MPU6000_registers.hpp
 	DEPENDS
-		drivers_accelerometer
-		drivers_gyroscope
 		px4_work_queue
 	)
diff --git a/src/drivers/imu/invensense/mpu6000/InvenSense_MPU6000_registers.hpp b/src/drivers/imu/invensense/mpu6000/InvenSense_MPU6000_registers.hpp
index 768f4a3de9..2849ab1a98 100644
--- a/src/drivers/imu/invensense/mpu6000/InvenSense_MPU6000_registers.hpp
+++ b/src/drivers/imu/invensense/mpu6000/InvenSense_MPU6000_registers.hpp
@@ -62,6 +62,8 @@ static constexpr uint8_t WHOAMI = 0x68;
 
 static constexpr float TEMPERATURE_SENSITIVITY = 340.f; // LSB/C
 static constexpr float TEMPERATURE_OFFSET = 36.53f; // C
+static constexpr float TEMPERATURE_SENSOR_MIN = -40.f; // °C
+static constexpr float TEMPERATURE_SENSOR_MAX = 85.f; // °C
 
 enum class Register : uint8_t {
 	CONFIG            = 0x1A,
diff --git a/src/drivers/imu/invensense/mpu6000/MPU6000.cpp b/src/drivers/imu/invensense/mpu6000/MPU6000.cpp
index 701a330db9..5765b91379 100644
--- a/src/drivers/imu/invensense/mpu6000/MPU6000.cpp
+++ b/src/drivers/imu/invensense/mpu6000/MPU6000.cpp
@@ -1,6 +1,6 @@
 /****************************************************************************
  *
- *   Copyright (c) 2020-2021 PX4 Development Team. All rights reserved.
+ *   Copyright (c) 2020-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
@@ -33,6 +33,8 @@
 
 #include "MPU6000.hpp"
 
+#include <lib/parameters/param.h>
+
 using namespace time_literals;
 
 static constexpr int16_t combine(uint8_t msb, uint8_t lsb)
@@ -44,14 +46,16 @@ MPU6000::MPU6000(const I2CSPIDriverConfig &config) :
 	SPI(config),
 	I2CSPIDriver(config),
 	_drdy_gpio(config.drdy_gpio),
-	_px4_accel(get_device_id(), config.rotation),
-	_px4_gyro(get_device_id(), config.rotation)
+	_rotation(config.rotation)
 {
-	if (config.drdy_gpio != 0) {
+	if (_drdy_gpio != 0) {
 		_drdy_missed_perf = perf_alloc(PC_COUNT, MODULE_NAME": DRDY missed");
 	}
 
-	ConfigureSampleRate(_px4_gyro.get_max_rate_hz());
+	int32_t imu_gyro_rate_max = 400;
+	param_get(param_find("IMU_GYRO_RATEMAX"), &imu_gyro_rate_max);
+
+	ConfigureSampleRate(imu_gyro_rate_max);
 }
 
 MPU6000::~MPU6000()
@@ -107,14 +111,19 @@ void MPU6000::print_status()
 
 int MPU6000::probe()
 {
-	const uint8_t whoami = RegisterRead(Register::WHO_AM_I);
+	// 3 retries
+	for (int retry = 0; retry < 3; retry++) {
+		const uint8_t whoami = RegisterRead(Register::WHO_AM_I);
 
-	if (whoami != WHOAMI) {
-		DEVICE_DEBUG("unexpected WHO_AM_I 0x%02x", whoami);
-		return PX4_ERROR;
+		if (whoami == WHOAMI) {
+			return PX4_OK;
+
+		} else {
+			DEVICE_DEBUG("unexpected WHO_AM_I 0x%02x", whoami);
+		}
 	}
 
-	return PX4_OK;
+	return PX4_ERROR;
 }
 
 void MPU6000::RunImpl()
@@ -156,8 +165,8 @@ void MPU6000::RunImpl()
 				ScheduleDelayed(100_ms);
 
 			} else {
-				PX4_DEBUG("Reset not complete, check again in 10 ms");
-				ScheduleDelayed(10_ms);
+				PX4_DEBUG("Reset not complete, check again in 100 ms");
+				ScheduleDelayed(100_ms);
 			}
 		}
 
@@ -165,6 +174,8 @@ void MPU6000::RunImpl()
 
 	case STATE::CONFIGURE:
 		if (Configure()) {
+			_temperature = ReadTemperature();
+
 			// if configure succeeded then start reading from FIFO
 			_state = STATE::FIFO_READ;
 
@@ -203,7 +214,7 @@ void MPU6000::RunImpl()
 				// scheduled from interrupt if _drdy_timestamp_sample was set as expected
 				const hrt_abstime drdy_timestamp_sample = _drdy_timestamp_sample.fetch_and(0);
 
-				if ((now - drdy_timestamp_sample) < _fifo_empty_interval_us) {
+				if (now < drdy_timestamp_sample + _fifo_empty_interval_us) {
 					timestamp_sample = drdy_timestamp_sample;
 
 				} else {
@@ -278,7 +289,7 @@ void MPU6000::RunImpl()
 			} else {
 				// periodically update temperature (~1 Hz)
 				if (hrt_elapsed_time(&_temperature_update_timestamp) >= 1_s) {
-					UpdateTemperature();
+					_temperature = ReadTemperature();
 					_temperature_update_timestamp = now;
 				}
 			}
@@ -288,65 +299,10 @@ void MPU6000::RunImpl()
 	}
 }
 
-void MPU6000::ConfigureAccel()
-{
-	const uint8_t AFS_SEL = RegisterRead(Register::ACCEL_CONFIG) & (Bit4 | Bit3); // [4:3] AFS_SEL[1:0]
-
-	switch (AFS_SEL) {
-	case AFS_SEL_2G:
-		_px4_accel.set_scale(CONSTANTS_ONE_G / 16384.f);
-		_px4_accel.set_range(2.f * CONSTANTS_ONE_G);
-		break;
-
-	case AFS_SEL_4G:
-		_px4_accel.set_scale(CONSTANTS_ONE_G / 8192.f);
-		_px4_accel.set_range(4.f * CONSTANTS_ONE_G);
-		break;
-
-	case AFS_SEL_8G:
-		_px4_accel.set_scale(CONSTANTS_ONE_G / 4096.f);
-		_px4_accel.set_range(8.f * CONSTANTS_ONE_G);
-		break;
-
-	case AFS_SEL_16G:
-		_px4_accel.set_scale(CONSTANTS_ONE_G / 2048.f);
-		_px4_accel.set_range(16.f * CONSTANTS_ONE_G);
-		break;
-	}
-}
-
-void MPU6000::ConfigureGyro()
-{
-	const uint8_t GYRO_FS_SEL = RegisterRead(Register::GYRO_CONFIG) & (Bit4 | Bit3); // [4:3] FS_SEL[1:0]
-
-	float range_dps = 0.f;
-
-	switch (GYRO_FS_SEL) {
-	case FS_SEL_250_DPS:
-		range_dps = 250.f;
-		break;
-
-	case FS_SEL_500_DPS:
-		range_dps = 500.f;
-		break;
-
-	case FS_SEL_1000_DPS:
-		range_dps = 1000.f;
-		break;
-
-	case FS_SEL_2000_DPS:
-		range_dps = 2000.f;
-		break;
-	}
-
-	_px4_gyro.set_scale(math::radians(range_dps / 32768.f));
-	_px4_gyro.set_range(math::radians(range_dps));
-}
-
 void MPU6000::ConfigureSampleRate(int sample_rate)
 {
 	// round down to nearest FIFO sample dt
-	const float min_interval = FIFO_SAMPLE_DT * 4; // limit to 2 kHz (500 us interval)
+	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));
@@ -371,9 +327,6 @@ bool MPU6000::Configure()
 		}
 	}
 
-	ConfigureAccel();
-	ConfigureGyro();
-
 	return success;
 }
 
@@ -400,7 +353,7 @@ bool MPU6000::DataReadyInterruptConfigure()
 	}
 
 	// Setup data ready on falling edge
-	return px4_arch_gpiosetevent(_drdy_gpio, false, true, true, &DataReadyInterruptCallback, this) == 0;
+	return (px4_arch_gpiosetevent(_drdy_gpio, false, true, false, &DataReadyInterruptCallback, this) == 0);
 }
 
 bool MPU6000::DataReadyInterruptDisable()
@@ -409,7 +362,7 @@ bool MPU6000::DataReadyInterruptDisable()
 		return false;
 	}
 
-	return px4_arch_gpiosetevent(_drdy_gpio, false, false, false, nullptr, nullptr) == 0;
+	return (px4_arch_gpiosetevent(_drdy_gpio, false, false, false, nullptr, nullptr) == 0);
 }
 
 bool MPU6000::RegisterCheck(const register_config_t &reg_cfg)
@@ -462,23 +415,33 @@ void MPU6000::RegisterSetAndClearBits(Register reg, uint8_t setbits, uint8_t cle
 
 uint16_t MPU6000::FIFOReadCount()
 {
-	// read FIFO count
-	uint8_t fifo_count_buf[3] {};
-	fifo_count_buf[0] = static_cast<uint8_t>(Register::FIFO_COUNTH) | DIR_READ;
-	set_frequency(SPI_SPEED_SENSOR);
+	// transfer buffer
+	struct TransferBuffer {
+		uint8_t cmd{static_cast<uint8_t>(Register::FIFO_COUNTH) | DIR_READ};
+		uint8_t FIFO_COUNTH{0};
+		uint8_t FIFO_COUNTL{0};
+	} buffer{};
 
-	if (transfer(fifo_count_buf, fifo_count_buf, sizeof(fifo_count_buf)) != PX4_OK) {
+	// read FIFO count
+	if (transfer((uint8_t *)&buffer, (uint8_t *)&buffer, sizeof(buffer)) != PX4_OK) {
 		perf_count(_bad_transfer_perf);
 		return 0;
 	}
 
-	return combine(fifo_count_buf[1], fifo_count_buf[2]);
+	return (buffer.FIFO_COUNTH << 8) + buffer.FIFO_COUNTL;
 }
 
 bool MPU6000::FIFORead(const hrt_abstime &timestamp_sample, uint8_t samples)
 {
-	FIFOTransferBuffer buffer{};
-	const size_t transfer_size = math::min(samples * sizeof(FIFO::DATA) + 1, FIFO::SIZE);
+	// FIFO transfer buffer
+	struct FIFOTransferBuffer {
+		uint8_t cmd{static_cast<uint8_t>(Register::FIFO_R_W) | DIR_READ};
+		FIFO::DATA f[FIFO_MAX_SAMPLES] {};
+	} buffer{};
+
+	// cmd + samples (FIFO::DATA)
+	const size_t transfer_size = 1 + math::min(samples * sizeof(FIFO::DATA), FIFO::SIZE);
+
 	set_frequency(SPI_SPEED_SENSOR);
 
 	if (transfer((uint8_t *)&buffer, (uint8_t *)&buffer, transfer_size) != PX4_OK) {
@@ -486,9 +449,43 @@ bool MPU6000::FIFORead(const hrt_abstime &timestamp_sample, uint8_t samples)
 		return false;
 	}
 
+	const uint8_t valid_samples = samples;
 
-	ProcessGyro(timestamp_sample, buffer.f, samples);
-	return ProcessAccel(timestamp_sample, buffer.f, samples);
+	if (valid_samples > 0) {
+		sensor_imu_fifo_s sensor_imu_fifo{};
+		sensor_imu_fifo.timestamp_sample = timestamp_sample;
+		sensor_imu_fifo.device_id = get_device_id();
+		sensor_imu_fifo.dt = FIFO_SAMPLE_DT;
+		sensor_imu_fifo.samples = valid_samples;
+		sensor_imu_fifo.accel_scale = ACCEL_SCALE;
+		sensor_imu_fifo.gyro_scale = GYRO_SCALE;
+
+		for (int i = 0; i < valid_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)
+			sensor_imu_fifo.accel_x[i] =              combine(buffer.f[i].ACCEL_XOUT_H, buffer.f[i].ACCEL_XOUT_L);
+			sensor_imu_fifo.accel_y[i] = math::negate(combine(buffer.f[i].ACCEL_YOUT_H, buffer.f[i].ACCEL_YOUT_L));
+			sensor_imu_fifo.accel_z[i] = math::negate(combine(buffer.f[i].ACCEL_ZOUT_H, buffer.f[i].ACCEL_ZOUT_L));
+			rotate_3i(_rotation, sensor_imu_fifo.accel_x[i], sensor_imu_fifo.accel_y[i], sensor_imu_fifo.accel_z[i]);
+
+			sensor_imu_fifo.gyro_x[i] =              combine(buffer.f[i].GYRO_XOUT_H, buffer.f[i].GYRO_XOUT_L);
+			sensor_imu_fifo.gyro_y[i] = math::negate(combine(buffer.f[i].GYRO_YOUT_H, buffer.f[i].GYRO_YOUT_L));
+			sensor_imu_fifo.gyro_z[i] = math::negate(combine(buffer.f[i].GYRO_ZOUT_H, buffer.f[i].GYRO_ZOUT_L));
+			rotate_3i(_rotation, sensor_imu_fifo.gyro_x[i], sensor_imu_fifo.gyro_y[i], sensor_imu_fifo.gyro_z[i]);
+		}
+
+		sensor_imu_fifo.temperature = (hrt_elapsed_time(&_temperature_update_timestamp) < 5_s) ? _temperature : NAN;
+
+		sensor_imu_fifo.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);
+
+		sensor_imu_fifo.timestamp = hrt_absolute_time();
+		_sensor_imu_fifo_pub.publish(sensor_imu_fifo);
+
+		return true;
+	}
+
+	return false;
 }
 
 void MPU6000::FIFOReset()
@@ -514,105 +511,32 @@ void MPU6000::FIFOReset()
 	}
 }
 
-static bool fifo_accel_equal(const FIFO::DATA &f0, const FIFO::DATA &f1)
+float MPU6000::ReadTemperature()
 {
-	return (memcmp(&f0.ACCEL_XOUT_H, &f1.ACCEL_XOUT_H, 6) == 0);
-}
+	// transfer buffer
+	struct TransferBuffer {
+		uint8_t cmd{static_cast<uint8_t>(Register::TEMP_OUT_H) | DIR_READ};
+		uint8_t TEMP_OUT_H{0};
+		uint8_t TEMP_OUT_L{0};
+	} buffer{};
 
-bool MPU6000::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 * SAMPLES_PER_TRANSFER;
-
-	bool bad_data = false;
-
-	// FIFO contains 8 duplicated accel samples per gyro sample
-	for (int i = 0; i < samples; i++) {
-		_fifo_accel_samples_count++;
-
-		// only process new FIFO samples (1 every 8 samples expected)
-		const bool new_sample = !fifo_accel_equal(fifo[i], _fifo_sample_last_new_accel);
-
-		// process every 8th sample
-		if (_fifo_accel_samples_count == SAMPLES_PER_TRANSFER) {
-			int16_t accel_x = combine(fifo[i].ACCEL_XOUT_H, fifo[i].ACCEL_XOUT_L);
-			int16_t accel_y = combine(fifo[i].ACCEL_YOUT_H, fifo[i].ACCEL_YOUT_L);
-			int16_t accel_z = combine(fifo[i].ACCEL_ZOUT_H, fifo[i].ACCEL_ZOUT_L);
-
-			// 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[accel.samples] = accel_x;
-			accel.y[accel.samples] = (accel_y == INT16_MIN) ? INT16_MAX : -accel_y;
-			accel.z[accel.samples] = (accel_z == INT16_MIN) ? INT16_MAX : -accel_z;
-			accel.samples++;
-
-		} else if (new_sample && (_fifo_accel_samples_count > 1)) {
-			// a new unique sample after fewer than 8 samples is an error
-			bad_data = true;
-			perf_count(_bad_transfer_perf);
-		}
-
-		// reset previous unique sample and counter
-		if (new_sample || (_fifo_accel_samples_count == SAMPLES_PER_TRANSFER)) {
-			_fifo_accel_samples_count = 0;
-			_fifo_sample_last_new_accel = fifo[i];
-		}
-	}
-
-	_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) {
-		_px4_accel.updateFIFO(accel);
-	}
-
-	return !bad_data;
-}
-
-void MPU6000::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 = samples;
-	gyro.dt = FIFO_SAMPLE_DT;
-
-	for (int i = 0; i < samples; i++) {
-		const int16_t gyro_x = combine(fifo[i].GYRO_XOUT_H, fifo[i].GYRO_XOUT_L);
-		const int16_t gyro_y = combine(fifo[i].GYRO_YOUT_H, fifo[i].GYRO_YOUT_L);
-		const int16_t gyro_z = combine(fifo[i].GYRO_ZOUT_H, fifo[i].GYRO_ZOUT_L);
-
-		// 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;
-		gyro.y[i] = (gyro_y == INT16_MIN) ? INT16_MAX : -gyro_y;
-		gyro.z[i] = (gyro_z == INT16_MIN) ? INT16_MAX : -gyro_z;
-	}
-
-	_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));
-
-	_px4_gyro.updateFIFO(gyro);
-}
-
-void MPU6000::UpdateTemperature()
-{
 	// read current temperature
-	uint8_t temperature_buf[3] {};
-	temperature_buf[0] = static_cast<uint8_t>(Register::TEMP_OUT_H) | DIR_READ;
 	set_frequency(SPI_SPEED_SENSOR);
 
-	if (transfer(temperature_buf, temperature_buf, sizeof(temperature_buf)) != PX4_OK) {
+	if (transfer((uint8_t *)&buffer, (uint8_t *)&buffer, sizeof(buffer)) != PX4_OK) {
 		perf_count(_bad_transfer_perf);
-		return;
+		return NAN;;
 	}
 
-	const int16_t TEMP_OUT = combine(temperature_buf[1], temperature_buf[2]);
+	const int16_t TEMP_OUT = combine(buffer.TEMP_OUT_H, buffer.TEMP_OUT_L);
 	const float TEMP_degC = (TEMP_OUT / TEMPERATURE_SENSITIVITY) + TEMPERATURE_OFFSET;
 
-	if (PX4_ISFINITE(TEMP_degC)) {
-		_px4_accel.set_temperature(TEMP_degC);
-		_px4_gyro.set_temperature(TEMP_degC);
+	if (PX4_ISFINITE(TEMP_degC)
+	    && (TEMP_degC >= TEMPERATURE_SENSOR_MIN)
+	    && (TEMP_degC <= TEMPERATURE_SENSOR_MAX)) {
+
+		return TEMP_degC;
 	}
+
+	return NAN;
 }
diff --git a/src/drivers/imu/invensense/mpu6000/MPU6000.hpp b/src/drivers/imu/invensense/mpu6000/MPU6000.hpp
index 2d5180013a..239d192928 100644
--- a/src/drivers/imu/invensense/mpu6000/MPU6000.hpp
+++ b/src/drivers/imu/invensense/mpu6000/MPU6000.hpp
@@ -1,6 +1,6 @@
 /****************************************************************************
  *
- *   Copyright (c) 2020-2021 PX4 Development Team. All rights reserved.
+ *   Copyright (c) 2020-2022 PX4 Development Team. All rights reserved.
  *
  * Redistribution and use in source and binary forms, with or without
  * modification, are permitted provided that the following conditions
@@ -43,14 +43,15 @@
 #include "InvenSense_MPU6000_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/PublicationMulti.hpp>
+#include <uORB/topics/sensor_imu_fifo.h>
+
 using namespace InvenSense_MPU6000;
 
 class MPU6000 : public device::SPI, public I2CSPIDriver<MPU6000>
@@ -71,20 +72,13 @@ private:
 
 	// Sensor Configuration
 	static constexpr float FIFO_SAMPLE_DT{1e6f / 8000.f};
-	static constexpr int32_t SAMPLES_PER_TRANSFER{8};                    // ensure at least 1 new accel sample per transfer
 	static constexpr float GYRO_RATE{1e6f / FIFO_SAMPLE_DT};             // 8000 Hz gyro
-	static constexpr float ACCEL_RATE{GYRO_RATE / SAMPLES_PER_TRANSFER}; // 1000 Hz accel
 
 	// maximum FIFO samples per transfer is limited to the size of sensor_accel_fifo/sensor_gyro_fifo
-	static constexpr int32_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 int32_t FIFO_MAX_SAMPLES{math::min(FIFO::SIZE / sizeof(FIFO::DATA), (size_t)sensor_imu_fifo_s::FIFO_SIZE)};
 
-	// Transfer data
-	struct FIFOTransferBuffer {
-		uint8_t cmd{static_cast<uint8_t>(Register::FIFO_R_W) | DIR_READ};
-		FIFO::DATA f[FIFO_MAX_SAMPLES] {};
-	};
-	// ensure no struct padding
-	static_assert(sizeof(FIFOTransferBuffer) == (1 + FIFO_MAX_SAMPLES *sizeof(FIFO::DATA)));
+	static constexpr float ACCEL_SCALE{CONSTANTS_ONE_G / 2048.f}; // AFS_SEL_16G:
+	static constexpr float GYRO_SCALE{math::radians(2000.f / 32768.f)}; // FS_SEL_2000_DPS:
 
 	struct register_config_t {
 		Register reg;
@@ -97,8 +91,6 @@ private:
 	bool Reset();
 
 	bool Configure();
-	void ConfigureAccel();
-	void ConfigureGyro();
 	void ConfigureSampleRate(int sample_rate);
 
 	static int DataReadyInterruptCallback(int irq, void *context, void *arg);
@@ -116,14 +108,15 @@ private:
 	bool FIFORead(const hrt_abstime &timestamp_sample, uint8_t samples);
 	void FIFOReset();
 
-	bool 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);
-	void UpdateTemperature();
+	float ReadTemperature();
 
 	const spi_drdy_gpio_t _drdy_gpio;
 
-	PX4Accelerometer _px4_accel;
-	PX4Gyroscope _px4_gyro;
+	uORB::PublicationMulti<sensor_imu_fifo_s> _sensor_imu_fifo_pub{ORB_ID(sensor_imu_fifo)};
+
+	const enum Rotation _rotation;
+
+	float _temperature{NAN};
 
 	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")};
@@ -137,9 +130,6 @@ private:
 	hrt_abstime _temperature_update_timestamp{0};
 	int _failure_count{0};
 
-	FIFO::DATA _fifo_sample_last_new_accel{};
-	uint32_t _fifo_accel_samples_count{0};
-
 	px4::atomic<hrt_abstime> _drdy_timestamp_sample{0};
 	int32_t _drdy_count{0};
 	bool _data_ready_interrupt_enabled{false};
diff --git a/src/drivers/imu/invensense/mpu6500/CMakeLists.txt b/src/drivers/imu/invensense/mpu6500/CMakeLists.txt
index c8c14ac3d4..c69216f7dc 100644
--- a/src/drivers/imu/invensense/mpu6500/CMakeLists.txt
+++ b/src/drivers/imu/invensense/mpu6500/CMakeLists.txt
@@ -40,7 +40,5 @@ px4_add_module(
 		MPU6500.hpp
 		mpu6500_main.cpp
 	DEPENDS
-		drivers_accelerometer
-		drivers_gyroscope
 		px4_work_queue
 	)
diff --git a/src/drivers/imu/invensense/mpu6500/InvenSense_MPU6500_registers.hpp b/src/drivers/imu/invensense/mpu6500/InvenSense_MPU6500_registers.hpp
index fee6661eb9..5f2dce7cc9 100644
--- a/src/drivers/imu/invensense/mpu6500/InvenSense_MPU6500_registers.hpp
+++ b/src/drivers/imu/invensense/mpu6500/InvenSense_MPU6500_registers.hpp
@@ -42,6 +42,8 @@
 
 #include <cstdint>
 
+namespace InvenSense_MPU6500
+{
 // TODO: move to a central header
 static constexpr uint8_t Bit0 = (1 << 0);
 static constexpr uint8_t Bit1 = (1 << 1);
@@ -52,8 +54,6 @@ static constexpr uint8_t Bit5 = (1 << 5);
 static constexpr uint8_t Bit6 = (1 << 6);
 static constexpr uint8_t Bit7 = (1 << 7);
 
-namespace InvenSense_MPU6500
-{
 static constexpr uint32_t SPI_SPEED = 1 * 1000 * 1000;
 static constexpr uint32_t SPI_SPEED_SENSOR = 10 * 1000 * 1000; // 20MHz for reading sensor and interrupt registers
 static constexpr uint8_t DIR_READ = 0x80;
@@ -62,6 +62,8 @@ static constexpr uint8_t WHOAMI = 0x70;
 
 static constexpr float TEMPERATURE_SENSITIVITY = 333.87f; // LSB/C
 static constexpr float TEMPERATURE_OFFSET = 21.f; // C
+static constexpr float TEMPERATURE_SENSOR_MIN = -40.f; // °C
+static constexpr float TEMPERATURE_SENSOR_MAX = 85.f; // °C
 
 enum class Register : uint8_t {
 
diff --git a/src/drivers/imu/invensense/mpu6500/MPU6500.cpp b/src/drivers/imu/invensense/mpu6500/MPU6500.cpp
index 2f29db0f79..7a414ddbbb 100644
--- a/src/drivers/imu/invensense/mpu6500/MPU6500.cpp
+++ b/src/drivers/imu/invensense/mpu6500/MPU6500.cpp
@@ -1,6 +1,6 @@
 /****************************************************************************
  *
- *   Copyright (c) 2020-2021 PX4 Development Team. All rights reserved.
+ *   Copyright (c) 2020-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
@@ -33,6 +33,8 @@
 
 #include "MPU6500.hpp"
 
+#include <lib/parameters/param.h>
+
 using namespace time_literals;
 
 static constexpr int16_t combine(uint8_t msb, uint8_t lsb)
@@ -44,14 +46,16 @@ MPU6500::MPU6500(const I2CSPIDriverConfig &config) :
 	SPI(config),
 	I2CSPIDriver(config),
 	_drdy_gpio(config.drdy_gpio),
-	_px4_accel(get_device_id(), config.rotation),
-	_px4_gyro(get_device_id(), config.rotation)
+	_rotation(config.rotation)
 {
-	if (config.drdy_gpio != 0) {
+	if (_drdy_gpio != 0) {
 		_drdy_missed_perf = perf_alloc(PC_COUNT, MODULE_NAME": DRDY missed");
 	}
 
-	ConfigureSampleRate(_px4_gyro.get_max_rate_hz());
+	int32_t imu_gyro_rate_max = 400;
+	param_get(param_find("IMU_GYRO_RATEMAX"), &imu_gyro_rate_max);
+
+	ConfigureSampleRate(imu_gyro_rate_max);
 }
 
 MPU6500::~MPU6500()
@@ -131,14 +135,19 @@ bool MPU6500::StoreCheckedRegisterValue(Register reg)
 
 int MPU6500::probe()
 {
-	const uint8_t whoami = RegisterRead(Register::WHO_AM_I);
+	// 3 retries
+	for (int retry = 0; retry < 3; retry++) {
+		const uint8_t whoami = RegisterRead(Register::WHO_AM_I);
 
-	if (whoami != WHOAMI) {
-		DEVICE_DEBUG("unexpected WHO_AM_I 0x%02x", whoami);
-		return PX4_ERROR;
+		if (whoami == WHOAMI) {
+			return PX4_OK;
+
+		} else {
+			DEVICE_DEBUG("unexpected WHO_AM_I 0x%02x", whoami);
+		}
 	}
 
-	return PX4_OK;
+	return PX4_ERROR;
 }
 
 void MPU6500::RunImpl()
@@ -188,8 +197,8 @@ void MPU6500::RunImpl()
 				ScheduleDelayed(100_ms);
 
 			} else {
-				PX4_DEBUG("Reset not complete, check again in 10 ms");
-				ScheduleDelayed(10_ms);
+				PX4_DEBUG("Reset not complete, check again in 100 ms");
+				ScheduleDelayed(100_ms);
 			}
 		}
 
@@ -197,6 +206,8 @@ void MPU6500::RunImpl()
 
 	case STATE::CONFIGURE:
 		if (Configure()) {
+			_temperature = ReadTemperature();
+
 			// if configure succeeded then start reading from FIFO
 			_state = STATE::FIFO_READ;
 
@@ -235,7 +246,7 @@ void MPU6500::RunImpl()
 				// scheduled from interrupt if _drdy_timestamp_sample was set as expected
 				const hrt_abstime drdy_timestamp_sample = _drdy_timestamp_sample.fetch_and(0);
 
-				if ((now - drdy_timestamp_sample) < _fifo_empty_interval_us) {
+				if (now < drdy_timestamp_sample + _fifo_empty_interval_us) {
 					timestamp_sample = drdy_timestamp_sample;
 
 				} else {
@@ -272,7 +283,7 @@ void MPU6500::RunImpl()
 					FIFOReset();
 					perf_count(_fifo_overflow_perf);
 
-				} else if (samples >= SAMPLES_PER_TRANSFER) {
+				} else if (samples >= 1) {
 					if (FIFORead(timestamp_sample, samples)) {
 						success = true;
 
@@ -288,6 +299,7 @@ void MPU6500::RunImpl()
 
 				// full reset if things are failing consistently
 				if (_failure_count > 10) {
+					PX4_DEBUG("Full reset because things are failing consistently");
 					Reset();
 					return;
 				}
@@ -302,13 +314,14 @@ void MPU6500::RunImpl()
 				} else {
 					// register check failed, force reset
 					perf_count(_bad_register_perf);
+					PX4_DEBUG("Force reset because register 0x%02hhX check failed ", (uint8_t)_register_cfg[_checked_register].reg);
 					Reset();
 				}
 
 			} else {
 				// periodically update temperature (~1 Hz)
 				if (hrt_elapsed_time(&_temperature_update_timestamp) >= 1_s) {
-					UpdateTemperature();
+					_temperature = ReadTemperature();
 					_temperature_update_timestamp = now;
 				}
 			}
@@ -318,65 +331,10 @@ void MPU6500::RunImpl()
 	}
 }
 
-void MPU6500::ConfigureAccel()
-{
-	const uint8_t ACCEL_FS_SEL = RegisterRead(Register::ACCEL_CONFIG) & (Bit4 | Bit3); // [4:3] ACCEL_FS_SEL[1:0]
-
-	switch (ACCEL_FS_SEL) {
-	case ACCEL_FS_SEL_2G:
-		_px4_accel.set_scale(CONSTANTS_ONE_G / 16384.f);
-		_px4_accel.set_range(2.f * CONSTANTS_ONE_G);
-		break;
-
-	case ACCEL_FS_SEL_4G:
-		_px4_accel.set_scale(CONSTANTS_ONE_G / 8192.f);
-		_px4_accel.set_range(4.f * CONSTANTS_ONE_G);
-		break;
-
-	case ACCEL_FS_SEL_8G:
-		_px4_accel.set_scale(CONSTANTS_ONE_G / 4096.f);
-		_px4_accel.set_range(8.f * CONSTANTS_ONE_G);
-		break;
-
-	case ACCEL_FS_SEL_16G:
-		_px4_accel.set_scale(CONSTANTS_ONE_G / 2048.f);
-		_px4_accel.set_range(16.f * CONSTANTS_ONE_G);
-		break;
-	}
-}
-
-void MPU6500::ConfigureGyro()
-{
-	const uint8_t GYRO_FS_SEL = RegisterRead(Register::GYRO_CONFIG) & (Bit4 | Bit3); // [4:3] GYRO_FS_SEL[1:0]
-
-	float range_dps = 0.f;
-
-	switch (GYRO_FS_SEL) {
-	case GYRO_FS_SEL_250_DPS:
-		range_dps = 250.f;
-		break;
-
-	case GYRO_FS_SEL_500_DPS:
-		range_dps = 500.f;
-		break;
-
-	case GYRO_FS_SEL_1000_DPS:
-		range_dps = 1000.f;
-		break;
-
-	case GYRO_FS_SEL_2000_DPS:
-		range_dps = 2000.f;
-		break;
-	}
-
-	_px4_gyro.set_scale(math::radians(range_dps / 32768.f));
-	_px4_gyro.set_range(math::radians(range_dps));
-}
-
 void MPU6500::ConfigureSampleRate(int sample_rate)
 {
-	// round down to nearest FIFO sample dt * SAMPLES_PER_TRANSFER
-	const float min_interval = FIFO_SAMPLE_DT * SAMPLES_PER_TRANSFER;
+	// 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));
@@ -401,9 +359,6 @@ bool MPU6500::Configure()
 		}
 	}
 
-	ConfigureAccel();
-	ConfigureGyro();
-
 	return success;
 }
 
@@ -430,7 +385,7 @@ bool MPU6500::DataReadyInterruptConfigure()
 	}
 
 	// Setup data ready on falling edge
-	return px4_arch_gpiosetevent(_drdy_gpio, false, true, true, &DataReadyInterruptCallback, this) == 0;
+	return (px4_arch_gpiosetevent(_drdy_gpio, false, true, false, &DataReadyInterruptCallback, this) == 0);
 }
 
 bool MPU6500::DataReadyInterruptDisable()
@@ -439,7 +394,7 @@ bool MPU6500::DataReadyInterruptDisable()
 		return false;
 	}
 
-	return px4_arch_gpiosetevent(_drdy_gpio, false, false, false, nullptr, nullptr) == 0;
+	return (px4_arch_gpiosetevent(_drdy_gpio, false, false, false, nullptr, nullptr) == 0);
 }
 
 bool MPU6500::RegisterCheck(const register_config_t &reg_cfg)
@@ -490,23 +445,33 @@ void MPU6500::RegisterSetAndClearBits(Register reg, uint8_t setbits, uint8_t cle
 
 uint16_t MPU6500::FIFOReadCount()
 {
-	// read FIFO count
-	uint8_t fifo_count_buf[3] {};
-	fifo_count_buf[0] = static_cast<uint8_t>(Register::FIFO_COUNTH) | DIR_READ;
-	set_frequency(SPI_SPEED_SENSOR);
+	// transfer buffer
+	struct TransferBuffer {
+		uint8_t cmd{static_cast<uint8_t>(Register::FIFO_COUNTH) | DIR_READ};
+		uint8_t FIFO_COUNTH{0};
+		uint8_t FIFO_COUNTL{0};
+	} buffer{};
 
-	if (transfer(fifo_count_buf, fifo_count_buf, sizeof(fifo_count_buf)) != PX4_OK) {
+	// read FIFO count
+	if (transfer((uint8_t *)&buffer, (uint8_t *)&buffer, sizeof(buffer)) != PX4_OK) {
 		perf_count(_bad_transfer_perf);
 		return 0;
 	}
 
-	return combine(fifo_count_buf[1], fifo_count_buf[2]);
+	return (buffer.FIFO_COUNTH << 8) + buffer.FIFO_COUNTL;
 }
 
 bool MPU6500::FIFORead(const hrt_abstime &timestamp_sample, uint8_t samples)
 {
-	FIFOTransferBuffer buffer{};
-	const size_t transfer_size = math::min(samples * sizeof(FIFO::DATA) + 1, FIFO::SIZE);
+	// FIFO transfer buffer
+	struct FIFOTransferBuffer {
+		uint8_t cmd{static_cast<uint8_t>(Register::FIFO_R_W) | DIR_READ};
+		FIFO::DATA f[FIFO_MAX_SAMPLES] {};
+	} buffer{};
+
+	// cmd + samples (FIFO::DATA)
+	const size_t transfer_size = 1 + math::min(samples * sizeof(FIFO::DATA), FIFO::SIZE);
+
 	set_frequency(SPI_SPEED_SENSOR);
 
 	if (transfer((uint8_t *)&buffer, (uint8_t *)&buffer, transfer_size) != PX4_OK) {
@@ -514,9 +479,43 @@ bool MPU6500::FIFORead(const hrt_abstime &timestamp_sample, uint8_t samples)
 		return false;
 	}
 
+	const uint8_t valid_samples = samples;
 
-	ProcessGyro(timestamp_sample, buffer.f, samples);
-	return ProcessAccel(timestamp_sample, buffer.f, samples);
+	if (valid_samples > 0) {
+		sensor_imu_fifo_s sensor_imu_fifo{};
+		sensor_imu_fifo.timestamp_sample = timestamp_sample;
+		sensor_imu_fifo.device_id = get_device_id();
+		sensor_imu_fifo.dt = FIFO_SAMPLE_DT;
+		sensor_imu_fifo.samples = valid_samples;
+		sensor_imu_fifo.accel_scale = ACCEL_SCALE;
+		sensor_imu_fifo.gyro_scale = GYRO_SCALE;
+
+		for (int i = 0; i < valid_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)
+			sensor_imu_fifo.accel_x[i] =              combine(buffer.f[i].ACCEL_XOUT_H, buffer.f[i].ACCEL_XOUT_L);
+			sensor_imu_fifo.accel_y[i] = math::negate(combine(buffer.f[i].ACCEL_YOUT_H, buffer.f[i].ACCEL_YOUT_L));
+			sensor_imu_fifo.accel_z[i] = math::negate(combine(buffer.f[i].ACCEL_ZOUT_H, buffer.f[i].ACCEL_ZOUT_L));
+			rotate_3i(_rotation, sensor_imu_fifo.accel_x[i], sensor_imu_fifo.accel_y[i], sensor_imu_fifo.accel_z[i]);
+
+			sensor_imu_fifo.gyro_x[i] =              combine(buffer.f[i].GYRO_XOUT_H, buffer.f[i].GYRO_XOUT_L);
+			sensor_imu_fifo.gyro_y[i] = math::negate(combine(buffer.f[i].GYRO_YOUT_H, buffer.f[i].GYRO_YOUT_L));
+			sensor_imu_fifo.gyro_z[i] = math::negate(combine(buffer.f[i].GYRO_ZOUT_H, buffer.f[i].GYRO_ZOUT_L));
+			rotate_3i(_rotation, sensor_imu_fifo.gyro_x[i], sensor_imu_fifo.gyro_y[i], sensor_imu_fifo.gyro_z[i]);
+		}
+
+		sensor_imu_fifo.temperature = (hrt_elapsed_time(&_temperature_update_timestamp) < 5_s) ? _temperature : NAN;
+
+		sensor_imu_fifo.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);
+
+		sensor_imu_fifo.timestamp = hrt_absolute_time();
+		_sensor_imu_fifo_pub.publish(sensor_imu_fifo);
+
+		return true;
+	}
+
+	return false;
 }
 
 void MPU6500::FIFOReset()
@@ -542,106 +541,32 @@ void MPU6500::FIFOReset()
 	}
 }
 
-static bool fifo_accel_equal(const FIFO::DATA &f0, const FIFO::DATA &f1)
+float MPU6500::ReadTemperature()
 {
-	return (memcmp(&f0.ACCEL_XOUT_H, &f1.ACCEL_XOUT_H, 6) == 0);
-}
+	// transfer buffer
+	struct TransferBuffer {
+		uint8_t cmd{static_cast<uint8_t>(Register::TEMP_OUT_H) | DIR_READ};
+		uint8_t TEMP_OUT_H{0};
+		uint8_t TEMP_OUT_L{0};
+	} buffer{};
 
-bool MPU6500::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 * SAMPLES_PER_TRANSFER;
-
-	bool bad_data = false;
-
-	// accel data is doubled in FIFO, but might be shifted
-	int accel_first_sample = 1;
-
-	if (samples >= 4) {
-		if (fifo_accel_equal(fifo[0], fifo[1]) && fifo_accel_equal(fifo[2], fifo[3])) {
-			// [A0, A1, A2, A3]
-			//  A0==A1, A2==A3
-			accel_first_sample = 1;
-
-		} else if (fifo_accel_equal(fifo[1], fifo[2])) {
-			// [A0, A1, A2, A3]
-			//  A0, A1==A2, A3
-			accel_first_sample = 0;
-
-		} else {
-			// no matching accel samples is an error
-			bad_data = true;
-			perf_count(_bad_transfer_perf);
-		}
-	}
-
-	for (int i = accel_first_sample; i < samples; i = i + SAMPLES_PER_TRANSFER) {
-		int16_t accel_x = combine(fifo[i].ACCEL_XOUT_H, fifo[i].ACCEL_XOUT_L);
-		int16_t accel_y = combine(fifo[i].ACCEL_YOUT_H, fifo[i].ACCEL_YOUT_L);
-		int16_t accel_z = combine(fifo[i].ACCEL_ZOUT_H, fifo[i].ACCEL_ZOUT_L);
-
-		// 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[accel.samples] = accel_x;
-		accel.y[accel.samples] = (accel_y == INT16_MIN) ? INT16_MAX : -accel_y;
-		accel.z[accel.samples] = (accel_z == INT16_MIN) ? INT16_MAX : -accel_z;
-		accel.samples++;
-	}
-
-	_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) {
-		_px4_accel.updateFIFO(accel);
-	}
-
-	return !bad_data;
-}
-
-void MPU6500::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 = samples;
-	gyro.dt = FIFO_SAMPLE_DT;
-
-	for (int i = 0; i < samples; i++) {
-		const int16_t gyro_x = combine(fifo[i].GYRO_XOUT_H, fifo[i].GYRO_XOUT_L);
-		const int16_t gyro_y = combine(fifo[i].GYRO_YOUT_H, fifo[i].GYRO_YOUT_L);
-		const int16_t gyro_z = combine(fifo[i].GYRO_ZOUT_H, fifo[i].GYRO_ZOUT_L);
-
-		// 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;
-		gyro.y[i] = (gyro_y == INT16_MIN) ? INT16_MAX : -gyro_y;
-		gyro.z[i] = (gyro_z == INT16_MIN) ? INT16_MAX : -gyro_z;
-	}
-
-	_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));
-
-	_px4_gyro.updateFIFO(gyro);
-}
-
-void MPU6500::UpdateTemperature()
-{
 	// read current temperature
-	uint8_t temperature_buf[3] {};
-	temperature_buf[0] = static_cast<uint8_t>(Register::TEMP_OUT_H) | DIR_READ;
 	set_frequency(SPI_SPEED_SENSOR);
 
-	if (transfer(temperature_buf, temperature_buf, sizeof(temperature_buf)) != PX4_OK) {
+	if (transfer((uint8_t *)&buffer, (uint8_t *)&buffer, sizeof(buffer)) != PX4_OK) {
 		perf_count(_bad_transfer_perf);
-		return;
+		return NAN;
 	}
 
-	const int16_t TEMP_OUT = combine(temperature_buf[1], temperature_buf[2]);
+	const int16_t TEMP_OUT = combine(buffer.TEMP_OUT_H, buffer.TEMP_OUT_L);
 	const float TEMP_degC = (TEMP_OUT / TEMPERATURE_SENSITIVITY) + TEMPERATURE_OFFSET;
 
-	if (PX4_ISFINITE(TEMP_degC)) {
-		_px4_accel.set_temperature(TEMP_degC);
-		_px4_gyro.set_temperature(TEMP_degC);
+	if (PX4_ISFINITE(TEMP_degC)
+	    && (TEMP_degC >= TEMPERATURE_SENSOR_MIN)
+	    && (TEMP_degC <= TEMPERATURE_SENSOR_MAX)) {
+
+		return TEMP_degC;
 	}
+
+	return NAN;
 }
diff --git a/src/drivers/imu/invensense/mpu6500/MPU6500.hpp b/src/drivers/imu/invensense/mpu6500/MPU6500.hpp
index 7686db5114..4b135a32dc 100644
--- a/src/drivers/imu/invensense/mpu6500/MPU6500.hpp
+++ b/src/drivers/imu/invensense/mpu6500/MPU6500.hpp
@@ -1,6 +1,6 @@
 /****************************************************************************
  *
- *   Copyright (c) 2020-2021 PX4 Development Team. All rights reserved.
+ *   Copyright (c) 2020-2022 PX4 Development Team. All rights reserved.
  *
  * Redistribution and use in source and binary forms, with or without
  * modification, are permitted provided that the following conditions
@@ -43,14 +43,15 @@
 #include "InvenSense_MPU6500_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/PublicationMulti.hpp>
+#include <uORB/topics/sensor_imu_fifo.h>
+
 using namespace InvenSense_MPU6500;
 
 class MPU6500 : public device::SPI, public I2CSPIDriver<MPU6500>
@@ -71,20 +72,13 @@ private:
 
 	// Sensor Configuration
 	static constexpr float FIFO_SAMPLE_DT{1e6f / 8000.f};
-	static constexpr int32_t SAMPLES_PER_TRANSFER{2};                    // ensure at least 1 new accel sample per transfer
 	static constexpr float GYRO_RATE{1e6f / FIFO_SAMPLE_DT};             // 8000 Hz gyro
-	static constexpr float ACCEL_RATE{GYRO_RATE / SAMPLES_PER_TRANSFER}; // 4000 Hz accel
 
 	// maximum FIFO samples per transfer is limited to the size of sensor_accel_fifo/sensor_gyro_fifo
-	static constexpr int32_t FIFO_MAX_SAMPLES{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 int32_t FIFO_MAX_SAMPLES{math::min(FIFO::SIZE / sizeof(FIFO::DATA), (size_t)sensor_imu_fifo_s::FIFO_SIZE)};
 
-	// Transfer data
-	struct FIFOTransferBuffer {
-		uint8_t cmd{static_cast<uint8_t>(Register::FIFO_R_W) | DIR_READ};
-		FIFO::DATA f[FIFO_MAX_SAMPLES] {};
-	};
-	// ensure no struct padding
-	static_assert(sizeof(FIFOTransferBuffer) == (1 + FIFO_MAX_SAMPLES *sizeof(FIFO::DATA)));
+	static constexpr float ACCEL_SCALE{CONSTANTS_ONE_G / 2048.f}; // ACCEL_FS_SEL_16G
+	static constexpr float GYRO_SCALE{math::radians(2000.f / 32768.f)}; // GYRO_FS_SEL_2000_DPS
 
 	struct register_config_t {
 		Register reg;
@@ -97,8 +91,6 @@ private:
 	bool Reset();
 
 	bool Configure();
-	void ConfigureAccel();
-	void ConfigureGyro();
 	void ConfigureSampleRate(int sample_rate);
 
 	static int DataReadyInterruptCallback(int irq, void *context, void *arg);
@@ -117,14 +109,15 @@ private:
 	bool FIFORead(const hrt_abstime &timestamp_sample, uint8_t samples);
 	void FIFOReset();
 
-	bool 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);
-	void UpdateTemperature();
+	float ReadTemperature();
 
 	const spi_drdy_gpio_t _drdy_gpio;
 
-	PX4Accelerometer _px4_accel;
-	PX4Gyroscope _px4_gyro;
+	uORB::PublicationMulti<sensor_imu_fifo_s> _sensor_imu_fifo_pub{ORB_ID(sensor_imu_fifo)};
+
+	const enum Rotation _rotation;
+
+	float _temperature{NAN};
 
 	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")};
diff --git a/src/drivers/imu/invensense/mpu9250/CMakeLists.txt b/src/drivers/imu/invensense/mpu9250/CMakeLists.txt
index a8afaa9f8c..47e6c57662 100644
--- a/src/drivers/imu/invensense/mpu9250/CMakeLists.txt
+++ b/src/drivers/imu/invensense/mpu9250/CMakeLists.txt
@@ -44,8 +44,6 @@ px4_add_module(
 		MPU9250.hpp
 		mpu9250_main.cpp
 	DEPENDS
-		drivers_accelerometer
-		drivers_gyroscope
 		drivers_magnetometer
 		px4_work_queue
 )
@@ -60,7 +58,5 @@ px4_add_module(
 		MPU9250_I2C.hpp
 		mpu9250_i2c_main.cpp
 	DEPENDS
-		drivers_accelerometer
-		drivers_gyroscope
 		px4_work_queue
 )
diff --git a/src/drivers/imu/invensense/mpu9250/InvenSense_MPU9250_registers.hpp b/src/drivers/imu/invensense/mpu9250/InvenSense_MPU9250_registers.hpp
index fdc907cb4f..28a89b2226 100644
--- a/src/drivers/imu/invensense/mpu9250/InvenSense_MPU9250_registers.hpp
+++ b/src/drivers/imu/invensense/mpu9250/InvenSense_MPU9250_registers.hpp
@@ -1,6 +1,6 @@
 /****************************************************************************
  *
- *   Copyright (c) 2020-2021 PX4 Development Team. All rights reserved.
+ *   Copyright (c) 2020-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
@@ -65,6 +65,8 @@ static constexpr uint8_t WHOAMI = 0x71;
 
 static constexpr float TEMPERATURE_SENSITIVITY = 333.87f; // LSB/C
 static constexpr float TEMPERATURE_OFFSET = 21.f; // C
+static constexpr float TEMPERATURE_SENSOR_MIN = -40.f; // °C
+static constexpr float TEMPERATURE_SENSOR_MAX = 85.f; // °C
 
 enum class Register : uint8_t {
 
diff --git a/src/drivers/imu/invensense/mpu9250/MPU9250.cpp b/src/drivers/imu/invensense/mpu9250/MPU9250.cpp
index 501f5f97db..d61d8016f5 100644
--- a/src/drivers/imu/invensense/mpu9250/MPU9250.cpp
+++ b/src/drivers/imu/invensense/mpu9250/MPU9250.cpp
@@ -1,6 +1,6 @@
 /****************************************************************************
  *
- *   Copyright (c) 2020-2021 PX4 Development Team. All rights reserved.
+ *   Copyright (c) 2020-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
@@ -35,6 +35,8 @@
 
 #include "AKM_AK8963_registers.hpp"
 
+#include <lib/parameters/param.h>
+
 using namespace time_literals;
 
 static constexpr int16_t combine(uint8_t msb, uint8_t lsb)
@@ -46,16 +48,18 @@ MPU9250::MPU9250(const I2CSPIDriverConfig &config) :
 	SPI(config),
 	I2CSPIDriver(config),
 	_drdy_gpio(config.drdy_gpio),
-	_px4_accel(get_device_id(), config.rotation),
-	_px4_gyro(get_device_id(), config.rotation)
+	_rotation(config.rotation)
 {
 	if (_drdy_gpio != 0) {
 		_drdy_missed_perf = perf_alloc(PC_COUNT, MODULE_NAME": DRDY missed");
 	}
 
-	ConfigureSampleRate(_px4_gyro.get_max_rate_hz());
+	int32_t imu_gyro_rate_max = 400;
+	param_get(param_find("IMU_GYRO_RATEMAX"), &imu_gyro_rate_max);
 
-	bool enable_magnetometer = config.custom1 == 1;
+	ConfigureSampleRate(imu_gyro_rate_max);
+
+	bool enable_magnetometer = (config.custom1 == 1);
 
 	if (enable_magnetometer) {
 		_slave_ak8963_magnetometer = new AKM_AK8963::MPU9250_AK8963(*this, config.rotation);
@@ -160,14 +164,19 @@ bool MPU9250::StoreCheckedRegisterValue(Register reg)
 
 int MPU9250::probe()
 {
-	const uint8_t whoami = RegisterRead(Register::WHO_AM_I);
+	// 3 retries
+	for (int retry = 0; retry < 3; retry++) {
+		const uint8_t whoami = RegisterRead(Register::WHO_AM_I);
 
-	if (whoami != WHOAMI) {
-		DEVICE_DEBUG("unexpected WHO_AM_I 0x%02x", whoami);
-		return PX4_ERROR;
+		if (whoami == WHOAMI) {
+			return PX4_OK;
+
+		} else {
+			DEVICE_DEBUG("unexpected WHO_AM_I 0x%02x", whoami);
+		}
 	}
 
-	return PX4_OK;
+	return PX4_ERROR;
 }
 
 void MPU9250::RunImpl()
@@ -218,8 +227,8 @@ void MPU9250::RunImpl()
 				ScheduleDelayed(100_ms);
 
 			} else {
-				PX4_DEBUG("Reset not complete, check again in 10 ms");
-				ScheduleDelayed(10_ms);
+				PX4_DEBUG("Reset not complete, check again in 100 ms");
+				ScheduleDelayed(100_ms);
 			}
 		}
 
@@ -232,6 +241,8 @@ void MPU9250::RunImpl()
 				_slave_ak8963_magnetometer->Reset();
 			}
 
+			_temperature = ReadTemperature();
+
 			// if configure succeeded then start reading from FIFO
 			_state = STATE::FIFO_READ;
 
@@ -270,7 +281,7 @@ void MPU9250::RunImpl()
 				// scheduled from interrupt if _drdy_timestamp_sample was set as expected
 				const hrt_abstime drdy_timestamp_sample = _drdy_timestamp_sample.fetch_and(0);
 
-				if ((now - drdy_timestamp_sample) < _fifo_empty_interval_us) {
+				if (now < drdy_timestamp_sample + _fifo_empty_interval_us) {
 					timestamp_sample = drdy_timestamp_sample;
 
 				} else {
@@ -307,7 +318,7 @@ void MPU9250::RunImpl()
 					FIFOReset();
 					perf_count(_fifo_overflow_perf);
 
-				} else if (samples >= SAMPLES_PER_TRANSFER) {
+				} else if (samples >= 1) {
 					if (FIFORead(timestamp_sample, samples)) {
 						success = true;
 
@@ -323,6 +334,7 @@ void MPU9250::RunImpl()
 
 				// full reset if things are failing consistently
 				if (_failure_count > 10) {
+					PX4_DEBUG("Full reset because things are failing consistently");
 					Reset();
 					return;
 				}
@@ -337,13 +349,14 @@ void MPU9250::RunImpl()
 				} else {
 					// register check failed, force reset
 					perf_count(_bad_register_perf);
+					PX4_DEBUG("Force reset because register 0x%02hhX check failed ", (uint8_t)_register_cfg[_checked_register].reg);
 					Reset();
 				}
 
 			} else {
 				// periodically update temperature (~1 Hz)
 				if (hrt_elapsed_time(&_temperature_update_timestamp) >= 1_s) {
-					UpdateTemperature();
+					_temperature = ReadTemperature();
 					_temperature_update_timestamp = now;
 				}
 			}
@@ -353,65 +366,10 @@ void MPU9250::RunImpl()
 	}
 }
 
-void MPU9250::ConfigureAccel()
-{
-	const uint8_t ACCEL_FS_SEL = RegisterRead(Register::ACCEL_CONFIG) & (Bit4 | Bit3); // [4:3] ACCEL_FS_SEL[1:0]
-
-	switch (ACCEL_FS_SEL) {
-	case ACCEL_FS_SEL_2G:
-		_px4_accel.set_scale(CONSTANTS_ONE_G / 16384.f);
-		_px4_accel.set_range(2.f * CONSTANTS_ONE_G);
-		break;
-
-	case ACCEL_FS_SEL_4G:
-		_px4_accel.set_scale(CONSTANTS_ONE_G / 8192.f);
-		_px4_accel.set_range(4.f * CONSTANTS_ONE_G);
-		break;
-
-	case ACCEL_FS_SEL_8G:
-		_px4_accel.set_scale(CONSTANTS_ONE_G / 4096.f);
-		_px4_accel.set_range(8.f * CONSTANTS_ONE_G);
-		break;
-
-	case ACCEL_FS_SEL_16G:
-		_px4_accel.set_scale(CONSTANTS_ONE_G / 2048.f);
-		_px4_accel.set_range(16.f * CONSTANTS_ONE_G);
-		break;
-	}
-}
-
-void MPU9250::ConfigureGyro()
-{
-	const uint8_t GYRO_FS_SEL = RegisterRead(Register::GYRO_CONFIG) & (Bit4 | Bit3); // [4:3] GYRO_FS_SEL[1:0]
-
-	float range_dps = 0.f;
-
-	switch (GYRO_FS_SEL) {
-	case GYRO_FS_SEL_250_DPS:
-		range_dps = 250.f;
-		break;
-
-	case GYRO_FS_SEL_500_DPS:
-		range_dps = 500.f;
-		break;
-
-	case GYRO_FS_SEL_1000_DPS:
-		range_dps = 1000.f;
-		break;
-
-	case GYRO_FS_SEL_2000_DPS:
-		range_dps = 2000.f;
-		break;
-	}
-
-	_px4_gyro.set_scale(math::radians(range_dps / 32768.f));
-	_px4_gyro.set_range(math::radians(range_dps));
-}
-
 void MPU9250::ConfigureSampleRate(int sample_rate)
 {
-	// round down to nearest FIFO sample dt * SAMPLES_PER_TRANSFER
-	const float min_interval = FIFO_SAMPLE_DT * SAMPLES_PER_TRANSFER;
+	// 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));
@@ -436,9 +394,6 @@ bool MPU9250::Configure()
 		}
 	}
 
-	ConfigureAccel();
-	ConfigureGyro();
-
 	return success;
 }
 
@@ -465,7 +420,7 @@ bool MPU9250::DataReadyInterruptConfigure()
 	}
 
 	// Setup data ready on falling edge
-	return px4_arch_gpiosetevent(_drdy_gpio, false, true, true, &DataReadyInterruptCallback, this) == 0;
+	return (px4_arch_gpiosetevent(_drdy_gpio, false, true, false, &DataReadyInterruptCallback, this) == 0);
 }
 
 bool MPU9250::DataReadyInterruptDisable()
@@ -474,7 +429,7 @@ bool MPU9250::DataReadyInterruptDisable()
 		return false;
 	}
 
-	return px4_arch_gpiosetevent(_drdy_gpio, false, false, false, nullptr, nullptr) == 0;
+	return (px4_arch_gpiosetevent(_drdy_gpio, false, false, false, nullptr, nullptr) == 0);
 }
 
 bool MPU9250::RegisterCheck(const register_config_t &reg_cfg)
@@ -525,28 +480,33 @@ void MPU9250::RegisterSetAndClearBits(Register reg, uint8_t setbits, uint8_t cle
 
 uint16_t MPU9250::FIFOReadCount()
 {
-	// read FIFO count
-	uint8_t fifo_count_buf[3] {};
-	fifo_count_buf[0] = static_cast<uint8_t>(Register::FIFO_COUNTH) | DIR_READ;
-	set_frequency(SPI_SPEED_SENSOR);
+	// transfer buffer
+	struct TransferBuffer {
+		uint8_t cmd{static_cast<uint8_t>(Register::FIFO_COUNTH) | DIR_READ};
+		uint8_t FIFO_COUNTH{0};
+		uint8_t FIFO_COUNTL{0};
+	} buffer{};
 
-	if (transfer(fifo_count_buf, fifo_count_buf, sizeof(fifo_count_buf)) != PX4_OK) {
+	// read FIFO count
+	if (transfer((uint8_t *)&buffer, (uint8_t *)&buffer, sizeof(buffer)) != PX4_OK) {
 		perf_count(_bad_transfer_perf);
 		return 0;
 	}
 
-	return combine(fifo_count_buf[1], fifo_count_buf[2]);
-}
-
-static bool fifo_accel_equal(const FIFO::DATA &f0, const FIFO::DATA &f1)
-{
-	return (memcmp(&f0.ACCEL_XOUT_H, &f1.ACCEL_XOUT_H, 6) == 0);
+	return (buffer.FIFO_COUNTH << 8) + buffer.FIFO_COUNTL;
 }
 
 bool MPU9250::FIFORead(const hrt_abstime &timestamp_sample, uint8_t samples)
 {
-	FIFOTransferBuffer buffer{};
-	const size_t transfer_size = math::min(samples * sizeof(FIFO::DATA) + 1, FIFO::SIZE);
+	// FIFO transfer buffer
+	struct FIFOTransferBuffer {
+		uint8_t cmd{static_cast<uint8_t>(Register::FIFO_R_W) | DIR_READ};
+		FIFO::DATA f[FIFO_MAX_SAMPLES] {};
+	} buffer{};
+
+	// cmd + samples (FIFO::DATA)
+	const size_t transfer_size = 1 + math::min(samples * sizeof(FIFO::DATA), FIFO::SIZE);
+
 	set_frequency(SPI_SPEED_SENSOR);
 
 	if (transfer((uint8_t *)&buffer, (uint8_t *)&buffer, transfer_size) != PX4_OK) {
@@ -554,42 +514,43 @@ bool MPU9250::FIFORead(const hrt_abstime &timestamp_sample, uint8_t samples)
 		return false;
 	}
 
-	uint8_t first_sample = 0;
+	const uint8_t valid_samples = samples;
 
-	if (samples >= 4) {
-		if (fifo_accel_equal(buffer.f[0], buffer.f[1]) && fifo_accel_equal(buffer.f[2], buffer.f[3])) {
-			// [A0, A1, A2, A3]
-			//  A0==A1, A2==A3
-			first_sample = 1;
+	if (valid_samples > 0) {
+		sensor_imu_fifo_s sensor_imu_fifo{};
+		sensor_imu_fifo.timestamp_sample = timestamp_sample;
+		sensor_imu_fifo.device_id = get_device_id();
+		sensor_imu_fifo.dt = FIFO_SAMPLE_DT;
+		sensor_imu_fifo.samples = valid_samples;
+		sensor_imu_fifo.accel_scale = ACCEL_SCALE;
+		sensor_imu_fifo.gyro_scale = GYRO_SCALE;
 
-		} else if (fifo_accel_equal(buffer.f[1], buffer.f[2])) {
-			// [A0, A1, A2, A3]
-			//  A0, A1==A2, A3
-			first_sample = 0;
+		for (int i = 0; i < valid_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)
+			sensor_imu_fifo.accel_x[i] =              combine(buffer.f[i].ACCEL_XOUT_H, buffer.f[i].ACCEL_XOUT_L);
+			sensor_imu_fifo.accel_y[i] = math::negate(combine(buffer.f[i].ACCEL_YOUT_H, buffer.f[i].ACCEL_YOUT_L));
+			sensor_imu_fifo.accel_z[i] = math::negate(combine(buffer.f[i].ACCEL_ZOUT_H, buffer.f[i].ACCEL_ZOUT_L));
+			rotate_3i(_rotation, sensor_imu_fifo.accel_x[i], sensor_imu_fifo.accel_y[i], sensor_imu_fifo.accel_z[i]);
 
-		} else if (_slave_ak8963_magnetometer && fifo_accel_equal(buffer.f[2], buffer.f[3])) {
-			// if the slave I2C magnetometer is active we tolerate these missing samples, but only if intermittant
-			// [A0, A1, A2, A3]
-			//  A0, A1, A2 == A3
-			first_sample = 2;
-			samples -= 2; // skip first 2 samples
-
-		} else {
-			// no matching accel samples is an error
-			if (!_slave_ak8963_magnetometer) {
-				// if the slave I2C magnetometer is active we tolerate these missing samples, but only if intermittant
-				// consecutive errors will still trigger a full sensor reset
-				perf_count(_bad_transfer_perf);
-			}
-
-			return false;
+			sensor_imu_fifo.gyro_x[i] =              combine(buffer.f[i].GYRO_XOUT_H, buffer.f[i].GYRO_XOUT_L);
+			sensor_imu_fifo.gyro_y[i] = math::negate(combine(buffer.f[i].GYRO_YOUT_H, buffer.f[i].GYRO_YOUT_L));
+			sensor_imu_fifo.gyro_z[i] = math::negate(combine(buffer.f[i].GYRO_ZOUT_H, buffer.f[i].GYRO_ZOUT_L));
+			rotate_3i(_rotation, sensor_imu_fifo.gyro_x[i], sensor_imu_fifo.gyro_y[i], sensor_imu_fifo.gyro_z[i]);
 		}
+
+		sensor_imu_fifo.temperature = (hrt_elapsed_time(&_temperature_update_timestamp) < 5_s) ? _temperature : NAN;
+
+		sensor_imu_fifo.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);
+
+		sensor_imu_fifo.timestamp = hrt_absolute_time();
+		_sensor_imu_fifo_pub.publish(sensor_imu_fifo);
+
+		return true;
 	}
 
-	ProcessGyro(timestamp_sample, &buffer.f[first_sample], samples);
-	ProcessAccel(timestamp_sample, &buffer.f[first_sample], samples);
-
-	return true;
+	return false;
 }
 
 void MPU9250::FIFOReset()
@@ -615,78 +576,34 @@ void MPU9250::FIFOReset()
 	}
 }
 
-void MPU9250::ProcessAccel(const hrt_abstime &timestamp_sample, const FIFO::DATA fifo[], const uint8_t samples)
+float MPU9250::ReadTemperature()
 {
-	sensor_accel_fifo_s accel{};
-	accel.timestamp_sample = timestamp_sample;
-	accel.samples = 0;
-	accel.dt = FIFO_SAMPLE_DT * SAMPLES_PER_TRANSFER;
+	// transfer buffer
+	struct TransferBuffer {
+		uint8_t cmd{static_cast<uint8_t>(Register::TEMP_OUT_H) | DIR_READ};
+		uint8_t TEMP_OUT_H{0};
+		uint8_t TEMP_OUT_L{0};
+	} buffer{};
 
-	for (int i = 0; i < samples; i = i + SAMPLES_PER_TRANSFER) {
-		int16_t accel_x = combine(fifo[i].ACCEL_XOUT_H, fifo[i].ACCEL_XOUT_L);
-		int16_t accel_y = combine(fifo[i].ACCEL_YOUT_H, fifo[i].ACCEL_YOUT_L);
-		int16_t accel_z = combine(fifo[i].ACCEL_ZOUT_H, fifo[i].ACCEL_ZOUT_L);
-
-		// 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[accel.samples] = accel_x;
-		accel.y[accel.samples] = (accel_y == INT16_MIN) ? INT16_MAX : -accel_y;
-		accel.z[accel.samples] = (accel_z == INT16_MIN) ? INT16_MAX : -accel_z;
-		accel.samples++;
-	}
-
-	_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) {
-		_px4_accel.updateFIFO(accel);
-	}
-}
-
-void MPU9250::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 = samples;
-	gyro.dt = FIFO_SAMPLE_DT;
-
-	for (int i = 0; i < samples; i++) {
-		const int16_t gyro_x = combine(fifo[i].GYRO_XOUT_H, fifo[i].GYRO_XOUT_L);
-		const int16_t gyro_y = combine(fifo[i].GYRO_YOUT_H, fifo[i].GYRO_YOUT_L);
-		const int16_t gyro_z = combine(fifo[i].GYRO_ZOUT_H, fifo[i].GYRO_ZOUT_L);
-
-		// 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;
-		gyro.y[i] = (gyro_y == INT16_MIN) ? INT16_MAX : -gyro_y;
-		gyro.z[i] = (gyro_z == INT16_MIN) ? INT16_MAX : -gyro_z;
-	}
-
-	_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));
-
-	_px4_gyro.updateFIFO(gyro);
-}
-
-void MPU9250::UpdateTemperature()
-{
 	// read current temperature
-	uint8_t temperature_buf[3] {};
-	temperature_buf[0] = static_cast<uint8_t>(Register::TEMP_OUT_H) | DIR_READ;
 	set_frequency(SPI_SPEED_SENSOR);
 
-	if (transfer(temperature_buf, temperature_buf, sizeof(temperature_buf)) != PX4_OK) {
+	if (transfer((uint8_t *)&buffer, (uint8_t *)&buffer, sizeof(buffer)) != PX4_OK) {
 		perf_count(_bad_transfer_perf);
-		return;
+		return NAN;
 	}
 
-	const int16_t TEMP_OUT = combine(temperature_buf[1], temperature_buf[2]);
+	const int16_t TEMP_OUT = combine(buffer.TEMP_OUT_H, buffer.TEMP_OUT_L);
 	const float TEMP_degC = (TEMP_OUT / TEMPERATURE_SENSITIVITY) + TEMPERATURE_OFFSET;
 
-	if (PX4_ISFINITE(TEMP_degC)) {
-		_px4_accel.set_temperature(TEMP_degC);
-		_px4_gyro.set_temperature(TEMP_degC);
+	if (PX4_ISFINITE(TEMP_degC)
+	    && (TEMP_degC >= TEMPERATURE_SENSOR_MIN)
+	    && (TEMP_degC <= TEMPERATURE_SENSOR_MAX)) {
+
+		return TEMP_degC;
 	}
+
+	return NAN;
 }
 
 void MPU9250::I2CSlaveRegisterWrite(uint8_t slave_i2c_addr, uint8_t reg, uint8_t val)
diff --git a/src/drivers/imu/invensense/mpu9250/MPU9250.hpp b/src/drivers/imu/invensense/mpu9250/MPU9250.hpp
index 369d2ec6e7..8d07395e60 100644
--- a/src/drivers/imu/invensense/mpu9250/MPU9250.hpp
+++ b/src/drivers/imu/invensense/mpu9250/MPU9250.hpp
@@ -1,6 +1,6 @@
 /****************************************************************************
  *
- *   Copyright (c) 2020-2021 PX4 Development Team. All rights reserved.
+ *   Copyright (c) 2020-2022 PX4 Development Team. All rights reserved.
  *
  * Redistribution and use in source and binary forms, with or without
  * modification, are permitted provided that the following conditions
@@ -43,14 +43,15 @@
 #include "InvenSense_MPU9250_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/PublicationMulti.hpp>
+#include <uORB/topics/sensor_imu_fifo.h>
+
 #include "MPU9250_AK8963.hpp"
 
 using namespace InvenSense_MPU9250;
@@ -73,20 +74,13 @@ private:
 
 	// Sensor Configuration
 	static constexpr float FIFO_SAMPLE_DT{1e6f / 8000.f};
-	static constexpr int32_t SAMPLES_PER_TRANSFER{2};                    // ensure at least 1 new accel sample per transfer
 	static constexpr float GYRO_RATE{1e6f / FIFO_SAMPLE_DT};             // 8000 Hz gyro
-	static constexpr float ACCEL_RATE{GYRO_RATE / SAMPLES_PER_TRANSFER}; // 4000 Hz accel
 
 	// maximum FIFO samples per transfer is limited to the size of sensor_accel_fifo/sensor_gyro_fifo
-	static constexpr int32_t FIFO_MAX_SAMPLES{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 int32_t FIFO_MAX_SAMPLES{math::min(FIFO::SIZE / sizeof(FIFO::DATA), (size_t)sensor_imu_fifo_s::FIFO_SIZE)};
 
-	// Transfer data
-	struct FIFOTransferBuffer {
-		uint8_t cmd{static_cast<uint8_t>(Register::FIFO_R_W) | DIR_READ};
-		FIFO::DATA f[FIFO_MAX_SAMPLES] {};
-	};
-	// ensure no struct padding
-	static_assert(sizeof(FIFOTransferBuffer) == (1 + FIFO_MAX_SAMPLES *sizeof(FIFO::DATA)));
+	static constexpr float ACCEL_SCALE{CONSTANTS_ONE_G / 2048.f}; // AFS_SEL_16G
+	static constexpr float GYRO_SCALE{math::radians(2000.f / 32768.f)}; // FS_SEL_2000_DPS
 
 	struct register_config_t {
 		Register reg;
@@ -99,8 +93,6 @@ private:
 	bool Reset();
 
 	bool Configure();
-	void ConfigureAccel();
-	void ConfigureGyro();
 	void ConfigureSampleRate(int sample_rate);
 
 	static int DataReadyInterruptCallback(int irq, void *context, void *arg);
@@ -121,12 +113,16 @@ private:
 	bool FIFORead(const hrt_abstime &timestamp_sample, uint8_t samples);
 	void FIFOReset();
 
-	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);
-	void UpdateTemperature();
+	float ReadTemperature();
 
 	const spi_drdy_gpio_t _drdy_gpio;
 
+	uORB::PublicationMulti<sensor_imu_fifo_s> _sensor_imu_fifo_pub{ORB_ID(sensor_imu_fifo)};
+
+	const enum Rotation _rotation;
+
+	float _temperature{NAN};
+
 	// I2C AUX interface (slave 1 - 4)
 	AKM_AK8963::MPU9250_AK8963 *_slave_ak8963_magnetometer{nullptr};
 	friend class AKM_AK8963::MPU9250_AK8963;
@@ -135,9 +131,6 @@ private:
 	void I2CSlaveExternalSensorDataEnable(uint8_t slave_i2c_addr, uint8_t reg, uint8_t size);
 	bool I2CSlaveExternalSensorDataRead(uint8_t *buffer, uint8_t length);
 
-	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")};
diff --git a/src/drivers/imu/invensense/mpu9250/MPU9250_I2C.cpp b/src/drivers/imu/invensense/mpu9250/MPU9250_I2C.cpp
index 73a87a71db..e52f9c70d9 100644
--- a/src/drivers/imu/invensense/mpu9250/MPU9250_I2C.cpp
+++ b/src/drivers/imu/invensense/mpu9250/MPU9250_I2C.cpp
@@ -1,6 +1,6 @@
 /****************************************************************************
  *
- *   Copyright (c) 2020-2021 PX4 Development Team. All rights reserved.
+ *   Copyright (c) 2020-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
@@ -33,6 +33,8 @@
 
 #include "MPU9250_I2C.hpp"
 
+#include <lib/parameters/param.h>
+
 using namespace time_literals;
 
 static constexpr int16_t combine(uint8_t msb, uint8_t lsb)
@@ -44,14 +46,16 @@ MPU9250_I2C::MPU9250_I2C(const I2CSPIDriverConfig &config) :
 	I2C(config),
 	I2CSPIDriver(config),
 	_drdy_gpio(config.drdy_gpio),
-	_px4_accel(get_device_id(), config.rotation),
-	_px4_gyro(get_device_id(), config.rotation)
+	_rotation(config.rotation)
 {
 	if (_drdy_gpio != 0) {
 		_drdy_missed_perf = perf_alloc(PC_COUNT, MODULE_NAME": DRDY missed");
 	}
 
-	ConfigureSampleRate(_px4_gyro.get_max_rate_hz());
+	int32_t imu_gyro_rate_max = 400;
+	param_get(param_find("IMU_GYRO_RATEMAX"), &imu_gyro_rate_max);
+
+	ConfigureSampleRate(imu_gyro_rate_max);
 }
 
 MPU9250_I2C::~MPU9250_I2C()
@@ -105,16 +109,44 @@ void MPU9250_I2C::print_status()
 	perf_print_counter(_drdy_missed_perf);
 }
 
-int MPU9250_I2C::probe()
+bool MPU9250_I2C::StoreCheckedRegisterValue(Register reg)
 {
-	const uint8_t whoami = RegisterRead(Register::WHO_AM_I);
+	// 3 retries
+	for (int i = 0; i < 3; i++) {
+		uint8_t read1 = RegisterRead(reg);
+		uint8_t read2 = RegisterRead(reg);
 
-	if (whoami != WHOAMI) {
-		DEVICE_DEBUG("unexpected WHO_AM_I 0x%02x", whoami);
-		return PX4_ERROR;
+		if (read1 == read2) {
+			for (auto &r : _register_cfg) {
+				if (r.reg == reg) {
+					r.set_bits = read1;
+					r.clear_bits = ~read1;
+					return true;
+				}
+			}
+
+		} else {
+			PX4_ERR("0x%02hhX read 1 != read 2 (0x%02hhX != 0x%02hhX)", static_cast<uint8_t>(reg), read1, read2);
+		}
 	}
 
-	return PX4_OK;
+	return false;
+}
+
+int MPU9250_I2C::probe()
+{
+	for (int retry = 0; retry < 3; retry++) {
+		const uint8_t whoami = RegisterRead(Register::WHO_AM_I);
+
+		if (whoami == WHOAMI) {
+			return PX4_OK;
+
+		} else {
+			DEVICE_DEBUG("unexpected WHO_AM_I 0x%02x", whoami);
+		}
+	}
+
+	return PX4_ERROR;
 }
 
 void MPU9250_I2C::RunImpl()
@@ -138,6 +170,14 @@ void MPU9250_I2C::RunImpl()
 		if ((RegisterRead(Register::WHO_AM_I) == WHOAMI)
 		    && (RegisterRead(Register::PWR_MGMT_1) == 0x01)) {
 
+			// offset registers (factory calibration) should not change during normal operation
+			StoreCheckedRegisterValue(Register::XA_OFFSET_H);
+			StoreCheckedRegisterValue(Register::XA_OFFSET_L);
+			StoreCheckedRegisterValue(Register::YA_OFFSET_H);
+			StoreCheckedRegisterValue(Register::YA_OFFSET_L);
+			StoreCheckedRegisterValue(Register::ZA_OFFSET_H);
+			StoreCheckedRegisterValue(Register::ZA_OFFSET_L);
+
 			// Wakeup and reset digital signal path
 			RegisterWrite(Register::PWR_MGMT_1, PWR_MGMT_1_BIT::CLKSEL_0);
 			RegisterWrite(Register::SIGNAL_PATH_RESET,
@@ -156,8 +196,8 @@ void MPU9250_I2C::RunImpl()
 				ScheduleDelayed(100_ms);
 
 			} else {
-				PX4_DEBUG("Reset not complete, check again in 10 ms");
-				ScheduleDelayed(10_ms);
+				PX4_DEBUG("Reset not complete, check again in 100 ms");
+				ScheduleDelayed(100_ms);
 			}
 		}
 
@@ -165,6 +205,8 @@ void MPU9250_I2C::RunImpl()
 
 	case STATE::CONFIGURE:
 		if (Configure()) {
+			_temperature = ReadTemperature();
+
 			// if configure succeeded then start reading from FIFO
 			_state = STATE::FIFO_READ;
 
@@ -203,7 +245,7 @@ void MPU9250_I2C::RunImpl()
 				// scheduled from interrupt if _drdy_timestamp_sample was set as expected
 				const hrt_abstime drdy_timestamp_sample = _drdy_timestamp_sample.fetch_and(0);
 
-				if ((now - drdy_timestamp_sample) < _fifo_empty_interval_us) {
+				if (now < drdy_timestamp_sample + _fifo_empty_interval_us) {
 					timestamp_sample = drdy_timestamp_sample;
 
 				} else {
@@ -240,7 +282,7 @@ void MPU9250_I2C::RunImpl()
 					FIFOReset();
 					perf_count(_fifo_overflow_perf);
 
-				} else if (samples >= SAMPLES_PER_TRANSFER) {
+				} else if (samples >= 1) {
 					if (FIFORead(timestamp_sample, samples)) {
 						success = true;
 
@@ -256,6 +298,7 @@ void MPU9250_I2C::RunImpl()
 
 				// full reset if things are failing consistently
 				if (_failure_count > 10) {
+					PX4_DEBUG("Full reset because things are failing consistently");
 					Reset();
 					return;
 				}
@@ -270,13 +313,14 @@ void MPU9250_I2C::RunImpl()
 				} else {
 					// register check failed, force reset
 					perf_count(_bad_register_perf);
+					PX4_DEBUG("Force reset because register 0x%02hhX check failed ", (uint8_t)_register_cfg[_checked_register].reg);
 					Reset();
 				}
 
 			} else {
 				// periodically update temperature (~1 Hz)
 				if (hrt_elapsed_time(&_temperature_update_timestamp) >= 1_s) {
-					UpdateTemperature();
+					_temperature = ReadTemperature();
 					_temperature_update_timestamp = now;
 				}
 			}
@@ -286,65 +330,10 @@ void MPU9250_I2C::RunImpl()
 	}
 }
 
-void MPU9250_I2C::ConfigureAccel()
-{
-	const uint8_t ACCEL_FS_SEL = RegisterRead(Register::ACCEL_CONFIG) & (Bit4 | Bit3); // [4:3] ACCEL_FS_SEL[1:0]
-
-	switch (ACCEL_FS_SEL) {
-	case ACCEL_FS_SEL_2G:
-		_px4_accel.set_scale(CONSTANTS_ONE_G / 16384.f);
-		_px4_accel.set_range(2.f * CONSTANTS_ONE_G);
-		break;
-
-	case ACCEL_FS_SEL_4G:
-		_px4_accel.set_scale(CONSTANTS_ONE_G / 8192.f);
-		_px4_accel.set_range(4.f * CONSTANTS_ONE_G);
-		break;
-
-	case ACCEL_FS_SEL_8G:
-		_px4_accel.set_scale(CONSTANTS_ONE_G / 4096.f);
-		_px4_accel.set_range(8.f * CONSTANTS_ONE_G);
-		break;
-
-	case ACCEL_FS_SEL_16G:
-		_px4_accel.set_scale(CONSTANTS_ONE_G / 2048.f);
-		_px4_accel.set_range(16.f * CONSTANTS_ONE_G);
-		break;
-	}
-}
-
-void MPU9250_I2C::ConfigureGyro()
-{
-	const uint8_t GYRO_FS_SEL = RegisterRead(Register::GYRO_CONFIG) & (Bit4 | Bit3); // [4:3] GYRO_FS_SEL[1:0]
-
-	float range_dps = 0.f;
-
-	switch (GYRO_FS_SEL) {
-	case GYRO_FS_SEL_250_DPS:
-		range_dps = 250.f;
-		break;
-
-	case GYRO_FS_SEL_500_DPS:
-		range_dps = 500.f;
-		break;
-
-	case GYRO_FS_SEL_1000_DPS:
-		range_dps = 1000.f;
-		break;
-
-	case GYRO_FS_SEL_2000_DPS:
-		range_dps = 2000.f;
-		break;
-	}
-
-	_px4_gyro.set_scale(math::radians(range_dps / 32768.f));
-	_px4_gyro.set_range(math::radians(range_dps));
-}
-
 void MPU9250_I2C::ConfigureSampleRate(int sample_rate)
 {
-	// round down to nearest FIFO sample dt * SAMPLES_PER_TRANSFER
-	const float min_interval = FIFO_SAMPLE_DT * SAMPLES_PER_TRANSFER;
+	// 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));
@@ -369,9 +358,6 @@ bool MPU9250_I2C::Configure()
 		}
 	}
 
-	ConfigureAccel();
-	ConfigureGyro();
-
 	return success;
 }
 
@@ -401,7 +387,7 @@ bool MPU9250_I2C::DataReadyInterruptConfigure()
 	}
 
 	// Setup data ready on falling edge
-	return px4_arch_gpiosetevent(_drdy_gpio, false, true, true, &DataReadyInterruptCallback, this) == 0;
+	return (px4_arch_gpiosetevent(_drdy_gpio, false, true, false, &DataReadyInterruptCallback, this) == 0);
 }
 
 bool MPU9250_I2C::DataReadyInterruptDisable()
@@ -410,7 +396,7 @@ bool MPU9250_I2C::DataReadyInterruptDisable()
 		return false;
 	}
 
-	return px4_arch_gpiosetevent(_drdy_gpio, false, false, false, nullptr, nullptr) == 0;
+	return (px4_arch_gpiosetevent(_drdy_gpio, false, false, false, nullptr, nullptr) == 0);
 }
 
 bool MPU9250_I2C::RegisterCheck(const register_config_t &reg_cfg)
@@ -436,8 +422,12 @@ uint8_t MPU9250_I2C::RegisterRead(Register reg)
 {
 	uint8_t cmd = static_cast<uint8_t>(reg);
 	uint8_t value = 0;
-	//set_frequency(SPI_SPEED); // low speed for regular registers
-	transfer(&cmd, 1, &value, 1);
+
+	if (transfer(&cmd, 1, &value, 1) != 0) {
+		perf_count(_bad_transfer_perf);
+		return 0;
+	}
+
 	return value;
 }
 
@@ -446,8 +436,10 @@ void MPU9250_I2C::RegisterWrite(Register reg, uint8_t value)
 	uint8_t cmd[2];
 	cmd[0] = static_cast<uint8_t>(reg);
 	cmd[1] = value;
-	//set_frequency(SPI_SPEED); // low speed for regular registers
-	transfer(cmd, sizeof(cmd), nullptr, 0);
+
+	if (transfer(cmd, sizeof(cmd), nullptr, 0) != 0) {
+		perf_count(_bad_transfer_perf);
+	}
 }
 
 void MPU9250_I2C::RegisterSetAndClearBits(Register reg, uint8_t setbits, uint8_t clearbits)
@@ -465,32 +457,76 @@ uint16_t MPU9250_I2C::FIFOReadCount()
 {
 	// read FIFO count
 	uint8_t cmd = static_cast<uint8_t>(Register::FIFO_COUNTH);
-	uint8_t fifo_count_buf[2] {};
-	//set_frequency(SPI_SPEED_SENSOR);
 
-	if (transfer(&cmd, 1, fifo_count_buf, 2) != PX4_OK) {
+	// transfer buffer
+	struct TransferBuffer {
+		uint8_t FIFO_COUNTH{0};
+		uint8_t FIFO_COUNTL{0};
+	} buffer{};
+
+	// read FIFO count
+	if (transfer(&cmd, 1, (uint8_t *)&buffer, sizeof(buffer)) != PX4_OK) {
 		perf_count(_bad_transfer_perf);
 		return 0;
 	}
 
-	return combine(fifo_count_buf[0], fifo_count_buf[1]);
+	return (buffer.FIFO_COUNTH << 8) + buffer.FIFO_COUNTL;
 }
 
 bool MPU9250_I2C::FIFORead(const hrt_abstime &timestamp_sample, uint8_t samples)
 {
 	uint8_t cmd = static_cast<uint8_t>(Register::FIFO_R_W);
-	FIFOTransferBuffer buffer{};
+
+	// FIFO transfer buffer
+	struct FIFOTransferBuffer {
+		FIFO::DATA f[FIFO_MAX_SAMPLES] {};
+	} buffer{};
+
+	// samples (FIFO::DATA)
 	const size_t transfer_size = math::min(samples * sizeof(FIFO::DATA), FIFO::SIZE);
-	//set_frequency(SPI_SPEED_SENSOR);
 
 	if (transfer(&cmd, 1, (uint8_t *)&buffer, transfer_size) != PX4_OK) {
 		perf_count(_bad_transfer_perf);
 		return false;
 	}
 
+	const uint8_t valid_samples = samples;
 
-	ProcessGyro(timestamp_sample, buffer.f, samples);
-	return ProcessAccel(timestamp_sample, buffer.f, samples);
+	if (valid_samples > 0) {
+		sensor_imu_fifo_s sensor_imu_fifo{};
+		sensor_imu_fifo.timestamp_sample = timestamp_sample;
+		sensor_imu_fifo.device_id = get_device_id();
+		sensor_imu_fifo.dt = FIFO_SAMPLE_DT;
+		sensor_imu_fifo.samples = valid_samples;
+		sensor_imu_fifo.accel_scale = ACCEL_SCALE;
+		sensor_imu_fifo.gyro_scale = GYRO_SCALE;
+
+		for (int i = 0; i < valid_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)
+			sensor_imu_fifo.accel_x[i] =              combine(buffer.f[i].ACCEL_XOUT_H, buffer.f[i].ACCEL_XOUT_L);
+			sensor_imu_fifo.accel_y[i] = math::negate(combine(buffer.f[i].ACCEL_YOUT_H, buffer.f[i].ACCEL_YOUT_L));
+			sensor_imu_fifo.accel_z[i] = math::negate(combine(buffer.f[i].ACCEL_ZOUT_H, buffer.f[i].ACCEL_ZOUT_L));
+			rotate_3i(_rotation, sensor_imu_fifo.accel_x[i], sensor_imu_fifo.accel_y[i], sensor_imu_fifo.accel_z[i]);
+
+			sensor_imu_fifo.gyro_x[i] =              combine(buffer.f[i].GYRO_XOUT_H, buffer.f[i].GYRO_XOUT_L);
+			sensor_imu_fifo.gyro_y[i] = math::negate(combine(buffer.f[i].GYRO_YOUT_H, buffer.f[i].GYRO_YOUT_L));
+			sensor_imu_fifo.gyro_z[i] = math::negate(combine(buffer.f[i].GYRO_ZOUT_H, buffer.f[i].GYRO_ZOUT_L));
+			rotate_3i(_rotation, sensor_imu_fifo.gyro_x[i], sensor_imu_fifo.gyro_y[i], sensor_imu_fifo.gyro_z[i]);
+		}
+
+		sensor_imu_fifo.temperature = (hrt_elapsed_time(&_temperature_update_timestamp) < 5_s) ? _temperature : NAN;
+
+		sensor_imu_fifo.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);
+
+		sensor_imu_fifo.timestamp = hrt_absolute_time();
+		_sensor_imu_fifo_pub.publish(sensor_imu_fifo);
+
+		return true;
+	}
+
+	return false;
 }
 
 void MPU9250_I2C::FIFOReset()
@@ -516,80 +552,31 @@ void MPU9250_I2C::FIFOReset()
 	}
 }
 
-bool MPU9250_I2C::ProcessAccel(const hrt_abstime &timestamp_sample, const FIFO::DATA fifo[], const uint8_t samples)
+float MPU9250_I2C::ReadTemperature()
 {
-	sensor_accel_fifo_s accel{};
-	accel.timestamp_sample = timestamp_sample;
-	accel.samples = 0;
-	accel.dt = FIFO_SAMPLE_DT * SAMPLES_PER_TRANSFER;
-
-	bool bad_data = false;
-
-	for (int i = 0; i < samples; i = i + SAMPLES_PER_TRANSFER) {
-		int16_t accel_x = combine(fifo[i].ACCEL_XOUT_H, fifo[i].ACCEL_XOUT_L);
-		int16_t accel_y = combine(fifo[i].ACCEL_YOUT_H, fifo[i].ACCEL_YOUT_L);
-		int16_t accel_z = combine(fifo[i].ACCEL_ZOUT_H, fifo[i].ACCEL_ZOUT_L);
-
-		// 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[accel.samples] = accel_x;
-		accel.y[accel.samples] = (accel_y == INT16_MIN) ? INT16_MAX : -accel_y;
-		accel.z[accel.samples] = (accel_z == INT16_MIN) ? INT16_MAX : -accel_z;
-		accel.samples++;
-	}
-
-	_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) {
-		_px4_accel.updateFIFO(accel);
-	}
-
-	return !bad_data;
-}
-
-void MPU9250_I2C::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 = samples;
-	gyro.dt = FIFO_SAMPLE_DT;
-
-	for (int i = 0; i < samples; i++) {
-		const int16_t gyro_x = combine(fifo[i].GYRO_XOUT_H, fifo[i].GYRO_XOUT_L);
-		const int16_t gyro_y = combine(fifo[i].GYRO_YOUT_H, fifo[i].GYRO_YOUT_L);
-		const int16_t gyro_z = combine(fifo[i].GYRO_ZOUT_H, fifo[i].GYRO_ZOUT_L);
-
-		// 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;
-		gyro.y[i] = (gyro_y == INT16_MIN) ? INT16_MAX : -gyro_y;
-		gyro.z[i] = (gyro_z == INT16_MIN) ? INT16_MAX : -gyro_z;
-	}
-
-	_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));
-
-	_px4_gyro.updateFIFO(gyro);
-}
-
-void MPU9250_I2C::UpdateTemperature()
-{
-	// read current temperature
 	uint8_t cmd = static_cast<uint8_t>(Register::TEMP_OUT_H);
-	uint8_t temperature_buf[2] {};
-	//set_frequency(SPI_SPEED_SENSOR);
 
-	if (transfer(&cmd, 1, temperature_buf, 2) != PX4_OK) {
+	// transfer buffer
+	struct TransferBuffer {
+		uint8_t TEMP_OUT_H{0};
+		uint8_t TEMP_OUT_L{0};
+	} buffer{};
+
+	// read current temperature
+	if (transfer(&cmd, 1, (uint8_t *)&buffer, sizeof(buffer)) != PX4_OK) {
 		perf_count(_bad_transfer_perf);
-		return;
+		return NAN;
 	}
 
-	const int16_t TEMP_OUT = combine(temperature_buf[0], temperature_buf[1]);
+	const int16_t TEMP_OUT = combine(buffer.TEMP_OUT_H, buffer.TEMP_OUT_L);
 	const float TEMP_degC = (TEMP_OUT / TEMPERATURE_SENSITIVITY) + TEMPERATURE_OFFSET;
 
-	if (PX4_ISFINITE(TEMP_degC)) {
-		_px4_accel.set_temperature(TEMP_degC);
-		_px4_gyro.set_temperature(TEMP_degC);
+	if (PX4_ISFINITE(TEMP_degC)
+	    && (TEMP_degC >= TEMPERATURE_SENSOR_MIN)
+	    && (TEMP_degC <= TEMPERATURE_SENSOR_MAX)) {
+
+		return TEMP_degC;
 	}
+
+	return NAN;
 }
diff --git a/src/drivers/imu/invensense/mpu9250/MPU9250_I2C.hpp b/src/drivers/imu/invensense/mpu9250/MPU9250_I2C.hpp
index 7af10d0b60..6abf0cc010 100644
--- a/src/drivers/imu/invensense/mpu9250/MPU9250_I2C.hpp
+++ b/src/drivers/imu/invensense/mpu9250/MPU9250_I2C.hpp
@@ -1,6 +1,6 @@
 /****************************************************************************
  *
- *   Copyright (c) 2020-2021 PX4 Development Team. All rights reserved.
+ *   Copyright (c) 2020-2022 PX4 Development Team. All rights reserved.
  *
  * Redistribution and use in source and binary forms, with or without
  * modification, are permitted provided that the following conditions
@@ -43,14 +43,15 @@
 #include "InvenSense_MPU9250_registers.hpp"
 
 #include <drivers/drv_hrt.h>
-#include <lib/drivers/accelerometer/PX4Accelerometer.hpp>
 #include <lib/drivers/device/i2c.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/PublicationMulti.hpp>
+#include <uORB/topics/sensor_imu_fifo.h>
+
 using namespace InvenSense_MPU9250;
 
 class MPU9250_I2C : public device::I2C, public I2CSPIDriver<MPU9250_I2C>
@@ -71,19 +72,13 @@ private:
 
 	// Sensor Configuration
 	static constexpr float FIFO_SAMPLE_DT{1e6f / 1000.f};
-	static constexpr int32_t SAMPLES_PER_TRANSFER{1};                    // ensure at least 1 new accel sample per transfer
 	static constexpr float GYRO_RATE{1e6f / FIFO_SAMPLE_DT};             // 1000 Hz gyro
-	static constexpr float ACCEL_RATE{GYRO_RATE / SAMPLES_PER_TRANSFER}; // 1000 Hz accel
 
 	// maximum FIFO samples per transfer is limited to the size of sensor_accel_fifo/sensor_gyro_fifo
-	static constexpr int32_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 int32_t FIFO_MAX_SAMPLES{math::min(FIFO::SIZE / sizeof(FIFO::DATA), (size_t)sensor_imu_fifo_s::FIFO_SIZE)};
 
-	// Transfer data
-	struct FIFOTransferBuffer {
-		FIFO::DATA f[FIFO_MAX_SAMPLES] {};
-	};
-	// ensure no struct padding
-	static_assert(sizeof(FIFOTransferBuffer) == (FIFO_MAX_SAMPLES *sizeof(FIFO::DATA)));
+	static constexpr float ACCEL_SCALE{CONSTANTS_ONE_G / 2048.f}; // AFS_SEL_16G:
+	static constexpr float GYRO_SCALE{math::radians(2000.f / 32768.f)}; // FS_SEL_2000_DPS:
 
 	struct register_config_t {
 		Register reg;
@@ -96,8 +91,6 @@ private:
 	bool Reset();
 
 	bool Configure();
-	void ConfigureAccel();
-	void ConfigureGyro();
 	void ConfigureSampleRate(int sample_rate);
 
 	static int DataReadyInterruptCallback(int irq, void *context, void *arg);
@@ -106,23 +99,27 @@ private:
 	bool DataReadyInterruptDisable();
 
 	bool RegisterCheck(const register_config_t &reg_cfg);
+	bool StoreCheckedRegisterValue(Register reg);
 
 	uint8_t RegisterRead(Register reg);
 	void RegisterWrite(Register reg, uint8_t value);
 	void RegisterSetAndClearBits(Register reg, uint8_t setbits, uint8_t clearbits);
+	void RegisterSetBits(Register reg, uint8_t setbits) { RegisterSetAndClearBits(reg, setbits, 0); }
+	void RegisterClearBits(Register reg, uint8_t clearbits) { RegisterSetAndClearBits(reg, 0, clearbits); }
 
 	uint16_t FIFOReadCount();
 	bool FIFORead(const hrt_abstime &timestamp_sample, uint8_t samples);
 	void FIFOReset();
 
-	bool 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);
-	void UpdateTemperature();
+	float ReadTemperature();
 
 	const spi_drdy_gpio_t _drdy_gpio;
 
-	PX4Accelerometer _px4_accel;
-	PX4Gyroscope _px4_gyro;
+	uORB::PublicationMulti<sensor_imu_fifo_s> _sensor_imu_fifo_pub{ORB_ID(sensor_imu_fifo)};
+
+	const enum Rotation _rotation;
+
+	float _temperature{NAN};
 
 	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")};
diff --git a/src/drivers/imu/l3gd20/CMakeLists.txt b/src/drivers/imu/l3gd20/CMakeLists.txt
index fd1e672436..6c5d6caf57 100644
--- a/src/drivers/imu/l3gd20/CMakeLists.txt
+++ b/src/drivers/imu/l3gd20/CMakeLists.txt
@@ -40,6 +40,5 @@ px4_add_module(
 		L3GD20.cpp
 		l3gd20_main.cpp
 	DEPENDS
-		drivers_gyroscope
 		px4_work_queue
 	)
diff --git a/src/drivers/imu/l3gd20/L3GD20.cpp b/src/drivers/imu/l3gd20/L3GD20.cpp
index 152673048c..4099099250 100644
--- a/src/drivers/imu/l3gd20/L3GD20.cpp
+++ b/src/drivers/imu/l3gd20/L3GD20.cpp
@@ -38,7 +38,7 @@ constexpr uint8_t L3GD20::_checked_registers[];
 L3GD20::L3GD20(const I2CSPIDriverConfig &config) :
 	SPI(config),
 	I2CSPIDriver(config),
-	_px4_gyro(get_device_id(), config.rotation),
+	_rotation(config.rotation),
 	_sample_perf(perf_alloc(PC_ELAPSED, MODULE_NAME": read")),
 	_errors(perf_alloc(PC_COUNT, MODULE_NAME": err")),
 	_bad_registers(perf_alloc(PC_COUNT, MODULE_NAME": bad_reg")),
@@ -177,7 +177,7 @@ L3GD20::set_range(unsigned max_dps)
 		return -EINVAL;
 	}
 
-	_px4_gyro.set_scale(new_range_scale_dps_digit / 180.0f * M_PI_F);
+	_gyro_scale = new_range_scale_dps_digit / 180.0f * M_PI_F;
 
 	write_checked_reg(ADDR_CTRL_REG4, bits);
 
@@ -349,31 +349,30 @@ L3GD20::RunImpl()
 	 *	 	  the offset is 74 from the origin and subtracting
 	 *		  74 from all measurements centers them around zero.
 	 */
-	_px4_gyro.set_error_count(perf_event_count(_bad_registers));
-
-	_px4_gyro.set_temperature(L3GD20_TEMP_OFFSET_CELSIUS - raw_report.temp);
+	sensor_gyro_s sensor_gyro{};
+	sensor_gyro.timestamp_sample = timestamp_sample;
+	sensor_gyro.device_id = get_device_id();
 
 	switch (_orientation) {
 	case SENSOR_BOARD_ROTATION_090_DEG:
 		/* swap x and y */
-		_px4_gyro.update(timestamp_sample, raw_report.y, raw_report.x, raw_report.z);
+		sensor_gyro.x = raw_report.x * _gyro_scale;
+		sensor_gyro.y = raw_report.y * _gyro_scale;
+		sensor_gyro.z = raw_report.z * _gyro_scale;
 		break;
 
-	case SENSOR_BOARD_ROTATION_180_DEG: {
-			/* swap x and y and negate both */
-			int16_t x = ((raw_report.x == -32768) ? 32767 : -raw_report.x);
-			int16_t y = ((raw_report.y == -32768) ? 32767 : -raw_report.y);
-			_px4_gyro.update(timestamp_sample, x, y, raw_report.z);
-		}
-
+	case SENSOR_BOARD_ROTATION_180_DEG:
+		/* swap x and y and negate both */
+		sensor_gyro.x = math::negate(raw_report.x) * _gyro_scale;
+		sensor_gyro.y = math::negate(raw_report.y) * _gyro_scale;
+		sensor_gyro.z = raw_report.z * _gyro_scale;
 		break;
 
-	case SENSOR_BOARD_ROTATION_270_DEG: {
-			/* swap x and y and negate y */
-			int16_t x = raw_report.y;
-			int16_t y = ((raw_report.x == -32768) ? 32767 : -raw_report.x);
-			_px4_gyro.update(timestamp_sample, x, y, raw_report.z);
-		}
+	case SENSOR_BOARD_ROTATION_270_DEG:
+		/* swap x and y and negate y */
+		sensor_gyro.x = raw_report.y * _gyro_scale;
+		sensor_gyro.y = math::negate(raw_report.x) * _gyro_scale;
+		sensor_gyro.z = raw_report.z * _gyro_scale;
 		break;
 
 	case SENSOR_BOARD_ROTATION_000_DEG:
@@ -381,9 +380,20 @@ L3GD20::RunImpl()
 	// FALLTHROUGH
 	default:
 		// keep axes in place
-		_px4_gyro.update(timestamp_sample, raw_report.x, raw_report.y, raw_report.z);
+		sensor_gyro.x = raw_report.x * _gyro_scale;
+		sensor_gyro.y = raw_report.y * _gyro_scale;
+		sensor_gyro.z = raw_report.z * _gyro_scale;
 	}
 
+	rotate_3f(_rotation, sensor_gyro.x, sensor_gyro.y, sensor_gyro.z);
+
+	sensor_gyro.range = _gyro_range;
+	sensor_gyro.temperature = L3GD20_TEMP_OFFSET_CELSIUS - raw_report.temp;
+	sensor_gyro.error_count = perf_event_count(_bad_registers);
+
+	sensor_gyro.timestamp = hrt_absolute_time();
+	_sensor_gyro_pub.publish(sensor_gyro);
+
 	_read++;
 
 	/* stop the perf counter */
diff --git a/src/drivers/imu/l3gd20/L3GD20.hpp b/src/drivers/imu/l3gd20/L3GD20.hpp
index 71bd532681..51cdc834b7 100644
--- a/src/drivers/imu/l3gd20/L3GD20.hpp
+++ b/src/drivers/imu/l3gd20/L3GD20.hpp
@@ -41,11 +41,13 @@
 
 #include <drivers/device/spi.h>
 #include <lib/conversion/rotation.h>
-#include <lib/drivers/gyroscope/PX4Gyroscope.hpp>
 #include <perf/perf_counter.h>
 #include <px4_platform_common/getopt.h>
 #include <px4_platform_common/i2c_spi_buses.h>
 
+#include <uORB/PublicationMulti.hpp>
+#include <uORB/topics/sensor_gyro.h>
+
 /* Orientation on board */
 #define SENSOR_BOARD_ROTATION_000_DEG	0
 #define SENSOR_BOARD_ROTATION_090_DEG	1
@@ -184,7 +186,11 @@ protected:
 
 private:
 
-	PX4Gyroscope		_px4_gyro;
+	uORB::PublicationMulti<sensor_gyro_s> _sensor_gyro_pub{ORB_ID(sensor_gyro)};
+
+	const enum Rotation _rotation;
+	float _gyro_scale{0.f};
+	float _gyro_range{0.f};
 
 	unsigned		_orientation{SENSOR_BOARD_ROTATION_DEFAULT};
 
diff --git a/src/drivers/imu/lsm303d/CMakeLists.txt b/src/drivers/imu/lsm303d/CMakeLists.txt
index 2ad45e295f..70b4804b38 100644
--- a/src/drivers/imu/lsm303d/CMakeLists.txt
+++ b/src/drivers/imu/lsm303d/CMakeLists.txt
@@ -39,7 +39,6 @@ px4_add_module(
 		lsm303d_main.cpp
 		LSM303D.cpp
 	DEPENDS
-		drivers_accelerometer
 		drivers_magnetometer
 		px4_work_queue
 	)
diff --git a/src/drivers/imu/lsm303d/LSM303D.cpp b/src/drivers/imu/lsm303d/LSM303D.cpp
index 7a43f9f0a7..f3d784ee05 100644
--- a/src/drivers/imu/lsm303d/LSM303D.cpp
+++ b/src/drivers/imu/lsm303d/LSM303D.cpp
@@ -56,7 +56,7 @@ static constexpr uint8_t _checked_registers[] = {
 LSM303D::LSM303D(const I2CSPIDriverConfig &config) :
 	SPI(config),
 	I2CSPIDriver(config),
-	_px4_accel(get_device_id(), config.rotation),
+	_rotation(config.rotation),
 	_px4_mag(get_device_id(), config.rotation),
 	_accel_sample_perf(perf_alloc(PC_ELAPSED, "lsm303d: acc_read")),
 	_mag_sample_perf(perf_alloc(PC_ELAPSED, "lsm303d: mag_read")),
@@ -236,7 +236,7 @@ LSM303D::accel_set_range(unsigned max_g)
 
 	float accel_range_scale = new_scale_g_digit * CONSTANTS_ONE_G;
 
-	_px4_accel.set_scale(accel_range_scale);
+	_accel_scale = accel_range_scale;
 
 	modify_reg(ADDR_CTRL_REG2, clearbits, setbits);
 
@@ -517,8 +517,23 @@ LSM303D::measureAccelerometer()
 	// register reads and bad values. This allows the higher level
 	// code to decide if it should use this sensor based on
 	// whether it has had failures
-	_px4_accel.set_error_count(perf_event_count(_bad_registers) + perf_event_count(_bad_values));
-	_px4_accel.update(timestamp_sample, raw_accel_report.x, raw_accel_report.y, raw_accel_report.z);
+
+	sensor_accel_s sensor_accel{};
+	sensor_accel.timestamp_sample = timestamp_sample;
+	sensor_accel.device_id = get_device_id();
+
+	sensor_accel.x = raw_accel_report.x * _accel_scale;
+	sensor_accel.y = raw_accel_report.y * _accel_scale;
+	sensor_accel.z = raw_accel_report.z * _accel_scale;
+
+	rotate_3f(_rotation, sensor_accel.x, sensor_accel.y, sensor_accel.z);
+
+	sensor_accel.range = _accel_range;
+	sensor_accel.temperature = _last_temperature;
+	sensor_accel.error_count = perf_event_count(_bad_registers) + perf_event_count(_bad_values);
+
+	sensor_accel.timestamp = hrt_absolute_time();
+	_sensor_accel_pub.publish(sensor_accel);
 
 	_last_accel[0] = raw_accel_report.x;
 	_last_accel[1] = raw_accel_report.y;
@@ -553,7 +568,6 @@ LSM303D::measureMagnetometer()
 	 * seems to be a signed offset from 25 degrees C in units of 0.125C
 	 */
 	_last_temperature = 25.0f + (raw_mag_report.temperature * 0.125f);
-	_px4_accel.set_temperature(_last_temperature);
 	_px4_mag.set_temperature(_last_temperature);
 
 	_px4_mag.set_error_count(perf_event_count(_bad_registers) + perf_event_count(_bad_values));
diff --git a/src/drivers/imu/lsm303d/LSM303D.hpp b/src/drivers/imu/lsm303d/LSM303D.hpp
index 62d3de5122..c505bdb45e 100644
--- a/src/drivers/imu/lsm303d/LSM303D.hpp
+++ b/src/drivers/imu/lsm303d/LSM303D.hpp
@@ -42,9 +42,11 @@
 #include <geo/geo.h>
 #include <perf/perf_counter.h>
 #include <px4_platform_common/i2c_spi_buses.h>
-#include <lib/drivers/accelerometer/PX4Accelerometer.hpp>
 #include <lib/drivers/magnetometer/PX4Magnetometer.hpp>
 
+#include <uORB/PublicationMulti.hpp>
+#include <uORB/topics/sensor_accel.h>
+
 /* SPI protocol address bits */
 #define DIR_READ			(1<<7)
 #define DIR_WRITE			(0<<7)
@@ -262,8 +264,12 @@ private:
 	 */
 	int			mag_set_samplerate(unsigned frequency);
 
+	uORB::PublicationMulti<sensor_accel_s> _sensor_accel_pub{ORB_ID(sensor_accel)};
+
+	const enum Rotation _rotation;
+	float _accel_range{0.f};
+	float _accel_scale{0.f};
 
-	PX4Accelerometer	_px4_accel;
 	PX4Magnetometer		_px4_mag;
 
 	unsigned		_call_accel_interval{1000000 / LSM303D_ACCEL_DEFAULT_RATE};
@@ -282,7 +288,7 @@ private:
 
 	hrt_abstime		_mag_last_measure{0};
 
-	float			_last_temperature{0.0f};
+	float			_last_temperature{NAN};
 
 	// this is used to support runtime checking of key
 	// configuration registers to detect SPI bus errors and sensor
diff --git a/src/drivers/imu/st/lsm9ds1/CMakeLists.txt b/src/drivers/imu/st/lsm9ds1/CMakeLists.txt
index fee777206e..be01a95602 100644
--- a/src/drivers/imu/st/lsm9ds1/CMakeLists.txt
+++ b/src/drivers/imu/st/lsm9ds1/CMakeLists.txt
@@ -41,7 +41,5 @@ px4_add_module(
 		lsm9ds1_main.cpp
 		ST_LSM9DS1_Registers.hpp
 	DEPENDS
-		drivers_accelerometer
-		drivers_gyroscope
 		px4_work_queue
 	)
diff --git a/src/drivers/imu/st/lsm9ds1/LSM9DS1.cpp b/src/drivers/imu/st/lsm9ds1/LSM9DS1.cpp
index b5577eff63..aa8445106e 100644
--- a/src/drivers/imu/st/lsm9ds1/LSM9DS1.cpp
+++ b/src/drivers/imu/st/lsm9ds1/LSM9DS1.cpp
@@ -1,6 +1,6 @@
 /****************************************************************************
  *
- *   Copyright (c) 2020-2021 PX4 Development Team. All rights reserved.
+ *   Copyright (c) 2020-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
@@ -33,6 +33,8 @@
 
 #include "LSM9DS1.hpp"
 
+#include <lib/parameters/param.h>
+
 using namespace time_literals;
 
 static constexpr int16_t combine(uint8_t msb, uint8_t lsb)
@@ -43,10 +45,12 @@ static constexpr int16_t combine(uint8_t msb, uint8_t lsb)
 LSM9DS1::LSM9DS1(const I2CSPIDriverConfig &config) :
 	SPI(config),
 	I2CSPIDriver(config),
-	_px4_accel(get_device_id(), config.rotation),
-	_px4_gyro(get_device_id(), config.rotation)
+	_rotation(config.rotation)
 {
-	ConfigureSampleRate(_px4_gyro.get_max_rate_hz());
+	int32_t imu_gyro_rate_max = 400;
+	param_get(param_find("IMU_GYRO_RATEMAX"), &imu_gyro_rate_max);
+
+	ConfigureSampleRate(imu_gyro_rate_max);
 }
 
 LSM9DS1::~LSM9DS1()
@@ -140,8 +144,8 @@ void LSM9DS1::RunImpl()
 				ScheduleDelayed(100_ms);
 
 			} else {
-				PX4_DEBUG("Reset not complete, check again in 10 ms");
-				ScheduleDelayed(10_ms);
+				PX4_DEBUG("Reset not complete, check again in 100 ms");
+				ScheduleDelayed(100_ms);
 			}
 		}
 
@@ -214,13 +218,14 @@ void LSM9DS1::RunImpl()
 
 				// full reset if things are failing consistently
 				if (_failure_count > 10) {
+					PX4_DEBUG("Full reset because things are failing consistently");
 					Reset();
 					return;
 				}
 			}
 
+			// check configuration registers periodically or immediately following any failure
 			if (!success || hrt_elapsed_time(&_last_config_check_timestamp) > 100_ms) {
-				// check configuration registers periodically or immediately following any failure
 				if (RegisterCheck(_register_cfg[_checked_register])) {
 					_last_config_check_timestamp = now;
 					_checked_register = (_checked_register + 1) % size_register_cfg;
@@ -228,13 +233,14 @@ void LSM9DS1::RunImpl()
 				} else {
 					// register check failed, force reset
 					perf_count(_bad_register_perf);
+					PX4_DEBUG("Force reset because register 0x%02hhX check failed ", (uint8_t)_register_cfg[_checked_register].reg);
 					Reset();
 				}
 
 			} else {
 				// periodically update temperature (~1 Hz)
 				if (hrt_elapsed_time(&_temperature_update_timestamp) >= 1_s) {
-					UpdateTemperature();
+					_temperature = ReadTemperature();
 					_temperature_update_timestamp = now;
 				}
 			}
@@ -272,14 +278,6 @@ bool LSM9DS1::Configure()
 		}
 	}
 
-	// Gyroscope configuration 2000 degrees/second
-	_px4_gyro.set_scale(math::radians(70.f / 1000.f)); // 70 mdps/LSB
-	_px4_gyro.set_range(math::radians(2000.f));
-
-	// Accelerometer configuration 16 G range
-	_px4_accel.set_scale(0.732f * (CONSTANTS_ONE_G / 1000.f)); // 0.732 mg/LSB
-	_px4_accel.set_range(16.f * CONSTANTS_ONE_G);
-
 	return success;
 }
 
@@ -329,15 +327,15 @@ void LSM9DS1::RegisterSetAndClearBits(Register reg, uint8_t setbits, uint8_t cle
 
 bool LSM9DS1::FIFORead(const hrt_abstime &timestamp_sample, uint8_t samples)
 {
-	sensor_gyro_fifo_s gyro{};
-	gyro.timestamp_sample = timestamp_sample;
-	gyro.samples = 0;
-	gyro.dt = FIFO_SAMPLE_DT;
+	sensor_imu_fifo_s sensor_imu_fifo{};
+	sensor_imu_fifo.timestamp_sample = timestamp_sample;
+	sensor_imu_fifo.samples = 0;
+	sensor_imu_fifo.dt = FIFO_SAMPLE_DT;
+	sensor_imu_fifo.accel_scale = ACCEL_SCALE;
+	sensor_imu_fifo.gyro_scale = GYRO_SCALE;
 
-	sensor_accel_fifo_s accel{};
-	accel.timestamp_sample = timestamp_sample;
-	accel.samples = 0;
-	accel.dt = FIFO_SAMPLE_DT;
+	int gyro_samples = 0;
+	int accel_samples = 0;
 
 	for (int i = 0; i < samples; i++) {
 		{
@@ -358,10 +356,10 @@ bool LSM9DS1::FIFORead(const hrt_abstime &timestamp_sample, uint8_t samples)
 
 				// 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[gyro.samples] = gyro_x;
-				gyro.y[gyro.samples] = gyro_y;
-				gyro.z[gyro.samples] = (gyro_z == INT16_MIN) ? INT16_MAX : -gyro_z;
-				gyro.samples++;
+				sensor_imu_fifo.gyro_x[gyro_samples] = gyro_x;
+				sensor_imu_fifo.gyro_y[gyro_samples] = gyro_y;
+				sensor_imu_fifo.gyro_z[gyro_samples] = math::negate(gyro_z);
+				gyro_samples++;
 
 			} else {
 				perf_count(_bad_transfer_perf);
@@ -386,10 +384,10 @@ bool LSM9DS1::FIFORead(const hrt_abstime &timestamp_sample, uint8_t samples)
 
 				// 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[accel.samples] = accel_x;
-				accel.y[accel.samples] = accel_y;
-				accel.z[accel.samples] = (accel_z == INT16_MIN) ? INT16_MAX : -accel_z;
-				accel.samples++;
+				sensor_imu_fifo.accel_x[accel_samples] = accel_x;
+				sensor_imu_fifo.accel_y[accel_samples] = accel_y;
+				sensor_imu_fifo.accel_z[accel_samples] = math::negate(accel_z);
+				accel_samples++;
 
 			} else {
 				perf_count(_bad_transfer_perf);
@@ -397,19 +395,21 @@ bool LSM9DS1::FIFORead(const hrt_abstime &timestamp_sample, uint8_t samples)
 		}
 	}
 
-	if (gyro.samples > 0) {
-		_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));
-		_px4_gyro.updateFIFO(gyro);
+	if (gyro_samples > 0 && accel_samples > 0) {
+
+		sensor_imu_fifo.samples = math::min(gyro_samples, accel_samples);
+
+		sensor_imu_fifo.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);
+
+		sensor_imu_fifo.timestamp = hrt_absolute_time();
+
+		_sensor_imu_fifo_pub.publish(sensor_imu_fifo);
+
+		return true;
 	}
 
-	if (accel.samples > 0) {
-		_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));
-		_px4_accel.updateFIFO(accel);
-	}
-
-	return (accel.samples > 0) && (gyro.samples > 0);
+	return false;
 }
 
 void LSM9DS1::FIFOReset()
@@ -427,26 +427,28 @@ void LSM9DS1::FIFOReset()
 	}
 }
 
-void LSM9DS1::UpdateTemperature()
+float LSM9DS1::ReadTemperature()
 {
-	// read current temperature
+	// transfer buffer
 	struct TransferBuffer {
 		uint8_t cmd{static_cast<uint8_t>(Register::OUT_TEMP_L) | DIR_READ};
 		uint8_t OUT_TEMP_L{0};
 		uint8_t OUT_TEMP_H{0};
 	} buffer{};
 
+	// read current temperature
 	if (transfer((uint8_t *)&buffer, (uint8_t *)&buffer, sizeof(buffer)) != PX4_OK) {
 		perf_count(_bad_transfer_perf);
-		return;
+		return NAN;
 	}
 
 	// 16 bits in two’s complement format with a sensitivity of 256 LSB/°C. The output zero level corresponds to 25 °C.
 	const int16_t OUT_TEMP = combine(buffer.OUT_TEMP_H, buffer.OUT_TEMP_L);
-	const float temperature = (OUT_TEMP / 256.0f) + 25.0f;
+	const float temperature = (OUT_TEMP / 256.f) + 25.f;
 
 	if (PX4_ISFINITE(temperature)) {
-		_px4_accel.set_temperature(temperature);
-		_px4_gyro.set_temperature(temperature);
+		return temperature;
 	}
+
+	return NAN;
 }
diff --git a/src/drivers/imu/st/lsm9ds1/LSM9DS1.hpp b/src/drivers/imu/st/lsm9ds1/LSM9DS1.hpp
index c02688f0e7..feb9df1954 100644
--- a/src/drivers/imu/st/lsm9ds1/LSM9DS1.hpp
+++ b/src/drivers/imu/st/lsm9ds1/LSM9DS1.hpp
@@ -1,6 +1,6 @@
 /****************************************************************************
  *
- *   Copyright (c) 2020-2021 PX4 Development Team. All rights reserved.
+ *   Copyright (c) 2020-2022 PX4 Development Team. All rights reserved.
  *
  * Redistribution and use in source and binary forms, with or without
  * modification, are permitted provided that the following conditions
@@ -43,13 +43,14 @@
 #include "ST_LSM9DS1_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/i2c_spi_buses.h>
 
+#include <uORB/PublicationMulti.hpp>
+#include <uORB/topics/sensor_imu_fifo.h>
+
 using namespace ST_LSM9DS1;
 
 class LSM9DS1 : public device::SPI, public I2CSPIDriver<LSM9DS1>
@@ -74,7 +75,10 @@ private:
 	static constexpr float ACCEL_RATE{ST_LSM9DS1::LA_ODR}; // 952 Hz accel
 
 	// maximum FIFO samples per transfer is limited to the size of sensor_accel_fifo/sensor_gyro_fifo
-	static constexpr int32_t FIFO_MAX_SAMPLES{math::min(math::min(FIFO::SIZE / 12, 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 int32_t FIFO_MAX_SAMPLES{math::min(FIFO::SIZE / 12, (size_t)sensor_imu_fifo_s::FIFO_SIZE)};
+
+	static constexpr float ACCEL_SCALE{0.732f * (CONSTANTS_ONE_G / 1000.f)}; // 16 G range: 0.732 mg/LSB
+	static constexpr float GYRO_SCALE{math::radians(70.f / 1000.f)}; // 2000 degrees/second: 70 mdps/LSB
 
 	struct register_config_t {
 		Register reg;
@@ -99,10 +103,11 @@ private:
 	bool FIFORead(const hrt_abstime &timestamp_sample, uint8_t samples);
 	void FIFOReset();
 
-	void UpdateTemperature();
+	float ReadTemperature();
 
-	PX4Accelerometer _px4_accel;
-	PX4Gyroscope _px4_gyro;
+	uORB::PublicationMulti<sensor_imu_fifo_s> _sensor_imu_fifo_pub{ORB_ID(sensor_imu_fifo)};
+
+	const enum Rotation _rotation;
 
 	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")};
@@ -115,6 +120,8 @@ private:
 	hrt_abstime _temperature_update_timestamp{0};
 	int _failure_count{0};
 
+	float _temperature{NAN};
+
 	enum class STATE : uint8_t {
 		RESET,
 		WAIT_FOR_RESET,
diff --git a/src/drivers/uavcan/sensors/accel.cpp b/src/drivers/uavcan/sensors/accel.cpp
index ad0327480e..9d37191283 100644
--- a/src/drivers/uavcan/sensors/accel.cpp
+++ b/src/drivers/uavcan/sensors/accel.cpp
@@ -1,6 +1,6 @@
 /****************************************************************************
  *
- *   Copyright (c) 2020 PX4 Development Team. All rights reserved.
+ *   Copyright (c) 2020-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
@@ -36,7 +36,6 @@
  */
 
 #include "accel.hpp"
-#include <lib/drivers/accelerometer/PX4Accelerometer.hpp>
 
 const char *const UavcanAccelBridge::NAME = "accel";
 
@@ -61,7 +60,7 @@ void UavcanAccelBridge::imu_sub_cb(const uavcan::ReceivedDataStructure<uavcan::e
 {
 	uavcan_bridge::Channel *channel = get_channel_for_node(msg.getSrcNodeID().get());
 
-	const hrt_abstime timestamp_sample = hrt_absolute_time();
+	//const hrt_abstime timestamp_sample = hrt_absolute_time();
 
 	if (channel == nullptr) {
 		// Something went wrong - no channel to publish on; return
@@ -69,37 +68,36 @@ void UavcanAccelBridge::imu_sub_cb(const uavcan::ReceivedDataStructure<uavcan::e
 	}
 
 	// Cast our generic CDev pointer to the sensor-specific driver class
-	PX4Accelerometer *accel = (PX4Accelerometer *)channel->h_driver;
 
-	if (accel == nullptr) {
-		return;
-	}
+	// if (accel == nullptr) {
+	// 	return;
+	// }
 
-	accel->set_error_count(0);
-	accel->update(timestamp_sample, msg.accelerometer_latest[0], msg.accelerometer_latest[1], msg.accelerometer_latest[2]);
+	// accel->set_error_count(0);
+	// accel->update(timestamp_sample, msg.accelerometer_latest[0], msg.accelerometer_latest[1], msg.accelerometer_latest[2]);
 }
 
 int UavcanAccelBridge::init_driver(uavcan_bridge::Channel *channel)
 {
-	// update device id as we now know our device node_id
-	DeviceId device_id{_device_id};
+	// // update device id as we now know our device node_id
+	// DeviceId device_id{_device_id};
 
-	device_id.devid_s.devtype = DRV_ACC_DEVTYPE_UAVCAN;
-	device_id.devid_s.address = static_cast<uint8_t>(channel->node_id);
+	// device_id.devid_s.devtype = DRV_ACC_DEVTYPE_UAVCAN;
+	// device_id.devid_s.address = static_cast<uint8_t>(channel->node_id);
 
-	channel->h_driver = new PX4Accelerometer(device_id.devid);
+	channel->h_driver = new uORB::PublicationMulti<sensor_accel_s>(ORB_ID(sensor_accel));
 
 	if (channel->h_driver == nullptr) {
 		return PX4_ERROR;
 	}
 
-	PX4Accelerometer *accel = (PX4Accelerometer *)channel->h_driver;
+	auto *accel_pub = static_cast<uORB::PublicationMulti<sensor_accel_s> *>(channel->h_driver);
 
-	channel->instance = accel->get_instance();
+	channel->instance = accel_pub->get_instance();
 
 	if (channel->instance < 0) {
 		PX4_ERR("UavcanAccel: Unable to get a class instance");
-		delete accel;
+		delete accel_pub;
 		channel->h_driver = nullptr;
 		return PX4_ERROR;
 	}
diff --git a/src/drivers/uavcan/sensors/accel.hpp b/src/drivers/uavcan/sensors/accel.hpp
index e1010b7eea..760ab49713 100644
--- a/src/drivers/uavcan/sensors/accel.hpp
+++ b/src/drivers/uavcan/sensors/accel.hpp
@@ -1,6 +1,6 @@
 /****************************************************************************
  *
- *   Copyright (c) 2020 PX4 Development Team. All rights reserved.
+ *   Copyright (c) 2020-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
@@ -41,6 +41,10 @@
 
 #include <uavcan/equipment/ahrs/RawIMU.hpp>
 
+#include <uORB/PublicationMulti.hpp>
+#include <uORB/topics/sensor_accel.h>
+#include <uORB/topics/sensor_gyro.h>
+
 class UavcanAccelBridge : public UavcanSensorBridgeBase
 {
 public:
diff --git a/src/drivers/uavcan/sensors/gyro.cpp b/src/drivers/uavcan/sensors/gyro.cpp
index 4a34213a03..ca5bc65ca8 100644
--- a/src/drivers/uavcan/sensors/gyro.cpp
+++ b/src/drivers/uavcan/sensors/gyro.cpp
@@ -1,6 +1,6 @@
 /****************************************************************************
  *
- *   Copyright (c) 2020 PX4 Development Team. All rights reserved.
+ *   Copyright (c) 2020-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
@@ -36,7 +36,6 @@
  */
 
 #include "gyro.hpp"
-#include <lib/drivers/gyroscope/PX4Gyroscope.hpp>
 
 const char *const UavcanGyroBridge::NAME = "gyro";
 
@@ -67,33 +66,33 @@ void UavcanGyroBridge::imu_sub_cb(const uavcan::ReceivedDataStructure<uavcan::eq
 	}
 
 	// Cast our generic CDev pointer to the sensor-specific driver class
-	PX4Gyroscope *gyro = (PX4Gyroscope *)channel->h_driver;
+	uORB::PublicationMulti<sensor_gyro_s> *gyro = (uORB::PublicationMulti<sensor_gyro_s> *)channel->h_driver;
 
 	if (gyro == nullptr) {
 		return;
 	}
 
-	gyro->update(hrt_absolute_time(),
-		     msg.rate_gyro_latest[0],
-		     msg.rate_gyro_latest[1],
-		     msg.rate_gyro_latest[2]);
+	// gyro->update(hrt_absolute_time(),
+	// 	     msg.rate_gyro_latest[0],
+	// 	     msg.rate_gyro_latest[1],
+	// 	     msg.rate_gyro_latest[2]);
 }
 
 int UavcanGyroBridge::init_driver(uavcan_bridge::Channel *channel)
 {
-	// update device id as we now know our device node_id
-	DeviceId device_id{_device_id};
+	// // update device id as we now know our device node_id
+	// DeviceId device_id{_device_id};
 
-	device_id.devid_s.devtype = DRV_GYR_DEVTYPE_UAVCAN;
-	device_id.devid_s.address = static_cast<uint8_t>(channel->node_id);
+	// device_id.devid_s.devtype = DRV_GYR_DEVTYPE_UAVCAN;
+	// device_id.devid_s.address = static_cast<uint8_t>(channel->node_id);
 
-	channel->h_driver = new PX4Gyroscope(device_id.devid);
+	channel->h_driver = new uORB::PublicationMulti<sensor_gyro_s>(ORB_ID(sensor_gyro));
 
 	if (channel->h_driver == nullptr) {
 		return PX4_ERROR;
 	}
 
-	PX4Gyroscope *gyro = (PX4Gyroscope *)channel->h_driver;
+	uORB::PublicationMulti<sensor_gyro_s> *gyro = (uORB::PublicationMulti<sensor_gyro_s> *)channel->h_driver;
 
 	channel->instance = gyro->get_instance();
 
diff --git a/src/drivers/uavcan/sensors/gyro.hpp b/src/drivers/uavcan/sensors/gyro.hpp
index 0ed5c8959f..804ae2a11a 100644
--- a/src/drivers/uavcan/sensors/gyro.hpp
+++ b/src/drivers/uavcan/sensors/gyro.hpp
@@ -1,6 +1,6 @@
 /****************************************************************************
  *
- *   Copyright (c) 2020 PX4 Development Team. All rights reserved.
+ *   Copyright (c) 2020-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
@@ -41,6 +41,10 @@
 
 #include <uavcan/equipment/ahrs/RawIMU.hpp>
 
+#include <uORB/PublicationMulti.hpp>
+#include <uORB/topics/sensor_accel.h>
+#include <uORB/topics/sensor_gyro.h>
+
 class UavcanGyroBridge : public UavcanSensorBridgeBase
 {
 public:
diff --git a/src/examples/fake_imu/CMakeLists.txt b/src/examples/fake_imu/CMakeLists.txt
index 2d037d7387..ee5155b8d7 100644
--- a/src/examples/fake_imu/CMakeLists.txt
+++ b/src/examples/fake_imu/CMakeLists.txt
@@ -40,7 +40,5 @@ px4_add_module(
 		FakeImu.cpp
 		FakeImu.hpp
 	DEPENDS
-		drivers_accelerometer
-		drivers_gyroscope
 		px4_work_queue
 )
diff --git a/src/examples/fake_imu/FakeImu.cpp b/src/examples/fake_imu/FakeImu.cpp
index d88a0ffb13..782d7057c7 100644
--- a/src/examples/fake_imu/FakeImu.cpp
+++ b/src/examples/fake_imu/FakeImu.cpp
@@ -1,6 +1,6 @@
 /****************************************************************************
  *
- *   Copyright (c) 2020-2021 PX4 Development Team. All rights reserved.
+ *   Copyright (c) 2020-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
@@ -33,21 +33,20 @@
 
 #include "FakeImu.hpp"
 
+#include <lib/parameters/param.h>
+
 using namespace time_literals;
 
 FakeImu::FakeImu() :
 	ModuleParams(nullptr),
-	ScheduledWorkItem(MODULE_NAME, px4::wq_configurations::hp_default),
-	_px4_accel(1310988), // 1310988: DRV_IMU_DEVTYPE_SIM, BUS: 1, ADDR: 1, TYPE: SIMULATION
-	_px4_gyro(1310988)   // 1310988: DRV_IMU_DEVTYPE_SIM, BUS: 1, ADDR: 1, TYPE: SIMULATION
+	ScheduledWorkItem(MODULE_NAME, px4::wq_configurations::hp_default)
 {
-	_sensor_interval_us = roundf(1.e6f / _px4_gyro.get_max_rate_hz());
+	int32_t imu_gyro_rate_max = 400;
+	param_get(param_find("IMU_GYRO_RATEMAX"), &imu_gyro_rate_max);
 
-	PX4_INFO("Rate %.3f, Interval: %" PRId32 " us", (double)_px4_gyro.get_max_rate_hz(), _sensor_interval_us);
+	_sensor_interval_us = roundf(1.e6f / imu_gyro_rate_max);
 
-	_px4_accel.set_range(2000.f); // don't care
-
-	_px4_gyro.set_scale(math::radians(2000.f) / static_cast<float>(INT16_MAX - 1)); // 2000 degrees/second max
+	PX4_INFO("Rate %.3f, Interval: %" PRId32 " us", (double)imu_gyro_rate_max, _sensor_interval_us);
 }
 
 bool FakeImu::init()
@@ -64,10 +63,14 @@ void FakeImu::Run()
 		return;
 	}
 
-	sensor_gyro_fifo_s gyro{};
-	gyro.timestamp_sample = hrt_absolute_time();
-	gyro.samples = roundf(IMU_RATE_HZ / (1e6 / _sensor_interval_us));
-	gyro.dt = 1e6 / IMU_RATE_HZ;
+	sensor_imu_fifo_s sensor_imu_fifo{};
+	sensor_imu_fifo.timestamp_sample = hrt_absolute_time();
+	sensor_imu_fifo.device_id = 1310988; // 1310988: DRV_IMU_DEVTYPE_SIM, BUS: 1, ADDR: 1, TYPE: SIMULATION
+	sensor_imu_fifo.dt = 1e6 / IMU_RATE_HZ;
+	sensor_imu_fifo.samples = math::min(roundf(IMU_RATE_HZ / (1e6 / _sensor_interval_us)),
+					    (float)sensor_imu_fifo_s::FIFO_SIZE);
+	sensor_imu_fifo.accel_scale = 1.f; // doesn't matter
+	sensor_imu_fifo.gyro_scale = math::radians(2000.f) / static_cast<float>(INT16_MAX - 1); // 2000 degrees/second max
 
 	const double dt_s = 1 / IMU_RATE_HZ;
 
@@ -84,41 +87,43 @@ void FakeImu::Run()
 	static constexpr double A = (INT16_MAX - 1);
 
 	if (_time_start_us == 0) {
-		_time_start_us = gyro.timestamp_sample;
+		_time_start_us = sensor_imu_fifo.timestamp_sample;
 	}
 
 	// 10 second sweep
 	const double T = 10.0;
 
-	const double timestamp_sample_s = static_cast<double>(gyro.timestamp_sample - _time_start_us) / 1e6;
+	const double timestamp_sample_s = static_cast<double>(sensor_imu_fifo.timestamp_sample - _time_start_us) / 1e6;
 
 	float x_freq = 0;
 	float y_freq = 0;
 	float z_freq = 0;
 
-	for (int n = 0; n < gyro.samples; n++) {
+	for (int n = 0; n < sensor_imu_fifo.samples; n++) {
 		// timestamp_sample corresponds to last sample
-		const double t = timestamp_sample_s - (gyro.samples - n - 1) * dt_s;
+		const double t = timestamp_sample_s - (sensor_imu_fifo.samples - n - 1) * dt_s;
 
 		// linear-frequency chirp, see https://en.wikipedia.org/wiki/Chirp
 		const double x_F = x_f0 + (x_f1 - x_f0) * t / (2 * T);
 		const double y_F = y_f0 + (y_f1 - y_f0) * t / (2 * T);
 		const double z_F = z_f0 + (z_f1 - z_f0) * t / (2 * T);
 
-		gyro.x[n] = roundf(A * sin(2 * M_PI * x_F * t));
-		gyro.y[n] = roundf(A * sin(2 * M_PI * y_F * t));
-		gyro.z[n] = roundf(A * sin(2 * M_PI * z_F * t));
+		sensor_imu_fifo.gyro_x[n] = roundf(A * sin(2 * M_PI * x_F * t));
+		sensor_imu_fifo.gyro_y[n] = roundf(A * sin(2 * M_PI * y_F * t));
+		sensor_imu_fifo.gyro_z[n] = roundf(A * sin(2 * M_PI * z_F * t));
 
-		if (n == 0) {
-			x_freq = (x_f1 - x_f0) * (t / T) + x_f0;
-			y_freq = (y_f1 - y_f0) * (t / T) + y_f0;
-			z_freq = (z_f1 - z_f0) * (t / T) + z_f0;
+		x_freq = (x_f1 - x_f0) * (t / T) + x_f0;
+		y_freq = (y_f1 - y_f0) * (t / T) + y_f0;
+		z_freq = (z_f1 - z_f0) * (t / T) + z_f0;
 
-			_px4_accel.update(gyro.timestamp_sample, x_freq, y_freq, z_freq);
-		}
+		// for logging
+		sensor_imu_fifo.accel_x[n] = x_freq;
+		sensor_imu_fifo.accel_y[n] = y_freq;
+		sensor_imu_fifo.accel_z[n] = z_freq;
 	}
 
-	_px4_gyro.updateFIFO(gyro);
+	sensor_imu_fifo.timestamp = hrt_absolute_time();
+	_sensor_imu_fifo_pub.publish(sensor_imu_fifo);
 
 #if defined(FAKE_IMU_FAKE_ESC_STATUS)
 
diff --git a/src/examples/fake_imu/FakeImu.hpp b/src/examples/fake_imu/FakeImu.hpp
index 489365557e..5f9436a32c 100644
--- a/src/examples/fake_imu/FakeImu.hpp
+++ b/src/examples/fake_imu/FakeImu.hpp
@@ -38,11 +38,9 @@
 #include <px4_platform_common/module_params.h>
 #include <px4_platform_common/posix.h>
 #include <px4_platform_common/px4_work_queue/ScheduledWorkItem.hpp>
-#include <lib/drivers/accelerometer/PX4Accelerometer.hpp>
-#include <lib/drivers/gyroscope/PX4Gyroscope.hpp>
 #include <uORB/PublicationMulti.hpp>
 #include <uORB/Subscription.hpp>
-#include <uORB/topics/sensor_gyro_fifo.h>
+#include <uORB/topics/sensor_imu_fifo.h>
 
 // fake ESC RPM for testing dynamic notch filtering
 //#define FAKE_IMU_FAKE_ESC_STATUS
@@ -73,11 +71,10 @@ private:
 
 	void Run() override;
 
-	PX4Accelerometer _px4_accel;
-	PX4Gyroscope _px4_gyro;
-
 	hrt_abstime _time_start_us{0};
 
+	uORB::Publication<sensor_imu_fifo_s> _sensor_imu_fifo_pub {ORB_ID(sensor_imu_fifo)};
+
 	uint32_t _sensor_interval_us{1250};
 
 #if defined(FAKE_IMU_FAKE_ESC_STATUS)
diff --git a/src/lib/drivers/CMakeLists.txt b/src/lib/drivers/CMakeLists.txt
index 00c45648d4..9978713d55 100644
--- a/src/lib/drivers/CMakeLists.txt
+++ b/src/lib/drivers/CMakeLists.txt
@@ -31,9 +31,7 @@
 #
 ############################################################################
 
-add_subdirectory(accelerometer)
 add_subdirectory(device)
-add_subdirectory(gyroscope)
 add_subdirectory(led)
 add_subdirectory(magnetometer)
 add_subdirectory(rangefinder)
diff --git a/src/lib/drivers/accelerometer/CMakeLists.txt b/src/lib/drivers/accelerometer/CMakeLists.txt
deleted file mode 100644
index 961c39f174..0000000000
--- a/src/lib/drivers/accelerometer/CMakeLists.txt
+++ /dev/null
@@ -1,39 +0,0 @@
-############################################################################
-#
-#   Copyright (c) 2018 PX4 Development Team. All rights reserved.
-#
-# Redistribution and use in source and binary forms, with or without
-# modification, are permitted provided that the following conditions
-# are met:
-#
-# 1. Redistributions of source code must retain the above copyright
-#    notice, this list of conditions and the following disclaimer.
-# 2. Redistributions in binary form must reproduce the above copyright
-#    notice, this list of conditions and the following disclaimer in
-#    the documentation and/or other materials provided with the
-#    distribution.
-# 3. Neither the name PX4 nor the names of its contributors may be
-#    used to endorse or promote products derived from this software
-#    without specific prior written permission.
-#
-# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
-# "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
-# LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
-# FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
-# COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
-# INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
-# BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
-# OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
-# AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
-# LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
-# ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
-# POSSIBILITY OF SUCH DAMAGE.
-#
-############################################################################
-
-px4_add_library(drivers_accelerometer
-	PX4Accelerometer.cpp
-	PX4Accelerometer.hpp
-)
-target_compile_options(drivers_accelerometer PRIVATE ${MAX_CUSTOM_OPT_LEVEL})
-target_link_libraries(drivers_accelerometer PRIVATE conversion)
diff --git a/src/lib/drivers/accelerometer/PX4Accelerometer.cpp b/src/lib/drivers/accelerometer/PX4Accelerometer.cpp
deleted file mode 100644
index 9994745a02..0000000000
--- a/src/lib/drivers/accelerometer/PX4Accelerometer.cpp
+++ /dev/null
@@ -1,183 +0,0 @@
-/****************************************************************************
- *
- *   Copyright (c) 2018-2021 PX4 Development Team. All rights reserved.
- *
- * Redistribution and use in source and binary forms, with or without
- * modification, are permitted provided that the following conditions
- * 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 "PX4Accelerometer.hpp"
-
-#include <lib/drivers/device/Device.hpp>
-#include <lib/parameters/param.h>
-
-using namespace time_literals;
-
-static constexpr int32_t sum(const int16_t samples[], uint8_t len)
-{
-	int32_t sum = 0;
-
-	for (int n = 0; n < len; n++) {
-		sum += samples[n];
-	}
-
-	return sum;
-}
-
-static constexpr uint8_t clipping(const int16_t samples[], uint8_t len)
-{
-	unsigned clip_count = 0;
-
-	for (int n = 0; n < len; n++) {
-		// - consider data clipped/saturated if it's INT16_MIN/INT16_MAX or within 1
-		// - this accommodates rotated data (|INT16_MIN| = INT16_MAX + 1)
-		//   and sensors that may re-use the lowest bit for other purposes (sync indicator, etc)
-		if ((samples[n] <= INT16_MIN + 1) || (samples[n] >= INT16_MAX - 1)) {
-			clip_count++;
-		}
-	}
-
-	return clip_count;
-}
-
-PX4Accelerometer::PX4Accelerometer(uint32_t device_id, enum Rotation rotation) :
-	_device_id{device_id},
-	_rotation{rotation}
-{
-	// advertise immediately to keep instance numbering in sync
-	_sensor_pub.advertise();
-
-	param_get(param_find("IMU_GYRO_RATEMAX"), &_imu_gyro_rate_max);
-}
-
-PX4Accelerometer::~PX4Accelerometer()
-{
-	_sensor_pub.unadvertise();
-	_sensor_fifo_pub.unadvertise();
-}
-
-void PX4Accelerometer::set_device_type(uint8_t devtype)
-{
-	// current DeviceStructure
-	union device::Device::DeviceId device_id;
-	device_id.devid = _device_id;
-
-	// update to new device type
-	device_id.devid_s.devtype = devtype;
-
-	// copy back
-	_device_id = device_id.devid;
-}
-
-void PX4Accelerometer::set_scale(float scale)
-{
-	if (fabsf(scale - _scale) > FLT_EPSILON) {
-		// rescale last sample on scale change
-		float rescale = _scale / scale;
-
-		for (auto &s : _last_sample) {
-			s = roundf(s * rescale);
-		}
-
-		_scale = scale;
-
-		UpdateClipLimit();
-	}
-}
-
-void PX4Accelerometer::update(const hrt_abstime &timestamp_sample, float x, float y, float z)
-{
-	// Apply rotation (before scaling)
-	rotate_3f(_rotation, x, y, z);
-
-	// publish
-	sensor_accel_s report;
-
-	report.timestamp_sample = timestamp_sample;
-	report.device_id = _device_id;
-	report.temperature = _temperature;
-	report.error_count = _error_count;
-	report.x = x * _scale;
-	report.y = y * _scale;
-	report.z = z * _scale;
-	report.clip_counter[0] = (fabsf(x) >= _clip_limit);
-	report.clip_counter[1] = (fabsf(y) >= _clip_limit);
-	report.clip_counter[2] = (fabsf(z) >= _clip_limit);
-	report.samples = 1;
-	report.timestamp = hrt_absolute_time();
-
-	_sensor_pub.publish(report);
-}
-
-void PX4Accelerometer::updateFIFO(sensor_accel_fifo_s &sample)
-{
-	// rotate all raw samples and publish fifo
-	const uint8_t N = sample.samples;
-
-	for (int n = 0; n < N; n++) {
-		rotate_3i(_rotation, sample.x[n], sample.y[n], sample.z[n]);
-	}
-
-	sample.device_id = _device_id;
-	sample.scale = _scale;
-	sample.timestamp = hrt_absolute_time();
-	_sensor_fifo_pub.publish(sample);
-
-
-	// publish
-	sensor_accel_s report;
-	report.timestamp_sample = sample.timestamp_sample;
-	report.device_id = _device_id;
-	report.temperature = _temperature;
-	report.error_count = _error_count;
-
-	// trapezoidal integration (equally spaced)
-	const float scale = _scale / (float)N;
-	report.x = (0.5f * (_last_sample[0] + sample.x[N - 1]) + sum(sample.x, N - 1)) * scale;
-	report.y = (0.5f * (_last_sample[1] + sample.y[N - 1]) + sum(sample.y, N - 1)) * scale;
-	report.z = (0.5f * (_last_sample[2] + sample.z[N - 1]) + sum(sample.z, N - 1)) * scale;
-
-	_last_sample[0] = sample.x[N - 1];
-	_last_sample[1] = sample.y[N - 1];
-	_last_sample[2] = sample.z[N - 1];
-
-	report.clip_counter[0] = clipping(sample.x, N);
-	report.clip_counter[1] = clipping(sample.y, N);
-	report.clip_counter[2] = clipping(sample.z, N);
-	report.samples = N;
-	report.timestamp = hrt_absolute_time();
-
-	_sensor_pub.publish(report);
-}
-
-void PX4Accelerometer::UpdateClipLimit()
-{
-	// 99.9% of potential max
-	_clip_limit = math::constrain((_range / _scale) * 0.999f, 0.f, (float)INT16_MAX);
-}
diff --git a/src/lib/drivers/accelerometer/PX4Accelerometer.hpp b/src/lib/drivers/accelerometer/PX4Accelerometer.hpp
deleted file mode 100644
index fa72d64053..0000000000
--- a/src/lib/drivers/accelerometer/PX4Accelerometer.hpp
+++ /dev/null
@@ -1,86 +0,0 @@
-/****************************************************************************
- *
- *   Copyright (c) 2018-2021 PX4 Development Team. All rights reserved.
- *
- * Redistribution and use in source and binary forms, with or without
- * modification, are permitted provided that the following conditions
- * 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.
- *
- ****************************************************************************/
-
-#pragma once
-
-#include <drivers/drv_hrt.h>
-#include <lib/conversion/rotation.h>
-#include <lib/geo/geo.h>
-#include <uORB/PublicationMulti.hpp>
-#include <uORB/topics/sensor_accel.h>
-#include <uORB/topics/sensor_accel_fifo.h>
-
-class PX4Accelerometer
-{
-public:
-	PX4Accelerometer(uint32_t device_id, enum Rotation rotation = ROTATION_NONE);
-	~PX4Accelerometer();
-
-	uint32_t get_device_id() const { return _device_id; }
-
-	int32_t get_max_rate_hz() const { return math::constrain(_imu_gyro_rate_max, static_cast<int32_t>(100), static_cast<int32_t>(4000)); }
-
-	void set_device_id(uint32_t device_id) { _device_id = device_id; }
-	void set_device_type(uint8_t devtype);
-	void set_error_count(uint32_t error_count) { _error_count = error_count; }
-	void set_range(float range) { _range = range; UpdateClipLimit(); }
-	void set_scale(float scale);
-	void set_temperature(float temperature) { _temperature = temperature; }
-
-	void update(const hrt_abstime &timestamp_sample, float x, float y, float z);
-
-	void updateFIFO(sensor_accel_fifo_s &sample);
-
-	int get_instance() { return _sensor_pub.get_instance(); };
-
-private:
-	void UpdateClipLimit();
-
-	uORB::PublicationMulti<sensor_accel_s> _sensor_pub{ORB_ID(sensor_accel)};
-	uORB::PublicationMulti<sensor_accel_fifo_s>  _sensor_fifo_pub{ORB_ID(sensor_accel_fifo)};
-
-	uint32_t		_device_id{0};
-	const enum Rotation	_rotation;
-
-	int32_t			_imu_gyro_rate_max{0}; // match gyro max rate
-
-	float			_range{16 * CONSTANTS_ONE_G};
-	float			_scale{1.f};
-	float			_temperature{NAN};
-
-	float			_clip_limit{_range / _scale};
-
-	uint32_t		_error_count{0};
-
-	int16_t			_last_sample[3] {};
-};
diff --git a/src/lib/drivers/gyroscope/CMakeLists.txt b/src/lib/drivers/gyroscope/CMakeLists.txt
deleted file mode 100644
index bb835cdc7b..0000000000
--- a/src/lib/drivers/gyroscope/CMakeLists.txt
+++ /dev/null
@@ -1,39 +0,0 @@
-############################################################################
-#
-#   Copyright (c) 2018 PX4 Development Team. All rights reserved.
-#
-# Redistribution and use in source and binary forms, with or without
-# modification, are permitted provided that the following conditions
-# are met:
-#
-# 1. Redistributions of source code must retain the above copyright
-#    notice, this list of conditions and the following disclaimer.
-# 2. Redistributions in binary form must reproduce the above copyright
-#    notice, this list of conditions and the following disclaimer in
-#    the documentation and/or other materials provided with the
-#    distribution.
-# 3. Neither the name PX4 nor the names of its contributors may be
-#    used to endorse or promote products derived from this software
-#    without specific prior written permission.
-#
-# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
-# "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
-# LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
-# FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
-# COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
-# INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
-# BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
-# OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
-# AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
-# LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
-# ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
-# POSSIBILITY OF SUCH DAMAGE.
-#
-############################################################################
-
-px4_add_library(drivers_gyroscope
-	PX4Gyroscope.cpp
-	PX4Gyroscope.hpp
-)
-target_compile_options(drivers_gyroscope PRIVATE ${MAX_CUSTOM_OPT_LEVEL})
-target_link_libraries(drivers_gyroscope PRIVATE conversion)
diff --git a/src/lib/drivers/gyroscope/PX4Gyroscope.cpp b/src/lib/drivers/gyroscope/PX4Gyroscope.cpp
deleted file mode 100644
index fdce2004a1..0000000000
--- a/src/lib/drivers/gyroscope/PX4Gyroscope.cpp
+++ /dev/null
@@ -1,182 +0,0 @@
-/****************************************************************************
- *
- *   Copyright (c) 2018-2022 PX4 Development Team. All rights reserved.
- *
- * Redistribution and use in source and binary forms, with or without
- * modification, are permitted provided that the following conditions
- * are met:
- *
- * 1. Redistributions of source code must retain the above copyright
- *    notice, this list of conditions and the following disclaimer.
- * 2. Redistributions in binary form must reproduce the above copyright
- *    notice, this list of conditions and the following disclaimer in
- *    the documentation and/or other materials provided with the
- *    distribution.
- * 3. Neither the name PX4 nor the names of its contributors may be
- *    used to endorse or promote products derived from this software
- *    without specific prior written permission.
- *
- * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
- * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
- * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
- * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
- * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
- * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
- * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
- * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
- * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
- * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
- * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
- * POSSIBILITY OF SUCH DAMAGE.
- *
- ****************************************************************************/
-
-
-#include "PX4Gyroscope.hpp"
-
-#include <lib/drivers/device/Device.hpp>
-#include <lib/parameters/param.h>
-
-using namespace time_literals;
-
-static constexpr int32_t sum(const int16_t samples[], uint8_t len)
-{
-	int32_t sum = 0;
-
-	for (int n = 0; n < len; n++) {
-		sum += samples[n];
-	}
-
-	return sum;
-}
-
-static constexpr uint8_t clipping(const int16_t samples[], uint8_t len)
-{
-	unsigned clip_count = 0;
-
-	for (int n = 0; n < len; n++) {
-		// - consider data clipped/saturated if it's INT16_MIN/INT16_MAX or within 1
-		// - this accommodates rotated data (|INT16_MIN| = INT16_MAX + 1)
-		//   and sensors that may re-use the lowest bit for other purposes (sync indicator, etc)
-		if ((samples[n] <= INT16_MIN + 1) || (samples[n] >= INT16_MAX - 1)) {
-			clip_count++;
-		}
-	}
-
-	return clip_count;
-}
-
-PX4Gyroscope::PX4Gyroscope(uint32_t device_id, enum Rotation rotation) :
-	_device_id{device_id},
-	_rotation{rotation}
-{
-	// advertise immediately to keep instance numbering in sync
-	_sensor_pub.advertise();
-
-	param_get(param_find("IMU_GYRO_RATEMAX"), &_imu_gyro_rate_max);
-}
-
-PX4Gyroscope::~PX4Gyroscope()
-{
-	_sensor_pub.unadvertise();
-	_sensor_fifo_pub.unadvertise();
-}
-
-void PX4Gyroscope::set_device_type(uint8_t devtype)
-{
-	// current DeviceStructure
-	union device::Device::DeviceId device_id;
-	device_id.devid = _device_id;
-
-	// update to new device type
-	device_id.devid_s.devtype = devtype;
-
-	// copy back
-	_device_id = device_id.devid;
-}
-
-void PX4Gyroscope::set_scale(float scale)
-{
-	if (fabsf(scale - _scale) > FLT_EPSILON) {
-		// rescale last sample on scale change
-		float rescale = _scale / scale;
-
-		for (auto &s : _last_sample) {
-			s = roundf(s * rescale);
-		}
-
-		_scale = scale;
-
-		UpdateClipLimit();
-	}
-}
-
-void PX4Gyroscope::update(const hrt_abstime &timestamp_sample, float x, float y, float z)
-{
-	// Apply rotation (before scaling)
-	rotate_3f(_rotation, x, y, z);
-
-	sensor_gyro_s report;
-
-	report.timestamp_sample = timestamp_sample;
-	report.device_id = _device_id;
-	report.temperature = _temperature;
-	report.error_count = _error_count;
-	report.x = x * _scale;
-	report.y = y * _scale;
-	report.z = z * _scale;
-	report.clip_counter[0] = (fabsf(x) >= _clip_limit);
-	report.clip_counter[1] = (fabsf(y) >= _clip_limit);
-	report.clip_counter[2] = (fabsf(z) >= _clip_limit);
-	report.samples = 1;
-	report.timestamp = hrt_absolute_time();
-
-	_sensor_pub.publish(report);
-}
-
-void PX4Gyroscope::updateFIFO(sensor_gyro_fifo_s &sample)
-{
-	// rotate all raw samples and publish fifo
-	const uint8_t N = sample.samples;
-
-	for (int n = 0; n < N; n++) {
-		rotate_3i(_rotation, sample.x[n], sample.y[n], sample.z[n]);
-	}
-
-	sample.device_id = _device_id;
-	sample.scale = _scale;
-	sample.timestamp = hrt_absolute_time();
-	_sensor_fifo_pub.publish(sample);
-
-
-	// publish
-	sensor_gyro_s report;
-	report.timestamp_sample = sample.timestamp_sample;
-	report.device_id = _device_id;
-	report.temperature = _temperature;
-	report.error_count = _error_count;
-
-	// trapezoidal integration (equally spaced)
-	const float scale = _scale / (float)N;
-	report.x = (0.5f * (_last_sample[0] + sample.x[N - 1]) + sum(sample.x, N - 1)) * scale;
-	report.y = (0.5f * (_last_sample[1] + sample.y[N - 1]) + sum(sample.y, N - 1)) * scale;
-	report.z = (0.5f * (_last_sample[2] + sample.z[N - 1]) + sum(sample.z, N - 1)) * scale;
-
-	_last_sample[0] = sample.x[N - 1];
-	_last_sample[1] = sample.y[N - 1];
-	_last_sample[2] = sample.z[N - 1];
-
-	report.clip_counter[0] = clipping(sample.x, N);
-	report.clip_counter[1] = clipping(sample.y, N);
-	report.clip_counter[2] = clipping(sample.z, N);
-	report.samples = N;
-	report.timestamp = hrt_absolute_time();
-
-	_sensor_pub.publish(report);
-}
-
-void PX4Gyroscope::UpdateClipLimit()
-{
-	// 99.9% of potential max
-	_clip_limit = math::constrain((_range / _scale) * 0.999f, 0.f, (float)INT16_MAX);
-}
diff --git a/src/lib/drivers/gyroscope/PX4Gyroscope.hpp b/src/lib/drivers/gyroscope/PX4Gyroscope.hpp
deleted file mode 100644
index f89514f7a1..0000000000
--- a/src/lib/drivers/gyroscope/PX4Gyroscope.hpp
+++ /dev/null
@@ -1,85 +0,0 @@
-/****************************************************************************
- *
- *   Copyright (c) 2018-2022 PX4 Development Team. All rights reserved.
- *
- * Redistribution and use in source and binary forms, with or without
- * modification, are permitted provided that the following conditions
- * are met:
- *
- * 1. Redistributions of source code must retain the above copyright
- *    notice, this list of conditions and the following disclaimer.
- * 2. Redistributions in binary form must reproduce the above copyright
- *    notice, this list of conditions and the following disclaimer in
- *    the documentation and/or other materials provided with the
- *    distribution.
- * 3. Neither the name PX4 nor the names of its contributors may be
- *    used to endorse or promote products derived from this software
- *    without specific prior written permission.
- *
- * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
- * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
- * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
- * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
- * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
- * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
- * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
- * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
- * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
- * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
- * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
- * POSSIBILITY OF SUCH DAMAGE.
- *
- ****************************************************************************/
-
-#pragma once
-
-#include <drivers/drv_hrt.h>
-#include <lib/conversion/rotation.h>
-#include <uORB/PublicationMulti.hpp>
-#include <uORB/topics/sensor_gyro.h>
-#include <uORB/topics/sensor_gyro_fifo.h>
-
-class PX4Gyroscope
-{
-public:
-	PX4Gyroscope(uint32_t device_id, enum Rotation rotation = ROTATION_NONE);
-	~PX4Gyroscope();
-
-	uint32_t get_device_id() const { return _device_id; }
-
-	int32_t get_max_rate_hz() const { return math::constrain(_imu_gyro_rate_max, static_cast<int32_t>(100), static_cast<int32_t>(4000)); }
-
-	void set_device_id(uint32_t device_id) { _device_id = device_id; }
-	void set_device_type(uint8_t devtype);
-	void set_error_count(uint32_t error_count) { _error_count = error_count; }
-	void set_range(float range) { _range = range; UpdateClipLimit(); }
-	void set_scale(float scale);
-	void set_temperature(float temperature) { _temperature = temperature; }
-
-	void update(const hrt_abstime &timestamp_sample, float x, float y, float z);
-
-	void updateFIFO(sensor_gyro_fifo_s &sample);
-
-	int get_instance() { return _sensor_pub.get_instance(); };
-
-private:
-	void UpdateClipLimit();
-
-	uORB::PublicationMulti<sensor_gyro_s> _sensor_pub{ORB_ID(sensor_gyro)};
-	uORB::PublicationMulti<sensor_gyro_fifo_s>  _sensor_fifo_pub{ORB_ID(sensor_gyro_fifo)};
-
-	uint32_t		_device_id{0};
-	const enum Rotation	_rotation;
-
-	int32_t			_imu_gyro_rate_max{0};
-
-	float			_range{math::radians(2000.f)};
-	float			_scale{1.f};
-	float			_temperature{NAN};
-
-	float			_clip_limit{_range / _scale};
-
-	uint32_t		_error_count{0};
-
-	int16_t			_last_sample[3] {};
-};
diff --git a/src/lib/sensor_calibration/Accelerometer.hpp b/src/lib/sensor_calibration/Accelerometer.hpp
index e22b58ab09..e1911431ec 100644
--- a/src/lib/sensor_calibration/Accelerometer.hpp
+++ b/src/lib/sensor_calibration/Accelerometer.hpp
@@ -84,6 +84,11 @@ public:
 		return _rotation * matrix::Vector3f{(data - _thermal_offset - _offset).emult(_scale)};
 	}
 
+	inline matrix::Vector3f Uncorrect(const matrix::Vector3f &corrected_data) const
+	{
+		return (_rotation.I() * corrected_data).edivide(_scale) + _thermal_offset + _offset;
+	}
+
 	// Compute sensor offset from bias (board frame)
 	matrix::Vector3f BiasCorrectedSensorOffset(const matrix::Vector3f &bias) const
 	{
diff --git a/src/lib/sensor_calibration/Utilities.cpp b/src/lib/sensor_calibration/Utilities.cpp
index dd3f2d8cb2..01c6d703ff 100644
--- a/src/lib/sensor_calibration/Utilities.cpp
+++ b/src/lib/sensor_calibration/Utilities.cpp
@@ -1,6 +1,6 @@
 /****************************************************************************
  *
- *   Copyright (c) 2020-2021 PX4 Development Team. All rights reserved.
+ *   Copyright (c) 2020-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
@@ -31,6 +31,8 @@
  *
  ****************************************************************************/
 
+#include "Utilities.hpp"
+
 #include <px4_platform_common/px4_config.h>
 #include <px4_platform_common/log.h>
 #include <lib/conversion/rotation.h>
@@ -284,4 +286,64 @@ bool DeviceExternal(uint32_t device_id)
 	return external;
 }
 
+matrix::Vector3f AverageFifoGyro(const sensor_imu_fifo_s &sensor_imu_fifo)
+{
+	if (sensor_imu_fifo.samples > 0 && sensor_imu_fifo.samples <= sensor_imu_fifo_s::FIFO_SIZE) {
+		// X axis
+		int32_t x = 0;
+
+		for (int i = 0; i < sensor_imu_fifo.samples; i++) {
+			x += sensor_imu_fifo.gyro_x[i];
+		}
+
+		// Y axis
+		int32_t y = 0;
+
+		for (int i = 0; i < sensor_imu_fifo.samples; i++) {
+			y += sensor_imu_fifo.gyro_y[i];
+		}
+
+		// Z axis
+		int32_t z = 0;
+
+		for (int i = 0; i < sensor_imu_fifo.samples; i++) {
+			z += sensor_imu_fifo.gyro_z[i];
+		}
+
+		return (matrix::Vector3f{(float)x, (float)y, (float)z} * sensor_imu_fifo.accel_scale) / sensor_imu_fifo.samples;
+	}
+
+	return matrix::Vector3f{0, 0, 0};
+}
+
+matrix::Vector3f AverageFifoAccel(const sensor_imu_fifo_s &sensor_imu_fifo)
+{
+	if (sensor_imu_fifo.samples > 0 && sensor_imu_fifo.samples <= sensor_imu_fifo_s::FIFO_SIZE) {
+		// X axis
+		int32_t x = 0;
+
+		for (int i = 0; i < sensor_imu_fifo.samples; i++) {
+			x += sensor_imu_fifo.accel_x[i];
+		}
+
+		// Y axis
+		int32_t y = 0;
+
+		for (int i = 0; i < sensor_imu_fifo.samples; i++) {
+			y += sensor_imu_fifo.accel_y[i];
+		}
+
+		// Z axis
+		int32_t z = 0;
+
+		for (int i = 0; i < sensor_imu_fifo.samples; i++) {
+			z += sensor_imu_fifo.accel_z[i];
+		}
+
+		return (matrix::Vector3f{(float)x, (float)y, (float)z} * sensor_imu_fifo.gyro_scale) / sensor_imu_fifo.samples;
+	}
+
+	return matrix::Vector3f{0, 0, 0};
+}
+
 } // namespace calibration
diff --git a/src/lib/sensor_calibration/Utilities.hpp b/src/lib/sensor_calibration/Utilities.hpp
index 4696f01f36..72aeb9b426 100644
--- a/src/lib/sensor_calibration/Utilities.hpp
+++ b/src/lib/sensor_calibration/Utilities.hpp
@@ -1,6 +1,6 @@
 /****************************************************************************
  *
- *   Copyright (c) 2020 PX4 Development Team. All rights reserved.
+ *   Copyright (c) 2020-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
@@ -40,6 +40,8 @@
 #include <lib/parameters/param.h>
 #include <matrix/math.hpp>
 
+#include <uORB/topics/sensor_imu_fifo.h>
+
 namespace calibration
 {
 
@@ -149,4 +151,7 @@ matrix::Dcmf GetBoardRotationMatrix();
  */
 bool DeviceExternal(uint32_t device_id);
 
+matrix::Vector3f AverageFifoGyro(const sensor_imu_fifo_s &sensor_imu_fifo);
+matrix::Vector3f AverageFifoAccel(const sensor_imu_fifo_s &sensor_imu_fifo);
+
 } // namespace calibration
diff --git a/src/modules/commander/gyro_calibration.cpp b/src/modules/commander/gyro_calibration.cpp
index a32a0511cf..840dffb2e4 100644
--- a/src/modules/commander/gyro_calibration.cpp
+++ b/src/modules/commander/gyro_calibration.cpp
@@ -56,8 +56,9 @@
 #include <lib/sensor_calibration/Utilities.hpp>
 #include <lib/systemlib/mavlink_log.h>
 #include <uORB/Subscription.hpp>
-#include <uORB/SubscriptionBlocking.hpp>
 #include <uORB/topics/sensor_gyro.h>
+#include <uORB/topics/sensor_imu_fifo.h>
+#include <uORB/SubscriptionMultiArray.hpp>
 
 static constexpr char sensor_name[] {"gyro"};
 static constexpr unsigned MAX_GYROS = 4;
@@ -82,12 +83,8 @@ static calibrate_return gyro_calibration_worker(gyro_worker_data_t &worker_data)
 	static constexpr unsigned CALIBRATION_COUNT = 250;
 	unsigned poll_errcount = 0;
 
-	uORB::SubscriptionBlocking<sensor_gyro_s> gyro_sub[MAX_GYROS] {
-		{ORB_ID(sensor_gyro), 0, 0},
-		{ORB_ID(sensor_gyro), 0, 1},
-		{ORB_ID(sensor_gyro), 0, 2},
-		{ORB_ID(sensor_gyro), 0, 3},
-	};
+	uORB::SubscriptionMultiArray<sensor_gyro_s, MAX_GYROS> sensor_gyro_subs{ORB_ID::sensor_gyro};
+	uORB::SubscriptionMultiArray<sensor_imu_fifo_s, MAX_GYROS> sensor_imu_fifo_subs{ORB_ID::sensor_imu_fifo};
 
 	/* use slowest gyro to pace, but count correctly per-gyro for statistics */
 	unsigned slow_count = 0;
@@ -97,39 +94,75 @@ static calibrate_return gyro_calibration_worker(gyro_worker_data_t &worker_data)
 			return calibrate_return_cancelled;
 		}
 
-		if (gyro_sub[0].updatedBlocking(100000)) {
+		if (sensor_gyro_subs.updated() || sensor_imu_fifo_subs.updated()) {
 			unsigned update_count = CALIBRATION_COUNT;
 
-			for (unsigned gyro_index = 0; gyro_index < MAX_GYROS; gyro_index++) {
-				if (worker_data.calibrations[gyro_index].device_id() != 0) {
+			int total_gyros = 0;
 
-					if (calibration_counter[gyro_index] >= CALIBRATION_COUNT) {
-						// Skip if instance has enough samples
-						continue;
-					}
+			for (unsigned imu_fifo_index = 0; imu_fifo_index < sensor_imu_fifo_subs.size(); imu_fifo_index++) {
+				if ((worker_data.calibrations[total_gyros].device_id() != 0)
+				    && (calibration_counter[total_gyros] < CALIBRATION_COUNT)) {
 
-					sensor_gyro_s gyro_report;
+					sensor_imu_fifo_s sensor_imu_fifo;
 
-					while (gyro_sub[gyro_index].update(&gyro_report)) {
+					while (sensor_imu_fifo_subs[imu_fifo_index].update(&sensor_imu_fifo)) {
 						// fetch optional thermal offset corrections in sensor frame
-						const Vector3f &thermal_offset{worker_data.calibrations[gyro_index].thermal_offset()};
+						const Vector3f &thermal_offset{worker_data.calibrations[total_gyros].thermal_offset()};
 
-						worker_data.offset[gyro_index] += Vector3f{gyro_report.x, gyro_report.y, gyro_report.z} - thermal_offset;
+						const Vector3f gyro_sample = calibration::AverageFifoGyro(sensor_imu_fifo);
 
-						calibration_counter[gyro_index]++;
+						worker_data.offset[total_gyros] += gyro_sample - thermal_offset;
 
-						if (gyro_index == 0) {
-							worker_data.filter[0].insert(gyro_report.x - thermal_offset(0));
-							worker_data.filter[1].insert(gyro_report.y - thermal_offset(1));
-							worker_data.filter[2].insert(gyro_report.z - thermal_offset(2));
+						calibration_counter[total_gyros]++;
+
+						if (total_gyros == 0) {
+							worker_data.filter[0].insert(gyro_sample(0) - thermal_offset(0));
+							worker_data.filter[1].insert(gyro_sample(1) - thermal_offset(1));
+							worker_data.filter[2].insert(gyro_sample(2) - thermal_offset(2));
 						}
 					}
 
 					// Maintain the sample count of the slowest sensor
-					if (calibration_counter[gyro_index] && calibration_counter[gyro_index] < update_count) {
-						update_count = calibration_counter[gyro_index];
+					if (calibration_counter[total_gyros] && calibration_counter[total_gyros] < update_count) {
+						update_count = calibration_counter[total_gyros];
 					}
 				}
+
+				if (sensor_imu_fifo_subs[imu_fifo_index].advertised()) {
+					total_gyros++;
+				}
+			}
+
+			for (unsigned gyro_index = 0; gyro_index < sensor_gyro_subs.size(); gyro_index++) {
+				if ((worker_data.calibrations[total_gyros].device_id() != 0)
+				    && (calibration_counter[total_gyros] < CALIBRATION_COUNT)) {
+
+					sensor_gyro_s sensor_gyro;
+
+					while (sensor_gyro_subs[gyro_index].update(&sensor_gyro)) {
+						// fetch optional thermal offset corrections in sensor frame
+						const Vector3f &thermal_offset{worker_data.calibrations[total_gyros].thermal_offset()};
+
+						worker_data.offset[total_gyros] += Vector3f{sensor_gyro.x, sensor_gyro.y, sensor_gyro.z} - thermal_offset;
+
+						calibration_counter[total_gyros]++;
+
+						if (total_gyros == 0) {
+							worker_data.filter[0].insert(sensor_gyro.x - thermal_offset(0));
+							worker_data.filter[1].insert(sensor_gyro.y - thermal_offset(1));
+							worker_data.filter[2].insert(sensor_gyro.z - thermal_offset(2));
+						}
+					}
+
+					// Maintain the sample count of the slowest sensor
+					if (calibration_counter[total_gyros] && calibration_counter[total_gyros] < update_count) {
+						update_count = calibration_counter[total_gyros];
+					}
+				}
+
+				if (sensor_gyro_subs[gyro_index].advertised()) {
+					total_gyros++;
+				}
 			}
 
 			const unsigned progress = (update_count * 100) / CALIBRATION_COUNT;
@@ -143,6 +176,8 @@ static calibrate_return gyro_calibration_worker(gyro_worker_data_t &worker_data)
 				slow_count = update_count;
 			}
 
+			px4_usleep(10'000);
+
 		} else {
 			poll_errcount++;
 		}
@@ -153,13 +188,13 @@ static calibrate_return gyro_calibration_worker(gyro_worker_data_t &worker_data)
 		}
 	}
 
-	for (unsigned s = 0; s < MAX_GYROS; s++) {
-		if ((worker_data.calibrations[s].device_id() != 0) && (calibration_counter[s] < CALIBRATION_COUNT / 2)) {
-			calibration_log_critical(worker_data.mavlink_log_pub, "ERROR: missing data, sensor %d", s)
+	for (unsigned i = 0; i < MAX_GYROS; i++) {
+		if ((worker_data.calibrations[i].device_id() != 0) && (calibration_counter[i] < CALIBRATION_COUNT / 2)) {
+			calibration_log_critical(worker_data.mavlink_log_pub, "ERROR: missing data, sensor %d", i)
 			return calibrate_return_error;
 		}
 
-		worker_data.offset[s] /= calibration_counter[s];
+		worker_data.offset[i] /= calibration_counter[i];
 	}
 
 	return calibrate_return_ok;
@@ -175,7 +210,7 @@ int do_gyro_calibration(orb_advert_t *mavlink_log_pub)
 	worker_data.mavlink_log_pub = mavlink_log_pub;
 
 	// We should not try to subscribe if the topic doesn't actually exist and can be counted.
-	const unsigned orb_gyro_count = orb_group_count(ORB_ID(sensor_gyro));
+	const unsigned orb_gyro_count = orb_group_count(ORB_ID(sensor_imu_fifo)) + orb_group_count(ORB_ID(sensor_gyro));
 
 	// Warn that we will not calibrate more than MAX_GYROS gyroscopes
 	if (orb_gyro_count > MAX_GYROS) {
@@ -186,11 +221,27 @@ int do_gyro_calibration(orb_advert_t *mavlink_log_pub)
 		return PX4_ERROR;
 	}
 
-	for (uint8_t cur_gyro = 0; cur_gyro < MAX_GYROS; cur_gyro++) {
-		uORB::SubscriptionData<sensor_gyro_s> gyro_sub{ORB_ID(sensor_gyro), cur_gyro};
+	int total_gyros = 0;
 
-		if (gyro_sub.advertised() && (gyro_sub.get().device_id != 0) && (gyro_sub.get().timestamp > 0)) {
-			worker_data.calibrations[cur_gyro].set_device_id(gyro_sub.get().device_id);
+	uORB::SubscriptionMultiArray<sensor_imu_fifo_s, MAX_GYROS> sensor_imu_fifo_subs{ORB_ID::sensor_imu_fifo};
+
+	for (uint8_t i = 0; i < sensor_imu_fifo_subs.size(); i++) {
+		sensor_imu_fifo_s sensor_imu_fifo;
+
+		if (sensor_imu_fifo_subs[i].update(&sensor_imu_fifo) && sensor_imu_fifo.device_id != 0) {
+			worker_data.calibrations[total_gyros].set_device_id(sensor_imu_fifo.device_id);
+			total_gyros++;
+		}
+	}
+
+	uORB::SubscriptionMultiArray<sensor_gyro_s, MAX_GYROS> sensor_gyro_subs{ORB_ID::sensor_gyro};
+
+	for (uint8_t i = 0; i < sensor_gyro_subs.size(); i++) {
+		sensor_gyro_s sensor_gyro;
+
+		if (sensor_gyro_subs[i].update(&sensor_gyro) && sensor_gyro.device_id != 0) {
+			worker_data.calibrations[total_gyros].set_device_id(sensor_gyro.device_id);
+			total_gyros++;
 		}
 	}
 
@@ -253,15 +304,15 @@ int do_gyro_calibration(orb_advert_t *mavlink_log_pub)
 		bool param_save = false;
 		bool failed = true;
 
-		for (unsigned uorb_index = 0; uorb_index < MAX_GYROS; uorb_index++) {
+		for (unsigned i = 0; i < MAX_GYROS; i++) {
 
-			auto &calibration = worker_data.calibrations[uorb_index];
+			auto &calibration = worker_data.calibrations[i];
 
 			if (calibration.device_id() != 0) {
-				calibration.set_offset(worker_data.offset[uorb_index]);
+				calibration.set_offset(worker_data.offset[i]);
 				calibration.PrintStatus();
 
-				if (calibration.ParametersSave(uorb_index, true)) {
+				if (calibration.ParametersSave(i, true)) {
 					param_save = true;
 					failed = false;
 
diff --git a/src/modules/gyro_calibration/GyroCalibration.cpp b/src/modules/gyro_calibration/GyroCalibration.cpp
index a832b3bfba..cbf401f64a 100644
--- a/src/modules/gyro_calibration/GyroCalibration.cpp
+++ b/src/modules/gyro_calibration/GyroCalibration.cpp
@@ -34,6 +34,7 @@
 #include "GyroCalibration.hpp"
 
 #include <lib/geo/geo.h>
+#include <lib/sensor_calibration/Utilities.hpp>
 
 using namespace time_literals;
 using matrix::Vector3f;
@@ -131,60 +132,132 @@ void GyroCalibration::Run()
 		return;
 	}
 
+	int total_accels = 0;
+	int total_gyros = 0;
 
 	// collect raw data from all available gyroscopes (sensor_gyro)
-	for (int gyro = 0; gyro < _sensor_gyro_subs.size(); gyro++) {
-		sensor_gyro_s sensor_gyro;
+	for (int imu_fifo_instance = 0; imu_fifo_instance < _sensor_imu_fifo_subs.size(); imu_fifo_instance++) {
+		sensor_imu_fifo_s sensor_imu_fifo;
 
-		while (_sensor_gyro_subs[gyro].update(&sensor_gyro)) {
-			if (PX4_ISFINITE(sensor_gyro.temperature)) {
-				if ((fabsf(_temperature[gyro] - sensor_gyro.temperature) > 1.f) || !PX4_ISFINITE(_temperature[gyro])) {
-					PX4_DEBUG("gyro %d temperature change, resetting all %.6f -> %.6f", gyro, (double)_temperature[gyro],
-						  (double)sensor_gyro.temperature);
+		while (_sensor_imu_fifo_subs[imu_fifo_instance].update(&sensor_imu_fifo)) {
+			if (PX4_ISFINITE(sensor_imu_fifo.temperature)) {
+				if ((fabsf(_temperature[total_gyros] - sensor_imu_fifo.temperature) > 1.f)
+				    || !PX4_ISFINITE(_temperature[total_gyros])) {
+					PX4_DEBUG("IMU %d temperature change, resetting all %.6f -> %.6f", total_gyros, (double)_temperature[total_gyros],
+						  (double)sensor_imu_fifo.temperature);
 
-					_temperature[gyro] = sensor_gyro.temperature;
+					_temperature[total_gyros] = sensor_imu_fifo.temperature;
 
 					// reset all on any temperature change
 					Reset();
 				}
 
 			} else {
-				_temperature[gyro] = NAN;
+				_temperature[total_gyros] = NAN;
 			}
 
-			if (_gyro_calibration[gyro].device_id() == sensor_gyro.device_id) {
-				const Vector3f val{Vector3f{sensor_gyro.x, sensor_gyro.y, sensor_gyro.z} - _gyro_calibration[gyro].thermal_offset()};
-				_gyro_mean[gyro].update(val);
-				_gyro_last_update[gyro] = sensor_gyro.timestamp;
+			if (_gyro_calibration[total_gyros].device_id() == sensor_imu_fifo.device_id && sensor_imu_fifo.samples > 0) {
+
+				Vector3f accel_val = calibration::AverageFifoAccel(sensor_imu_fifo);
+				Vector3f gyro_val = calibration::AverageFifoGyro(sensor_imu_fifo);
+
+				_gyro_mean[total_gyros].update(gyro_val);
+				_gyro_last_update[total_gyros] = sensor_imu_fifo.timestamp;
+
+				// check accelerometer for possible movement
+				if ((accel_val - _acceleration[total_accels]).longerThan(0.5f)) {
+					// reset all on any change
+					PX4_DEBUG("accel %d changed, resetting all %.5f", total_accels,
+						  (double)(accel_val - _acceleration[total_accels]).length());
+
+					_acceleration[total_accels] = accel_val;
+					Reset();
+					return;
+
+				} else if (accel_val.longerThan(CONSTANTS_ONE_G * 1.3f)) {
+					Reset();
+					return;
+				}
 
 			} else {
 				// setting device id, reset all
-				_gyro_calibration[gyro].set_device_id(sensor_gyro.device_id);
+				_gyro_calibration[total_gyros].set_device_id(sensor_imu_fifo.device_id);
 				Reset();
 			}
 		}
 
-		if ((_gyro_last_update[gyro] != 0) && (hrt_elapsed_time(&_gyro_last_update[gyro]) > 100_ms)) {
+		if ((_gyro_last_update[total_gyros] != 0) && (hrt_elapsed_time(&_gyro_last_update[total_gyros]) > 100_ms)) {
 			// reset on any timeout
 			Reset();
-			_gyro_last_update[gyro] = 0;
+			_gyro_last_update[total_gyros] = 0;
 			return;
 		}
+
+		if (_sensor_imu_fifo_subs[imu_fifo_instance].advertised()) {
+			total_accels++;
+			total_gyros++;
+		}
+	}
+
+
+	// collect raw data from all available gyroscopes (sensor_gyro)
+	for (int gyro_instance = 0; gyro_instance < _sensor_gyro_subs.size(); gyro_instance++) {
+		sensor_gyro_s sensor_gyro;
+
+		while (_sensor_gyro_subs[gyro_instance].update(&sensor_gyro)) {
+			if (PX4_ISFINITE(sensor_gyro.temperature)) {
+				if ((fabsf(_temperature[total_gyros] - sensor_gyro.temperature) > 1.f) || !PX4_ISFINITE(_temperature[total_gyros])) {
+					PX4_DEBUG("gyro %d temperature change, resetting all %.6f -> %.6f", total_gyros, (double)_temperature[total_gyros],
+						  (double)sensor_gyro.temperature);
+
+					_temperature[total_gyros] = sensor_gyro.temperature;
+
+					// reset all on any temperature change
+					Reset();
+				}
+
+			} else {
+				_temperature[total_gyros] = NAN;
+			}
+
+			if (_gyro_calibration[total_gyros].device_id() == sensor_gyro.device_id) {
+				const Vector3f val{Vector3f{sensor_gyro.x, sensor_gyro.y, sensor_gyro.z} - _gyro_calibration[total_gyros].thermal_offset()};
+				_gyro_mean[total_gyros].update(val);
+				_gyro_last_update[total_gyros] = sensor_gyro.timestamp;
+
+			} else {
+				// setting device id, reset all
+				_gyro_calibration[total_gyros].set_device_id(sensor_gyro.device_id);
+				Reset();
+			}
+		}
+
+		if ((_gyro_last_update[total_gyros] != 0) && (hrt_elapsed_time(&_gyro_last_update[total_gyros]) > 100_ms)) {
+			// reset on any timeout
+			Reset();
+			_gyro_last_update[total_gyros] = 0;
+			return;
+		}
+
+		if (_sensor_gyro_subs[gyro_instance].advertised()) {
+			total_gyros++;
+		}
 	}
 
 
 	// check all accelerometers for possible movement
-	for (int accel = 0; accel < _sensor_accel_subs.size(); accel++) {
+	for (int accel_instance = 0; accel_instance < _sensor_accel_subs.size(); accel_instance++) {
 		sensor_accel_s sensor_accel;
 
-		if (_sensor_accel_subs[accel].update(&sensor_accel)) {
+		if (_sensor_accel_subs[accel_instance].update(&sensor_accel)) {
 			const Vector3f acceleration{sensor_accel.x, sensor_accel.y, sensor_accel.z};
 
-			if ((acceleration - _acceleration[accel]).longerThan(0.5f)) {
+			if ((acceleration - _acceleration[total_accels]).longerThan(0.5f)) {
 				// reset all on any change
-				PX4_DEBUG("accel %d changed, resetting all %.5f", accel, (double)(acceleration - _acceleration[accel]).length());
+				PX4_DEBUG("accel %d changed, resetting all %.5f", total_accels,
+					  (double)(acceleration - _acceleration[total_accels]).length());
 
-				_acceleration[accel] = acceleration;
+				_acceleration[total_accels] = acceleration;
 				Reset();
 				return;
 
@@ -193,30 +266,32 @@ void GyroCalibration::Run()
 				return;
 			}
 		}
-	}
 
+		if (_sensor_accel_subs[accel_instance].advertised()) {
+			total_accels++;
+		}
+	}
 
 	// check if sufficient data has been gathered to update calibration
 	bool sufficient_samples = false;
 
-	for (int gyro = 0; gyro < _sensor_gyro_subs.size(); gyro++) {
-		if ((_gyro_calibration[gyro].device_id() != 0) && _gyro_mean[gyro].valid()) {
+	for (int i = 0; i < total_gyros; i++) {
+		if ((_gyro_calibration[i].device_id() != 0) && _gyro_mean[i].valid()) {
 			// periodically check variance
-			if (_gyro_mean[gyro].count() % 100 == 0) {
-				PX4_DEBUG("gyro %d (%" PRIu32 ") variance, [%.9f, %.9f, %.9f] %.9f", gyro, _gyro_calibration[gyro].device_id(),
-					  (double)_gyro_mean[gyro].variance()(0), (double)_gyro_mean[gyro].variance()(1), (double)_gyro_mean[gyro].variance()(2),
-					  (double)_gyro_mean[gyro].variance().length());
+			if (_gyro_mean[i].count() % 100 == 0) {
+				PX4_DEBUG("gyro %d (%" PRIu32 ") variance, [%.9f, %.9f, %.9f] %.9f", i, _gyro_calibration[i].device_id(),
+					  (double)_gyro_mean[i].variance()(0), (double)_gyro_mean[i].variance()(1), (double)_gyro_mean[i].variance()(2),
+					  (double)_gyro_mean[i].variance().length());
 
-				if (_gyro_mean[gyro].variance().longerThan(0.001f)) {
+				if (_gyro_mean[i].variance().longerThan(0.001f)) {
 					// reset all
-					PX4_DEBUG("gyro %d variance longer than 0.001f (%.3f), resetting all",
-						  gyro, (double)_gyro_mean[gyro].variance().length());
+					PX4_DEBUG("gyro %d variance longer than 0.001f (%.3f), resetting all", i, (double)_gyro_mean[i].variance().length());
 					Reset();
 					return;
 				}
 			}
 
-			if (_gyro_mean[gyro].count() > 5000) {
+			if (_gyro_mean[i].count() > 5000) {
 				sufficient_samples = true;
 
 			} else {
@@ -231,16 +306,15 @@ void GyroCalibration::Run()
 	if (sufficient_samples && (hrt_elapsed_time(&_last_calibration_update) > 10_s)) {
 		bool calibration_updated = false;
 
-		for (int gyro = 0; gyro < _sensor_gyro_subs.size(); gyro++) {
+		for (int gyro = 0; gyro < total_gyros; gyro++) {
 			if (_gyro_calibration[gyro].device_id() != 0 && _gyro_mean[gyro].valid()) {
 
 				// check variance again before saving
 				if (_gyro_mean[gyro].variance().longerThan(0.001f)) {
 					// reset all
-					PX4_DEBUG("gyro %d variance longer than 0.001f (%.3f), resetting all",
+					PX4_DEBUG("gyro %d variance longer than 0.001f (%.6f), resetting all",
 						  gyro, (double)_gyro_mean[gyro].variance().length());
-					Reset();
-					return;
+					break;
 				}
 
 				const Vector3f old_offset{_gyro_calibration[gyro].offset()};
@@ -254,8 +328,6 @@ void GyroCalibration::Run()
 						 (double)old_offset(0), (double)old_offset(1), (double)old_offset(2),
 						 (double)_gyro_mean[gyro].mean()(0), (double)_gyro_mean[gyro].mean()(1), (double)_gyro_mean[gyro].mean()(2),
 						 (double)_temperature[gyro]);
-
-					perf_count(_calibration_updated_perf);
 				}
 			}
 		}
@@ -264,7 +336,7 @@ void GyroCalibration::Run()
 		if (calibration_updated) {
 			bool param_save = false;
 
-			for (int gyro = 0; gyro < _sensor_gyro_subs.size(); gyro++) {
+			for (int gyro = 0; gyro < total_gyros; gyro++) {
 				if (_gyro_calibration[gyro].device_id() != 0) {
 					if (_gyro_calibration[gyro].ParametersSave(gyro)) {
 						param_save = true;
@@ -275,6 +347,7 @@ void GyroCalibration::Run()
 			if (param_save) {
 				param_notify_changes();
 				_last_calibration_update = hrt_absolute_time();
+				perf_count(_calibration_updated_perf);
 			}
 		}
 
diff --git a/src/modules/gyro_calibration/GyroCalibration.hpp b/src/modules/gyro_calibration/GyroCalibration.hpp
index c12d24f8bc..daa380033d 100644
--- a/src/modules/gyro_calibration/GyroCalibration.hpp
+++ b/src/modules/gyro_calibration/GyroCalibration.hpp
@@ -48,6 +48,7 @@
 #include <uORB/topics/parameter_update.h>
 #include <uORB/topics/sensor_accel.h>
 #include <uORB/topics/sensor_gyro.h>
+#include <uORB/topics/sensor_imu_fifo.h>
 #include <uORB/topics/vehicle_status.h>
 #include <uORB/topics/vehicle_status_flags.h>
 
@@ -91,6 +92,7 @@ private:
 
 	uORB::SubscriptionMultiArray<sensor_accel_s, MAX_SENSORS> _sensor_accel_subs{ORB_ID::sensor_accel};
 	uORB::SubscriptionMultiArray<sensor_gyro_s, MAX_SENSORS>  _sensor_gyro_subs{ORB_ID::sensor_gyro};
+	uORB::SubscriptionMultiArray<sensor_imu_fifo_s, MAX_SENSORS> _sensor_imu_fifo_subs{ORB_ID::sensor_imu_fifo};
 
 	calibration::Gyroscope _gyro_calibration[MAX_SENSORS] {};
 	math::WelfordMean<matrix::Vector3f> _gyro_mean[MAX_SENSORS] {};
diff --git a/src/modules/gyro_fft/GyroFFT.cpp b/src/modules/gyro_fft/GyroFFT.cpp
index 51818c3425..6fe899ab51 100644
--- a/src/modules/gyro_fft/GyroFFT.cpp
+++ b/src/modules/gyro_fft/GyroFFT.cpp
@@ -172,14 +172,14 @@ bool GyroFFT::SensorSelectionUpdate(bool force)
 		_sensor_selection_sub.copy(&sensor_selection);
 
 		if ((sensor_selection.gyro_device_id != 0) && (_selected_sensor_device_id != sensor_selection.gyro_device_id)) {
-			// prefer sensor_gyro_fifo if available
+			// prefer sensor_imu_fifo if available
 			for (uint8_t i = 0; i < MAX_SENSOR_COUNT; i++) {
-				uORB::SubscriptionData<sensor_gyro_fifo_s> sensor_gyro_fifo_sub{ORB_ID(sensor_gyro_fifo), i};
+				uORB::SubscriptionData<sensor_imu_fifo_s> sensor_imu_fifo_sub{ORB_ID(sensor_imu_fifo), i};
 
-				if (sensor_gyro_fifo_sub.get().device_id == sensor_selection.gyro_device_id) {
-					if (_sensor_gyro_fifo_sub.ChangeInstance(i) && _sensor_gyro_fifo_sub.registerCallback()) {
+				if (sensor_imu_fifo_sub.get().device_id == sensor_selection.gyro_device_id) {
+					if (_sensor_imu_fifo_sub.ChangeInstance(i) && _sensor_imu_fifo_sub.registerCallback()) {
 						_sensor_gyro_sub.unregisterCallback();
-						_sensor_gyro_fifo_sub.set_required_updates(sensor_gyro_fifo_s::ORB_QUEUE_LENGTH / 2);
+						_sensor_imu_fifo_sub.set_required_updates(sensor_imu_fifo_s::ORB_QUEUE_LENGTH / 2);
 						_selected_sensor_device_id = sensor_selection.gyro_device_id;
 						_gyro_fifo = true;
 
@@ -198,7 +198,7 @@ bool GyroFFT::SensorSelectionUpdate(bool force)
 
 				if (sensor_gyro_sub.get().device_id == sensor_selection.gyro_device_id) {
 					if (_sensor_gyro_sub.ChangeInstance(i) && _sensor_gyro_sub.registerCallback()) {
-						_sensor_gyro_fifo_sub.unregisterCallback();
+						_sensor_imu_fifo_sub.unregisterCallback();
 						_sensor_gyro_sub.set_required_updates(sensor_gyro_s::ORB_QUEUE_LENGTH / 2);
 						_selected_sensor_device_id = sensor_selection.gyro_device_id;
 						_gyro_fifo = false;
@@ -303,7 +303,7 @@ void GyroFFT::Run()
 {
 	if (should_exit()) {
 		_sensor_gyro_sub.unregisterCallback();
-		_sensor_gyro_fifo_sub.unregisterCallback();
+		_sensor_imu_fifo_sub.unregisterCallback();
 		exit_and_cleanup();
 		return;
 	}
@@ -331,10 +331,10 @@ void GyroFFT::Run()
 
 	if (_gyro_fifo) {
 		// run on sensor gyro fifo updates
-		sensor_gyro_fifo_s sensor_gyro_fifo;
+		sensor_imu_fifo_s sensor_imu_fifo;
 
-		while (_sensor_gyro_fifo_sub.update(&sensor_gyro_fifo)) {
-			if (_sensor_gyro_fifo_sub.get_last_generation() != _gyro_last_generation + 1) {
+		while (_sensor_imu_fifo_sub.update(&sensor_imu_fifo)) {
+			if (_sensor_imu_fifo_sub.get_last_generation() != _gyro_last_generation + 1) {
 				// force reset if we've missed a sample
 				_fft_buffer_index[0] = 0;
 				_fft_buffer_index[1] = 0;
@@ -343,19 +343,19 @@ void GyroFFT::Run()
 				perf_count(_gyro_fifo_generation_gap_perf);
 			}
 
-			_gyro_last_generation = _sensor_gyro_fifo_sub.get_last_generation();
+			_gyro_last_generation = _sensor_imu_fifo_sub.get_last_generation();
 
-			if (fabsf(sensor_gyro_fifo.scale - _fifo_last_scale) > FLT_EPSILON) {
+			if (fabsf(sensor_imu_fifo.gyro_scale - _fifo_last_scale) > FLT_EPSILON) {
 				// force reset if scale has changed
 				_fft_buffer_index[0] = 0;
 				_fft_buffer_index[1] = 0;
 				_fft_buffer_index[2] = 0;
 
-				_fifo_last_scale = sensor_gyro_fifo.scale;
+				_fifo_last_scale = sensor_imu_fifo.gyro_scale;
 			}
 
-			int16_t *input[] {sensor_gyro_fifo.x, sensor_gyro_fifo.y, sensor_gyro_fifo.z};
-			Update(sensor_gyro_fifo.timestamp_sample, input, sensor_gyro_fifo.samples);
+			int16_t *input[] {sensor_imu_fifo.gyro_x, sensor_imu_fifo.gyro_y, sensor_imu_fifo.gyro_z};
+			Update(sensor_imu_fifo.timestamp_sample, input, sensor_imu_fifo.samples);
 		}
 
 	} else {
diff --git a/src/modules/gyro_fft/GyroFFT.hpp b/src/modules/gyro_fft/GyroFFT.hpp
index 1edfe2ff5e..30d442a8af 100644
--- a/src/modules/gyro_fft/GyroFFT.hpp
+++ b/src/modules/gyro_fft/GyroFFT.hpp
@@ -49,7 +49,7 @@
 #include <uORB/topics/parameter_update.h>
 #include <uORB/topics/sensor_gyro.h>
 #include <uORB/topics/sensor_gyro_fft.h>
-#include <uORB/topics/sensor_gyro_fifo.h>
+#include <uORB/topics/sensor_imu_fifo.h>
 #include <uORB/topics/sensor_selection.h>
 #include <uORB/topics/vehicle_imu_status.h>
 
@@ -120,7 +120,7 @@ private:
 	uORB::Subscription _vehicle_imu_status_sub{ORB_ID(vehicle_imu_status)};
 
 	uORB::SubscriptionCallbackWorkItem _sensor_gyro_sub{this, ORB_ID(sensor_gyro)};
-	uORB::SubscriptionCallbackWorkItem _sensor_gyro_fifo_sub{this, ORB_ID(sensor_gyro_fifo)};
+	uORB::SubscriptionCallbackWorkItem _sensor_imu_fifo_sub{this, ORB_ID(sensor_imu_fifo)};
 
 	perf_counter_t _cycle_perf{perf_alloc(PC_ELAPSED, MODULE_NAME": cycle")};
 	perf_counter_t _cycle_interval_perf{perf_alloc(PC_INTERVAL, MODULE_NAME": cycle interval")};
diff --git a/src/modules/logger/logged_topics.cpp b/src/modules/logger/logged_topics.cpp
index a835ddfeba..6a23b3b7f9 100644
--- a/src/modules/logger/logged_topics.cpp
+++ b/src/modules/logger/logged_topics.cpp
@@ -352,12 +352,12 @@ void LoggedTopics::add_vision_and_avoidance_topics()
 
 void LoggedTopics::add_raw_imu_gyro_fifo()
 {
-	add_topic("sensor_gyro_fifo");
+	add_topic("sensor_imu_fifo");
 }
 
 void LoggedTopics::add_raw_imu_accel_fifo()
 {
-	add_topic("sensor_accel_fifo");
+	//add_topic("sensor_accel_fifo");
 }
 
 void LoggedTopics::add_system_identification_topics()
diff --git a/src/modules/mavlink/CMakeLists.txt b/src/modules/mavlink/CMakeLists.txt
index 196559d01f..060db3b97a 100644
--- a/src/modules/mavlink/CMakeLists.txt
+++ b/src/modules/mavlink/CMakeLists.txt
@@ -100,8 +100,6 @@ px4_add_module(
 	DEPENDS
 		airspeed
 		component_general_json # for checksums.h
-		drivers_accelerometer
-		drivers_gyroscope
 		drivers_magnetometer
 		conversion
 		sensor_calibration
diff --git a/src/modules/mavlink/mavlink_receiver.cpp b/src/modules/mavlink/mavlink_receiver.cpp
index 38efbdecdc..be5a834136 100644
--- a/src/modules/mavlink/mavlink_receiver.cpp
+++ b/src/modules/mavlink/mavlink_receiver.cpp
@@ -72,8 +72,6 @@
 MavlinkReceiver::~MavlinkReceiver()
 {
 	delete _tune_publisher;
-	delete _px4_accel;
-	delete _px4_gyro;
 	delete _px4_mag;
 #if !defined(CONSTRAINED_FLASH)
 	delete[] _received_msg_stats;
@@ -2313,7 +2311,7 @@ MavlinkReceiver::handle_message_hil_sensor(mavlink_message_t *msg)
 	mavlink_hil_sensor_t hil_sensor;
 	mavlink_msg_hil_sensor_decode(msg, &hil_sensor);
 
-	const uint64_t timestamp = hrt_absolute_time();
+	const uint64_t timestamp_sample = hrt_absolute_time();
 
 	// temperature only updated with baro
 	float temperature = NAN;
@@ -2324,34 +2322,30 @@ MavlinkReceiver::handle_message_hil_sensor(mavlink_message_t *msg)
 
 	// gyro
 	if ((hil_sensor.fields_updated & SensorSource::GYRO) == SensorSource::GYRO) {
-		if (_px4_gyro == nullptr) {
-			// 1310988: DRV_IMU_DEVTYPE_SIM, BUS: 1, ADDR: 1, TYPE: SIMULATION
-			_px4_gyro = new PX4Gyroscope(1310988);
-		}
-
-		if (_px4_gyro != nullptr) {
-			if (PX4_ISFINITE(temperature)) {
-				_px4_gyro->set_temperature(temperature);
-			}
-
-			_px4_gyro->update(timestamp, hil_sensor.xgyro, hil_sensor.ygyro, hil_sensor.zgyro);
-		}
+		sensor_gyro_s sensor_gyro{};
+		sensor_gyro.timestamp_sample = timestamp_sample;
+		sensor_gyro.device_id = 1310988; // 1310988: DRV_IMU_DEVTYPE_SIM, BUS: 1, ADDR: 1, TYPE: SIMULATION
+		sensor_gyro.x = hil_sensor.xgyro;
+		sensor_gyro.y = hil_sensor.ygyro;
+		sensor_gyro.z = hil_sensor.zgyro;
+		sensor_gyro.range = math::radians(2000.f); // TODO
+		sensor_gyro.temperature = temperature;
+		sensor_gyro.timestamp = hrt_absolute_time();
+		_sensor_gyro_pub.publish(sensor_gyro);
 	}
 
 	// accelerometer
 	if ((hil_sensor.fields_updated & SensorSource::ACCEL) == SensorSource::ACCEL) {
-		if (_px4_accel == nullptr) {
-			// 1310988: DRV_IMU_DEVTYPE_SIM, BUS: 1, ADDR: 1, TYPE: SIMULATION
-			_px4_accel = new PX4Accelerometer(1310988);
-		}
-
-		if (_px4_accel != nullptr) {
-			if (PX4_ISFINITE(temperature)) {
-				_px4_accel->set_temperature(temperature);
-			}
-
-			_px4_accel->update(timestamp, hil_sensor.xacc, hil_sensor.yacc, hil_sensor.zacc);
-		}
+		sensor_accel_s sensor_accel{};
+		sensor_accel.timestamp_sample = timestamp_sample;
+		sensor_accel.device_id = 1310988; // 1310988: DRV_IMU_DEVTYPE_SIM, BUS: 1, ADDR: 1, TYPE: SIMULATION
+		sensor_accel.x = hil_sensor.xacc;
+		sensor_accel.y = hil_sensor.yacc;
+		sensor_accel.z = hil_sensor.zacc;
+		sensor_accel.range = 16.f; // TODO
+		sensor_accel.temperature = temperature;
+		sensor_accel.timestamp = hrt_absolute_time();
+		_sensor_accel_pub.publish(sensor_accel);
 	}
 
 	// magnetometer
@@ -2366,7 +2360,7 @@ MavlinkReceiver::handle_message_hil_sensor(mavlink_message_t *msg)
 				_px4_mag->set_temperature(temperature);
 			}
 
-			_px4_mag->update(timestamp, hil_sensor.xmag, hil_sensor.ymag, hil_sensor.zmag);
+			_px4_mag->update(timestamp_sample, hil_sensor.xmag, hil_sensor.ymag, hil_sensor.zmag);
 		}
 	}
 
@@ -2374,7 +2368,7 @@ MavlinkReceiver::handle_message_hil_sensor(mavlink_message_t *msg)
 	if ((hil_sensor.fields_updated & SensorSource::BARO) == SensorSource::BARO) {
 		// publish
 		sensor_baro_s sensor_baro{};
-		sensor_baro.timestamp_sample = timestamp;
+		sensor_baro.timestamp_sample = timestamp_sample;
 		sensor_baro.device_id = 6620172; // 6620172: DRV_BARO_DEVTYPE_BAROSIM, BUS: 1, ADDR: 4, TYPE: SIMULATION
 		sensor_baro.pressure = hil_sensor.abs_pressure * 100.0f; // hPa to Pa
 		sensor_baro.temperature = hil_sensor.temperature;
@@ -2386,7 +2380,7 @@ MavlinkReceiver::handle_message_hil_sensor(mavlink_message_t *msg)
 	// differential pressure
 	if ((hil_sensor.fields_updated & SensorSource::DIFF_PRESS) == SensorSource::DIFF_PRESS) {
 		differential_pressure_s report{};
-		report.timestamp_sample = timestamp;
+		report.timestamp_sample = timestamp_sample;
 		report.device_id = 1377548; // 1377548: DRV_DIFF_PRESS_DEVTYPE_SIM, BUS: 1, ADDR: 5, TYPE: SIMULATION
 		report.temperature = hil_sensor.temperature;
 		report.differential_pressure_pa = hil_sensor.diff_pressure * 100.0f; // hPa to Pa
@@ -2398,7 +2392,7 @@ MavlinkReceiver::handle_message_hil_sensor(mavlink_message_t *msg)
 	{
 		battery_status_s hil_battery_status{};
 
-		hil_battery_status.timestamp = timestamp;
+		hil_battery_status.timestamp = timestamp_sample;
 		hil_battery_status.voltage_v = 16.0f;
 		hil_battery_status.voltage_filtered_v = 16.0f;
 		hil_battery_status.current_a = 10.0f;
@@ -2800,36 +2794,30 @@ MavlinkReceiver::handle_message_hil_state_quaternion(mavlink_message_t *msg)
 
 	/* accelerometer */
 	{
-		if (_px4_accel == nullptr) {
-			// 1310988: DRV_IMU_DEVTYPE_SIM, BUS: 1, ADDR: 1, TYPE: SIMULATION
-			_px4_accel = new PX4Accelerometer(1310988);
-
-			if (_px4_accel == nullptr) {
-				PX4_ERR("PX4Accelerometer alloc failed");
-			}
-		}
-
-		if (_px4_accel != nullptr) {
-			// accel in mG
-			_px4_accel->set_scale(CONSTANTS_ONE_G / 1000.0f);
-			_px4_accel->update(timestamp_sample, hil_state.xacc, hil_state.yacc, hil_state.zacc);
-		}
+		sensor_accel_s sensor_accel{};
+		sensor_accel.timestamp_sample = timestamp_sample;
+		sensor_accel.device_id = 1310988; // 1310988: DRV_IMU_DEVTYPE_SIM, BUS: 1, ADDR: 1, TYPE: SIMULATION
+		sensor_accel.x = hil_state.xacc;
+		sensor_accel.y = hil_state.yacc;
+		sensor_accel.z = hil_state.zacc;
+		sensor_accel.range = 16.f; // TODO
+		sensor_accel.temperature = NAN;
+		sensor_accel.timestamp = hrt_absolute_time();
+		_sensor_accel_pub.publish(sensor_accel);
 	}
 
 	/* gyroscope */
 	{
-		if (_px4_gyro == nullptr) {
-			// 1310988: DRV_IMU_DEVTYPE_SIM, BUS: 1, ADDR: 1, TYPE: SIMULATION
-			_px4_gyro = new PX4Gyroscope(1310988);
-
-			if (_px4_gyro == nullptr) {
-				PX4_ERR("PX4Gyroscope alloc failed");
-			}
-		}
-
-		if (_px4_gyro != nullptr) {
-			_px4_gyro->update(timestamp_sample, hil_state.rollspeed, hil_state.pitchspeed, hil_state.yawspeed);
-		}
+		sensor_gyro_s sensor_gyro{};
+		sensor_gyro.timestamp_sample = timestamp_sample;
+		sensor_gyro.device_id = 1310988; // 1310988: DRV_IMU_DEVTYPE_SIM, BUS: 1, ADDR: 1, TYPE: SIMULATION
+		sensor_gyro.x = hil_state.rollspeed;
+		sensor_gyro.y = hil_state.pitchspeed;
+		sensor_gyro.z = hil_state.yawspeed;
+		sensor_gyro.range = math::radians(2000.f); // TODO
+		sensor_gyro.temperature = NAN;
+		sensor_gyro.timestamp = hrt_absolute_time();
+		_sensor_gyro_pub.publish(sensor_gyro);
 	}
 
 	/* battery status */
diff --git a/src/modules/mavlink/mavlink_receiver.h b/src/modules/mavlink/mavlink_receiver.h
index ae652d0d56..1f4f690793 100644
--- a/src/modules/mavlink/mavlink_receiver.h
+++ b/src/modules/mavlink/mavlink_receiver.h
@@ -52,8 +52,6 @@
 #include "tune_publisher.h"
 
 #include <geo/geo.h>
-#include <lib/drivers/accelerometer/PX4Accelerometer.hpp>
-#include <lib/drivers/gyroscope/PX4Gyroscope.hpp>
 #include <lib/drivers/magnetometer/PX4Magnetometer.hpp>
 #include <lib/systemlib/mavlink_log.h>
 #include <px4_platform_common/module_params.h>
@@ -92,8 +90,10 @@
 #include <uORB/topics/position_setpoint_triplet.h>
 #include <uORB/topics/radio_status.h>
 #include <uORB/topics/rc_channels.h>
+#include <uORB/topics/sensor_accel.h>
 #include <uORB/topics/sensor_baro.h>
 #include <uORB/topics/sensor_gps.h>
+#include <uORB/topics/sensor_gyro.h>
 #include <uORB/topics/sensor_optical_flow.h>
 #include <uORB/topics/telemetry_status.h>
 #include <uORB/topics/transponder_report.h>
@@ -337,8 +337,10 @@ private:
 	uORB::PublicationMulti<manual_control_setpoint_s>	_manual_control_input_pub{ORB_ID(manual_control_input)};
 	uORB::PublicationMulti<ping_s>				_ping_pub{ORB_ID(ping)};
 	uORB::PublicationMulti<radio_status_s>			_radio_status_pub{ORB_ID(radio_status)};
+	uORB::PublicationMulti<sensor_accel_s>			_sensor_accel_pub{ORB_ID(sensor_accel)};
 	uORB::PublicationMulti<sensor_baro_s>			_sensor_baro_pub{ORB_ID(sensor_baro)};
 	uORB::PublicationMulti<sensor_gps_s>			_sensor_gps_pub{ORB_ID(sensor_gps)};
+	uORB::PublicationMulti<sensor_gyro_s>			_sensor_gyro_pub{ORB_ID(sensor_gyro)};
 	uORB::PublicationMulti<sensor_optical_flow_s>         _sensor_optical_flow_pub{ORB_ID(sensor_optical_flow)};
 
 	// ORB publications (queue length > 1)
@@ -365,8 +367,6 @@ private:
 		BARO		= 0b1101000000000,
 		DIFF_PRESS	= 0b10000000000
 	};
-	PX4Accelerometer *_px4_accel{nullptr};
-	PX4Gyroscope *_px4_gyro{nullptr};
 	PX4Magnetometer *_px4_mag{nullptr};
 
 	float _global_local_alt0{NAN};
diff --git a/src/modules/sensors/Integrator.hpp b/src/modules/sensors/Integrator.hpp
index b6c26f331e..ed99c9cdbc 100644
--- a/src/modules/sensors/Integrator.hpp
+++ b/src/modules/sensors/Integrator.hpp
@@ -90,6 +90,8 @@ public:
 	void set_reset_samples(uint8_t reset_samples) { _reset_samples_min = reset_samples; }
 	uint8_t get_reset_samples() const { return _reset_samples_min; }
 
+	uint8_t integrated_samples() const { return _integrated_samples; }
+
 	/**
 	 * Is the Integrator ready to reset?
 	 *
diff --git a/src/modules/sensors/sensors.cpp b/src/modules/sensors/sensors.cpp
index 6e77293580..2084d24c79 100644
--- a/src/modules/sensors/sensors.cpp
+++ b/src/modules/sensors/sensors.cpp
@@ -87,6 +87,7 @@ Sensors::Sensors(bool hil_enabled) :
 	parameters_update();
 
 	InitializeVehicleIMU();
+	InitializeVehicleIMUFifo();
 }
 
 Sensors::~Sensors()
@@ -147,6 +148,13 @@ Sensors::~Sensors()
 		}
 	}
 
+	for (auto &vehicle_imu_fifo : _vehicle_imu_fifo_list) {
+		if (vehicle_imu_fifo) {
+			vehicle_imu_fifo->Stop();
+			delete vehicle_imu_fifo;
+		}
+	}
+
 	perf_free(_loop_perf);
 }
 
@@ -462,12 +470,13 @@ void Sensors::InitializeVehicleIMU()
 				const bool multi_mode = (_param_sens_imu_mode.get() == 0);
 				const px4::wq_config_t &wq_config = multi_mode ? px4::ins_instance_to_wq(i) : px4::wq_configurations::INS0;
 
-				VehicleIMU *imu = new VehicleIMU(i, i, i, wq_config);
+				VehicleIMU *imu = new VehicleIMU(_total_imu_count, i, i, wq_config);
 
 				if (imu != nullptr) {
 					// Start VehicleIMU instance and store
 					if (imu->Start()) {
 						_vehicle_imu_list[i] = imu;
+						_total_imu_count++;
 
 					} else {
 						delete imu;
@@ -482,6 +491,42 @@ void Sensors::InitializeVehicleIMU()
 	}
 }
 
+void Sensors::InitializeVehicleIMUFifo()
+{
+	// create a VehicleIMU instance for each accel/gyro pair
+	for (uint8_t i = 0; i < MAX_SENSOR_COUNT; i++) {
+		if (_vehicle_imu_fifo_list[i] == nullptr) {
+
+			uORB::Subscription sensor_imu_fifo_sub{ORB_ID(sensor_imu_fifo), i};
+
+			if (sensor_imu_fifo_sub.advertised()) {
+				// if the sensors module is responsible for voting (SENS_IMU_MODE 1) then run every VehicleIMU in the same WQ
+				//   otherwise each VehicleIMU runs in a corresponding INSx WQ
+				const bool multi_mode = (_param_sens_imu_mode.get() == 0);
+				const px4::wq_config_t &wq_config = multi_mode ? px4::ins_instance_to_wq(_total_imu_count) :
+								    px4::wq_configurations::INS0;
+
+				VehicleIMUFifo *imu_fifo = new VehicleIMUFifo(_total_imu_count, i, wq_config);
+
+				if (imu_fifo != nullptr) {
+					// Start VehicleIMU instance and store
+					if (imu_fifo->Start()) {
+						_vehicle_imu_fifo_list[i] = imu_fifo;
+						_total_imu_count++;
+
+					} else {
+						delete imu_fifo;
+					}
+				}
+
+			} else {
+				// abort on first failure, try again later
+				return;
+			}
+		}
+	}
+}
+
 #if defined(CONFIG_SENSORS_VEHICLE_MAGNETOMETER)
 void Sensors::InitializeVehicleMagnetometer()
 {
@@ -597,6 +642,7 @@ void Sensors::Run()
 		// sensor device id (not just orb_group_count) must be populated before IMU init can succeed
 		_voted_sensors_update.initializeSensors();
 		InitializeVehicleIMU();
+		InitializeVehicleIMUFifo();
 
 		_last_config_update = hrt_absolute_time();
 
@@ -750,6 +796,13 @@ int Sensors::print_status()
 		}
 	}
 
+	for (auto &i : _vehicle_imu_fifo_list) {
+		if (i != nullptr) {
+			PX4_INFO_RAW("\n");
+			i->PrintStatus();
+		}
+	}
+
 	return 0;
 }
 
diff --git a/src/modules/sensors/sensors.hpp b/src/modules/sensors/sensors.hpp
index c5ecae266c..db481bf93c 100644
--- a/src/modules/sensors/sensors.hpp
+++ b/src/modules/sensors/sensors.hpp
@@ -51,6 +51,7 @@
 #include <uORB/SubscriptionCallback.hpp>
 #include "voted_sensors_update.h"
 #include "vehicle_imu/VehicleIMU.hpp"
+#include "vehicle_imu/VehicleIMUFifo.hpp"
 
 #if defined(CONFIG_SENSORS_VEHICLE_ACCELERATION)
 # include "vehicle_acceleration/VehicleAcceleration.hpp"
@@ -127,6 +128,7 @@ private:
 	void		InitializeVehicleGPSPosition();
 
 	void		InitializeVehicleIMU();
+	void		InitializeVehicleIMUFifo();
 
 	void		InitializeVehicleMagnetometer();
 
@@ -137,6 +139,12 @@ private:
 	perf_counter_t	_loop_perf;	/**< loop performance counter */
 
 	VehicleIMU *_vehicle_imu_list[MAX_SENSOR_COUNT] {};
+	VehicleIMUFifo  *_vehicle_imu_fifo_list[MAX_SENSOR_COUNT] {};
+
+	int _total_imu_count{0};
+
+	uint8_t _n_accel{0};
+	uint8_t _n_gyro{0};
 
 	VotedSensorsUpdate _voted_sensors_update;
 
@@ -145,9 +153,6 @@ private:
 	hrt_abstime     _last_config_update{0};
 	hrt_abstime     _sensor_combined_prev_timestamp{0};
 
-	uint8_t _n_accel{0};
-	uint8_t _n_gyro{0};
-
 	bool _armed{false};		/**< arming status of the vehicle */
 
 	uORB::SubscriptionCallbackWorkItem _vehicle_imu_sub[MAX_SENSOR_COUNT] {
@@ -263,4 +268,4 @@ private:
 #endif // CONFIG_SENSORS_VEHICLE_MAGNETOMETER
 		(ParamBool<px4::params::SENS_IMU_MODE>) _param_sens_imu_mode
 	)
-};
\ No newline at end of file
+};
diff --git a/src/modules/sensors/vehicle_acceleration/VehicleAcceleration.cpp b/src/modules/sensors/vehicle_acceleration/VehicleAcceleration.cpp
index c2507eb20f..1e687eb083 100644
--- a/src/modules/sensors/vehicle_acceleration/VehicleAcceleration.cpp
+++ b/src/modules/sensors/vehicle_acceleration/VehicleAcceleration.cpp
@@ -47,13 +47,15 @@ VehicleAcceleration::VehicleAcceleration() :
 	ScheduledWorkItem(MODULE_NAME, px4::wq_configurations::nav_and_controllers)
 {
 	_vehicle_acceleration_pub.advertise();
-
-	CheckAndUpdateFilters();
 }
 
 VehicleAcceleration::~VehicleAcceleration()
 {
 	Stop();
+
+	perf_free(_cycle_perf);
+	perf_free(_filter_reset_perf);
+	perf_free(_selection_changed_perf);
 }
 
 bool VehicleAcceleration::Start()
@@ -68,7 +70,7 @@ bool VehicleAcceleration::Start()
 	}
 
 	if (!SensorSelectionUpdate(true)) {
-		_sensor_sub.registerCallback();
+		ScheduleNow();
 	}
 
 	return true;
@@ -78,53 +80,90 @@ void VehicleAcceleration::Stop()
 {
 	// clear all registered callbacks
 	_sensor_sub.unregisterCallback();
+	_sensor_fifo_sub.unregisterCallback();
 	_sensor_selection_sub.unregisterCallback();
 
 	Deinit();
 }
 
-void VehicleAcceleration::CheckAndUpdateFilters()
+bool VehicleAcceleration::UpdateSampleRate()
 {
-	bool sample_rate_changed = false;
+	float sample_rate_hz = NAN;
+	float publish_rate_hz = NAN;
 
-	// get sample rate from vehicle_imu_status publication
 	for (uint8_t i = 0; i < MAX_SENSOR_COUNT; i++) {
 		uORB::SubscriptionData<vehicle_imu_status_s> imu_status{ORB_ID(vehicle_imu_status), i};
 
-		const float sample_rate_hz = imu_status.get().accel_rate_hz;
+		if (imu_status.get().accel_device_id == _selected_sensor_device_id) {
+			sample_rate_hz = imu_status.get().accel_raw_rate_hz;
+			publish_rate_hz = imu_status.get().accel_rate_hz;
+			break;
+		}
+	}
 
-		if (imu_status.advertised() && (imu_status.get().timestamp != 0)
-		    && (imu_status.get().accel_device_id != 0) && (imu_status.get().accel_device_id == _calibration.device_id())
-		    && PX4_ISFINITE(sample_rate_hz) && (sample_rate_hz > 0)) {
+	// calculate sensor update rate
+	if ((sample_rate_hz > 0) && PX4_ISFINITE(sample_rate_hz) && (publish_rate_hz > 0) && PX4_ISFINITE(publish_rate_hz)) {
+		// check if sample rate error is greater than 1%
+		const bool sample_rate_changed = (fabsf(sample_rate_hz - _filter_sample_rate_hz) / sample_rate_hz) > 0.01f;
 
-			// check if sample rate error is greater than 1%
-			if (!PX4_ISFINITE(_filter_sample_rate) || (fabsf(sample_rate_hz - _filter_sample_rate) / _filter_sample_rate) > 0.01f) {
-				PX4_DEBUG("sample rate changed: %.3f Hz -> %.3f Hz", (double)_filter_sample_rate, (double)sample_rate_hz);
-				_filter_sample_rate = sample_rate_hz;
-				sample_rate_changed = true;
+		if (_update_sample_rate || sample_rate_changed
+		    || (_filter_sample_rate_hz <= FLT_EPSILON) || !PX4_ISFINITE(_filter_sample_rate_hz)) {
 
+			PX4_DEBUG("updating sample rate: %.3f Hz -> %.3f Hz", (double)_filter_sample_rate_hz, (double)sample_rate_hz);
+
+			if (sample_rate_changed || !PX4_ISFINITE(_filter_sample_rate_hz)) {
+				_reset_filters = true;
+			}
+
+			_filter_sample_rate_hz = sample_rate_hz;
+			_update_sample_rate = false;
+
+			if (_param_imu_integ_rate.get() > 0.f) {
 				// determine number of sensor samples that will get closest to the desired rate
-				if (_param_imu_integ_rate.get() > 0) {
-					const float configured_interval_us = 1e6f / _param_imu_integ_rate.get();
-					const float sample_interval_avg = 1e6f / sample_rate_hz;
-					const uint8_t samples = math::constrain(roundf(configured_interval_us / sample_interval_avg), 1.f,
-										(float)sensor_accel_s::ORB_QUEUE_LENGTH);
+				const float configured_interval_us = 1e6f / _param_imu_integ_rate.get();
+				const float publish_interval_us = 1e6f / publish_rate_hz;
 
-					_sensor_sub.set_required_updates(samples);
+				const uint8_t samples = roundf(configured_interval_us / publish_interval_us);
+
+				if (_fifo_available) {
+					_sensor_fifo_sub.set_required_updates(math::constrain(samples, (uint8_t)1, sensor_imu_fifo_s::ORB_QUEUE_LENGTH));
 
 				} else {
-					_sensor_sub.set_required_updates(1);
+					_sensor_sub.set_required_updates(math::constrain(samples, (uint8_t)1, sensor_accel_s::ORB_QUEUE_LENGTH));
 				}
 
-				break;
+				// publish interval (constrained 100 Hz - 8 kHz)
+				_publish_interval_min_us = math::constrain((int)roundf(configured_interval_us - (publish_interval_us * 0.5f)), 125,
+							   10000);
+
+			} else {
+				_sensor_sub.set_required_updates(1);
+				_sensor_fifo_sub.set_required_updates(1);
+				_publish_interval_min_us = 0;
 			}
 		}
 	}
 
-	// update software low pass filters
-	if (sample_rate_changed || (fabsf(_lp_filter.get_cutoff_freq() - _param_imu_accel_cutoff.get()) > 0.1f)) {
-		_lp_filter.set_cutoff_frequency(_filter_sample_rate, _param_imu_accel_cutoff.get());
-		_lp_filter.reset(_acceleration_prev);
+	return PX4_ISFINITE(_filter_sample_rate_hz) && (_filter_sample_rate_hz > 0);
+}
+
+void VehicleAcceleration::ResetFilters(const hrt_abstime &time_now_us)
+{
+	if ((_filter_sample_rate_hz > 0) && PX4_ISFINITE(_filter_sample_rate_hz)) {
+
+		const Vector3f acceleration_uncalibrated{GetResetAcceleration()};
+
+		PX4_INFO("reset acceleration [%.5f, %.5f, %.5f]", (double)acceleration_uncalibrated(0),
+			 (double)acceleration_uncalibrated(1), (double)acceleration_uncalibrated(2));
+
+		for (int axis = 0; axis < 3; axis++) {
+			// acceleration low pass
+			_lp_filter[axis].set_cutoff_frequency(_filter_sample_rate_hz, _param_imu_accel_cutoff.get());
+			_lp_filter[axis].reset(acceleration_uncalibrated(axis));
+		}
+
+		_reset_filters = false;
+		perf_count(_filter_reset_perf);
 	}
 }
 
@@ -142,49 +181,141 @@ void VehicleAcceleration::SensorBiasUpdate(bool force)
 	if (_estimator_sensor_bias_sub.updated() || force) {
 		estimator_sensor_bias_s bias;
 
-		if (_estimator_sensor_bias_sub.copy(&bias)) {
-			if (bias.accel_device_id == _calibration.device_id()) {
-				_bias = Vector3f{bias.accel_bias};
+		if (_estimator_sensor_bias_sub.copy(&bias)
+		    && (bias.accel_device_id == _selected_sensor_device_id)
+		    && bias.accel_bias_valid) {
 
-			} else {
-				_bias.zero();
-			}
+			_bias = Vector3f{bias.accel_bias};
+
+		} else {
+			_bias.zero();
 		}
 	}
 }
 
-bool VehicleAcceleration::SensorSelectionUpdate(bool force)
+bool VehicleAcceleration::SensorSelectionUpdate(const hrt_abstime &time_now_us, bool force)
 {
-	if (_sensor_selection_sub.updated() || (_calibration.device_id() == 0) || force) {
+	if (_sensor_selection_sub.updated() || (_selected_sensor_device_id == 0) || force) {
 		sensor_selection_s sensor_selection{};
 		_sensor_selection_sub.copy(&sensor_selection);
 
-		if ((sensor_selection.accel_device_id != 0) && (_calibration.device_id() != sensor_selection.accel_device_id)) {
+		bool selected_device_id_valid = false;
+		uint32_t device_id = sensor_selection.accel_device_id;
+		uint32_t device_id_first_valid_imu = 0;
+
+		// use vehicle_imu_status to do basic sensor selection validation
+		for (uint8_t i = 0; i < MAX_SENSOR_COUNT; i++) {
+			uORB::SubscriptionData<vehicle_imu_status_s> imu_status{ORB_ID(vehicle_imu_status), i};
+
+			if (imu_status.advertised()
+			    && (imu_status.get().timestamp != 0) && (time_now_us < imu_status.get().timestamp + 1_s)
+			    && (imu_status.get().accel_device_id != 0)) {
+				// vehicle_imu_status accel valid
+
+				if ((device_id != 0) && (imu_status.get().accel_device_id == device_id)) {
+					selected_device_id_valid = true;
+				}
+
+				// record first valid IMU as a backup option
+				if (device_id_first_valid_imu == 0) {
+					device_id_first_valid_imu = imu_status.get().accel_device_id;
+				}
+			}
+		}
+
+		// if no accel selected or healthy then use fallback
+		if ((device_id == 0) || !selected_device_id_valid) {
+			device_id = device_id_first_valid_imu;
+		}
+
+		if ((_selected_sensor_device_id != device_id) || force) {
+
+			const bool device_id_valid = (device_id != 0);
+
+			// see if the selected sensor publishes sensor_imu_fifo
 			for (uint8_t i = 0; i < MAX_SENSOR_COUNT; i++) {
-				uORB::SubscriptionData<sensor_accel_s> sensor_accel_sub{ORB_ID(sensor_accel), i};
+				uORB::SubscriptionData<sensor_imu_fifo_s> sensor_imu_fifo_sub{ORB_ID(sensor_imu_fifo), i};
 
-				const uint32_t device_id = sensor_accel_sub.get().device_id;
+				if (sensor_imu_fifo_sub.get().device_id != 0) {
+					// if no accel was selected use the first valid sensor_imu_fifo
+					if (!device_id_valid && (time_now_us < sensor_imu_fifo_sub.get().timestamp + 1_s)) {
+						device_id = sensor_imu_fifo_sub.get().device_id;
+					}
 
-				if (sensor_accel_sub.advertised() && (sensor_accel_sub.get().timestamp != 0)
-				    && (device_id != 0) && (device_id == sensor_selection.accel_device_id)) {
+					if ((sensor_imu_fifo_sub.get().device_id == device_id)
+					    && _sensor_fifo_sub.ChangeInstance(i) && _sensor_fifo_sub.registerCallback()) {
 
-					if (_sensor_sub.ChangeInstance(i) && _sensor_sub.registerCallback()) {
-						PX4_DEBUG("selected sensor changed %" PRIu32 " -> %" PRIu32 "", _calibration.device_id(), device_id);
+						// make sure non-FIFO sub is unregistered
+						_sensor_sub.unregisterCallback();
 
-						// clear bias and corrections
-						_bias.zero();
+						_calibration.set_device_id(sensor_imu_fifo_sub.get().device_id);
 
-						_calibration.set_device_id(device_id);
+						if (_calibration.enabled()) {
+							_selected_sensor_device_id = sensor_imu_fifo_sub.get().device_id;
 
-						CheckAndUpdateFilters();
+							_filter_sample_rate_hz = 1.f / (sensor_imu_fifo_sub.get().dt * 1e-6f);
+							_update_sample_rate = true;
+							_reset_filters = true;
+							_bias.zero();
+							_fifo_available = true;
 
-						return true;
+							perf_count(_selection_changed_perf);
+							PX4_DEBUG("selecting sensor_imu_fifo:%" PRIu8 " %" PRIu32, i, _selected_sensor_device_id);
+							return true;
+
+						} else {
+							_selected_sensor_device_id = 0;
+						}
 					}
 				}
 			}
 
-			PX4_ERR("unable to find or subscribe to selected sensor (%" PRIu32 ")", sensor_selection.accel_device_id);
-			_calibration.set_device_id(0);
+			for (uint8_t i = 0; i < MAX_SENSOR_COUNT; i++) {
+				uORB::SubscriptionData<sensor_accel_s> sensor_accel_sub{ORB_ID(sensor_accel), i};
+
+				if (sensor_accel_sub.advertised()
+				    && (sensor_accel_sub.get().timestamp != 0)
+				    && (sensor_accel_sub.get().device_id != 0)
+				    && (time_now_us < sensor_accel_sub.get().timestamp + 1_s)) {
+
+					// if no accel was selected use the first valid sensor_accel
+					if (!device_id_valid) {
+						device_id = sensor_accel_sub.get().device_id;
+						PX4_WARN("no accel selected, using sensor_accel:%" PRIu8 " %" PRIu32, i, sensor_accel_sub.get().device_id);
+					}
+
+					if (sensor_accel_sub.get().device_id == device_id) {
+						if (_sensor_sub.ChangeInstance(i) && _sensor_sub.registerCallback()) {
+							// make sure FIFO sub is unregistered
+							_sensor_fifo_sub.unregisterCallback();
+
+							_calibration.set_device_id(sensor_accel_sub.get().device_id);
+
+							_selected_sensor_device_id = sensor_accel_sub.get().device_id;
+
+							_filter_sample_rate_hz = NAN;
+							_update_sample_rate = true;
+							_reset_filters = true;
+							_bias.zero();
+							_fifo_available = false;
+
+							perf_count(_selection_changed_perf);
+							PX4_DEBUG("selecting sensor_accel:%" PRIu8 " %" PRIu32, i, _selected_sensor_device_id);
+							return true;
+
+						} else {
+							PX4_ERR("unable to register callback for sensor_accel:%" PRIu8 " %" PRIu32,
+								i, sensor_accel_sub.get().device_id);
+						}
+					}
+				}
+			}
+
+			if (device_id != 0) {
+				PX4_ERR("unable to find or subscribe to selected sensor (%" PRIu32 ")", device_id);
+			}
+
+			_selected_sensor_device_id = 0;
 		}
 	}
 
@@ -203,69 +334,192 @@ void VehicleAcceleration::ParametersUpdate(bool force)
 
 		_calibration.ParametersUpdate();
 
-		CheckAndUpdateFilters();
-	}
-}
-
-void VehicleAcceleration::Run()
-{
-	// backup schedule
-	ScheduleDelayed(10_ms);
-
-	// update corrections first to set _selected_sensor
-	bool selection_updated = SensorSelectionUpdate();
-
-	_calibration.SensorCorrectionsUpdate(selection_updated);
-	SensorBiasUpdate(selection_updated);
-	ParametersUpdate();
-
-	// require valid sensor sample rate to run
-	if (!PX4_ISFINITE(_filter_sample_rate)) {
-		CheckAndUpdateFilters();
-
-		if (!PX4_ISFINITE(_filter_sample_rate)) {
-			return;
-		}
-	}
-
-	// process all outstanding messages
-	sensor_accel_s sensor_data;
-
-	while (_sensor_sub.update(&sensor_data)) {
-		const Vector3f accel_raw{sensor_data.x, sensor_data.y, sensor_data.z};
-
-		if (math::isFinite(accel_raw)) {
-			// Apply calibration and filter
-			//  - calibration offsets, scale factors, and thermal scale (if available)
-			//  - estimated in run bias (if available)
-			//  - biquad low-pass filter
-			const Vector3f accel_corrected = _calibration.Correct(accel_raw) - _bias;
-			const Vector3f accel_filtered = _lp_filter.apply(accel_corrected);
-
-			_acceleration_prev = accel_corrected;
-
-			// publish once all new samples are processed
-			if (!_sensor_sub.updated()) {
-				// Publish vehicle_acceleration
-				vehicle_acceleration_s v_acceleration;
-				v_acceleration.timestamp_sample = sensor_data.timestamp_sample;
-				accel_filtered.copyTo(v_acceleration.xyz);
-				v_acceleration.timestamp = hrt_absolute_time();
-				_vehicle_acceleration_pub.publish(v_acceleration);
-
-				return;
+		// accel low pass cutoff frequency changed
+		for (auto &lp : _lp_filter) {
+			if (fabsf(lp.get_cutoff_freq() - _param_imu_accel_cutoff.get()) > 0.01f) {
+				_reset_filters = true;
+				break;
 			}
 		}
 	}
 }
 
+Vector3f VehicleAcceleration::GetResetAcceleration() const
+{
+	if (_last_publish != 0) {
+		// acceleration filtering is performed on raw unscaled data
+		//  start with last valid vehicle body frame acceleration and compute equivalent raw data (for current sensor selection)
+		Vector3f acceleration_uncalibrated{_calibration.Uncorrect(_acceleration + _bias)};
+
+		if (PX4_ISFINITE(acceleration_uncalibrated(0))
+		    && PX4_ISFINITE(acceleration_uncalibrated(1))
+		    && PX4_ISFINITE(acceleration_uncalibrated(2))) {
+
+			return acceleration_uncalibrated;
+		}
+	}
+
+	return Vector3f{0.f, 0.f, 0.f};
+}
+
+float VehicleAcceleration::FilterAcceleration(int axis, float data[], int N)
+{
+	// Apply general low-pass filter (IMU_ACCEL_CUTOFF)
+	_lp_filter[axis].applyArray(data, N);
+
+	// return last filtered sample
+	return data[N - 1];
+}
+
+void VehicleAcceleration::Run()
+{
+	perf_begin(_cycle_perf);
+
+	// backup schedule
+	ScheduleDelayed(10_ms);
+
+	const hrt_abstime time_now_us = hrt_absolute_time();
+
+	// update corrections first to set _selected_sensor
+	const bool selection_updated = SensorSelectionUpdate(time_now_us);
+
+	if (selection_updated || _update_sample_rate) {
+		if (!UpdateSampleRate()) {
+			// sensor sample rate required to run
+			perf_end(_cycle_perf);
+			return;
+		}
+	}
+
+	ParametersUpdate();
+
+	_calibration.SensorCorrectionsUpdate(selection_updated);
+	SensorBiasUpdate(selection_updated);
+
+	if (_reset_filters) {
+		ResetFilters(time_now_us);
+
+		if (_reset_filters) {
+			// not safe to run until filters configured
+			perf_end(_cycle_perf);
+			return;
+		}
+	}
+
+	if (_fifo_available) {
+		// process all outstanding fifo messages
+		sensor_imu_fifo_s sensor_fifo_data;
+
+		while (_sensor_fifo_sub.update(&sensor_fifo_data)) {
+			const int N = sensor_fifo_data.samples;
+			static constexpr int FIFO_SIZE_MAX = sizeof(sensor_fifo_data.accel_x) / sizeof(sensor_fifo_data.accel_x[0]);
+
+			if ((sensor_fifo_data.dt > 0) && (N > 0) && (N <= FIFO_SIZE_MAX)) {
+				Vector3f acceleration_uncalibrated;
+
+				int16_t *raw_data_array[] {sensor_fifo_data.accel_x, sensor_fifo_data.accel_y, sensor_fifo_data.accel_z};
+
+				for (int axis = 0; axis < 3; axis++) {
+					// copy raw int16 sensor samples to float array for filtering
+					float data[FIFO_SIZE_MAX];
+
+					for (int n = 0; n < N; n++) {
+						data[n] = sensor_fifo_data.accel_scale * raw_data_array[axis][n];
+					}
+
+					// save last filtered sample
+					acceleration_uncalibrated(axis) = FilterAcceleration(axis, data, N);
+				}
+
+				// Publish
+				if (!_sensor_fifo_sub.updated()) {
+					if (CalibrateAndPublish(sensor_fifo_data.timestamp_sample, acceleration_uncalibrated)) {
+
+						perf_end(_cycle_perf);
+						return;
+					}
+				}
+			}
+		}
+
+	} else {
+		// process all outstanding messages
+		sensor_accel_s sensor_data;
+
+		while (_sensor_sub.update(&sensor_data)) {
+			if (PX4_ISFINITE(sensor_data.x) && PX4_ISFINITE(sensor_data.y) && PX4_ISFINITE(sensor_data.z)) {
+
+				Vector3f acceleration_uncalibrated;
+
+				float raw_data_array[] {sensor_data.x, sensor_data.y, sensor_data.z};
+
+				for (int axis = 0; axis < 3; axis++) {
+					// copy sensor sample to float array for filtering
+					float data[1] {raw_data_array[axis]};
+
+					// save last filtered sample
+					acceleration_uncalibrated(axis) = FilterAcceleration(axis, data);
+				}
+
+				// Publish
+				if (!_sensor_sub.updated()) {
+					if (CalibrateAndPublish(sensor_data.timestamp_sample, acceleration_uncalibrated)) {
+
+						perf_end(_cycle_perf);
+						return;
+					}
+				}
+			}
+		}
+	}
+
+	// force reselection on timeout
+	if (time_now_us > _last_publish + 500_ms) {
+		SensorSelectionUpdate(true);
+	}
+
+	perf_end(_cycle_perf);
+}
+
+bool VehicleAcceleration::CalibrateAndPublish(const hrt_abstime &timestamp_sample,
+		const Vector3f &acceleration_uncalibrated)
+{
+	if (timestamp_sample >= _last_publish + _publish_interval_min_us) {
+
+		// Publish vehicle_acceleration
+		vehicle_acceleration_s v_acceleration;
+		v_acceleration.timestamp_sample = timestamp_sample;
+
+		// Acceleration: rotate sensor frame to board, scale raw data to SI, apply calibration, and remove in-run estimated bias
+		_acceleration = _calibration.Correct(acceleration_uncalibrated) - _bias;
+		_acceleration.copyTo(v_acceleration.xyz);
+
+		v_acceleration.timestamp = hrt_absolute_time();
+		_vehicle_acceleration_pub.publish(v_acceleration);
+
+
+		// shift last publish time forward, but don't let it get further behind than the interval
+		_last_publish = math::constrain(_last_publish + _publish_interval_min_us,
+						timestamp_sample - _publish_interval_min_us, timestamp_sample);
+
+		return true;
+	}
+
+	return false;
+}
+
 void VehicleAcceleration::PrintStatus()
 {
-	PX4_INFO_RAW("[vehicle_acceleration] selected sensor: %" PRIu32 ", rate: %.1f Hz, estimated bias: [%.4f %.4f %.4f]\n",
-		     _calibration.device_id(), (double)_filter_sample_rate,
+	PX4_INFO_RAW("[vehicle_acceleration] selected sensor: %" PRIu32
+		     ", rate: %.1f Hz %s, estimated bias: [%.5f %.5f %.5f]\n",
+		     _calibration.device_id(), (double)_filter_sample_rate_hz, _fifo_available ? "FIFO" : "",
 		     (double)_bias(0), (double)_bias(1), (double)_bias(2));
 
 	_calibration.PrintStatus();
+
+	perf_print_counter(_cycle_perf);
+	perf_print_counter(_filter_reset_perf);
+	perf_print_counter(_selection_changed_perf);
 }
 
 } // namespace sensors
diff --git a/src/modules/sensors/vehicle_acceleration/VehicleAcceleration.hpp b/src/modules/sensors/vehicle_acceleration/VehicleAcceleration.hpp
index 68c0652082..16a8a2064b 100644
--- a/src/modules/sensors/vehicle_acceleration/VehicleAcceleration.hpp
+++ b/src/modules/sensors/vehicle_acceleration/VehicleAcceleration.hpp
@@ -1,6 +1,6 @@
 /****************************************************************************
  *
- *   Copyright (c) 2019 PX4 Development Team. All rights reserved.
+ *   Copyright (c) 2019-2022 PX4 Development Team. All rights reserved.
  *
  * Redistribution and use in source and binary forms, with or without
  * modification, are permitted provided that the following conditions
@@ -48,6 +48,7 @@
 #include <uORB/topics/estimator_sensor_bias.h>
 #include <uORB/topics/parameter_update.h>
 #include <uORB/topics/sensor_accel.h>
+#include <uORB/topics/sensor_imu_fifo.h>
 #include <uORB/topics/sensor_selection.h>
 #include <uORB/topics/vehicle_acceleration.h>
 
@@ -62,18 +63,26 @@ public:
 	VehicleAcceleration();
 	~VehicleAcceleration() override;
 
+	void PrintStatus();
 	bool Start();
 	void Stop();
 
-	void PrintStatus();
-
 private:
 	void Run() override;
 
-	void CheckAndUpdateFilters();
+	bool CalibrateAndPublish(const hrt_abstime &timestamp_sample, const matrix::Vector3f &acceleration_uncalibrated);
+
+	inline float FilterAcceleration(int axis, float data[], int N = 1);
+
 	void ParametersUpdate(bool force = false);
+
+	void ResetFilters(const hrt_abstime &time_now_us);
 	void SensorBiasUpdate(bool force = false);
-	bool SensorSelectionUpdate(bool force = false);
+	bool SensorSelectionUpdate(const hrt_abstime &time_now_us, bool force = false);
+	bool UpdateSampleRate();
+
+	// scaled appropriately for current sensor
+	matrix::Vector3f GetResetAcceleration() const;
 
 	static constexpr int MAX_SENSOR_COUNT = 4;
 
@@ -86,16 +95,30 @@ private:
 
 	uORB::SubscriptionCallbackWorkItem _sensor_selection_sub{this, ORB_ID(sensor_selection)};
 	uORB::SubscriptionCallbackWorkItem _sensor_sub{this, ORB_ID(sensor_accel)};
+	uORB::SubscriptionCallbackWorkItem _sensor_fifo_sub{this, ORB_ID(sensor_imu_fifo)};
 
 	calibration::Accelerometer _calibration{};
 
 	matrix::Vector3f _bias{};
 
-	matrix::Vector3f _acceleration_prev{};
+	matrix::Vector3f _acceleration{};
 
-	float _filter_sample_rate{NAN};
+	hrt_abstime _publish_interval_min_us{0};
+	hrt_abstime _last_publish{0};
 
-	math::LowPassFilter2p<matrix::Vector3f> _lp_filter{};
+	float _filter_sample_rate_hz{NAN};
+
+	math::LowPassFilter2p<float> _lp_filter[3] {};
+
+	uint32_t _selected_sensor_device_id{0};
+
+	bool _reset_filters{true};
+	bool _fifo_available{false};
+	bool _update_sample_rate{true};
+
+	perf_counter_t _cycle_perf{perf_alloc(PC_ELAPSED, MODULE_NAME": accel filter")};
+	perf_counter_t _filter_reset_perf{perf_alloc(PC_COUNT, MODULE_NAME": accel filter reset")};
+	perf_counter_t _selection_changed_perf{perf_alloc(PC_COUNT, MODULE_NAME": accel selection changed")};
 
 	DEFINE_PARAMETERS(
 		(ParamFloat<px4::params::IMU_ACCEL_CUTOFF>) _param_imu_accel_cutoff,
diff --git a/src/modules/sensors/vehicle_angular_velocity/VehicleAngularVelocity.cpp b/src/modules/sensors/vehicle_angular_velocity/VehicleAngularVelocity.cpp
index a49b32d7d2..4b4af880e7 100644
--- a/src/modules/sensors/vehicle_angular_velocity/VehicleAngularVelocity.cpp
+++ b/src/modules/sensors/vehicle_angular_velocity/VehicleAngularVelocity.cpp
@@ -90,7 +90,7 @@ void VehicleAngularVelocity::Stop()
 {
 	// clear all registered callbacks
 	_sensor_sub.unregisterCallback();
-	_sensor_gyro_fifo_sub.unregisterCallback();
+	_sensor_fifo_sub.unregisterCallback();
 	_sensor_selection_sub.unregisterCallback();
 
 	Deinit();
@@ -138,7 +138,7 @@ bool VehicleAngularVelocity::UpdateSampleRate()
 				const uint8_t samples = roundf(configured_interval_us / publish_interval_us);
 
 				if (_fifo_available) {
-					_sensor_gyro_fifo_sub.set_required_updates(math::constrain(samples, (uint8_t)1, sensor_gyro_fifo_s::ORB_QUEUE_LENGTH));
+					_sensor_fifo_sub.set_required_updates(math::constrain(samples, (uint8_t)1, sensor_imu_fifo_s::ORB_QUEUE_LENGTH));
 
 				} else {
 					_sensor_sub.set_required_updates(math::constrain(samples, (uint8_t)1, sensor_gyro_s::ORB_QUEUE_LENGTH));
@@ -150,7 +150,7 @@ bool VehicleAngularVelocity::UpdateSampleRate()
 
 			} else {
 				_sensor_sub.set_required_updates(1);
-				_sensor_gyro_fifo_sub.set_required_updates(1);
+				_sensor_fifo_sub.set_required_updates(1);
 				_publish_interval_min_us = 0;
 			}
 		}
@@ -217,7 +217,10 @@ void VehicleAngularVelocity::SensorBiasUpdate(bool force)
 	if (_estimator_sensor_bias_sub.updated() || force) {
 		estimator_sensor_bias_s bias;
 
-		if (_estimator_sensor_bias_sub.copy(&bias) && (bias.gyro_device_id == _selected_sensor_device_id)) {
+		if (_estimator_sensor_bias_sub.copy(&bias)
+		    && (bias.gyro_device_id == _selected_sensor_device_id)
+		    && bias.gyro_bias_valid) {
+
 			_bias = Vector3f{bias.gyro_bias};
 
 		} else {
@@ -265,44 +268,44 @@ bool VehicleAngularVelocity::SensorSelectionUpdate(const hrt_abstime &time_now_u
 
 			const bool device_id_valid = (device_id != 0);
 
-			// see if the selected sensor publishes sensor_gyro_fifo
+			// see if the selected sensor publishes sensor_imu_fifo
 			for (uint8_t i = 0; i < MAX_SENSOR_COUNT; i++) {
-				uORB::SubscriptionData<sensor_gyro_fifo_s> sensor_gyro_fifo_sub{ORB_ID(sensor_gyro_fifo), i};
+				uORB::SubscriptionData<sensor_imu_fifo_s> sensor_imu_fifo_sub{ORB_ID(sensor_imu_fifo), i};
 
-				if (sensor_gyro_fifo_sub.advertised()
-				    && (sensor_gyro_fifo_sub.get().timestamp != 0)
-				    && (sensor_gyro_fifo_sub.get().device_id != 0)
-				    && (time_now_us < sensor_gyro_fifo_sub.get().timestamp + 1_s)) {
+				if (sensor_imu_fifo_sub.advertised()
+				    && (sensor_imu_fifo_sub.get().timestamp != 0)
+				    && (sensor_imu_fifo_sub.get().device_id != 0)
+				    && (time_now_us < sensor_imu_fifo_sub.get().timestamp + 1_s)) {
 
 					// if no gyro was selected use the first valid sensor_gyro_fifo
 					if (!device_id_valid) {
-						device_id = sensor_gyro_fifo_sub.get().device_id;
-						PX4_WARN("no gyro selected, using sensor_gyro_fifo:%" PRIu8 " %" PRIu32, i, sensor_gyro_fifo_sub.get().device_id);
+						device_id = sensor_imu_fifo_sub.get().device_id;
+						PX4_WARN("no gyro selected, using sensor_imu_fifo:%" PRIu8 " %" PRIu32, i, sensor_imu_fifo_sub.get().device_id);
 					}
 
-					if (sensor_gyro_fifo_sub.get().device_id == device_id) {
-						if (_sensor_gyro_fifo_sub.ChangeInstance(i) && _sensor_gyro_fifo_sub.registerCallback()) {
+					if (sensor_imu_fifo_sub.get().device_id == device_id) {
+						if (_sensor_fifo_sub.ChangeInstance(i) && _sensor_fifo_sub.registerCallback()) {
 							// make sure non-FIFO sub is unregistered
 							_sensor_sub.unregisterCallback();
 
-							_calibration.set_device_id(sensor_gyro_fifo_sub.get().device_id);
+							_calibration.set_device_id(sensor_imu_fifo_sub.get().device_id);
 
-							_selected_sensor_device_id = sensor_gyro_fifo_sub.get().device_id;
+							_selected_sensor_device_id = sensor_imu_fifo_sub.get().device_id;
 
 							_timestamp_sample_last = 0;
-							_filter_sample_rate_hz = 1.f / (sensor_gyro_fifo_sub.get().dt * 1e-6f);
+							_filter_sample_rate_hz = 1.f / (sensor_imu_fifo_sub.get().dt * 1e-6f);
 							_update_sample_rate = true;
 							_reset_filters = true;
 							_bias.zero();
 							_fifo_available = true;
 
 							perf_count(_selection_changed_perf);
-							PX4_DEBUG("selecting sensor_gyro_fifo:%" PRIu8 " %" PRIu32, i, _selected_sensor_device_id);
+							PX4_DEBUG("selecting sensor_imu_fifo:%" PRIu8 " %" PRIu32, i, _selected_sensor_device_id);
 							return true;
 
 						} else {
-							PX4_ERR("unable to register callback for sensor_gyro_fifo:%" PRIu8 " %" PRIu32,
-								i, sensor_gyro_fifo_sub.get().device_id);
+							PX4_ERR("unable to register callback for sensor_imu_fifo:%" PRIu8 " %" PRIu32,
+								i, sensor_imu_fifo_sub.get().device_id);
 						}
 					}
 				}
@@ -325,7 +328,7 @@ bool VehicleAngularVelocity::SensorSelectionUpdate(const hrt_abstime &time_now_u
 					if (sensor_gyro_sub.get().device_id == device_id) {
 						if (_sensor_sub.ChangeInstance(i) && _sensor_sub.registerCallback()) {
 							// make sure FIFO sub is unregistered
-							_sensor_gyro_fifo_sub.unregisterCallback();
+							_sensor_fifo_sub.unregisterCallback();
 
 							_calibration.set_device_id(sensor_gyro_sub.get().device_id);
 
@@ -805,25 +808,25 @@ void VehicleAngularVelocity::Run()
 
 	if (_fifo_available) {
 		// process all outstanding fifo messages
-		sensor_gyro_fifo_s sensor_fifo_data;
+		sensor_imu_fifo_s sensor_fifo_data;
 
-		while (_sensor_gyro_fifo_sub.update(&sensor_fifo_data)) {
+		while (_sensor_fifo_sub.update(&sensor_fifo_data)) {
 			const float inverse_dt_s = 1e6f / sensor_fifo_data.dt;
 			const int N = sensor_fifo_data.samples;
-			static constexpr int FIFO_SIZE_MAX = sizeof(sensor_fifo_data.x) / sizeof(sensor_fifo_data.x[0]);
+			static constexpr int FIFO_SIZE_MAX = sizeof(sensor_fifo_data.gyro_x) / sizeof(sensor_fifo_data.gyro_x[0]);
 
 			if ((sensor_fifo_data.dt > 0) && (N > 0) && (N <= FIFO_SIZE_MAX)) {
 				Vector3f angular_velocity_uncalibrated;
 				Vector3f angular_acceleration_uncalibrated;
 
-				int16_t *raw_data_array[] {sensor_fifo_data.x, sensor_fifo_data.y, sensor_fifo_data.z};
+				int16_t *raw_data_array[] {sensor_fifo_data.gyro_x, sensor_fifo_data.gyro_y, sensor_fifo_data.gyro_z};
 
 				for (int axis = 0; axis < 3; axis++) {
 					// copy raw int16 sensor samples to float array for filtering
 					float data[FIFO_SIZE_MAX];
 
 					for (int n = 0; n < N; n++) {
-						data[n] = sensor_fifo_data.scale * raw_data_array[axis][n];
+						data[n] = sensor_fifo_data.gyro_scale * raw_data_array[axis][n];
 					}
 
 					// save last filtered sample
@@ -832,10 +835,9 @@ void VehicleAngularVelocity::Run()
 				}
 
 				// Publish
-				if (!_sensor_gyro_fifo_sub.updated()) {
+				if (!_sensor_fifo_sub.updated()) {
 					if (CalibrateAndPublish(sensor_fifo_data.timestamp_sample,
-								angular_velocity_uncalibrated,
-								angular_acceleration_uncalibrated)) {
+								angular_velocity_uncalibrated, angular_acceleration_uncalibrated)) {
 
 						perf_end(_cycle_perf);
 						return;
@@ -876,8 +878,7 @@ void VehicleAngularVelocity::Run()
 				// Publish
 				if (!_sensor_sub.updated()) {
 					if (CalibrateAndPublish(sensor_data.timestamp_sample,
-								angular_velocity_uncalibrated,
-								angular_acceleration_uncalibrated)) {
+								angular_velocity_uncalibrated, angular_acceleration_uncalibrated)) {
 
 						perf_end(_cycle_perf);
 						return;
@@ -896,32 +897,33 @@ void VehicleAngularVelocity::Run()
 }
 
 bool VehicleAngularVelocity::CalibrateAndPublish(const hrt_abstime &timestamp_sample,
-		const Vector3f &angular_velocity_uncalibrated,
-		const Vector3f &angular_acceleration_uncalibrated)
+		const Vector3f &angular_velocity_uncalibrated, const Vector3f &angular_acceleration_uncalibrated)
 {
 	if (timestamp_sample >= _last_publish + _publish_interval_min_us) {
 
 		// Publish vehicle_angular_acceleration
-		vehicle_angular_acceleration_s angular_acceleration;
-		angular_acceleration.timestamp_sample = timestamp_sample;
+		vehicle_angular_acceleration_s v_angular_acceleration;
+		v_angular_acceleration.timestamp_sample = timestamp_sample;
 
 		// Angular acceleration: rotate sensor frame to board, scale raw data to SI, apply any additional configured rotation
 		_angular_acceleration = _calibration.rotation() * angular_acceleration_uncalibrated;
-		_angular_acceleration.copyTo(angular_acceleration.xyz);
+		_angular_acceleration.copyTo(v_angular_acceleration.xyz);
+
+		v_angular_acceleration.timestamp = hrt_absolute_time();
+		_vehicle_angular_acceleration_pub.publish(v_angular_acceleration);
 
-		angular_acceleration.timestamp = hrt_absolute_time();
-		_vehicle_angular_acceleration_pub.publish(angular_acceleration);
 
 		// Publish vehicle_angular_velocity
-		vehicle_angular_velocity_s angular_velocity;
-		angular_velocity.timestamp_sample = timestamp_sample;
+		vehicle_angular_velocity_s v_angular_velocity;
+		v_angular_velocity.timestamp_sample = timestamp_sample;
 
 		// Angular velocity: rotate sensor frame to board, scale raw data to SI, apply calibration, and remove in-run estimated bias
 		_angular_velocity = _calibration.Correct(angular_velocity_uncalibrated) - _bias;
-		_angular_velocity.copyTo(angular_velocity.xyz);
+		_angular_velocity.copyTo(v_angular_velocity.xyz);
+
+		v_angular_velocity.timestamp = hrt_absolute_time();
+		_vehicle_angular_velocity_pub.publish(v_angular_velocity);
 
-		angular_velocity.timestamp = hrt_absolute_time();
-		_vehicle_angular_velocity_pub.publish(angular_velocity);
 
 		// shift last publish time forward, but don't let it get further behind than the interval
 		_last_publish = math::constrain(_last_publish + _publish_interval_min_us,
diff --git a/src/modules/sensors/vehicle_angular_velocity/VehicleAngularVelocity.hpp b/src/modules/sensors/vehicle_angular_velocity/VehicleAngularVelocity.hpp
index 47b9bf98a0..83aee8627b 100644
--- a/src/modules/sensors/vehicle_angular_velocity/VehicleAngularVelocity.hpp
+++ b/src/modules/sensors/vehicle_angular_velocity/VehicleAngularVelocity.hpp
@@ -53,7 +53,7 @@
 #include <uORB/topics/parameter_update.h>
 #include <uORB/topics/sensor_gyro.h>
 #include <uORB/topics/sensor_gyro_fft.h>
-#include <uORB/topics/sensor_gyro_fifo.h>
+#include <uORB/topics/sensor_imu_fifo.h>
 #include <uORB/topics/sensor_selection.h>
 #include <uORB/topics/vehicle_angular_acceleration.h>
 #include <uORB/topics/vehicle_angular_velocity.h>
@@ -113,7 +113,7 @@ private:
 
 	uORB::SubscriptionCallbackWorkItem _sensor_selection_sub{this, ORB_ID(sensor_selection)};
 	uORB::SubscriptionCallbackWorkItem _sensor_sub{this, ORB_ID(sensor_gyro)};
-	uORB::SubscriptionCallbackWorkItem _sensor_gyro_fifo_sub{this, ORB_ID(sensor_gyro_fifo)};
+	uORB::SubscriptionCallbackWorkItem _sensor_fifo_sub{this, ORB_ID(sensor_imu_fifo)};
 
 	calibration::Gyroscope _calibration{};
 
diff --git a/src/modules/sensors/vehicle_imu/CMakeLists.txt b/src/modules/sensors/vehicle_imu/CMakeLists.txt
index fe6ddeb8a4..8a74c0dbf8 100644
--- a/src/modules/sensors/vehicle_imu/CMakeLists.txt
+++ b/src/modules/sensors/vehicle_imu/CMakeLists.txt
@@ -34,6 +34,8 @@
 px4_add_library(vehicle_imu
 	VehicleIMU.cpp
 	VehicleIMU.hpp
+	VehicleIMUFifo.cpp
+	VehicleIMUFifo.hpp
 )
 
 target_compile_options(vehicle_imu
diff --git a/src/modules/sensors/vehicle_imu/VehicleIMU.cpp b/src/modules/sensors/vehicle_imu/VehicleIMU.cpp
index 261b09a6ff..f67c9b6627 100644
--- a/src/modules/sensors/vehicle_imu/VehicleIMU.cpp
+++ b/src/modules/sensors/vehicle_imu/VehicleIMU.cpp
@@ -291,9 +291,9 @@ bool VehicleIMU::UpdateAccel()
 		} else {
 			// collect sample interval average for filters
 			if (accel.timestamp_sample > _accel_timestamp_sample_last) {
-				if ((_accel_timestamp_sample_last != 0) && (accel.samples > 0)) {
+				if (_accel_timestamp_sample_last != 0) {
 					float interval_us = accel.timestamp_sample - _accel_timestamp_sample_last;
-					_accel_interval_mean.update(Vector2f{interval_us, interval_us / accel.samples});
+					_accel_interval_mean.update(Vector2f{interval_us, interval_us});
 				}
 
 			} else {
@@ -378,51 +378,49 @@ bool VehicleIMU::UpdateAccel()
 
 		updated = true;
 
-		if (accel.clip_counter[0] > 0 || accel.clip_counter[1] > 0 || accel.clip_counter[2] > 0) {
-			// rotate sensor clip counts into vehicle body frame
-			const Vector3f clipping{_accel_calibration.rotation() *
-						Vector3f{(float)accel.clip_counter[0], (float)accel.clip_counter[1], (float)accel.clip_counter[2]}};
+		// if (accel.clip_counter[0] > 0 || accel.clip_counter[1] > 0 || accel.clip_counter[2] > 0) {
+		// 	// rotate sensor clip counts into vehicle body frame
+		// 	const Vector3f clipping{_accel_calibration.rotation() *
+		// 				Vector3f{(float)accel.clip_counter[0], (float)accel.clip_counter[1], (float)accel.clip_counter[2]}};
 
-			// round to get reasonble clip counts per axis (after board rotation)
-			const uint8_t clip_x = roundf(fabsf(clipping(0)));
-			const uint8_t clip_y = roundf(fabsf(clipping(1)));
-			const uint8_t clip_z = roundf(fabsf(clipping(2)));
+		// 	// round to get reasonble clip counts per axis (after board rotation)
+		// 	const uint8_t clip_x = roundf(fabsf(clipping(0)));
+		// 	const uint8_t clip_y = roundf(fabsf(clipping(1)));
+		// 	const uint8_t clip_z = roundf(fabsf(clipping(2)));
 
-			_status.accel_clipping[0] += clip_x;
-			_status.accel_clipping[1] += clip_y;
-			_status.accel_clipping[2] += clip_z;
+		// 	_status.accel_clipping[0] += clip_x;
+		// 	_status.accel_clipping[1] += clip_y;
+		// 	_status.accel_clipping[2] += clip_z;
 
-			if (clip_x > 0) {
-				_delta_velocity_clipping |= vehicle_imu_s::CLIPPING_X;
-			}
+		// 	if (clip_x > 0) {
+		// 		_delta_velocity_clipping |= vehicle_imu_s::CLIPPING_X;
+		// 	}
 
-			if (clip_y > 0) {
-				_delta_velocity_clipping |= vehicle_imu_s::CLIPPING_Y;
-			}
+		// 	if (clip_y > 0) {
+		// 		_delta_velocity_clipping |= vehicle_imu_s::CLIPPING_Y;
+		// 	}
 
-			if (clip_z > 0) {
-				_delta_velocity_clipping |= vehicle_imu_s::CLIPPING_Z;
-			}
+		// 	if (clip_z > 0) {
+		// 		_delta_velocity_clipping |= vehicle_imu_s::CLIPPING_Z;
+		// 	}
 
-			_publish_status = true;
+		// 	_publish_status = true;
 
-			if (_accel_calibration.enabled() && (hrt_elapsed_time(&_last_accel_clipping_notify_time) > 3_s)) {
-				// start notifying the user periodically if there's significant continuous clipping
-				const uint64_t clipping_total = _status.accel_clipping[0] + _status.accel_clipping[1] + _status.accel_clipping[2];
+		// if (_accel_calibration.enabled() && (hrt_elapsed_time(&_last_clipping_notify_time) > 3_s)) {
+		// 	// start notifying the user periodically if there's significant continuous clipping
+		// 	const uint64_t clipping_total = _status.accel_clipping[0] + _status.accel_clipping[1] + _status.accel_clipping[2];
 
-				if (clipping_total > _last_accel_clipping_notify_total_count + 1000) {
-					mavlink_log_critical(&_mavlink_log_pub, "Accel %" PRIu8 " clipping, not safe to fly!\t", _instance);
-					/* EVENT
-					 * @description Land now, and check the vehicle setup.
-					 * Clipping can lead to fly-aways.
-					 */
-					events::send<uint8_t>(events::ID("vehicle_imu_accel_clipping"), events::Log::Critical,
-							      "Accel {1} clipping, not safe to fly!", _instance);
-					_last_accel_clipping_notify_time = accel.timestamp_sample;
-					_last_accel_clipping_notify_total_count = clipping_total;
-				}
-			}
-		}
+		// 	if (clipping_total > _last_clipping_notify_total_count + 1000) {
+		// 		mavlink_log_critical(&_mavlink_log_pub, "Accel %" PRIu8 " clipping, not safe to fly!\t", _instance);
+		// 		/* EVENT
+		// 		 * @description Land now, and check the vehicle setup.
+		// 		 * Clipping can lead to fly-aways.
+		// 		 */
+		// 		events::send<uint8_t>(events::ID("vehicle_imu_accel_clipping"), events::Log::Critical,
+		// 				      "Accel {1} clipping, not safe to fly!", _instance);
+		// 		_last_clipping_notify_time = accel.timestamp_sample;
+		// 		_last_clipping_notify_total_count = clipping_total;
+		// 	}
 	}
 
 	return updated;
@@ -446,9 +444,9 @@ bool VehicleIMU::UpdateGyro()
 		} else {
 			// collect sample interval average for filters
 			if (gyro.timestamp_sample > _gyro_timestamp_sample_last) {
-				if ((_gyro_timestamp_sample_last != 0) && (gyro.samples > 0)) {
+				if (_gyro_timestamp_sample_last != 0) {
 					float interval_us = gyro.timestamp_sample - _gyro_timestamp_sample_last;
-					_gyro_interval_mean.update(Vector2f{interval_us, interval_us / gyro.samples});
+					_gyro_interval_mean.update(Vector2f{interval_us, interval_us});
 				}
 
 			} else {
@@ -534,51 +532,51 @@ bool VehicleIMU::UpdateGyro()
 
 		updated = true;
 
-		if (gyro.clip_counter[0] > 0 || gyro.clip_counter[1] > 0 || gyro.clip_counter[2] > 0) {
-			// rotate sensor clip counts into vehicle body frame
-			const Vector3f clipping{_gyro_calibration.rotation() *
-						Vector3f{(float)gyro.clip_counter[0], (float)gyro.clip_counter[1], (float)gyro.clip_counter[2]}};
+		// if (gyro.clip_counter[0] > 0 || gyro.clip_counter[1] > 0 || gyro.clip_counter[2] > 0) {
+		// 	// rotate sensor clip counts into vehicle body frame
+		// 	const Vector3f clipping{_gyro_calibration.rotation() *
+		// 				Vector3f{(float)gyro.clip_counter[0], (float)gyro.clip_counter[1], (float)gyro.clip_counter[2]}};
 
-			// round to get reasonble clip counts per axis (after board rotation)
-			const uint8_t clip_x = roundf(fabsf(clipping(0)));
-			const uint8_t clip_y = roundf(fabsf(clipping(1)));
-			const uint8_t clip_z = roundf(fabsf(clipping(2)));
+		// 	// round to get reasonble clip counts per axis (after board rotation)
+		// 	const uint8_t clip_x = roundf(fabsf(clipping(0)));
+		// 	const uint8_t clip_y = roundf(fabsf(clipping(1)));
+		// 	const uint8_t clip_z = roundf(fabsf(clipping(2)));
 
-			_status.gyro_clipping[0] += clip_x;
-			_status.gyro_clipping[1] += clip_y;
-			_status.gyro_clipping[2] += clip_z;
+		// 	_status.gyro_clipping[0] += clip_x;
+		// 	_status.gyro_clipping[1] += clip_y;
+		// 	_status.gyro_clipping[2] += clip_z;
 
-			if (clip_x > 0) {
-				_delta_angle_clipping |= vehicle_imu_s::CLIPPING_X;
-			}
+		// 	if (clip_x > 0) {
+		// 		_delta_angle_clipping |= vehicle_imu_s::CLIPPING_X;
+		// 	}
 
-			if (clip_y > 0) {
-				_delta_angle_clipping |= vehicle_imu_s::CLIPPING_Y;
-			}
+		// 	if (clip_y > 0) {
+		// 		_delta_angle_clipping |= vehicle_imu_s::CLIPPING_Y;
+		// 	}
 
-			if (clip_z > 0) {
-				_delta_angle_clipping |= vehicle_imu_s::CLIPPING_Z;
-			}
+		// 	if (clip_z > 0) {
+		// 		_delta_angle_clipping |= vehicle_imu_s::CLIPPING_Z;
+		// 	}
 
-			_publish_status = true;
+		// 	_publish_status = true;
 
-			if (_gyro_calibration.enabled() && (hrt_elapsed_time(&_last_gyro_clipping_notify_time) > 3_s)) {
-				// start notifying the user periodically if there's significant continuous clipping
-				const uint64_t clipping_total = _status.gyro_clipping[0] + _status.gyro_clipping[1] + _status.gyro_clipping[2];
+		// 	if (_gyro_calibration.enabled() && (hrt_elapsed_time(&_last_gyro_clipping_notify_time) > 3_s)) {
+		// 		// start notifying the user periodically if there's significant continuous clipping
+		// 		const uint64_t clipping_total = _status.gyro_clipping[0] + _status.gyro_clipping[1] + _status.gyro_clipping[2];
 
-				if (clipping_total > _last_gyro_clipping_notify_total_count + 1000) {
-					mavlink_log_critical(&_mavlink_log_pub, "Gyro %" PRIu8 " clipping, not safe to fly!\t", _instance);
-					/* EVENT
-					 * @description Land now, and check the vehicle setup.
-					 * Clipping can lead to fly-aways.
-					 */
-					events::send<uint8_t>(events::ID("vehicle_imu_gyro_clipping"), events::Log::Critical,
-							      "Gyro {1} clipping, not safe to fly!", _instance);
-					_last_gyro_clipping_notify_time = gyro.timestamp_sample;
-					_last_gyro_clipping_notify_total_count = clipping_total;
-				}
-			}
-		}
+		// 		if (clipping_total > _last_gyro_clipping_notify_total_count + 1000) {
+		// 			mavlink_log_critical(&_mavlink_log_pub, "Gyro %" PRIu8 " clipping, not safe to fly!\t", _instance);
+		// 			/* EVENT
+		// 			 * @description Land now, and check the vehicle setup.
+		// 			 * Clipping can lead to fly-aways.
+		// 			 */
+		// 			events::send<uint8_t>(events::ID("vehicle_imu_gyro_clipping"), events::Log::Critical,
+		// 					      "Gyro {1} clipping, not safe to fly!", _instance);
+		// 			_last_gyro_clipping_notify_time = gyro.timestamp_sample;
+		// 			_last_gyro_clipping_notify_total_count = clipping_total;
+		// 		}
+		// 	}
+		// }
 	}
 
 	return updated;
diff --git a/src/modules/sensors/vehicle_imu/VehicleIMU.hpp b/src/modules/sensors/vehicle_imu/VehicleIMU.hpp
index f4f5f520c1..f6414c8be1 100644
--- a/src/modules/sensors/vehicle_imu/VehicleIMU.hpp
+++ b/src/modules/sensors/vehicle_imu/VehicleIMU.hpp
@@ -168,6 +168,8 @@ private:
 
 	const uint8_t _instance;
 
+
+	// calibration
 	bool _armed{false};
 
 	bool _accel_cal_available{false};
@@ -186,6 +188,7 @@ private:
 
 	hrt_abstime _in_flight_calibration_check_timestamp_last{0};
 
+
 	perf_counter_t _accel_generation_gap_perf{perf_alloc(PC_COUNT, MODULE_NAME": accel data gap")};
 	perf_counter_t _gyro_generation_gap_perf{perf_alloc(PC_COUNT, MODULE_NAME": gyro data gap")};
 
diff --git a/src/modules/sensors/vehicle_imu/VehicleIMUBase.hpp b/src/modules/sensors/vehicle_imu/VehicleIMUBase.hpp
new file mode 100644
index 0000000000..e5bea21a7b
--- /dev/null
+++ b/src/modules/sensors/vehicle_imu/VehicleIMUBase.hpp
@@ -0,0 +1,193 @@
+/****************************************************************************
+ *
+ *   Copyright (c) 2020-2022 PX4 Development Team. All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright
+ *    notice, this list of conditions and the following disclaimer.
+ * 2. Redistributions in binary form must reproduce the above copyright
+ *    notice, this list of conditions and the following disclaimer in
+ *    the documentation and/or other materials provided with the
+ *    distribution.
+ * 3. Neither the name PX4 nor the names of its contributors may be
+ *    used to endorse or promote products derived from this software
+ *    without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
+ * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
+ * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *
+ ****************************************************************************/
+
+#pragma once
+
+#include <Integrator.hpp>
+
+#include <lib/mathlib/math/Limits.hpp>
+#include <lib/mathlib/math/WelfordMean.hpp>
+#include <lib/matrix/matrix/math.hpp>
+#include <lib/perf/perf_counter.h>
+#include <lib/sensor_calibration/Accelerometer.hpp>
+#include <lib/sensor_calibration/Gyroscope.hpp>
+#include <px4_platform_common/log.h>
+#include <px4_platform_common/module_params.h>
+#include <px4_platform_common/px4_config.h>
+#include <px4_platform_common/px4_work_queue/ScheduledWorkItem.hpp>
+#include <uORB/PublicationMulti.hpp>
+#include <uORB/Subscription.hpp>
+#include <uORB/SubscriptionMultiArray.hpp>
+#include <uORB/SubscriptionCallback.hpp>
+#include <uORB/topics/estimator_sensor_bias.h>
+#include <uORB/topics/parameter_update.h>
+#include <uORB/topics/sensor_accel.h>
+#include <uORB/topics/sensor_gyro.h>
+#include <uORB/topics/vehicle_control_mode.h>
+#include <uORB/topics/vehicle_imu.h>
+#include <uORB/topics/vehicle_imu_status.h>
+
+using namespace time_literals;
+
+namespace sensors
+{
+
+class VehicleIMUBase : public ModuleParams, public px4::ScheduledWorkItem
+{
+public:
+	VehicleIMUBase() = delete;
+	~VehicleIMUBase() override;
+
+	bool Start();
+	void Stop();
+
+	void PrintStatus();
+
+private:
+	bool ParametersUpdate(bool force = false);
+	bool Publish();
+	void Run() override;
+
+	bool UpdateAccel();
+	bool UpdateGyro();
+
+	void UpdateIntegratorConfiguration();
+
+	inline void UpdateAccelVibrationMetrics(const matrix::Vector3f &acceleration);
+	inline void UpdateGyroVibrationMetrics(const matrix::Vector3f &angular_velocity);
+
+	void SensorCalibrationUpdate();
+	void SensorCalibrationSaveAccel();
+	void SensorCalibrationSaveGyro();
+
+	uORB::PublicationMulti<vehicle_imu_s> _vehicle_imu_pub{ORB_ID(vehicle_imu)};
+	uORB::PublicationMulti<vehicle_imu_status_s> _vehicle_imu_status_pub{ORB_ID(vehicle_imu_status)};
+
+	uORB::SubscriptionInterval _parameter_update_sub{ORB_ID(parameter_update), 1_s};
+
+	// Used to check, save and use learned magnetometer biases
+	uORB::SubscriptionMultiArray<estimator_sensor_bias_s> _estimator_sensor_bias_subs{ORB_ID::estimator_sensor_bias};
+
+	uORB::Subscription _vehicle_control_mode_sub{ORB_ID(vehicle_control_mode)};
+
+	calibration::Accelerometer _accel_calibration{};
+	calibration::Gyroscope _gyro_calibration{};
+
+	sensors::Integrator       _accel_integrator{};
+	sensors::IntegratorConing _gyro_integrator{};
+
+	uint32_t _imu_integration_interval_us{5000};
+
+	hrt_abstime _accel_timestamp_sample_last{0};
+	hrt_abstime _gyro_timestamp_sample_last{0};
+	hrt_abstime _gyro_timestamp_last{0};
+
+	math::WelfordMean<matrix::Vector3f> _raw_accel_mean{};
+	math::WelfordMean<matrix::Vector3f> _raw_gyro_mean{};
+
+	math::WelfordMean<matrix::Vector2f> _accel_interval_mean{};
+	math::WelfordMean<matrix::Vector2f> _gyro_interval_mean{};
+
+	math::WelfordMean<matrix::Vector2f> _gyro_update_latency_mean{};
+
+	float _accel_interval_best_variance{(float)INFINITY};
+	float _gyro_interval_best_variance{(float)INFINITY};
+
+	float _accel_interval_us{NAN};
+	float _gyro_interval_us{NAN};
+
+	unsigned _accel_last_generation{0};
+	unsigned _gyro_last_generation{0};
+
+	float _accel_temperature_sum{NAN};
+	float _gyro_temperature_sum{NAN};
+
+	int _accel_temperature_sum_count{0};
+	int _gyro_temperature_sum_count{0};
+
+	matrix::Vector3f _acceleration_prev{};     // acceleration from the previous IMU measurement for vibration metrics
+	matrix::Vector3f _angular_velocity_prev{}; // angular velocity from the previous IMU measurement for vibration metrics
+
+	vehicle_imu_status_s _status{};
+
+	float _coning_norm_accum{0};
+	float _coning_norm_accum_total_time_s{0};
+
+	uint8_t     _delta_angle_clipping{0};
+	uint8_t     _delta_velocity_clipping{0};
+
+	hrt_abstime _last_accel_clipping_notify_time{0};
+	hrt_abstime _last_gyro_clipping_notify_time{0};
+
+	uint64_t    _last_accel_clipping_notify_total_count{0};
+	uint64_t    _last_gyro_clipping_notify_total_count{0};
+
+	orb_advert_t _mavlink_log_pub{nullptr};
+
+	uint32_t _backup_schedule_timeout_us{20000};
+
+	bool _data_gap{false};
+	bool _update_integrator_config{true};
+	bool _intervals_configured{false};
+	bool _publish_status{false};
+
+	const uint8_t _instance;
+
+	bool _armed{false};
+
+	bool _accel_cal_available{false};
+	bool _gyro_cal_available{false};
+
+	struct InFlightCalibration {
+		matrix::Vector3f offset{};
+		matrix::Vector3f bias_variance{};
+		bool valid{false};
+	};
+
+	InFlightCalibration _accel_learned_calibration[ORB_MULTI_MAX_INSTANCES] {};
+	InFlightCalibration _gyro_learned_calibration[ORB_MULTI_MAX_INSTANCES] {};
+
+	static constexpr hrt_abstime INFLIGHT_CALIBRATION_QUIET_PERIOD_US{30_s};
+
+	hrt_abstime _in_flight_calibration_check_timestamp_last{0};
+
+	perf_counter_t _accel_generation_gap_perf{perf_alloc(PC_COUNT, MODULE_NAME": accel data gap")};
+	perf_counter_t _gyro_generation_gap_perf{perf_alloc(PC_COUNT, MODULE_NAME": gyro data gap")};
+
+	DEFINE_PARAMETERS(
+		(ParamInt<px4::params::IMU_INTEG_RATE>) _param_imu_integ_rate,
+		(ParamBool<px4::params::SENS_IMU_AUTOCAL>) _param_sens_imu_autocal
+	)
+};
+
+} // namespace sensors
diff --git a/src/modules/sensors/vehicle_imu/VehicleIMUFifo.cpp b/src/modules/sensors/vehicle_imu/VehicleIMUFifo.cpp
new file mode 100644
index 0000000000..fc57cb217e
--- /dev/null
+++ b/src/modules/sensors/vehicle_imu/VehicleIMUFifo.cpp
@@ -0,0 +1,808 @@
+/****************************************************************************
+ *
+ *   Copyright (c) 2022 PX4 Development Team. All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright
+ *    notice, this list of conditions and the following disclaimer.
+ * 2. Redistributions in binary form must reproduce the above copyright
+ *    notice, this list of conditions and the following disclaimer in
+ *    the documentation and/or other materials provided with the
+ *    distribution.
+ * 3. Neither the name PX4 nor the names of its contributors may be
+ *    used to endorse or promote products derived from this software
+ *    without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
+ * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
+ * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *
+ ****************************************************************************/
+
+#include "VehicleIMUFifo.hpp"
+
+#include <px4_platform_common/log.h>
+#include <px4_platform_common/events.h>
+#include <lib/sensor_calibration/Utilities.hpp>
+#include <lib/systemlib/mavlink_log.h>
+
+#include <float.h>
+
+using namespace matrix;
+
+using math::constrain;
+
+namespace sensors
+{
+
+static constexpr uint8_t clipping(const int16_t samples[], int first_sample, int last_sample)
+{
+	unsigned clip_count = 0;
+
+	for (int n = first_sample; n <= last_sample; n++) {
+		if ((samples[n] == INT16_MIN) || (samples[n] == INT16_MAX)) {
+			clip_count++;
+		}
+	}
+
+	return clip_count;
+}
+
+static constexpr int32_t sum(const int16_t samples[], int first_sample, int last_sample)
+{
+	int32_t sum = 0;
+
+	for (int n = first_sample; n <= last_sample; n++) {
+		sum += samples[n];
+	}
+
+	return sum;
+}
+
+VehicleIMUFifo::VehicleIMUFifo(int instance, uint8_t sensor_imu_fifo_index, const px4::wq_config_t &config) :
+	ModuleParams(nullptr),
+	ScheduledWorkItem(MODULE_NAME, config),
+	_sensor_imu_fifo_sub(this, ORB_ID(sensor_imu_fifo), sensor_imu_fifo_index),
+	_instance(instance)
+{
+	_imu_integration_interval_us = 1e6f / _param_imu_integ_rate.get();
+
+	_gyro_integrator.set_reset_interval(_imu_integration_interval_us);
+	_gyro_integrator.set_reset_samples(sensor_imu_fifo_s::ORB_QUEUE_LENGTH);
+
+#if defined(ENABLE_LOCKSTEP_SCHEDULER)
+	// currently with lockstep every raw sample needs a corresponding vehicle_imu publication
+	_sensor_imu_fifo_sub.set_required_updates(1);
+#else
+	// schedule conservatively until the actual accel & gyro rates are known
+	_sensor_imu_fifo_sub.set_required_updates(sensor_imu_fifo_s::ORB_QUEUE_LENGTH / 2);
+#endif
+
+	// advertise immediately to ensure consistent ordering
+	_vehicle_imu_pub.advertise();
+	_vehicle_imu_status_pub.advertise();
+}
+
+VehicleIMUFifo::~VehicleIMUFifo()
+{
+	Stop();
+
+	perf_free(_imu_generation_gap_perf);
+
+	_vehicle_imu_pub.unadvertise();
+	_vehicle_imu_status_pub.unadvertise();
+}
+
+bool VehicleIMUFifo::Start()
+{
+	// force initial updates
+	ParametersUpdate(true);
+
+	_sensor_imu_fifo_sub.registerCallback();
+	ScheduleNow();
+	return true;
+}
+
+void VehicleIMUFifo::Stop()
+{
+	// clear all registered callbacks
+	_sensor_imu_fifo_sub.unregisterCallback();
+
+	Deinit();
+}
+
+bool VehicleIMUFifo::ParametersUpdate(bool force)
+{
+	bool updated = false;
+
+	// Check if parameters have changed
+	if (_parameter_update_sub.updated() || force) {
+		// clear update
+		parameter_update_s param_update;
+		_parameter_update_sub.copy(&param_update);
+
+		const auto imu_integ_rate_prev = _param_imu_integ_rate.get();
+
+		updateParams();
+
+		updated = true;
+
+		const auto accel_calibration_count = _accel_calibration.calibration_count();
+		const auto gyro_calibration_count = _gyro_calibration.calibration_count();
+		_accel_calibration.ParametersUpdate();
+		_gyro_calibration.ParametersUpdate();
+
+		if (accel_calibration_count != _accel_calibration.calibration_count()) {
+			// if calibration changed reset any existing learned calibration
+			_accel_cal_available = false;
+			_in_flight_calibration_check_timestamp_last = hrt_absolute_time() + INFLIGHT_CALIBRATION_QUIET_PERIOD_US;
+
+			for (auto &learned_cal : _accel_learned_calibration) {
+				learned_cal = {};
+			}
+		}
+
+		if (gyro_calibration_count != _gyro_calibration.calibration_count()) {
+			// if calibration changed reset any existing learned calibration
+			_gyro_cal_available = false;
+			_in_flight_calibration_check_timestamp_last = hrt_absolute_time() + INFLIGHT_CALIBRATION_QUIET_PERIOD_US;
+
+			for (auto &learned_cal : _gyro_learned_calibration) {
+				learned_cal = {};
+			}
+		}
+
+		// constrain IMU integration time 1-20 milliseconds (50-1000 Hz)
+		const int32_t imu_integration_rate_hz = constrain(_param_imu_integ_rate.get(), (int32_t)50, (int32_t)1000);
+
+		if (imu_integration_rate_hz != _param_imu_integ_rate.get()) {
+			PX4_WARN("IMU_INTEG_RATE updated %" PRId32 " -> %" PRIu32, _param_imu_integ_rate.get(), imu_integration_rate_hz);
+			_param_imu_integ_rate.set(imu_integration_rate_hz);
+			_param_imu_integ_rate.commit_no_notification();
+		}
+
+		_imu_integration_interval_us = 1e6f / imu_integration_rate_hz;
+
+		if (_param_imu_integ_rate.get() != imu_integ_rate_prev) {
+			// force update
+			_update_integrator_config = true;
+		}
+	}
+
+	return updated;
+}
+
+void VehicleIMUFifo::Run()
+{
+	const hrt_abstime now_us = hrt_absolute_time();
+
+	const bool parameters_updated = ParametersUpdate();
+
+	if (!_accel_calibration.enabled() || !_gyro_calibration.enabled()) {
+		_sensor_imu_fifo_sub.unregisterCallback();
+		ScheduleDelayed(1_s);
+		return;
+	}
+
+	// backup schedule
+	ScheduleDelayed(_backup_schedule_timeout_us);
+
+	// check vehicle status for changes to armed state
+	if (_vehicle_control_mode_sub.updated()) {
+		vehicle_control_mode_s vehicle_control_mode;
+
+		if (_vehicle_control_mode_sub.copy(&vehicle_control_mode)) {
+			_armed = vehicle_control_mode.flag_armed;
+		}
+	}
+
+	// reset data gap monitor
+	_data_gap = false;
+
+	sensor_imu_fifo_s sensor_imu_fifo;
+
+	while (_sensor_imu_fifo_sub.update(&sensor_imu_fifo)) {
+
+		bool consume_all = !_intervals_configured || _data_gap;
+
+		// monitor scheduling latency and force catch up with latest gyro if falling behind
+		if (_sensor_imu_fifo_sub.updated() && (_update_latency_mean.count() > 100)
+		    && (_update_latency_mean.mean()(1) > _interval_us * 1e-6f)) {
+
+			PX4_INFO("update mean sample latency: %.6f, publish latency %.6f",
+				 (double)_update_latency_mean.mean()(0),
+				 (double)_update_latency_mean.mean()(1));
+
+			consume_all = true;
+		}
+
+		if (_sensor_imu_fifo_sub.get_last_generation() != _last_generation + 1) {
+			perf_count(_imu_generation_gap_perf);
+
+			// reset average sample measurement
+			_interval_mean.reset();
+
+		} else {
+			// collect sample interval average for filters
+			if (sensor_imu_fifo.timestamp_sample > _timestamp_sample_last) {
+				if ((_timestamp_sample_last != 0) && (sensor_imu_fifo.samples > 0)) {
+					float interval_us = sensor_imu_fifo.timestamp_sample - _timestamp_sample_last;
+					_interval_mean.update(Vector2f{interval_us, (float)sensor_imu_fifo.samples});
+				}
+
+			} else {
+				PX4_ERR("%d - accel %" PRIu32 " timestamp error timestamp_sample: %" PRIu64 ", previous timestamp_sample: %" PRIu64,
+					_instance, sensor_imu_fifo.device_id, sensor_imu_fifo.timestamp_sample, _timestamp_sample_last);
+			}
+
+			if (sensor_imu_fifo.timestamp < sensor_imu_fifo.timestamp_sample) {
+				PX4_ERR("%d - accel %" PRIu32 " timestamp (%" PRIu64 ") < timestamp_sample (%" PRIu64 ")",
+					_instance, sensor_imu_fifo.device_id, sensor_imu_fifo.timestamp, sensor_imu_fifo.timestamp_sample);
+			}
+
+			const int interval_count = _interval_mean.count();
+			const float interval_variance = _interval_mean.variance()(0);
+
+			// check measured interval periodically
+			if ((_interval_mean.valid() && (interval_count % 10 == 0))
+			    && (!PX4_ISFINITE(_interval_best_variance)
+				|| (interval_variance < _interval_best_variance)
+				|| (interval_count > 1000))) {
+
+				const float interval_mean = _interval_mean.mean()(0);
+				const float interval_mean_fifo = interval_mean / _interval_mean.mean()(1);
+
+				// update sample rate if previously invalid or changed
+				const float interval_delta_us = fabsf(interval_mean - _interval_us);
+				const float percent_changed = interval_delta_us / _interval_us;
+
+				if (!PX4_ISFINITE(_interval_us) || (percent_changed > 0.001f)) {
+					if (PX4_ISFINITE(interval_mean) && PX4_ISFINITE(interval_mean_fifo) && PX4_ISFINITE(interval_variance)) {
+						// update integrator configuration if interval has changed by more than 10%
+						if (interval_delta_us > 0.1f * _interval_us) {
+							_update_integrator_config = true;
+						}
+
+						_interval_us = interval_mean;
+						_interval_samples = _interval_mean.mean()(1);
+
+						_interval_best_variance = interval_variance;
+
+						_status.accel_rate_hz = 1e6f / interval_mean;
+						_status.gyro_rate_hz = 1e6f / interval_mean;
+
+						_status.accel_raw_rate_hz = 1e6f / interval_mean_fifo; // FIFO
+						_status.gyro_raw_rate_hz = 1e6f / interval_mean_fifo; // FIFO
+
+						_publish_status = true;
+
+					} else {
+						_interval_mean.reset();
+					}
+				}
+			}
+
+			if (interval_count > 10000) {
+				// reset periodically to prevent numerical issues
+				_interval_mean.reset();
+			}
+		}
+
+		_last_generation = _sensor_imu_fifo_sub.get_last_generation();
+
+		_accel_calibration.set_device_id(sensor_imu_fifo.device_id);
+		_gyro_calibration.set_device_id(sensor_imu_fifo.device_id);
+
+		if (sensor_imu_fifo.error_count != _status.accel_error_count) {
+			_publish_status = true;
+			_status.accel_error_count = sensor_imu_fifo.error_count;
+			_status.gyro_error_count = sensor_imu_fifo.error_count;
+		}
+
+		const int N = sensor_imu_fifo.samples;
+
+		_timestamp_last = sensor_imu_fifo.timestamp;
+		_timestamp_sample_last = sensor_imu_fifo.timestamp_sample;
+
+		_temperature_sum += sensor_imu_fifo.temperature;
+		_temperature_sum_count++;
+
+		if (fabsf(sensor_imu_fifo.accel_scale - _accel_scale) > FLT_EPSILON) {
+			// rescale last sample on scale change
+			float rescale = _accel_scale / sensor_imu_fifo.accel_scale;
+
+			for (auto &s : _last_accel_sample) {
+				s = roundf(s * rescale);
+			}
+
+			_accel_scale = sensor_imu_fifo.accel_scale;
+		}
+
+		const float dt_s = sensor_imu_fifo.dt * 1e-6f;
+
+		int last_sample = N - 1;
+
+		if (!consume_all && (_gyro_integrator.integrated_samples() + N > _gyro_integrator.get_reset_samples())) {
+			// TODO: publish early and then integrate the rest?
+			if (_gyro_integrator.get_reset_samples() > _gyro_integrator.integrated_samples()) {
+				int required_samples = _gyro_integrator.get_reset_samples() - _gyro_integrator.integrated_samples();
+
+				if (required_samples < sensor_imu_fifo_s::FIFO_SIZE / 2) {
+					last_sample = N - required_samples;
+
+					PX4_DEBUG("intgrated samples (%d) + N (%d) > reset samples (%d)   %" PRIu8 " - IMU: %" PRIu32 ", Req samples: %d",
+						  _gyro_integrator.integrated_samples(), N, _gyro_integrator.get_reset_samples(),
+						  _instance, _accel_calibration.device_id(), required_samples);
+				}
+			}
+		}
+
+		IntegrateAccel(sensor_imu_fifo.accel_x, sensor_imu_fifo.accel_y, sensor_imu_fifo.accel_z, 0, last_sample);
+		IntegrateGyro(sensor_imu_fifo.gyro_x, sensor_imu_fifo.gyro_y, sensor_imu_fifo.gyro_z, 0, last_sample,
+			      sensor_imu_fifo.gyro_scale, dt_s);
+
+		// reconfigure integrators if calculated sensor intervals have changed
+		if (_update_integrator_config || !_intervals_configured) {
+			UpdateIntegratorConfiguration();
+		}
+
+		// publish if both accel & gyro integrators are ready
+		bool publish = _intervals_configured && _gyro_integrator.integral_ready();
+
+		if (consume_all && _sensor_imu_fifo_sub.updated()) {
+			publish = false;
+		}
+
+		if (publish) {
+			Vector3f delta_angle;
+			uint16_t delta_angle_dt;
+
+			if (_gyro_integrator.reset(delta_angle, delta_angle_dt)) {
+				Vector3f delta_velocity{_accel_integral *_accel_scale * dt_s};
+				_accel_integral.zero();
+				uint16_t delta_velocity_dt = delta_angle_dt;
+
+				if (Publish(delta_angle, delta_angle_dt, delta_velocity, delta_velocity_dt)) {
+					// record gyro publication latency and integrated samples
+					if (_update_latency_mean.count() > 10000) {
+						// reset periodically to avoid numerical issues
+						_update_latency_mean.reset();
+					}
+
+					const float time_run_s = now_us * 1e-6f;
+
+					_update_latency_mean.update(Vector2f{time_run_s - _timestamp_sample_last * 1e-6f, time_run_s - _timestamp_last * 1e-6f});
+
+
+					// integrate leftover samples
+					if (last_sample < N - 1) {
+						IntegrateAccel(sensor_imu_fifo.accel_x, sensor_imu_fifo.accel_y, sensor_imu_fifo.accel_z, last_sample + 1, N - 1);
+						IntegrateGyro(sensor_imu_fifo.gyro_x, sensor_imu_fifo.gyro_y, sensor_imu_fifo.gyro_z, last_sample + 1, N - 1,
+							      sensor_imu_fifo.gyro_scale, dt_s);
+
+					}
+
+
+					return;
+				}
+			}
+		}
+	}
+
+
+	if (_param_sens_imu_autocal.get() && !parameters_updated) {
+		if ((_armed || !_accel_calibration.calibrated() || !_gyro_calibration.calibrated())
+		    && (now_us > _in_flight_calibration_check_timestamp_last + 1_s)) {
+
+			SensorCalibrationUpdate();
+			_in_flight_calibration_check_timestamp_last = now_us;
+
+		} else if (!_armed) {
+			SensorCalibrationSaveAccel();
+			SensorCalibrationSaveGyro();
+		}
+	}
+
+}
+
+void VehicleIMUFifo::IntegrateAccel(const int16_t x[], const int16_t y[], const int16_t z[],
+				    int first_sample, int last_sample)
+{
+	// integrate accel
+	// trapezoidal integration (equally spaced)
+	_accel_integral(0) += (0.5f * (_last_accel_sample[0] + x[last_sample]) + sum(x, first_sample, last_sample - 1));
+	_accel_integral(1) += (0.5f * (_last_accel_sample[1] + y[last_sample]) + sum(y, first_sample, last_sample - 1));
+	_accel_integral(2) += (0.5f * (_last_accel_sample[2] + z[last_sample]) + sum(z, first_sample, last_sample - 1));
+
+	_last_accel_sample[0] = x[last_sample];
+	_last_accel_sample[1] = y[last_sample];
+	_last_accel_sample[2] = z[last_sample];
+
+	int clip_counter[3] {
+		clipping(x, first_sample, last_sample),
+		clipping(y, first_sample, last_sample),
+		clipping(z, first_sample, last_sample),
+	};
+
+	if (clip_counter[0] > 0 || clip_counter[1] > 0 || clip_counter[2] > 0) {
+		// rotate sensor clip counts into vehicle body frame
+		const Vector3f clipping{_accel_calibration.rotation() *Vector3f{(float)clip_counter[0], (float)clip_counter[1], (float)clip_counter[2]}};
+
+		// round to get reasonble clip counts per axis (after board rotation)
+		const uint8_t clip_x = roundf(fabsf(clipping(0)));
+		const uint8_t clip_y = roundf(fabsf(clipping(1)));
+		const uint8_t clip_z = roundf(fabsf(clipping(2)));
+
+		_status.accel_clipping[0] += clip_x;
+		_status.accel_clipping[1] += clip_y;
+		_status.accel_clipping[2] += clip_z;
+
+		if (clip_x > 0) {
+			_delta_velocity_clipping |= vehicle_imu_s::CLIPPING_X;
+		}
+
+		if (clip_y > 0) {
+			_delta_velocity_clipping |= vehicle_imu_s::CLIPPING_Y;
+		}
+
+		if (clip_z > 0) {
+			_delta_velocity_clipping |= vehicle_imu_s::CLIPPING_Z;
+		}
+
+		if (_accel_calibration.enabled() && (hrt_elapsed_time(&_last_clipping_notify_time) > 3_s)) {
+			// start notifying the user periodically if there's significant continuous clipping
+			const uint64_t clipping_total = _status.accel_clipping[0] + _status.accel_clipping[1] + _status.accel_clipping[2];
+
+			if (clipping_total > _last_clipping_notify_total_count + 1000) {
+				mavlink_log_critical(&_mavlink_log_pub, "Accel %" PRIu8 " clipping, not safe to fly!\t", _instance);
+				/* EVENT
+				 * @description Land now, and check the vehicle setup.
+				 * Clipping can lead to fly-aways.
+				 */
+				events::send<uint8_t>(events::ID("vehicle_imu_accel_clipping"), events::Log::Critical,
+						      "Accel {1} clipping, not safe to fly!", _instance);
+				_last_clipping_notify_time = hrt_absolute_time();
+				_last_clipping_notify_total_count = clipping_total;
+			}
+		}
+	}
+}
+
+void VehicleIMUFifo::IntegrateGyro(const int16_t x[], const int16_t y[], const int16_t z[], int first_sample,
+				   int last_sample, float scale, float dt_s)
+{
+	for (int n = first_sample; n <= last_sample; n++) {
+		Vector3f gyro_raw{(float)x[n] *scale, (float)y[n] *scale, (float)z[n] *scale};
+		_gyro_integrator.put(gyro_raw, dt_s);
+	}
+}
+
+bool VehicleIMUFifo::Publish(const Vector3f &delta_angle, uint16_t delta_angle_dt, const Vector3f &delta_velocity,
+			     uint16_t delta_velocity_dt)
+{
+	bool updated = false;
+
+	if (_accel_calibration.enabled() && _gyro_calibration.enabled()) {
+
+		const Vector3f accumulated_coning_corrections = _gyro_integrator.accumulated_coning_corrections();
+
+		// delta angle: apply offsets, scale, and board rotation
+		_gyro_calibration.SensorCorrectionsUpdate();
+		const float gyro_dt_s = 1.e-6f * delta_angle_dt;
+		const Vector3f angular_velocity{_gyro_calibration.Correct(delta_angle / gyro_dt_s)};
+		_gyro_sum += angular_velocity;
+		_gyro_sum_count++;
+		UpdateGyroVibrationMetrics(angular_velocity);
+		const Vector3f delta_angle_corrected{angular_velocity * gyro_dt_s};
+
+		// accumulate delta angle coning corrections
+		_coning_norm_accum += accumulated_coning_corrections.norm() * gyro_dt_s;
+		_coning_norm_accum_total_time_s += gyro_dt_s;
+
+
+		// delta velocity: apply offsets, scale, and board rotation
+		_accel_calibration.SensorCorrectionsUpdate();
+		const float accel_dt_s = 1.e-6f * delta_velocity_dt;
+		const Vector3f acceleration{_accel_calibration.Correct(delta_velocity / accel_dt_s)};
+		_accel_sum += acceleration;
+		_accel_sum_count++;
+		UpdateAccelVibrationMetrics(acceleration);
+		const Vector3f delta_velocity_corrected{acceleration * accel_dt_s};
+
+		// vehicle_imu_status
+		//  publish before vehicle_imu so that error counts are available synchronously if needed
+		if ((_accel_sum_count > 0) && (_gyro_sum_count > 0)
+		    && (_publish_status || (hrt_elapsed_time(&_status.timestamp) >= 100_ms))) {
+
+			_status.accel_device_id = _accel_calibration.device_id();
+			_status.gyro_device_id = _gyro_calibration.device_id();
+
+			// temperature
+			float temperature = _temperature_sum / _temperature_sum_count;
+			// reset
+			_temperature_sum = 0.f;
+			_temperature_sum_count = 0;
+
+
+			// mean accel
+			const Vector3f accel_mean{_accel_sum / _accel_sum_count};
+			accel_mean.copyTo(_status.mean_accel);
+			_status.temperature_accel = temperature;
+			// reset
+			_accel_sum.zero();
+			_accel_sum_count = 0;
+
+
+			// mean gyro
+			const Vector3f gyro_mean{_gyro_sum / _gyro_sum_count};
+			gyro_mean.copyTo(_status.mean_gyro);
+			_status.temperature_gyro = temperature;
+			// reset
+			_gyro_sum.zero();
+			_gyro_sum_count = 0;
+
+			// Gyro delta angle coning metric = length of coning corrections averaged since last status publication
+			_status.delta_angle_coning_metric = _coning_norm_accum / _coning_norm_accum_total_time_s;
+			_coning_norm_accum = 0;
+			_coning_norm_accum_total_time_s = 0;
+
+
+			_status.timestamp = hrt_absolute_time();
+			_vehicle_imu_status_pub.publish(_status);
+
+			_publish_status = false;
+		}
+
+		// publish vehicle_imu
+		vehicle_imu_s imu;
+		imu.timestamp_sample = _timestamp_sample_last;
+		imu.accel_device_id = _accel_calibration.device_id();
+		imu.gyro_device_id = _gyro_calibration.device_id();
+		delta_angle_corrected.copyTo(imu.delta_angle);
+		delta_velocity_corrected.copyTo(imu.delta_velocity);
+		imu.delta_angle_dt = delta_angle_dt;
+		imu.delta_velocity_dt = delta_velocity_dt;
+		imu.delta_velocity_clipping = _delta_velocity_clipping;
+		imu.accel_calibration_count = _accel_calibration.calibration_count();
+		imu.gyro_calibration_count = _gyro_calibration.calibration_count();
+		imu.timestamp = hrt_absolute_time();
+		_vehicle_imu_pub.publish(imu);
+
+		// reset clip counts
+		_delta_velocity_clipping = 0;
+
+		updated = true;
+	}
+
+	return updated;
+}
+
+void VehicleIMUFifo::UpdateIntegratorConfiguration()
+{
+	if (PX4_ISFINITE(_interval_us) && PX4_ISFINITE(_interval_us)) {
+
+		// determine number of sensor samples that will get closest to the desired integration interval
+		uint8_t gyro_integral_samples = math::max(1, (int)roundf(_imu_integration_interval_us / _interval_us));
+
+		// if gyro samples exceeds queue depth, instead round to nearest even integer to improve scheduling options
+		if (gyro_integral_samples > sensor_imu_fifo_s::ORB_QUEUE_LENGTH) {
+			gyro_integral_samples = math::max(1, (int)roundf(_imu_integration_interval_us / _interval_us / 2) * 2);
+		}
+
+		//uint32_t integration_interval_us = roundf(gyro_integral_samples * _interval_us);
+
+		_gyro_integrator.set_reset_interval(_imu_integration_interval_us);
+		_gyro_integrator.set_reset_samples(gyro_integral_samples * _interval_samples);
+
+		_backup_schedule_timeout_us = sensor_imu_fifo_s::ORB_QUEUE_LENGTH * _interval_us;
+
+		// gyro: find largest integer multiple of gyro_integral_samples
+		for (int n = sensor_imu_fifo_s::ORB_QUEUE_LENGTH; n > 0; n--) {
+			if (gyro_integral_samples > sensor_imu_fifo_s::ORB_QUEUE_LENGTH) {
+				gyro_integral_samples /= 2;
+			}
+
+			if (gyro_integral_samples % n == 0) {
+				_sensor_imu_fifo_sub.set_required_updates(n);
+				_sensor_imu_fifo_sub.registerCallback();
+
+				_intervals_configured = true;
+				_update_integrator_config = false;
+
+				PX4_INFO("IMU FIFO (%" PRIu32 "), FIFO samples: %" PRIu8 ", interval: %.1f",
+					 _accel_calibration.device_id(), gyro_integral_samples, (double)_interval_us);
+
+				break;
+			}
+		}
+	}
+}
+
+void VehicleIMUFifo::UpdateAccelVibrationMetrics(const Vector3f &acceleration)
+{
+	// Accel high frequency vibe = filtered length of (acceleration - acceleration_prev)
+	_status.accel_vibration_metric = 0.99f * _status.accel_vibration_metric
+					 + 0.01f * Vector3f(acceleration - _acceleration_prev).norm();
+
+	_acceleration_prev = acceleration;
+}
+
+void VehicleIMUFifo::UpdateGyroVibrationMetrics(const Vector3f &angular_velocity)
+{
+	// Gyro high frequency vibe = filtered length of (angular_velocity - angular_velocity_prev)
+	_status.gyro_vibration_metric = 0.99f * _status.gyro_vibration_metric
+					+ 0.01f * Vector3f(angular_velocity - _angular_velocity_prev).norm();
+
+	_angular_velocity_prev = angular_velocity;
+}
+
+void VehicleIMUFifo::PrintStatus()
+{
+	PX4_INFO_RAW("[vehicle_imu_fifo] %" PRIu8 " - IMU: %" PRIu32 ", interval: %.1f us (SD %.1f us)\n",
+		     _instance, _accel_calibration.device_id(), (double)_interval_us, (double)sqrtf(_interval_best_variance));
+
+	PX4_INFO_RAW("gyro update mean sample latency: %.6f s, publish latency %.6f s, gyro interval %.6f s",
+		     (double)_update_latency_mean.mean()(0),
+		     (double)_update_latency_mean.mean()(1),
+		     (double)(_interval_us * 1e-6f));
+
+	perf_print_counter(_imu_generation_gap_perf);
+
+	_accel_calibration.PrintStatus();
+	_gyro_calibration.PrintStatus();
+}
+
+void VehicleIMUFifo::SensorCalibrationUpdate()
+{
+	for (int i = 0; i < _estimator_sensor_bias_subs.size(); i++) {
+		estimator_sensor_bias_s estimator_sensor_bias;
+
+		if (_estimator_sensor_bias_subs[i].update(&estimator_sensor_bias)
+		    && (hrt_elapsed_time(&estimator_sensor_bias.timestamp) < 1_s)) {
+
+			// find corresponding accel bias
+			if (estimator_sensor_bias.accel_bias_valid && estimator_sensor_bias.accel_bias_stable
+			    && (estimator_sensor_bias.accel_device_id != 0)
+			    && (estimator_sensor_bias.accel_device_id == _accel_calibration.device_id())) {
+
+				const Vector3f bias{estimator_sensor_bias.accel_bias};
+
+				_accel_learned_calibration[i].offset = _accel_calibration.BiasCorrectedSensorOffset(bias);
+				_accel_learned_calibration[i].bias_variance = Vector3f{estimator_sensor_bias.accel_bias_variance};
+				_accel_learned_calibration[i].valid = true;
+				_accel_cal_available = true;
+			}
+
+			// find corresponding gyro calibration
+			if (estimator_sensor_bias.gyro_bias_valid && estimator_sensor_bias.gyro_bias_stable
+			    && (estimator_sensor_bias.gyro_device_id != 0)
+			    && (estimator_sensor_bias.gyro_device_id == _gyro_calibration.device_id())) {
+
+				const Vector3f bias{estimator_sensor_bias.gyro_bias};
+
+				_gyro_learned_calibration[i].offset = _gyro_calibration.BiasCorrectedSensorOffset(bias);
+				_gyro_learned_calibration[i].bias_variance = Vector3f{estimator_sensor_bias.gyro_bias_variance};
+				_gyro_learned_calibration[i].valid = true;
+				_gyro_cal_available = true;
+			}
+		}
+	}
+}
+
+void VehicleIMUFifo::SensorCalibrationSaveAccel()
+{
+	if (_accel_cal_available) {
+		const Vector3f cal_orig{_accel_calibration.offset()};
+		bool initialised = false;
+		Vector3f offset_estimate{};
+		Vector3f bias_variance{};
+
+		// apply all valid saved offsets
+		for (int i = 0; i < ORB_MULTI_MAX_INSTANCES; i++) {
+			if (_accel_learned_calibration[i].valid) {
+				if (!initialised) {
+					bias_variance = _accel_learned_calibration[i].bias_variance;
+					offset_estimate = _accel_learned_calibration[i].offset;
+					initialised = true;
+
+				} else {
+					for (int axis_index = 0; axis_index < 3; axis_index++) {
+						const float sum_of_variances = _accel_learned_calibration[i].bias_variance(axis_index) + bias_variance(axis_index);
+						const float k1 = bias_variance(axis_index) / sum_of_variances;
+						const float k2 = _accel_learned_calibration[i].bias_variance(axis_index) / sum_of_variances;
+						offset_estimate(axis_index) = k2 * offset_estimate(axis_index) + k1 * _accel_learned_calibration[i].offset(axis_index);
+						bias_variance(axis_index) *= k2;
+					}
+				}
+
+				// reset
+				_accel_learned_calibration[i] = {};
+			}
+		}
+
+		if (initialised && ((cal_orig - offset_estimate).longerThan(0.05f) || !_accel_calibration.calibrated())) {
+			if (_accel_calibration.set_offset(offset_estimate)) {
+				PX4_INFO("%s %d (%" PRIu32 ") offset committed: [%.3f %.3f %.3f]->[%.3f %.3f %.3f])",
+					 _accel_calibration.SensorString(), _instance, _accel_calibration.device_id(),
+					 (double)cal_orig(0), (double)cal_orig(1), (double)cal_orig(2),
+					 (double)offset_estimate(0), (double)offset_estimate(1), (double)offset_estimate(2));
+
+				// save parameters with preferred calibration slot to current sensor index
+				if (_accel_calibration.ParametersSave(_sensor_imu_fifo_sub.get_instance())) {
+					param_notify_changes();
+				}
+
+				_in_flight_calibration_check_timestamp_last = hrt_absolute_time() + INFLIGHT_CALIBRATION_QUIET_PERIOD_US;
+			}
+		}
+
+		// reset
+		_accel_cal_available = false;
+	}
+}
+
+void VehicleIMUFifo::SensorCalibrationSaveGyro()
+{
+	if (_gyro_cal_available) {
+		const Vector3f cal_orig{_gyro_calibration.offset()};
+		bool initialised = false;
+		Vector3f offset_estimate{};
+		Vector3f bias_variance{};
+
+		// apply all valid saved offsets
+		for (int i = 0; i < ORB_MULTI_MAX_INSTANCES; i++) {
+			if (_gyro_learned_calibration[i].valid) {
+				if (!initialised) {
+					bias_variance = _gyro_learned_calibration[i].bias_variance;
+					offset_estimate = _gyro_learned_calibration[i].offset;
+					initialised = true;
+
+				} else {
+					for (int axis_index = 0; axis_index < 3; axis_index++) {
+						const float sum_of_variances = _gyro_learned_calibration[i].bias_variance(axis_index) + bias_variance(axis_index);
+						const float k1 = bias_variance(axis_index) / sum_of_variances;
+						const float k2 = _gyro_learned_calibration[i].bias_variance(axis_index) / sum_of_variances;
+						offset_estimate(axis_index) = k2 * offset_estimate(axis_index) + k1 * _gyro_learned_calibration[i].offset(axis_index);
+						bias_variance(axis_index) *= k2;
+					}
+				}
+
+				// reset
+				_gyro_learned_calibration[i] = {};
+			}
+		}
+
+		if (initialised && ((cal_orig - offset_estimate).longerThan(0.01f) || !_gyro_calibration.calibrated())) {
+			if (_gyro_calibration.set_offset(offset_estimate)) {
+				PX4_INFO("%s %d (%" PRIu32 ") offset committed: [%.3f %.3f %.3f]->[%.3f %.3f %.3f])",
+					 _gyro_calibration.SensorString(), _instance, _gyro_calibration.device_id(),
+					 (double)cal_orig(0), (double)cal_orig(1), (double)cal_orig(2),
+					 (double)offset_estimate(0), (double)offset_estimate(1), (double)offset_estimate(2));
+
+				// save parameters with preferred calibration slot to current sensor index
+				if (_gyro_calibration.ParametersSave(_sensor_imu_fifo_sub.get_instance())) {
+					param_notify_changes();
+				}
+
+				_in_flight_calibration_check_timestamp_last = hrt_absolute_time() + INFLIGHT_CALIBRATION_QUIET_PERIOD_US;
+			}
+		}
+
+		// reset
+		_gyro_cal_available = false;
+	}
+}
+
+} // namespace sensors
diff --git a/src/modules/sensors/vehicle_imu/VehicleIMUFifo.hpp b/src/modules/sensors/vehicle_imu/VehicleIMUFifo.hpp
new file mode 100644
index 0000000000..8eaead8c6c
--- /dev/null
+++ b/src/modules/sensors/vehicle_imu/VehicleIMUFifo.hpp
@@ -0,0 +1,193 @@
+/****************************************************************************
+ *
+ *   Copyright (c) 2022 PX4 Development Team. All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright
+ *    notice, this list of conditions and the following disclaimer.
+ * 2. Redistributions in binary form must reproduce the above copyright
+ *    notice, this list of conditions and the following disclaimer in
+ *    the documentation and/or other materials provided with the
+ *    distribution.
+ * 3. Neither the name PX4 nor the names of its contributors may be
+ *    used to endorse or promote products derived from this software
+ *    without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
+ * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
+ * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *
+ ****************************************************************************/
+
+#pragma once
+
+#include "Integrator.hpp"
+
+#include <lib/mathlib/math/Limits.hpp>
+#include <lib/mathlib/math/WelfordMean.hpp>
+#include <lib/matrix/matrix/math.hpp>
+#include <lib/perf/perf_counter.h>
+#include <lib/sensor_calibration/Accelerometer.hpp>
+#include <lib/sensor_calibration/Gyroscope.hpp>
+#include <px4_platform_common/log.h>
+#include <px4_platform_common/module_params.h>
+#include <px4_platform_common/px4_config.h>
+#include <px4_platform_common/px4_work_queue/ScheduledWorkItem.hpp>
+#include <uORB/PublicationMulti.hpp>
+#include <uORB/Subscription.hpp>
+#include <uORB/SubscriptionMultiArray.hpp>
+#include <uORB/SubscriptionCallback.hpp>
+#include <uORB/topics/estimator_sensor_bias.h>
+#include <uORB/topics/parameter_update.h>
+#include <uORB/topics/sensor_imu_fifo.h>
+#include <uORB/topics/vehicle_control_mode.h>
+#include <uORB/topics/vehicle_imu.h>
+#include <uORB/topics/vehicle_imu_status.h>
+
+using namespace time_literals;
+
+namespace sensors
+{
+
+class VehicleIMUFifo : public ModuleParams, public px4::ScheduledWorkItem
+{
+public:
+	VehicleIMUFifo() = delete;
+	VehicleIMUFifo(int instance, uint8_t sensor_imu_fifo_index, const px4::wq_config_t &config);
+
+	~VehicleIMUFifo() override;
+
+	bool Start();
+	void Stop();
+
+	void PrintStatus();
+
+private:
+	bool ParametersUpdate(bool force = false);
+	bool Publish(const matrix::Vector3f &delta_angle, uint16_t delta_angle_dt,
+		     const matrix::Vector3f &delta_velocity, uint16_t delta_velocity_dt);
+	void Run() override;
+
+	void IntegrateAccel(const int16_t x[], const int16_t y[], const int16_t z[], int first_sample, int last_sample);
+	void IntegrateGyro(const int16_t x[], const int16_t y[], const int16_t z[], int first_sample, int last_sample,
+			   float scale, float dt_s);
+
+	void UpdateIntegratorConfiguration();
+
+	inline void UpdateAccelVibrationMetrics(const matrix::Vector3f &acceleration);
+	inline void UpdateGyroVibrationMetrics(const matrix::Vector3f &angular_velocity);
+
+	void SensorCalibrationUpdate();
+	void SensorCalibrationSaveAccel();
+	void SensorCalibrationSaveGyro();
+
+	uORB::PublicationMulti<vehicle_imu_s> _vehicle_imu_pub{ORB_ID(vehicle_imu)};
+	uORB::PublicationMulti<vehicle_imu_status_s> _vehicle_imu_status_pub{ORB_ID(vehicle_imu_status)};
+
+	uORB::SubscriptionInterval _parameter_update_sub{ORB_ID(parameter_update), 1_s};
+
+	// Used to check, save and use learned magnetometer biases
+	uORB::SubscriptionMultiArray<estimator_sensor_bias_s> _estimator_sensor_bias_subs{ORB_ID::estimator_sensor_bias};
+
+	uORB::SubscriptionCallbackWorkItem _sensor_imu_fifo_sub;
+
+	uORB::Subscription _vehicle_control_mode_sub{ORB_ID(vehicle_control_mode)};
+
+	calibration::Accelerometer _accel_calibration{};
+	calibration::Gyroscope _gyro_calibration{};
+
+	IntegratorConing _gyro_integrator{};
+
+	matrix::Vector3f _accel_integral{};
+	int16_t _last_accel_sample[3] {};
+	float _accel_scale{0.f};
+
+	uint32_t _imu_integration_interval_us{5000};
+
+	hrt_abstime _timestamp_sample_last{0};
+	hrt_abstime _timestamp_last{0};
+
+	math::WelfordMean<matrix::Vector2f> _interval_mean{};
+
+	math::WelfordMean<matrix::Vector2f> _update_latency_mean{};
+
+	float _interval_best_variance{(float)INFINITY};
+
+	float _interval_us{NAN};
+	int _interval_samples{1};
+
+	unsigned _last_generation{0};
+
+	matrix::Vector3f _accel_sum{};
+	matrix::Vector3f _gyro_sum{};
+	int _accel_sum_count{0};
+	int _gyro_sum_count{0};
+
+	float _temperature_sum{0};
+	int _temperature_sum_count{0};
+
+	matrix::Vector3f _acceleration_prev{};     // acceleration from the previous IMU measurement for vibration metrics
+	matrix::Vector3f _angular_velocity_prev{}; // angular velocity from the previous IMU measurement for vibration metrics
+
+	vehicle_imu_status_s _status{};
+
+	float _coning_norm_accum{0};
+	float _coning_norm_accum_total_time_s{0};
+
+	uint8_t _delta_velocity_clipping{0};
+
+	hrt_abstime _last_clipping_notify_time{0};
+	uint64_t _last_clipping_notify_total_count{0};
+	orb_advert_t _mavlink_log_pub{nullptr};
+
+	uint32_t _backup_schedule_timeout_us{20000};
+
+	bool _data_gap{false};
+	bool _update_integrator_config{true};
+	bool _intervals_configured{false};
+	bool _publish_status{false};
+
+	const uint8_t _instance;
+
+
+	// calibration
+	bool _armed{false};
+
+	bool _accel_cal_available{false};
+	bool _gyro_cal_available{false};
+
+	struct InFlightCalibration {
+		matrix::Vector3f offset{};
+		matrix::Vector3f bias_variance{};
+		bool valid{false};
+	};
+
+	InFlightCalibration _accel_learned_calibration[ORB_MULTI_MAX_INSTANCES] {};
+	InFlightCalibration _gyro_learned_calibration[ORB_MULTI_MAX_INSTANCES] {};
+
+	static constexpr hrt_abstime INFLIGHT_CALIBRATION_QUIET_PERIOD_US{30_s};
+
+	hrt_abstime _in_flight_calibration_check_timestamp_last{0};
+
+
+	perf_counter_t _imu_generation_gap_perf{perf_alloc(PC_COUNT, MODULE_NAME": imu data gap")};
+
+	DEFINE_PARAMETERS(
+		(ParamInt<px4::params::IMU_INTEG_RATE>) _param_imu_integ_rate,
+		(ParamBool<px4::params::SENS_IMU_AUTOCAL>) _param_sens_imu_autocal
+	)
+};
+
+} // namespace sensors
diff --git a/src/modules/sensors/voted_sensors_update.cpp b/src/modules/sensors/voted_sensors_update.cpp
index 3e89e18644..b0d35786eb 100644
--- a/src/modules/sensors/voted_sensors_update.cpp
+++ b/src/modules/sensors/voted_sensors_update.cpp
@@ -354,11 +354,13 @@ void VotedSensorsUpdate::initSensorClass(SensorData &sensor_data, uint8_t sensor
 	bool added = false;
 	int max_sensor_index = -1;
 
-	for (unsigned i = 0; i < sensor_count_max; i++) {
+	for (uint8_t i = 0; i < sensor_count_max; i++) {
+
+		uORB::SubscriptionData<vehicle_imu_s> imu_sub{ORB_ID(vehicle_imu), i};
 
 		max_sensor_index = i;
 
-		if (!sensor_data.advertised[i] && sensor_data.subscription[i].advertised()) {
+		if (!sensor_data.advertised[i] && imu_sub.advertised()) {
 			sensor_data.advertised[i] = true;
 			sensor_data.priority[i] = DEFAULT_PRIORITY;
 			sensor_data.priority_configured[i] = DEFAULT_PRIORITY;
@@ -369,7 +371,7 @@ void VotedSensorsUpdate::initSensorClass(SensorData &sensor_data, uint8_t sensor
 					added = true;
 
 				} else {
-					PX4_ERR("failed to add validator for sensor %s %i", sensor_data.subscription[i].get_topic()->o_name, i);
+					PX4_ERR("failed to add validator for sensor %i", i);
 				}
 			}
 		}
diff --git a/src/modules/sensors/voted_sensors_update.h b/src/modules/sensors/voted_sensors_update.h
index 9e8d40a6e9..f690311706 100644
--- a/src/modules/sensors/voted_sensors_update.h
+++ b/src/modules/sensors/voted_sensors_update.h
@@ -107,10 +107,6 @@ private:
 	static constexpr uint8_t DEFAULT_PRIORITY = 50;
 
 	struct SensorData {
-		SensorData() = delete;
-		explicit SensorData(ORB_ID meta) : subscription{{meta, 0}, {meta, 1}, {meta, 2}, {meta, 3}} {}
-
-		uORB::Subscription subscription[MAX_SENSOR_COUNT]; /**< raw sensor data subscription */
 		DataValidatorGroup voter{1};
 		unsigned int last_failover_count{0};
 		int32_t priority[MAX_SENSOR_COUNT] {};
@@ -146,8 +142,8 @@ private:
 	 */
 	void calcGyroInconsistency();
 
-	SensorData _accel{ORB_ID::sensor_accel};
-	SensorData _gyro{ORB_ID::sensor_gyro};
+	SensorData _accel{};
+	SensorData _gyro{};
 
 	hrt_abstime _last_error_message{0};
 	orb_advert_t _mavlink_log_pub{nullptr};
diff --git a/src/modules/simulation/simulator_ignition_bridge/SimulatorIgnitionBridge.cpp b/src/modules/simulation/simulator_ignition_bridge/SimulatorIgnitionBridge.cpp
index 129e6decec..4b6d3887f5 100644
--- a/src/modules/simulation/simulator_ignition_bridge/SimulatorIgnitionBridge.cpp
+++ b/src/modules/simulation/simulator_ignition_bridge/SimulatorIgnitionBridge.cpp
@@ -330,7 +330,6 @@ void SimulatorIgnitionBridge::imuCallback(const ignition::msgs::IMU &imu)
 	sensor_accel.y = accel_b.Y();
 	sensor_accel.z = accel_b.Z();
 	sensor_accel.temperature = NAN;
-	sensor_accel.samples = 1;
 	sensor_accel.timestamp = time_us; // hrt_absolute_time();
 	_sensor_accel_pub.publish(sensor_accel);
 
@@ -348,7 +347,6 @@ void SimulatorIgnitionBridge::imuCallback(const ignition::msgs::IMU &imu)
 	sensor_gyro.y = gyro_b.Y();
 	sensor_gyro.z = gyro_b.Z();
 	sensor_gyro.temperature = NAN;
-	sensor_gyro.samples = 1;
 	sensor_gyro.timestamp = time_us; // hrt_absolute_time();
 	_sensor_gyro_pub.publish(sensor_gyro);
 
diff --git a/src/modules/simulation/simulator_mavlink/CMakeLists.txt b/src/modules/simulation/simulator_mavlink/CMakeLists.txt
index bd431f7d94..8006846297 100644
--- a/src/modules/simulation/simulator_mavlink/CMakeLists.txt
+++ b/src/modules/simulation/simulator_mavlink/CMakeLists.txt
@@ -48,8 +48,6 @@ px4_add_module(
 		mavlink_c_generate
 		conversion
 		geo
-		drivers_accelerometer
-		drivers_gyroscope
 		drivers_magnetometer
 	)
 
diff --git a/src/modules/simulation/simulator_mavlink/SimulatorMavlink.cpp b/src/modules/simulation/simulator_mavlink/SimulatorMavlink.cpp
index cd496f44cb..93e161558a 100644
--- a/src/modules/simulation/simulator_mavlink/SimulatorMavlink.cpp
+++ b/src/modules/simulation/simulator_mavlink/SimulatorMavlink.cpp
@@ -193,89 +193,124 @@ void SimulatorMavlink::update_sensors(const hrt_abstime &time, const mavlink_hil
 		}
 	}
 
-	// accel
-	if ((sensors.fields_updated & SensorSource::ACCEL) == SensorSource::ACCEL) {
-		if (sensors.id >= ACCEL_COUNT_MAX) {
-			PX4_ERR("Number of simulated accelerometer %d out of range. Max: %d", sensors.id, ACCEL_COUNT_MAX);
-			return;
+	// 1310988: DRV_IMU_DEVTYPE_SIM, BUS: 1, ADDR: 1, TYPE: SIMULATION
+	// 1310996: DRV_IMU_DEVTYPE_SIM, BUS: 2, ADDR: 1, TYPE: SIMULATION
+	// 1311004: DRV_IMU_DEVTYPE_SIM, BUS: 3, ADDR: 1, TYPE: SIMULATION
+
+	// accel/gyro instance 0 is a simulated FIFO
+	if (((sensors.fields_updated & SensorSource::ACCEL) == SensorSource::ACCEL)
+	    && ((sensors.fields_updated & SensorSource::GYRO) == SensorSource::GYRO)) {
+
+		// accel/gyro 0 is simulated FIFO
+		static constexpr float ACCEL_FIFO_SCALE = CONSTANTS_ONE_G / 2048.f;
+		//static constexpr float ACCEL_FIFO_RANGE = 16.f * CONSTANTS_ONE_G;
+		static constexpr float GYRO_FIFO_SCALE = math::radians(2000.f / 32768.f);
+		//static constexpr float GYRO_FIFO_RANGE = math::radians(2000.f);
+
+		sensor_imu_fifo_s sensor_imu_fifo{};
+		sensor_imu_fifo.timestamp_sample = time;
+		sensor_imu_fifo.device_id = 1310988; // 1310988: DRV_IMU_DEVTYPE_SIM, BUS: 1, ADDR: 1, TYPE: SIMULATION;
+		sensor_imu_fifo.dt = time - _last_imu_fifo.timestamp_sample;
+		sensor_imu_fifo.samples = 1;
+		sensor_imu_fifo.accel_scale = ACCEL_FIFO_SCALE;
+		sensor_imu_fifo.gyro_scale = GYRO_FIFO_SCALE;
+		sensor_imu_fifo.temperature = _sensors_temperature;
+
+		if (_accel_stuck[0]) {
+			for (int i = 0; i < _last_imu_fifo.samples; i++) {
+				sensor_imu_fifo.accel_x[i] = _last_imu_fifo.accel_x[i];
+				sensor_imu_fifo.accel_y[i] = _last_imu_fifo.accel_y[i];
+				sensor_imu_fifo.accel_z[i] = _last_imu_fifo.accel_z[i];
+			}
+
+		} else if (!_accel_blocked[0]) {
+			sensor_imu_fifo.accel_x[0] = sensors.xacc / ACCEL_FIFO_SCALE;
+			sensor_imu_fifo.accel_y[0] = sensors.yacc / ACCEL_FIFO_SCALE;
+			sensor_imu_fifo.accel_z[0] = sensors.zacc / ACCEL_FIFO_SCALE;
 		}
 
-		if (sensors.id == 0) {
-			// accel 0 is simulated FIFO
-			static constexpr float ACCEL_FIFO_SCALE = CONSTANTS_ONE_G / 2048.f;
-			static constexpr float ACCEL_FIFO_RANGE = 16.f * CONSTANTS_ONE_G;
-
-			_px4_accel[sensors.id].set_scale(ACCEL_FIFO_SCALE);
-			_px4_accel[sensors.id].set_range(ACCEL_FIFO_RANGE);
-
-			if (_accel_stuck[sensors.id]) {
-				_px4_accel[sensors.id].updateFIFO(_last_accel_fifo);
-
-			} else if (!_accel_blocked[sensors.id]) {
-				_px4_accel[sensors.id].set_temperature(_sensors_temperature);
-
-				_last_accel_fifo.samples = 1;
-				_last_accel_fifo.dt = time - _last_accel_fifo.timestamp_sample;
-				_last_accel_fifo.timestamp_sample = time;
-				_last_accel_fifo.x[0] = sensors.xacc / ACCEL_FIFO_SCALE;
-				_last_accel_fifo.y[0] = sensors.yacc / ACCEL_FIFO_SCALE;
-				_last_accel_fifo.z[0] = sensors.zacc / ACCEL_FIFO_SCALE;
-
-				_px4_accel[sensors.id].updateFIFO(_last_accel_fifo);
+		if (_gyro_stuck[0]) {
+			for (int i = 0; i < _last_imu_fifo.samples; i++) {
+				sensor_imu_fifo.gyro_x[i] = _last_imu_fifo.gyro_x[i];
+				sensor_imu_fifo.gyro_y[i] = _last_imu_fifo.gyro_y[i];
+				sensor_imu_fifo.gyro_z[i] = _last_imu_fifo.gyro_z[i];
 			}
 
-		} else {
-			if (_accel_stuck[sensors.id]) {
-				_px4_accel[sensors.id].update(time, _last_accel[sensors.id](0), _last_accel[sensors.id](1), _last_accel[sensors.id](2));
+		} else if (!_gyro_blocked[0]) {
+			sensor_imu_fifo.gyro_x[0] = sensors.xgyro / GYRO_FIFO_SCALE;
+			sensor_imu_fifo.gyro_y[0] = sensors.ygyro / GYRO_FIFO_SCALE;
+			sensor_imu_fifo.gyro_z[0] = sensors.zgyro / GYRO_FIFO_SCALE;
+		}
 
-			} else if (!_accel_blocked[sensors.id]) {
-				_px4_accel[sensors.id].set_temperature(_sensors_temperature);
-				_px4_accel[sensors.id].update(time, sensors.xacc, sensors.yacc, sensors.zacc);
-				_last_accel[sensors.id] = matrix::Vector3f{sensors.xacc, sensors.yacc, sensors.zacc};
+		sensor_imu_fifo.timestamp = hrt_absolute_time();
+		_sensor_imu_fifo_pub.publish(sensor_imu_fifo);
+
+		_last_imu_fifo = sensor_imu_fifo;
+	}
+
+	// accel
+	if ((sensors.fields_updated & SensorSource::ACCEL) == SensorSource::ACCEL) {
+		for (int i = 0; i < ACCEL_COUNT_MAX - 1; i++) {
+			sensor_accel_s sensor_accel{};
+			sensor_accel.timestamp_sample = time;
+
+			if (i == 0) {
+				sensor_accel.device_id = 1310996; // 1310996: DRV_IMU_DEVTYPE_SIM, BUS: 2, ADDR: 1, TYPE: SIMULATION
+
+			} else if (i == 1) {
+				sensor_accel.device_id = 1311004; // 1311004: DRV_IMU_DEVTYPE_SIM, BUS: 3, ADDR: 1, TYPE: SIMULATION
 			}
+
+			sensor_accel.temperature = _sensors_temperature;
+
+			if (_accel_stuck[i + 1]) {
+				sensor_accel.x = _last_accel[i](0);
+				sensor_accel.y = _last_accel[i](1);
+				sensor_accel.z = _last_accel[i](2);
+
+			} else if (!_accel_blocked[i + 1]) {
+				sensor_accel.x = sensors.xacc;
+				sensor_accel.y = sensors.yacc;
+				sensor_accel.z = sensors.zacc;
+			}
+
+			sensor_accel.timestamp = hrt_absolute_time();
+			_accel_pub[i].publish(sensor_accel);
+
+			_last_accel[i] = matrix::Vector3f{sensor_accel.x, sensor_accel.y, sensor_accel.z};
 		}
 	}
 
 	// gyro
 	if ((sensors.fields_updated & SensorSource::GYRO) == SensorSource::GYRO) {
-		if (sensors.id >= GYRO_COUNT_MAX) {
-			PX4_ERR("Number of simulated gyroscope %d out of range. Max: %d", sensors.id, GYRO_COUNT_MAX);
-			return;
-		}
+		for (int i = 0; i < GYRO_COUNT_MAX - 1; i++) {
+			sensor_gyro_s sensor_gyro{};
+			sensor_gyro.timestamp_sample = time;
 
-		if (sensors.id == 0) {
-			// gyro 0 is simulated FIFO
-			static constexpr float GYRO_FIFO_SCALE = math::radians(2000.f / 32768.f);
-			static constexpr float GYRO_FIFO_RANGE = math::radians(2000.f);
+			if (i == 0) {
+				sensor_gyro.device_id = 1310996; // 1310996: DRV_IMU_DEVTYPE_SIM, BUS: 2, ADDR: 1, TYPE: SIMULATION
 
-			_px4_gyro[sensors.id].set_scale(GYRO_FIFO_SCALE);
-			_px4_gyro[sensors.id].set_range(GYRO_FIFO_RANGE);
-
-			if (_gyro_stuck[sensors.id]) {
-				_px4_gyro[sensors.id].updateFIFO(_last_gyro_fifo);
-
-			} else if (!_gyro_blocked[sensors.id]) {
-				_px4_gyro[sensors.id].set_temperature(_sensors_temperature);
-
-				_last_gyro_fifo.samples = 1;
-				_last_gyro_fifo.dt = time - _last_gyro_fifo.timestamp_sample;
-				_last_gyro_fifo.timestamp_sample = time;
-				_last_gyro_fifo.x[0] = sensors.xgyro / GYRO_FIFO_SCALE;
-				_last_gyro_fifo.y[0] = sensors.ygyro / GYRO_FIFO_SCALE;
-				_last_gyro_fifo.z[0] = sensors.zgyro / GYRO_FIFO_SCALE;
-
-				_px4_gyro[sensors.id].updateFIFO(_last_gyro_fifo);
+			} else if (i == 1) {
+				sensor_gyro.device_id = 1311004; // 1311004: DRV_IMU_DEVTYPE_SIM, BUS: 3, ADDR: 1, TYPE: SIMULATION
 			}
 
-		} else {
-			if (_gyro_stuck[sensors.id]) {
-				_px4_gyro[sensors.id].update(time, _last_gyro[sensors.id](0), _last_gyro[sensors.id](1), _last_gyro[sensors.id](2));
+			sensor_gyro.temperature = _sensors_temperature;
 
-			} else if (!_gyro_blocked[sensors.id]) {
-				_px4_gyro[sensors.id].set_temperature(_sensors_temperature);
-				_px4_gyro[sensors.id].update(time, sensors.xgyro, sensors.ygyro, sensors.zgyro);
-				_last_gyro[sensors.id] = matrix::Vector3f{sensors.xgyro, sensors.ygyro, sensors.zgyro};
+			if (_gyro_stuck[i + 1]) {
+				sensor_gyro.x = _last_gyro[i](0);
+				sensor_gyro.y = _last_gyro[i](1);
+				sensor_gyro.z = _last_gyro[i](2);
+
+			} else if (!_gyro_blocked[i + 1]) {
+				sensor_gyro.x = sensors.xgyro;
+				sensor_gyro.y = sensors.ygyro;
+				sensor_gyro.z = sensors.zgyro;
 			}
+
+			sensor_gyro.timestamp = hrt_absolute_time();
+			_gyro_pub[i].publish(sensor_gyro);
+
+			_last_gyro[i] = matrix::Vector3f{sensor_gyro.x, sensor_gyro.y, sensor_gyro.z};
 		}
 	}
 
diff --git a/src/modules/simulation/simulator_mavlink/SimulatorMavlink.hpp b/src/modules/simulation/simulator_mavlink/SimulatorMavlink.hpp
index a84729e09a..fc5bd7debe 100644
--- a/src/modules/simulation/simulator_mavlink/SimulatorMavlink.hpp
+++ b/src/modules/simulation/simulator_mavlink/SimulatorMavlink.hpp
@@ -43,8 +43,6 @@
 #pragma once
 
 #include <drivers/drv_hrt.h>
-#include <lib/drivers/accelerometer/PX4Accelerometer.hpp>
-#include <lib/drivers/gyroscope/PX4Gyroscope.hpp>
 #include <lib/drivers/magnetometer/PX4Magnetometer.hpp>
 #include <lib/geo/geo.h>
 #include <lib/perf/perf_counter.h>
@@ -65,8 +63,11 @@
 #include <uORB/topics/irlock_report.h>
 #include <uORB/topics/manual_control_setpoint.h>
 #include <uORB/topics/parameter_update.h>
+#include <uORB/topics/sensor_accel.h>
 #include <uORB/topics/sensor_baro.h>
 #include <uORB/topics/sensor_gps.h>
+#include <uORB/topics/sensor_gyro.h>
+#include <uORB/topics/sensor_imu_fifo.h>
 #include <uORB/topics/sensor_optical_flow.h>
 #include <uORB/topics/vehicle_angular_velocity.h>
 #include <uORB/topics/vehicle_attitude.h>
@@ -164,19 +165,14 @@ private:
 	static SimulatorMavlink *_instance;
 
 	// simulated sensor instances
+	uORB::PublicationMulti<sensor_imu_fifo_s> _sensor_imu_fifo_pub{ORB_ID(sensor_imu_fifo)};
+	sensor_imu_fifo_s _last_imu_fifo{};
+
 	static constexpr uint8_t ACCEL_COUNT_MAX = 3;
-	PX4Accelerometer _px4_accel[ACCEL_COUNT_MAX] {
-		{1310988, ROTATION_NONE}, // 1310988: DRV_IMU_DEVTYPE_SIM, BUS: 1, ADDR: 1, TYPE: SIMULATION
-		{1310996, ROTATION_NONE}, // 1310996: DRV_IMU_DEVTYPE_SIM, BUS: 2, ADDR: 1, TYPE: SIMULATION
-		{1311004, ROTATION_NONE}, // 1311004: DRV_IMU_DEVTYPE_SIM, BUS: 3, ADDR: 1, TYPE: SIMULATION
-	};
+	uORB::PublicationMulti<sensor_accel_s> _accel_pub[ACCEL_COUNT_MAX - 1] {ORB_ID(sensor_accel), ORB_ID(sensor_accel)};
 
 	static constexpr uint8_t GYRO_COUNT_MAX = 3;
-	PX4Gyroscope _px4_gyro[GYRO_COUNT_MAX] {
-		{1310988, ROTATION_NONE}, // 1310988: DRV_IMU_DEVTYPE_SIM, BUS: 1, ADDR: 1, TYPE: SIMULATION
-		{1310996, ROTATION_NONE}, // 1310996: DRV_IMU_DEVTYPE_SIM, BUS: 2, ADDR: 1, TYPE: SIMULATION
-		{1311004, ROTATION_NONE}, // 1311004: DRV_IMU_DEVTYPE_SIM, BUS: 3, ADDR: 1, TYPE: SIMULATION
-	};
+	uORB::PublicationMulti<sensor_gyro_s> _gyro_pub[GYRO_COUNT_MAX - 1] {ORB_ID(sensor_gyro), ORB_ID(sensor_gyro)};
 
 	static constexpr uint8_t MAG_COUNT_MAX = 2;
 	PX4Magnetometer _px4_mag[MAG_COUNT_MAX] {
@@ -279,12 +275,10 @@ private:
 
 	bool _accel_blocked[ACCEL_COUNT_MAX] {};
 	bool _accel_stuck[ACCEL_COUNT_MAX] {};
-	sensor_accel_fifo_s _last_accel_fifo{};
 	matrix::Vector3f _last_accel[GYRO_COUNT_MAX] {};
 
 	bool _gyro_blocked[GYRO_COUNT_MAX] {};
 	bool _gyro_stuck[GYRO_COUNT_MAX] {};
-	sensor_gyro_fifo_s _last_gyro_fifo{};
 	matrix::Vector3f _last_gyro[GYRO_COUNT_MAX] {};
 
 	bool _baro_blocked{false};
diff --git a/src/modules/simulation/simulator_sih/CMakeLists.txt b/src/modules/simulation/simulator_sih/CMakeLists.txt
index a30565ff40..3593b87879 100644
--- a/src/modules/simulation/simulator_sih/CMakeLists.txt
+++ b/src/modules/simulation/simulator_sih/CMakeLists.txt
@@ -42,8 +42,6 @@ px4_add_module(
 		sih.hpp
 	DEPENDS
 		mathlib
-		drivers_accelerometer
-		drivers_gyroscope
 	)
 
 if(PX4_PLATFORM MATCHES "posix")
diff --git a/src/modules/simulation/simulator_sih/sih.cpp b/src/modules/simulation/simulator_sih/sih.cpp
index 16b7568d22..28c5e3e60d 100644
--- a/src/modules/simulation/simulator_sih/sih.cpp
+++ b/src/modules/simulation/simulator_sih/sih.cpp
@@ -65,9 +65,6 @@ Sih::~Sih()
 
 void Sih::run()
 {
-	_px4_accel.set_temperature(T1_C);
-	_px4_gyro.set_temperature(T1_C);
-
 	parameters_updated();
 	init_variables();
 
@@ -440,9 +437,29 @@ void Sih::reconstruct_sensors_signals(const hrt_abstime &time_now_us)
 	Vector3f acc = _C_IB.transpose() * (_v_I_dot - Vector3f(0.0f, 0.0f, CONSTANTS_ONE_G)) + noiseGauss3f(0.5f, 1.7f, 1.4f);
 	Vector3f gyro = _w_B + noiseGauss3f(0.14f, 0.07f, 0.03f);
 
-	// update IMU every iteration
-	_px4_accel.update(time_now_us, acc(0), acc(1), acc(2));
-	_px4_gyro.update(time_now_us, gyro(0), gyro(1), gyro(2));
+	// sensor_accel
+	sensor_accel_s sensor_accel{};
+	sensor_accel.timestamp_sample = time_now_us;
+	sensor_accel.device_id = 1;
+	sensor_accel.x = acc(0);
+	sensor_accel.y = acc(1);
+	sensor_accel.z = acc(2);
+	sensor_accel.range = 16.f * CONSTANTS_ONE_G;
+	sensor_accel.temperature = NAN;
+	sensor_accel.timestamp = hrt_absolute_time();
+	_sensor_accel_pub.publish(sensor_accel);
+
+	// sensor_gyro
+	sensor_gyro_s sensor_gyro{};
+	sensor_gyro.timestamp_sample = time_now_us;
+	sensor_gyro.device_id = 1;
+	sensor_gyro.x = gyro(0);
+	sensor_gyro.y = gyro(1);
+	sensor_gyro.z = gyro(2);
+	sensor_gyro.range = math::radians(2000.f);
+	sensor_gyro.temperature = NAN;
+	sensor_gyro.timestamp = hrt_absolute_time();
+	_sensor_gyro_pub.publish(sensor_gyro);
 }
 
 void Sih::send_airspeed(const hrt_abstime &time_now_us)
diff --git a/src/modules/simulation/simulator_sih/sih.hpp b/src/modules/simulation/simulator_sih/sih.hpp
index e77b7d15e2..50fecfd844 100644
--- a/src/modules/simulation/simulator_sih/sih.hpp
+++ b/src/modules/simulation/simulator_sih/sih.hpp
@@ -63,16 +63,16 @@
 #include <conversion/rotation.h>    // math::radians,
 #include <lib/geo/geo.h>        // to get the physical constants
 #include <drivers/drv_hrt.h>        // to get the real time
-#include <lib/drivers/accelerometer/PX4Accelerometer.hpp>
-#include <lib/drivers/gyroscope/PX4Gyroscope.hpp>
 #include <lib/perf/perf_counter.h>
-#include <uORB/Publication.hpp>
+#include <uORB/PublicationMulti.hpp>
 #include <uORB/Subscription.hpp>
 #include <uORB/SubscriptionInterval.hpp>
 #include <uORB/topics/parameter_update.h>
 #include <uORB/topics/airspeed.h>
 #include <uORB/topics/actuator_outputs.h>
 #include <uORB/topics/distance_sensor.h>
+#include <uORB/topics/sensor_accel.h>
+#include <uORB/topics/sensor_gyro.h>
 #include <uORB/topics/vehicle_angular_velocity.h>
 #include <uORB/topics/vehicle_attitude.h>
 #include <uORB/topics/vehicle_global_position.h>
@@ -117,8 +117,8 @@ private:
 	void parameters_updated();
 
 	// simulated sensors
-	PX4Accelerometer _px4_accel{1310988}; // 1310988: DRV_IMU_DEVTYPE_SIM, BUS: 1, ADDR: 1, TYPE: SIMULATION
-	PX4Gyroscope     _px4_gyro{1310988};  // 1310988: DRV_IMU_DEVTYPE_SIM, BUS: 1, ADDR: 1, TYPE: SIMULATION
+	uORB::PublicationMulti<sensor_accel_s> _sensor_accel_pub{ORB_ID(sensor_accel)};
+	uORB::PublicationMulti<sensor_gyro_s> _sensor_gyro_pub{ORB_ID(sensor_gyro)};
 	uORB::Publication<distance_sensor_s>  _distance_snsr_pub{ORB_ID(distance_sensor)};
 	uORB::Publication<airspeed_s>         _airspeed_pub{ORB_ID(airspeed)};
 
diff --git a/src/systemcmds/microbench/test_microbench_uorb.cpp b/src/systemcmds/microbench/test_microbench_uorb.cpp
index 02c7cdf303..1a0efd18c8 100644
--- a/src/systemcmds/microbench/test_microbench_uorb.cpp
+++ b/src/systemcmds/microbench/test_microbench_uorb.cpp
@@ -1,6 +1,6 @@
 /****************************************************************************
  *
- *  Copyright (C) 2018-2021 PX4 Development Team. All rights reserved.
+ *  Copyright (C) 2018-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
@@ -50,7 +50,7 @@
 #include <uORB/Subscription.hpp>
 #include <uORB/topics/sensor_accel.h>
 #include <uORB/topics/sensor_gyro.h>
-#include <uORB/topics/sensor_gyro_fifo.h>
+#include <uORB/topics/sensor_imu_fifo.h>
 #include <uORB/topics/vehicle_local_position.h>
 #include <uORB/topics/vehicle_status.h>
 
@@ -108,7 +108,7 @@ private:
 	vehicle_status_s status;
 	vehicle_local_position_s lpos;
 	sensor_gyro_s gyro;
-	sensor_gyro_fifo_s gyro_fifo;
+	sensor_imu_fifo_s imu_fifo;
 };
 
 bool MicroBenchORB::run_tests()
@@ -139,7 +139,7 @@ void MicroBenchORB::reset()
 
 	gyro.timestamp = rand();
 
-	gyro_fifo.timestamp = rand();
+	imu_fifo.timestamp = rand();
 }
 
 ut_declare_test_c(test_microbench_uorb, MicroBenchORB)
@@ -149,7 +149,7 @@ bool MicroBenchORB::time_px4_uorb()
 	int fd_status = orb_subscribe(ORB_ID(vehicle_status));
 	int fd_lpos = orb_subscribe(ORB_ID(vehicle_local_position));
 	int fd_gyro = orb_subscribe(ORB_ID(sensor_gyro));
-	int fd_gyro_fifo = orb_subscribe(ORB_ID(sensor_gyro_fifo));
+	int fd_imu_fifo = orb_subscribe(ORB_ID(sensor_imu_fifo));
 
 	int ret = 0;
 	bool updated = false;
@@ -169,8 +169,8 @@ bool MicroBenchORB::time_px4_uorb()
 
 	printf("\n");
 
-	PERF("orb_check sensor_gyro_fifo", ret = orb_check(fd_gyro_fifo, &updated), 100);
-	PERF("orb_copy sensor_gyro_fifo", ret = orb_copy(ORB_ID(sensor_gyro_fifo), fd_gyro_fifo, &gyro_fifo), 100);
+	PERF("orb_check sensor_imu_fifo", ret = orb_check(fd_imu_fifo, &updated), 100);
+	PERF("orb_copy sensor_imu_fifo", ret = orb_copy(ORB_ID(sensor_imu_fifo), fd_imu_fifo, &imu_fifo), 100);
 
 	printf("\n");
 
@@ -189,7 +189,7 @@ bool MicroBenchORB::time_px4_uorb()
 	orb_unsubscribe(fd_status);
 	orb_unsubscribe(fd_lpos);
 	orb_unsubscribe(fd_gyro);
-	orb_unsubscribe(fd_gyro_fifo);
+	orb_unsubscribe(fd_imu_fifo);
 
 	return true;
 }
@@ -238,9 +238,9 @@ bool MicroBenchORB::time_px4_uorb_direct()
 
 	{
 		printf("\n");
-		uORB::Subscription sens_gyro_fifo0{ORB_ID(sensor_gyro_fifo), 0};
-		PERF("uORB::Subscription orb_check sensor_gyro_fifo:0", ret = sens_gyro_fifo0.updated(), 100);
-		PERF("uORB::Subscription orb_copy sensor_gyro_fifo:0", ret = sens_gyro_fifo0.copy(&gyro_fifo), 100);
+		uORB::Subscription sens_imu_fifo0{ORB_ID(sensor_imu_fifo), 0};
+		PERF("uORB::Subscription orb_check sensor_imu_fifo:0", ret = sens_imu_fifo0.updated(), 100);
+		PERF("uORB::Subscription orb_copy sensor_imu_fifo:0", ret = sens_imu_fifo0.copy(&imu_fifo), 100);
 	}
 
 	return true;