diff --git a/src/drivers/imu/invensense/mpu9250/InvenSense_MPU9250_registers.hpp b/src/drivers/imu/invensense/mpu9250/InvenSense_MPU9250_registers.hpp
index bb3fc899e7..0aa23f7543 100644
--- a/src/drivers/imu/invensense/mpu9250/InvenSense_MPU9250_registers.hpp
+++ b/src/drivers/imu/invensense/mpu9250/InvenSense_MPU9250_registers.hpp
@@ -54,7 +54,8 @@ static constexpr uint8_t Bit7 = (1 << 7);
 
 namespace InvenSense_MPU9250
 {
-static constexpr uint32_t SPI_SPEED = 20 * 1000 * 1000;
+static constexpr uint32_t SPI_SPEED = 1 * 1000 * 1000;
+static constexpr uint32_t SPI_SPEED_SENSOR = 20 * 1000 * 1000; // 20MHz for reading sensor and interrupt registers
 static constexpr uint8_t DIR_READ = 0x80;
 
 static constexpr uint8_t WHOAMI = 0x71;
diff --git a/src/drivers/imu/invensense/mpu9250/MPU9250.cpp b/src/drivers/imu/invensense/mpu9250/MPU9250.cpp
index 359ae27f3a..f459984a79 100644
--- a/src/drivers/imu/invensense/mpu9250/MPU9250.cpp
+++ b/src/drivers/imu/invensense/mpu9250/MPU9250.cpp
@@ -42,11 +42,6 @@ static constexpr int16_t combine(uint8_t msb, uint8_t lsb)
 	return (msb << 8u) | lsb;
 }
 
-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);
-}
-
 MPU9250::MPU9250(int bus, uint32_t device, enum Rotation rotation) :
 	SPI(MODULE_NAME, nullptr, bus, device, SPIDEV_MODE3, SPI_SPEED),
 	ScheduledWorkItem(MODULE_NAME, px4::device_bus_to_wq(get_device_id())),
@@ -54,8 +49,11 @@ MPU9250::MPU9250(int bus, uint32_t device, enum Rotation rotation) :
 	_px4_gyro(get_device_id(), ORB_PRIO_VERY_HIGH, rotation)
 {
 	set_device_type(DRV_ACC_DEVTYPE_MPU9250);
+
 	_px4_accel.set_device_type(DRV_ACC_DEVTYPE_MPU9250);
 	_px4_gyro.set_device_type(DRV_GYR_DEVTYPE_MPU9250);
+
+	ConfigureSampleRate(_px4_gyro.get_max_rate_hz());
 }
 
 MPU9250::~MPU9250()
@@ -75,38 +73,6 @@ MPU9250::~MPU9250()
 	perf_free(_drdy_interval_perf);
 }
 
-void MPU9250::ConfigureSampleRate(int sample_rate)
-{
-	if (sample_rate == 0) {
-		sample_rate = 1000; // default to 1 kHz
-	}
-
-	sample_rate = math::constrain(sample_rate, 250, 2000); // limit 250 - 2000 Hz
-
-	_fifo_empty_interval_us = math::max(((1000000 / sample_rate) / 250) * 250, 500); // round down to nearest 250 us
-	_fifo_gyro_samples = math::min(_fifo_empty_interval_us / (1000000 / GYRO_RATE), FIFO_MAX_SAMPLES);
-
-	// recompute FIFO empty interval (us) with actual gyro sample limit
-	_fifo_empty_interval_us = _fifo_gyro_samples * (1000000 / GYRO_RATE);
-
-	_fifo_accel_samples = math::min(_fifo_empty_interval_us / (1000000 / ACCEL_RATE), FIFO_MAX_SAMPLES);
-
-	_px4_accel.set_update_rate(1000000 / _fifo_empty_interval_us);
-	_px4_gyro.set_update_rate(1000000 / _fifo_empty_interval_us);
-}
-
-int MPU9250::probe()
-{
-	const uint8_t whoami = RegisterRead(Register::WHO_AM_I);
-
-	if (whoami != WHOAMI) {
-		PX4_WARN("unexpected WHO_AM_I 0x%02x", whoami);
-		return PX4_ERROR;
-	}
-
-	return PX4_OK;
-}
-
 bool MPU9250::Init()
 {
 	if (SPI::init() != PX4_OK) {
@@ -122,31 +88,199 @@ bool MPU9250::Init()
 		return false;
 	}
 
-	if (!Reset()) {
-		PX4_ERR("reset failed");
-		return false;
+	return Reset();
+}
+
+void MPU9250::Stop()
+{
+	// wait until stopped
+	while (_state.load() != STATE::STOPPED) {
+		_state.store(STATE::REQUEST_STOP);
+		ScheduleNow();
+		px4_usleep(10);
 	}
-
-	Start();
-
-	return true;
 }
 
 bool MPU9250::Reset()
 {
-	// PWR_MGMT_1: Device Reset
-	RegisterWrite(Register::PWR_MGMT_1, PWR_MGMT_1_BIT::H_RESET);
+	_state.store(STATE::RESET);
+	ScheduleClear();
+	ScheduleNow();
+	return true;
+}
+
+void MPU9250::PrintInfo()
+{
+	PX4_INFO("FIFO empty interval: %d us (%.3f Hz)", _fifo_empty_interval_us,
+		 static_cast<double>(1000000 / _fifo_empty_interval_us));
+
+	perf_print_counter(_transfer_perf);
+	perf_print_counter(_bad_register_perf);
+	perf_print_counter(_bad_transfer_perf);
+	perf_print_counter(_fifo_empty_perf);
+	perf_print_counter(_fifo_overflow_perf);
+	perf_print_counter(_fifo_reset_perf);
+	perf_print_counter(_drdy_interval_perf);
+
+	_px4_accel.print_status();
+	_px4_gyro.print_status();
+}
+
+int MPU9250::probe()
+{
+	const uint8_t whoami = RegisterRead(Register::WHO_AM_I);
+
+	if (whoami != WHOAMI) {
+		PX4_WARN("unexpected WHO_AM_I 0x%02x", whoami);
+		return PX4_ERROR;
+	}
+
+	return PX4_OK;
+}
+
+void MPU9250::Run()
+{
+	switch (_state.load()) {
+	case STATE::RESET:
+		// PWR_MGMT_1: Device Reset
+		RegisterWrite(Register::PWR_MGMT_1, PWR_MGMT_1_BIT::H_RESET);
+		_reset_timestamp = hrt_absolute_time();
+		_state.store(STATE::WAIT_FOR_RESET);
+		ScheduleDelayed(100);
+		break;
+
+	case STATE::WAIT_FOR_RESET:
 
-	for (int i = 0; i < 100; i++) {
 		// The reset value is 0x00 for all registers other than the registers below
 		//  Document Number: RM-MPU-9250A-00 Page 9 of 55
 		if ((RegisterRead(Register::WHO_AM_I) == WHOAMI)
 		    && (RegisterRead(Register::PWR_MGMT_1) == 0x01)) {
-			return true;
-		}
-	}
 
-	return false;
+			// if reset succeeded then configure
+			_state.store(STATE::CONFIGURE);
+			ScheduleNow();
+
+		} else {
+			// RESET not complete
+			if (hrt_elapsed_time(&_reset_timestamp) > 10_ms) {
+				PX4_ERR("Reset failed, retrying");
+				_state.store(STATE::RESET);
+				ScheduleDelayed(10_ms);
+
+			} else {
+				PX4_DEBUG("Reset not complete, check again in 1 ms");
+				ScheduleDelayed(1_ms);
+			}
+		}
+
+		break;
+
+	case STATE::CONFIGURE:
+		if (Configure()) {
+			// if configure succeeded then start reading from FIFO
+			_state.store(STATE::FIFO_READ);
+
+			if (DataReadyInterruptConfigure()) {
+				_data_ready_interrupt_enabled = true;
+
+				// backup schedule as a watchdog timeout
+				ScheduleDelayed(10_ms);
+
+			} else {
+				_data_ready_interrupt_enabled = false;
+				ScheduleOnInterval(_fifo_empty_interval_us, _fifo_empty_interval_us);
+			}
+
+			FIFOReset();
+
+		} else {
+			PX4_DEBUG("Configure failed, retrying");
+			// try again in 1 ms
+			ScheduleDelayed(1_ms);
+		}
+
+		break;
+
+	case STATE::FIFO_READ: {
+			hrt_abstime timestamp_sample = 0;
+			uint8_t samples = 0;
+
+			if (_data_ready_interrupt_enabled) {
+				// re-schedule as watchdog timeout
+				ScheduleDelayed(10_ms);
+
+				// timestamp set in data ready interrupt
+				samples = _fifo_read_samples.load();
+				timestamp_sample = _fifo_watermark_interrupt_timestamp;
+			}
+
+			bool failure = false;
+
+			// manually check FIFO count if no samples from DRDY or timestamp looks bogus
+			if (!_data_ready_interrupt_enabled || (samples == 0)
+			    || (hrt_elapsed_time(&timestamp_sample) > (_fifo_empty_interval_us / 2))) {
+
+				// use the time now roughly corresponding with the last sample we'll pull from the FIFO
+				timestamp_sample = hrt_absolute_time();
+				const uint16_t fifo_count = FIFOReadCount();
+
+				if (fifo_count == 0) {
+					failure = true;
+					perf_count(_fifo_empty_perf);
+				}
+
+				samples = (fifo_count / sizeof(FIFO::DATA) / 2) * 2; // round down to nearest 2
+			}
+
+			if (samples > FIFO_MAX_SAMPLES) {
+				// not technically an overflow, but more samples than we expected or can publish
+				perf_count(_fifo_overflow_perf);
+				failure = true;
+				FIFOReset();
+
+			} else if (samples >= 2) {
+				// require at least 2 samples (we want at least 1 new accel sample per transfer)
+				if (!FIFORead(timestamp_sample, samples)) {
+					failure = true;
+					_px4_accel.increase_error_count();
+					_px4_gyro.increase_error_count();
+				}
+			}
+
+			if (failure || hrt_elapsed_time(&_last_config_check_timestamp) > 10_ms) {
+				// check registers incrementally
+				if (RegisterCheck(_register_cfg[_checked_register], true)) {
+					_last_config_check_timestamp = timestamp_sample;
+					_checked_register = (_checked_register + 1) % size_register_cfg;
+
+				} else {
+					// register check failed, force reconfigure
+					PX4_DEBUG("Health check failed, reconfiguring");
+					_state.store(STATE::CONFIGURE);
+					ScheduleNow();
+				}
+
+			} else {
+				// periodically update temperature (1 Hz)
+				if (hrt_elapsed_time(&_temperature_update_timestamp) > 1_s) {
+					UpdateTemperature();
+					_temperature_update_timestamp = timestamp_sample;
+				}
+			}
+		}
+
+		break;
+
+	case STATE::REQUEST_STOP:
+		DataReadyInterruptDisable();
+		ScheduleClear();
+		_state.store(STATE::STOPPED);
+		break;
+
+	case STATE::STOPPED:
+		// DO NOTHING
+		break;
+	}
 }
 
 void MPU9250::ConfigureAccel()
@@ -203,57 +337,98 @@ void MPU9250::ConfigureGyro()
 	}
 }
 
-void MPU9250::ResetFIFO()
+void MPU9250::ConfigureSampleRate(int sample_rate)
 {
-	perf_count(_fifo_reset_perf);
+	if (sample_rate == 0) {
+		sample_rate = 1000; // default to 1 kHz
+	}
 
-	// USER_CTRL: disable FIFO and reset all signal paths
-	RegisterSetAndClearBits(Register::USER_CTRL, USER_CTRL_BIT::FIFO_RST | USER_CTRL_BIT::SIG_COND_RST,
-				USER_CTRL_BIT::FIFO_EN);
+	_fifo_empty_interval_us = math::max(((1000000 / sample_rate) / 250) * 250, 250); // round down to nearest 250 us
+	_fifo_gyro_samples = math::min(_fifo_empty_interval_us / (1000000 / GYRO_RATE), FIFO_MAX_SAMPLES);
 
-	_data_ready_count.store(0);
+	// recompute FIFO empty interval (us) with actual gyro sample limit
+	_fifo_empty_interval_us = _fifo_gyro_samples * (1000000 / GYRO_RATE);
 
-	// FIFO_EN: enable both gyro and accel
-	RegisterWrite(Register::FIFO_EN, FIFO_EN_BIT::GYRO_XOUT | FIFO_EN_BIT::GYRO_YOUT | FIFO_EN_BIT::GYRO_ZOUT |
-		      FIFO_EN_BIT::ACCEL);
+	_fifo_accel_samples = math::min(_fifo_empty_interval_us / (1000000 / ACCEL_RATE), FIFO_MAX_SAMPLES);
 
-	// USER_CTRL: re-enable FIFO
-	RegisterSetAndClearBits(Register::USER_CTRL, USER_CTRL_BIT::FIFO_EN,
-				USER_CTRL_BIT::FIFO_RST | USER_CTRL_BIT::SIG_COND_RST);
+	_px4_accel.set_update_rate(1000000 / _fifo_empty_interval_us);
+	_px4_gyro.set_update_rate(1000000 / _fifo_empty_interval_us);
 }
 
-bool MPU9250::Configure(bool notify)
+bool MPU9250::Configure()
 {
 	bool success = true;
 
 	for (const auto &reg : _register_cfg) {
-		if (!CheckRegister(reg, notify)) {
+		if (!RegisterCheck(reg)) {
 			success = false;
 		}
 	}
 
+	ConfigureAccel();
+	ConfigureGyro();
+
 	return success;
 }
 
-bool MPU9250::CheckRegister(const register_config_t &reg_cfg, bool notify)
+int MPU9250::DataReadyInterruptCallback(int irq, void *context, void *arg)
+{
+	static_cast<MPU9250 *>(arg)->DataReady();
+	return 0;
+}
+
+void MPU9250::DataReady()
+{
+	if (_data_ready_count.fetch_add(1) >= (_fifo_gyro_samples - 1)) {
+		_data_ready_count.store(0);
+		_fifo_watermark_interrupt_timestamp = hrt_absolute_time();
+		_fifo_read_samples.store(_fifo_gyro_samples);
+		ScheduleNow();
+	}
+
+	perf_count(_drdy_interval_perf);
+}
+
+bool MPU9250::DataReadyInterruptConfigure()
+{
+	int ret_setevent = -1;
+
+	// Setup data ready on rising edge
+	// TODO: cleanup horrible DRDY define mess
+#if defined(GPIO_DRDY_PORTD_PIN15)
+	ret_setevent = px4_arch_gpiosetevent(GPIO_DRDY_PORTD_PIN15, true, false, true, &MPU9250::DataReadyInterruptCallback,
+					     this);
+#endif
+
+	return (ret_setevent == 0);
+}
+
+bool MPU9250::DataReadyInterruptDisable()
+{
+	int ret_setevent = -1;
+
+	// Disable data ready callback
+	// TODO: cleanup horrible DRDY define mess
+#if defined(GPIO_DRDY_PORTD_PIN15)
+	ret_setevent = px4_arch_gpiosetevent(GPIO_DRDY_PORTD_PIN15, false, false, false, nullptr, nullptr);
+#endif
+
+	return (ret_setevent == 0);
+}
+
+bool MPU9250::RegisterCheck(const register_config_t &reg_cfg, bool notify)
 {
 	bool success = true;
 
 	const uint8_t reg_value = RegisterRead(reg_cfg.reg);
 
 	if (reg_cfg.set_bits && !(reg_value & reg_cfg.set_bits)) {
-		if (notify) {
-			PX4_ERR("0x%02hhX: 0x%02hhX (0x%02hhX not set)", (uint8_t)reg_cfg.reg, reg_value, reg_cfg.set_bits);
-		}
-
+		PX4_DEBUG("0x%02hhX: 0x%02hhX (0x%02hhX not set)", (uint8_t)reg_cfg.reg, reg_value, reg_cfg.set_bits);
 		success = false;
 	}
 
 	if (reg_cfg.clear_bits && (reg_value & reg_cfg.clear_bits)) {
-		if (notify) {
-			PX4_ERR("0x%02hhX: 0x%02hhX (0x%02hhX not cleared)", (uint8_t)reg_cfg.reg, reg_value, reg_cfg.clear_bits);
-		}
-
+		PX4_DEBUG("0x%02hhX: 0x%02hhX (0x%02hhX not cleared)", (uint8_t)reg_cfg.reg, reg_value, reg_cfg.clear_bits);
 		success = false;
 	}
 
@@ -269,6 +444,8 @@ bool MPU9250::CheckRegister(const register_config_t &reg_cfg, bool notify)
 
 		if (notify) {
 			perf_count(_bad_register_perf);
+			_px4_accel.increase_error_count();
+			_px4_gyro.increase_error_count();
 		}
 	}
 
@@ -315,145 +492,84 @@ void MPU9250::RegisterClearBits(Register reg, uint8_t clearbits)
 	RegisterSetAndClearBits(reg, 0, clearbits);
 }
 
-int MPU9250::DataReadyInterruptCallback(int irq, void *context, void *arg)
+uint16_t MPU9250::FIFOReadCount()
 {
-	MPU9250 *dev = reinterpret_cast<MPU9250 *>(arg);
-	dev->DataReady();
-	return 0;
-}
-
-void MPU9250::DataReady()
-{
-	perf_count(_drdy_interval_perf);
-
-	if (_data_ready_count.fetch_add(1) >= (_fifo_gyro_samples - 1)) {
-		// make another measurement
-		ScheduleNow();
-		_data_ready_count.store(0);
-	}
-}
-
-void MPU9250::Start()
-{
-	ConfigureSampleRate(_px4_gyro.get_max_rate_hz());
-
-	// attempt to configure 3 times
-	for (int i = 0; i < 3; i++) {
-		if (Configure(false)) {
-			break;
-		}
-	}
-
-	// TODO: cleanup horrible DRDY define mess
-#if defined(GPIO_DRDY_PORTD_PIN15)
-	_using_data_ready_interrupt_enabled = true;
-	// Setup data ready on rising edge
-	px4_arch_gpiosetevent(GPIO_DRDY_PORTD_PIN15, true, false, true, &MPU9250::DataReadyInterruptCallback, this);
-#else
-	_using_data_ready_interrupt_enabled = false;
-	ScheduleOnInterval(FIFO_INTERVAL, FIFO_INTERVAL);
-#endif
-
-	ResetFIFO();
-
-	// schedule as watchdog
-	if (_using_data_ready_interrupt_enabled) {
-		ScheduleDelayed(100_ms);
-	}
-}
-
-void MPU9250::Stop()
-{
-	Reset();
-
-	// TODO: cleanup horrible DRDY define mess
-#if defined(GPIO_DRDY_PORTD_PIN15)
-	// Disable data ready callback
-	px4_arch_gpiosetevent(GPIO_DRDY_PORTD_PIN15, false, false, false, nullptr, nullptr);
-#endif
-
-	ScheduleClear();
-}
-
-void MPU9250::Run()
-{
-	// use the time now roughly corresponding with the last sample we'll pull from the FIFO
-	const hrt_abstime timestamp_sample = hrt_absolute_time();
-
 	// read FIFO count
 	uint8_t fifo_count_buf[3] {};
 	fifo_count_buf[0] = static_cast<uint8_t>(Register::FIFO_COUNTH) | DIR_READ;
 
 	if (transfer(fifo_count_buf, fifo_count_buf, sizeof(fifo_count_buf)) != PX4_OK) {
 		perf_count(_bad_transfer_perf);
+		return 0;
 	}
 
-	if (_using_data_ready_interrupt_enabled) {
-		// re-schedule as watchdog
-		ScheduleDelayed(100_ms);
-	}
-
-	// check registers
-	if (hrt_elapsed_time(&_last_config_check) > 100_ms) {
-		_checked_register = (_checked_register + 1) % size_register_cfg;
-
-		if (CheckRegister(_register_cfg[_checked_register])) {
-			// delay next register check if current succeeded
-			_last_config_check = hrt_absolute_time();
-
-		} else {
-			// if register check failed reconfigure all
-			Configure();
-			ResetFIFO();
-			return;
-		}
-	}
-
-	const uint16_t fifo_count = combine(fifo_count_buf[1], fifo_count_buf[2]);
-	const uint8_t samples = (fifo_count / sizeof(FIFO::DATA) / 2) * 2; // round down to nearest 2
-
-	if (samples < 2) {
-		perf_count(_fifo_empty_perf);
-		return;
-
-	} else if (samples > FIFO_MAX_SAMPLES) {
-		// not technically an overflow, but more samples than we expected or can publish
-		perf_count(_fifo_overflow_perf);
-		ResetFIFO();
-
-		return;
-	}
-
-	// Transfer data
-	struct TransferBuffer {
-		uint8_t cmd;
-		FIFO::DATA f[FIFO_MAX_SAMPLES];
-	};
-	// ensure no struct padding
-	static_assert(sizeof(TransferBuffer) == (sizeof(uint8_t) + FIFO_MAX_SAMPLES * sizeof(FIFO::DATA)));
+	return combine(fifo_count_buf[1], fifo_count_buf[2]);
+}
 
+bool MPU9250::FIFORead(const hrt_abstime &timestamp_sample, uint16_t samples)
+{
 	TransferBuffer *report = (TransferBuffer *)_dma_data_buffer;
 	const size_t transfer_size = math::min(samples * sizeof(FIFO::DATA) + 1, FIFO::SIZE);
 	memset(report, 0, transfer_size);
 	report->cmd = static_cast<uint8_t>(Register::FIFO_R_W) | DIR_READ;
 
 	perf_begin(_transfer_perf);
+	set_frequency(SPI_SPEED_SENSOR);
 
 	if (transfer(_dma_data_buffer, _dma_data_buffer, transfer_size) != PX4_OK) {
+		set_frequency(SPI_SPEED); // restore normal speed
 		perf_end(_transfer_perf);
 		perf_count(_bad_transfer_perf);
-		return;
+		return false;
 	}
 
+	set_frequency(SPI_SPEED); // restore normal speed
 	perf_end(_transfer_perf);
 
+	ProcessGyro(timestamp_sample, report, samples);
+	return ProcessAccel(timestamp_sample, report, samples);
+}
 
+void MPU9250::FIFOReset()
+{
+	perf_count(_fifo_reset_perf);
+
+	// FIFO_EN: disable FIFO
+	RegisterWrite(Register::FIFO_EN, 0);
+
+	// USER_CTRL: disable FIFO and reset all signal paths
+	RegisterSetAndClearBits(Register::USER_CTRL, USER_CTRL_BIT::FIFO_RST | USER_CTRL_BIT::SIG_COND_RST,
+				USER_CTRL_BIT::FIFO_EN);
+
+	// reset while FIFO is disabled
+	_data_ready_count.store(0);
+	_fifo_watermark_interrupt_timestamp = 0;
+	_fifo_read_samples.store(0);
+
+	// FIFO_EN: enable both gyro and accel
+	// USER_CTRL: re-enable FIFO
+	for (const auto &r : _register_cfg) {
+		if ((r.reg == Register::FIFO_EN) || (r.reg == Register::USER_CTRL)) {
+			RegisterSetAndClearBits(r.reg, r.set_bits, r.clear_bits);
+		}
+	}
+}
+
+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 MPU9250::ProcessAccel(const hrt_abstime &timestamp_sample, const TransferBuffer *const report, uint8_t samples)
+{
 	PX4Accelerometer::FIFOSample accel;
 	accel.timestamp_sample = timestamp_sample;
 	accel.dt = _fifo_empty_interval_us / _fifo_accel_samples;
 
+	bool bad_data = false;
+
 	// accel data is doubled in FIFO, but might be shifted
-	int accel_first_sample = 0;
+	int accel_first_sample = 1;
 
 	if (samples >= 3) {
 		if (fifo_accel_equal(report->f[0], report->f[1])) {
@@ -468,7 +584,7 @@ void MPU9250::Run()
 
 		} else {
 			perf_count(_bad_transfer_perf);
-			return;
+			bad_data = true;
 		}
 	}
 
@@ -480,7 +596,8 @@ void MPU9250::Run()
 		int16_t accel_y = combine(fifo_sample.ACCEL_YOUT_H, fifo_sample.ACCEL_YOUT_L);
 		int16_t accel_z = combine(fifo_sample.ACCEL_ZOUT_H, fifo_sample.ACCEL_ZOUT_L);
 
-		// sensor's frame is +x forward, +y left, +z up, flip y & z to publish right handed (x forward, y right, z down)
+		// 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;
@@ -489,7 +606,13 @@ void MPU9250::Run()
 
 	accel.samples = accel_samples;
 
+	_px4_accel.updateFIFO(accel);
 
+	return !bad_data;
+}
+
+void MPU9250::ProcessGyro(const hrt_abstime &timestamp_sample, const TransferBuffer *const report, uint8_t samples)
+{
 	PX4Gyroscope::FIFOSample gyro;
 	gyro.timestamp_sample = timestamp_sample;
 	gyro.samples = samples;
@@ -502,46 +625,32 @@ void MPU9250::Run()
 		const int16_t gyro_y = combine(fifo_sample.GYRO_YOUT_H, fifo_sample.GYRO_YOUT_L);
 		const int16_t gyro_z = combine(fifo_sample.GYRO_ZOUT_H, fifo_sample.GYRO_ZOUT_L);
 
-		// sensor's frame is +x forward, +y left, +z up, flip y & z to publish right handed (x forward, y right, z down)
+		// 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;
 	}
 
-	// Temperature
-	if (hrt_elapsed_time(&_time_last_temperature_update) > 1_s) {
-		// read current temperature
-		uint8_t temperature_buf[3] {};
-		temperature_buf[0] = static_cast<uint8_t>(Register::TEMP_OUT_H) | DIR_READ;
+	_px4_gyro.updateFIFO(gyro);
+}
 
-		if (transfer(temperature_buf, temperature_buf, sizeof(temperature_buf)) != PX4_OK) {
-			return;
-		}
+void MPU9250::UpdateTemperature()
+{
+	// read current temperature
+	uint8_t temperature_buf[3] {};
+	temperature_buf[0] = static_cast<uint8_t>(Register::TEMP_OUT_H) | DIR_READ;
 
-		const int16_t TEMP_OUT = combine(temperature_buf[1], temperature_buf[2]);
-		const float TEMP_degC = ((TEMP_OUT - ROOM_TEMPERATURE_OFFSET) / TEMPERATURE_SENSITIVITY) + ROOM_TEMPERATURE_OFFSET;
+	if (transfer(temperature_buf, temperature_buf, sizeof(temperature_buf)) != PX4_OK) {
+		perf_count(_bad_transfer_perf);
+		return;
+	}
 
+	const int16_t TEMP_OUT = combine(temperature_buf[1], temperature_buf[2]);
+	const float TEMP_degC = ((TEMP_OUT - ROOM_TEMPERATURE_OFFSET) / TEMPERATURE_SENSITIVITY) + ROOM_TEMPERATURE_OFFSET;
+
+	if (PX4_ISFINITE(TEMP_degC)) {
 		_px4_accel.set_temperature(TEMP_degC);
 		_px4_gyro.set_temperature(TEMP_degC);
 	}
-
-	_px4_gyro.updateFIFO(gyro);
-	_px4_accel.updateFIFO(accel);
-}
-
-void MPU9250::PrintInfo()
-{
-	PX4_INFO("FIFO empty interval: %d us (%.3f Hz)", _fifo_empty_interval_us,
-		 static_cast<double>(1000000 / _fifo_empty_interval_us));
-
-	perf_print_counter(_transfer_perf);
-	perf_print_counter(_bad_register_perf);
-	perf_print_counter(_bad_transfer_perf);
-	perf_print_counter(_fifo_empty_perf);
-	perf_print_counter(_fifo_overflow_perf);
-	perf_print_counter(_fifo_reset_perf);
-	perf_print_counter(_drdy_interval_perf);
-
-	_px4_accel.print_status();
-	_px4_gyro.print_status();
 }
diff --git a/src/drivers/imu/invensense/mpu9250/MPU9250.hpp b/src/drivers/imu/invensense/mpu9250/MPU9250.hpp
index 61cfafc0bb..0c660668b4 100644
--- a/src/drivers/imu/invensense/mpu9250/MPU9250.hpp
+++ b/src/drivers/imu/invensense/mpu9250/MPU9250.hpp
@@ -67,6 +67,19 @@ public:
 
 private:
 
+	// Sensor Configuration
+	static constexpr uint32_t GYRO_RATE{8000};  // 8 kHz gyro
+	static constexpr uint32_t ACCEL_RATE{4000}; // 4 kHz accel
+	static constexpr uint32_t FIFO_MAX_SAMPLES{ math::min(FIFO::SIZE / sizeof(FIFO::DATA) + 1, sizeof(PX4Gyroscope::FIFOSample::x) / sizeof(PX4Gyroscope::FIFOSample::x[0]))};
+
+	// Transfer data
+	struct TransferBuffer {
+		uint8_t cmd;
+		FIFO::DATA f[FIFO_MAX_SAMPLES];
+	};
+	// ensure no struct padding
+	static_assert(sizeof(TransferBuffer) == (sizeof(uint8_t) + FIFO_MAX_SAMPLES *sizeof(FIFO::DATA)));
+
 	struct register_config_t {
 		Register reg;
 		uint8_t set_bits{0};
@@ -75,17 +88,19 @@ private:
 
 	int probe() override;
 
-	static int DataReadyInterruptCallback(int irq, void *context, void *arg);
-	void DataReady();
-
 	void Run() override;
 
-	void ConfigureSampleRate(int sample_rate);
-	bool CheckRegister(const register_config_t &reg_cfg, bool notify = true);
-	bool Configure(bool notify = true);
-
+	bool Configure();
 	void ConfigureAccel();
 	void ConfigureGyro();
+	void ConfigureSampleRate(int sample_rate);
+
+	static int DataReadyInterruptCallback(int irq, void *context, void *arg);
+	void DataReady();
+	bool DataReadyInterruptConfigure();
+	bool DataReadyInterruptDisable();
+
+	bool RegisterCheck(const register_config_t &reg_cfg, bool notify = false);
 
 	uint8_t RegisterRead(Register reg);
 	void RegisterWrite(Register reg, uint8_t value);
@@ -93,7 +108,13 @@ private:
 	void RegisterSetBits(Register reg, uint8_t setbits);
 	void RegisterClearBits(Register reg, uint8_t clearbits);
 
-	void ResetFIFO();
+	uint16_t FIFOReadCount();
+	bool FIFORead(const hrt_abstime &timestamp_sample, uint16_t samples);
+	void FIFOReset();
+
+	bool ProcessAccel(const hrt_abstime &timestamp_sample, const TransferBuffer *const buffer, uint8_t samples);
+	void ProcessGyro(const hrt_abstime &timestamp_sample, const TransferBuffer *const buffer, uint8_t samples);
+	void UpdateTemperature();
 
 	uint8_t *_dma_data_buffer{nullptr};
 
@@ -103,24 +124,31 @@ private:
 	perf_counter_t _transfer_perf{perf_alloc(PC_ELAPSED, MODULE_NAME": transfer")};
 	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_interval_perf{perf_alloc(PC_INTERVAL, MODULE_NAME": drdy interval")};
+	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_interval_perf{perf_alloc(PC_INTERVAL, MODULE_NAME": DRDY interval")};
 
-	hrt_abstime _last_config_check{0};
-	hrt_abstime _time_last_temperature_update{0};
-
-	px4::atomic<int> _data_ready_count{0};
+	hrt_abstime _reset_timestamp{0};
+	hrt_abstime _last_config_check_timestamp{0};
+	hrt_abstime _fifo_watermark_interrupt_timestamp{0};
+	hrt_abstime _temperature_update_timestamp{0};
 
+	px4::atomic<uint8_t> _data_ready_count{0};
+	px4::atomic<uint8_t> _fifo_read_samples{0};
+	bool _data_ready_interrupt_enabled{false};
 	uint8_t _checked_register{0};
 
-	bool _using_data_ready_interrupt_enabled{false};
+	enum class STATE : uint8_t {
+		RESET,
+		WAIT_FOR_RESET,
+		CONFIGURE,
+		FIFO_READ,
+		REQUEST_STOP,
+		STOPPED,
+	};
 
-	// Sensor Configuration
-	static constexpr uint32_t GYRO_RATE{8000};  // 8 kHz gyro
-	static constexpr uint32_t ACCEL_RATE{4000}; // 4 kHz accel
-	static constexpr uint32_t FIFO_MAX_SAMPLES{ math::min(FIFO::SIZE / sizeof(FIFO::DATA) + 1, sizeof(PX4Gyroscope::FIFOSample::x) / sizeof(PX4Gyroscope::FIFOSample::x[0]))};
+	px4::atomic<STATE> _state{STATE::RESET};
 
 	uint16_t _fifo_empty_interval_us{1000}; // 1000 us / 1000 Hz transfer interval
 	uint8_t _fifo_gyro_samples{static_cast<uint8_t>(_fifo_empty_interval_us / (1000000 / GYRO_RATE))};
@@ -136,6 +164,6 @@ private:
 		{ Register::CONFIG,        CONFIG_BIT::DLPF_CFG_BYPASS_DLPF_8KHZ, Bit7 | CONFIG_BIT::FIFO_MODE },
 		{ Register::USER_CTRL,     USER_CTRL_BIT::FIFO_EN, 0 },
 		{ Register::FIFO_EN,       FIFO_EN_BIT::GYRO_XOUT | FIFO_EN_BIT::GYRO_YOUT | FIFO_EN_BIT::GYRO_ZOUT | FIFO_EN_BIT::ACCEL, 0 },
-		{ Register::INT_ENABLE,    INT_ENABLE_BIT::FIFO_OFLOW_EN | INT_ENABLE_BIT::RAW_RDY_EN }
+		{ Register::INT_ENABLE,    INT_ENABLE_BIT::RAW_RDY_EN, 0 }
 	};
 };