diff --git a/src/drivers/imu/invensense/icm40609d/CMakeLists.txt b/src/drivers/imu/invensense/icm40609d/CMakeLists.txt
index 16ee7e8019..148a612aab 100644
--- a/src/drivers/imu/invensense/icm40609d/CMakeLists.txt
+++ b/src/drivers/imu/invensense/icm40609d/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__icm40609d
 	MAIN icm40609d
 	COMPILE_FLAGS
+		${MAX_CUSTOM_OPT_LEVEL}
 	SRCS
 		icm40609d_main.cpp
 		ICM40609D.cpp
diff --git a/src/drivers/imu/invensense/icm40609d/ICM40609D.cpp b/src/drivers/imu/invensense/icm40609d/ICM40609D.cpp
index 8dc0f4b795..fe30183a0c 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
@@ -146,7 +146,8 @@ void ICM40609D::RunImpl()
 
 	case STATE::WAIT_FOR_RESET:
 		if ((RegisterRead(Register::BANK_0::WHO_AM_I) == WHOAMI)
-		    && (RegisterRead(Register::BANK_0::DEVICE_CONFIG) == 0x00)) {
+		    && (RegisterRead(Register::BANK_0::DEVICE_CONFIG) == 0x00)
+		    && (RegisterRead(Register::BANK_0::INT_STATUS) & INT_STATUS_BIT::RESET_DONE_INT)) {
 
 			// Wakeup accel and gyro and schedule remaining configuration
 			RegisterWrite(Register::BANK_0::PWR_MGMT0, PWR_MGMT0_BIT::GYRO_MODE_LOW_NOISE | PWR_MGMT0_BIT::ACCEL_MODE_LOW_NOISE);
@@ -273,8 +274,8 @@ void ICM40609D::RunImpl()
 				}
 			}
 
+			// 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 +286,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,65 +293,6 @@ 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
@@ -418,8 +353,11 @@ bool ICM40609D::Configure()
 		}
 	}
 
-	ConfigureAccel();
-	ConfigureGyro();
+	_px4_accel.set_range(32.f * CONSTANTS_ONE_G);
+	_px4_accel.set_scale(CONSTANTS_ONE_G / 1024.f);
+
+	_px4_gyro.set_range(math::radians(2000.f));
+	_px4_gyro.set_scale(math::radians(2000.f / 32768.f));
 
 	return success;
 }
@@ -584,9 +522,11 @@ bool ICM40609D::FIFORead(const hrt_abstime &timestamp_sample, uint8_t samples)
 	}
 
 	if (valid_samples > 0) {
-		ProcessGyro(timestamp_sample, buffer.f, valid_samples);
-		ProcessAccel(timestamp_sample, buffer.f, valid_samples);
-		return true;
+		if (ProcessTemperature(buffer.f, valid_samples)) {
+			ProcessGyro(timestamp_sample, buffer.f, valid_samples);
+			ProcessAccel(timestamp_sample, buffer.f, valid_samples);
+			return true;
+		}
 	}
 
 	return false;
@@ -607,7 +547,7 @@ void ICM40609D::ProcessAccel(const hrt_abstime &timestamp_sample, const FIFO::DA
 {
 	sensor_accel_fifo_s accel{};
 	accel.timestamp_sample = timestamp_sample;
-	accel.samples = 0;
+	accel.samples = samples;
 	accel.dt = FIFO_SAMPLE_DT;
 
 	for (int i = 0; i < samples; i++) {
@@ -617,18 +557,15 @@ void ICM40609D::ProcessAccel(const hrt_abstime &timestamp_sample, const FIFO::DA
 
 		// 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[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));
 
-	if (accel.samples > 0) {
-		_px4_accel.updateFIFO(accel);
-	}
+	_px4_accel.updateFIFO(accel);
 }
 
 void ICM40609D::ProcessGyro(const hrt_abstime &timestamp_sample, const FIFO::DATA fifo[], const uint8_t samples)
@@ -639,15 +576,15 @@ void ICM40609D::ProcessGyro(const hrt_abstime &timestamp_sample, const FIFO::DAT
 	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);
+		int16_t gyro_x = combine(fifo[i].GYRO_DATA_X1, fifo[i].GYRO_DATA_X0);
+		int16_t gyro_y = combine(fifo[i].GYRO_DATA_Y1, fifo[i].GYRO_DATA_Y0);
+		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;
+		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) +
@@ -656,25 +593,32 @@ void ICM40609D::ProcessGyro(const hrt_abstime &timestamp_sample, const FIFO::DAT
 	_px4_gyro.updateFIFO(gyro);
 }
 
-void ICM40609D::UpdateTemperature()
+bool ICM40609D::ProcessTemperature(const FIFO::DATA fifo[], const uint8_t samples)
 {
-	// 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);
+	float temperature_sum{0};
+	int valid_samples = 0;
 
-	if (transfer(temperature_buf, temperature_buf, sizeof(temperature_buf)) != PX4_OK) {
-		perf_count(_bad_transfer_perf);
-		return;
+	for (int i = 0; i < samples; i++) {
+		// Temperature data stored in FIFO is an 8-bit quantity, FIFO_TEMP_DATA
+		// It can be converted to degrees centigrade by using the following formula:
+		//  Temperature in Degrees Centigrade = (FIFO_TEMP_DATA / 2.07) + 25
+		temperature_sum += (fifo[i].FIFO_TEMP_DATA / 2.07f) + 25.f;
+		valid_samples++;
 	}
 
-	const int16_t TEMP_DATA = combine(temperature_buf[1], temperature_buf[2]);
+	if (valid_samples > 0) {
+		// use average temperature reading
+		const float temperature_avg = temperature_sum / valid_samples;
 
-	// Temperature in Degrees Centigrade
-	const float TEMP_degC = (TEMP_DATA / TEMPERATURE_SENSITIVITY) + TEMPERATURE_OFFSET;
+		if (PX4_ISFINITE(temperature_avg)) {
+			_px4_accel.set_temperature(temperature_avg);
+			_px4_gyro.set_temperature(temperature_avg);
+			return true;
 
-	if (PX4_ISFINITE(TEMP_degC)) {
-		_px4_accel.set_temperature(TEMP_degC);
-		_px4_gyro.set_temperature(TEMP_degC);
+		} else {
+			perf_count(_bad_transfer_perf);
+		}
 	}
+
+	return false;
 }
diff --git a/src/drivers/imu/invensense/icm40609d/ICM40609D.hpp b/src/drivers/imu/invensense/icm40609d/ICM40609D.hpp
index ab6fcfa1f2..6fe77d0fd3 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
@@ -70,12 +70,12 @@ private:
 	void exit_and_cleanup() override;
 
 	// Sensor Configuration
-	static constexpr float FIFO_SAMPLE_DT{1e6f / 8000.f};     // 8000 Hz accel & gyro ODR configured
+	static constexpr float FIFO_SAMPLE_DT{1e6f / 8000.f};     // 8000 Hz accel & gyro ODR configured (must match GYRO_CONFIG0/ACCEL_CONFIG0)
 	static constexpr float GYRO_RATE{1e6f / FIFO_SAMPLE_DT};
 	static constexpr float ACCEL_RATE{1e6f / FIFO_SAMPLE_DT};
 
 	// maximum FIFO samples per transfer is limited to the size of sensor_accel_fifo/sensor_gyro_fifo
-	static constexpr 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), 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 {
@@ -99,8 +99,6 @@ private:
 	bool Reset();
 
 	bool Configure();
-	void ConfigureAccel();
-	void ConfigureGyro();
 	void ConfigureSampleRate(int sample_rate);
 	void ConfigureFIFOWatermark(uint8_t samples);
 
@@ -125,7 +123,7 @@ private:
 
 	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();
+	bool ProcessTemperature(const FIFO::DATA fifo[], const uint8_t samples);
 
 	const spi_drdy_gpio_t _drdy_gpio;
 
@@ -160,18 +158,21 @@ private:
 	int32_t _fifo_gyro_samples{static_cast<int32_t>(_fifo_empty_interval_us / (1000000 / GYRO_RATE))};
 
 	uint8_t _checked_register_bank0{0};
-	static constexpr uint8_t size_register_bank0_cfg{10};
+	static constexpr uint8_t size_register_bank0_cfg{13};
 	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_ODR_8kHz, Bit7 | Bit6 | Bit5 | Bit3 | Bit2 },
-		{ Register::BANK_0::ACCEL_CONFIG0, ACCEL_CONFIG0_BIT::ACCEL_ODR_8kHz, Bit7 | Bit6 | Bit5 | Bit3 | Bit2 },
-		{ Register::BANK_0::FIFO_CONFIG1,  FIFO_CONFIG1_BIT::FIFO_WM_GT_TH | FIFO_CONFIG1_BIT::FIFO_GYRO_EN | FIFO_CONFIG1_BIT::FIFO_ACCEL_EN, FIFO_CONFIG1_BIT::FIFO_TEMP_EN },
-		{ 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                              | 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_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_32G_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::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_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::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 },
 	};
 };
diff --git a/src/drivers/imu/invensense/icm40609d/InvenSense_ICM40609D_registers.hpp b/src/drivers/imu/invensense/icm40609d/InvenSense_ICM40609D_registers.hpp
index 4d359526ba..cbe4503754 100644
--- a/src/drivers/imu/invensense/icm40609d/InvenSense_ICM40609D_registers.hpp
+++ b/src/drivers/imu/invensense/icm40609d/InvenSense_ICM40609D_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
@@ -66,36 +66,38 @@ namespace Register
 {
 
 enum class BANK_0 : uint8_t {
-	DEVICE_CONFIG     = 0x11,
+	DEVICE_CONFIG      = 0x11,
 
-	INT_CONFIG        = 0x14,
+	INT_CONFIG         = 0x14,
 
-	FIFO_CONFIG       = 0x16,
+	FIFO_CONFIG        = 0x16,
 
-	TEMP_DATA1        = 0x1D,
-	TEMP_DATA0        = 0x1E,
+	TEMP_DATA1         = 0x1D,
+	TEMP_DATA0         = 0x1E,
 
-	INT_STATUS        = 0x2D,
-	FIFO_COUNTH       = 0x2E,
-	FIFO_COUNTL       = 0x2F,
-	FIFO_DATA         = 0x30,
+	INT_STATUS         = 0x2D,
+	FIFO_COUNTH        = 0x2E,
+	FIFO_COUNTL        = 0x2F,
+	FIFO_DATA          = 0x30,
 
-	SIGNAL_PATH_RESET = 0x4B,
+	SIGNAL_PATH_RESET  = 0x4B,
 
-	PWR_MGMT0         = 0x4E,
-	GYRO_CONFIG0      = 0x4F,
-	ACCEL_CONFIG0     = 0x50,
+	PWR_MGMT0          = 0x4E,
+	GYRO_CONFIG0       = 0x4F,
+	ACCEL_CONFIG0      = 0x50,
+	GYRO_CONFIG1       = 0x51,
+	ACCEL_CONFIG1      = 0x53,
 
-	FIFO_CONFIG1      = 0x5F,
-	FIFO_CONFIG2      = 0x60,
-	FIFO_CONFIG3      = 0x61,
+	FIFO_CONFIG1       = 0x5F,
+	FIFO_CONFIG2       = 0x60,
+	FIFO_CONFIG3       = 0x61,
 
-	INT_CONFIG0       = 0x63,
+	INT_CONFIG0        = 0x63,
+	INT_CONFIG1        = 0x64,
+	INT_SOURCE0        = 0x65,
 
-	INT_SOURCE0       = 0x65,
-
-	WHO_AM_I          = 0x75,
-	REG_BANK_SEL      = 0x76,
+	WHO_AM_I           = 0x75,
+	REG_BANK_SEL       = 0x76,
 };
 
 };
@@ -104,7 +106,7 @@ enum class BANK_0 : uint8_t {
 
 // DEVICE_CONFIG
 enum DEVICE_CONFIG_BIT : uint8_t {
-	SOFT_RESET_CONFIG = Bit0, //
+	SOFT_RESET_CONFIG = Bit0,
 };
 
 // INT_CONFIG
@@ -122,14 +124,15 @@ enum FIFO_CONFIG_BIT : uint8_t {
 
 // INT_STATUS
 enum INT_STATUS_BIT : uint8_t {
-	DATA_RDY_INT   = Bit3,
+	RESET_DONE_INT = Bit4,
+
 	FIFO_THS_INT   = Bit2,
 	FIFO_FULL_INT  = Bit1,
 };
 
 // SIGNAL_PATH_RESET
 enum SIGNAL_PATH_RESET_BIT : uint8_t {
-	FIFO_FLUSH      = Bit1,
+	FIFO_FLUSH = Bit1,
 };
 
 // PWR_MGMT0
@@ -141,42 +144,52 @@ 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,         // 0b001 = ±1000 dps
-	GYRO_FS_SEL_500_DPS  = Bit6,         // 0b010 = ±500 dps
-	GYRO_FS_SEL_250_DPS  = Bit6 | Bit5,  // 0b011 = ±250 dps
-	GYRO_FS_SEL_125_DPS  = Bit7,         // 0b100 = ±125 dps
+	//  0b000: ±2000dps (default)
+	GYRO_FS_SEL_2000_DPS_CLEAR = Bit7 | Bit6 | Bit5,
 
 	// 3:0 GYRO_ODR
-	GYRO_ODR_32kHz       = Bit0,         // 0001: 32kHz
-	GYRO_ODR_16kHz       = Bit1,         // 0010: 16kHz
-	GYRO_ODR_8kHz        = Bit1 | Bit0,  // 0011: 8kHz
-	GYRO_ODR_4kHz        = Bit2,         // 0100: 4kHz
-	GYRO_ODR_2kHz        = Bit2 | Bit0,  // 0101: 2kHz
-	GYRO_ODR_1kHz        = Bit2 | Bit1,  // 0110: 1kHz (default)
+	//  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_32G = 0,           // 000: ±32g (default)
-	ACCEL_FS_SEL_16G = Bit5,        // 001: ±16g
-	ACCEL_FS_SEL_8G  = Bit6,        // 010: ±8g
-	ACCEL_FS_SEL_4G  = Bit6 | Bit5, // 011: ±4g
+	//  0b000: ±32g (default)
+	ACCEL_FS_SEL_32G_CLEAR     = Bit7 | Bit6 | Bit5,
 
 	// 3:0 ACCEL_ODR
-	ACCEL_ODR_32kHz  = Bit0,        // 0001: 32kHz
-	ACCEL_ODR_16kHz  = Bit1,        // 0010: 16kHz
-	ACCEL_ODR_8kHz   = Bit1 | Bit0, // 0011: 8kHz
-	ACCEL_ODR_4kHz   = Bit2,        // 0100: 4kHz
-	ACCEL_ODR_2kHz   = Bit2 | Bit0, // 0101: 2kHz
-	ACCEL_ODR_1kHz   = Bit2 | Bit1, // 0110: 1kHz (default)
+	//  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_1ST_CLEAR = Bit3 | Bit2, // 00: 1st Order UI filter
+};
+
+// ACCEL_CONFIG1
+enum ACCEL_CONFIG1_BIT : uint8_t {
+	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_TEMP_EN           = Bit2,
 	FIFO_GYRO_EN           = Bit1,
@@ -186,14 +199,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_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,
 };
 
 // REG_BANK_SEL
@@ -225,9 +245,9 @@ struct DATA {
 	uint8_t GYRO_DATA_Y0;
 	uint8_t GYRO_DATA_Z1;
 	uint8_t GYRO_DATA_Z0;
-	uint8_t temperature;  // Temperature[7:0]
-	uint8_t timestamp_l;
-	uint8_t timestamp_h;
+	uint8_t FIFO_TEMP_DATA; // Temperature[7:0]
+	uint8_t TimeStamp_h;    // TimeStamp[15:8]
+	uint8_t TimeStamp_l;    // TimeStamp[7:0]
 };
 
 // With FIFO_ACCEL_EN and FIFO_GYRO_EN header should be 8’b_0110_10xx
diff --git a/src/drivers/imu/invensense/icm40609d/icm40609d_main.cpp b/src/drivers/imu/invensense/icm40609d/icm40609d_main.cpp
index 92480f0599..7b338c1cef 100644
--- a/src/drivers/imu/invensense/icm40609d/icm40609d_main.cpp
+++ b/src/drivers/imu/invensense/icm40609d/icm40609d_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 icm40609d_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/icm42605/CMakeLists.txt b/src/drivers/imu/invensense/icm42605/CMakeLists.txt
index fdb1ac7c0c..bfa447ba34 100644
--- a/src/drivers/imu/invensense/icm42605/CMakeLists.txt
+++ b/src/drivers/imu/invensense/icm42605/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__icm42605
 	MAIN icm42605
 	COMPILE_FLAGS
+		${MAX_CUSTOM_OPT_LEVEL}
 	SRCS
 		icm42605_main.cpp
 		ICM42605.cpp
diff --git a/src/drivers/imu/invensense/icm42605/ICM42605.cpp b/src/drivers/imu/invensense/icm42605/ICM42605.cpp
index e805382194..c89b31cfe9 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
@@ -274,8 +274,8 @@ void ICM42605::RunImpl()
 				}
 			}
 
+			// 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 +286,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,65 +293,6 @@ 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
@@ -419,8 +353,11 @@ bool ICM42605::Configure()
 		}
 	}
 
-	ConfigureAccel();
-	ConfigureGyro();
+	_px4_accel.set_range(16.f * CONSTANTS_ONE_G);
+	_px4_accel.set_scale(CONSTANTS_ONE_G / 2048.f);
+
+	_px4_gyro.set_range(math::radians(2000.f));
+	_px4_gyro.set_scale(math::radians(2000.f / 32768.f));
 
 	return success;
 }
@@ -585,9 +522,11 @@ bool ICM42605::FIFORead(const hrt_abstime &timestamp_sample, uint8_t samples)
 	}
 
 	if (valid_samples > 0) {
-		ProcessGyro(timestamp_sample, buffer.f, valid_samples);
-		ProcessAccel(timestamp_sample, buffer.f, valid_samples);
-		return true;
+		if (ProcessTemperature(buffer.f, valid_samples)) {
+			ProcessGyro(timestamp_sample, buffer.f, valid_samples);
+			ProcessAccel(timestamp_sample, buffer.f, valid_samples);
+			return true;
+		}
 	}
 
 	return false;
@@ -608,7 +547,7 @@ void ICM42605::ProcessAccel(const hrt_abstime &timestamp_sample, const FIFO::DAT
 {
 	sensor_accel_fifo_s accel{};
 	accel.timestamp_sample = timestamp_sample;
-	accel.samples = 0;
+	accel.samples = samples;
 	accel.dt = FIFO_SAMPLE_DT;
 
 	for (int i = 0; i < samples; i++) {
@@ -618,18 +557,15 @@ void ICM42605::ProcessAccel(const hrt_abstime &timestamp_sample, const FIFO::DAT
 
 		// 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[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));
 
-	if (accel.samples > 0) {
-		_px4_accel.updateFIFO(accel);
-	}
+	_px4_accel.updateFIFO(accel);
 }
 
 void ICM42605::ProcessGyro(const hrt_abstime &timestamp_sample, const FIFO::DATA fifo[], const uint8_t samples)
@@ -640,15 +576,15 @@ void ICM42605::ProcessGyro(const hrt_abstime &timestamp_sample, const FIFO::DATA
 	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);
+		int16_t gyro_x = combine(fifo[i].GYRO_DATA_X1, fifo[i].GYRO_DATA_X0);
+		int16_t gyro_y = combine(fifo[i].GYRO_DATA_Y1, fifo[i].GYRO_DATA_Y0);
+		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;
+		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) +
@@ -657,25 +593,32 @@ void ICM42605::ProcessGyro(const hrt_abstime &timestamp_sample, const FIFO::DATA
 	_px4_gyro.updateFIFO(gyro);
 }
 
-void ICM42605::UpdateTemperature()
+bool ICM42605::ProcessTemperature(const FIFO::DATA fifo[], const uint8_t samples)
 {
-	// 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);
+	float temperature_sum{0};
+	int valid_samples = 0;
 
-	if (transfer(temperature_buf, temperature_buf, sizeof(temperature_buf)) != PX4_OK) {
-		perf_count(_bad_transfer_perf);
-		return;
+	for (int i = 0; i < samples; i++) {
+		// Temperature data stored in FIFO is an 8-bit quantity, FIFO_TEMP_DATA
+		// It can be converted to degrees centigrade by using the following formula:
+		//  Temperature in Degrees Centigrade = (FIFO_TEMP_DATA / 2.07) + 25
+		temperature_sum += (fifo[i].FIFO_TEMP_DATA / 2.07f) + 25.f;
+		valid_samples++;
 	}
 
-	const int16_t TEMP_DATA = combine(temperature_buf[1], temperature_buf[2]);
+	if (valid_samples > 0) {
+		// use average temperature reading
+		const float temperature_avg = temperature_sum / valid_samples;
 
-	// Temperature in Degrees Centigrade
-	const float TEMP_degC = (TEMP_DATA / TEMPERATURE_SENSITIVITY) + TEMPERATURE_OFFSET;
+		if (PX4_ISFINITE(temperature_avg)) {
+			_px4_accel.set_temperature(temperature_avg);
+			_px4_gyro.set_temperature(temperature_avg);
+			return true;
 
-	if (PX4_ISFINITE(TEMP_degC)) {
-		_px4_accel.set_temperature(TEMP_degC);
-		_px4_gyro.set_temperature(TEMP_degC);
+		} else {
+			perf_count(_bad_transfer_perf);
+		}
 	}
+
+	return false;
 }
diff --git a/src/drivers/imu/invensense/icm42605/ICM42605.hpp b/src/drivers/imu/invensense/icm42605/ICM42605.hpp
index 98ad916d90..b1bafef6db 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
@@ -70,7 +70,7 @@ private:
 	void exit_and_cleanup() override;
 
 	// Sensor Configuration
-	static constexpr float FIFO_SAMPLE_DT{1e6f / 8000.f};     // 8000 Hz accel & gyro ODR configured
+	static constexpr float FIFO_SAMPLE_DT{1e6f / 8000.f};     // 8000 Hz accel & gyro ODR configured (must match GYRO_CONFIG0/ACCEL_CONFIG0)
 	static constexpr float GYRO_RATE{1e6f / FIFO_SAMPLE_DT};
 	static constexpr float ACCEL_RATE{1e6f / FIFO_SAMPLE_DT};
 
@@ -99,8 +99,6 @@ private:
 	bool Reset();
 
 	bool Configure();
-	void ConfigureAccel();
-	void ConfigureGyro();
 	void ConfigureSampleRate(int sample_rate);
 	void ConfigureFIFOWatermark(uint8_t samples);
 
@@ -125,7 +123,7 @@ private:
 
 	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();
+	bool ProcessTemperature(const FIFO::DATA fifo[], const uint8_t samples);
 
 	const spi_drdy_gpio_t _drdy_gpio;
 
@@ -160,18 +158,21 @@ private:
 	int32_t _fifo_gyro_samples{static_cast<int32_t>(_fifo_empty_interval_us / (1000000 / GYRO_RATE))};
 
 	uint8_t _checked_register_bank0{0};
-	static constexpr uint8_t size_register_bank0_cfg{10};
+	static constexpr uint8_t size_register_bank0_cfg{13};
 	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_ODR_8kHz, Bit7 | Bit6 | Bit5 | Bit3 | Bit2 },
-		{ Register::BANK_0::ACCEL_CONFIG0, ACCEL_CONFIG0_BIT::ACCEL_ODR_8kHz, Bit7 | Bit6 | Bit5 | Bit3 | Bit2 },
-		{ Register::BANK_0::FIFO_CONFIG1,  FIFO_CONFIG1_BIT::FIFO_WM_GT_TH | FIFO_CONFIG1_BIT::FIFO_GYRO_EN | FIFO_CONFIG1_BIT::FIFO_ACCEL_EN, FIFO_CONFIG1_BIT::FIFO_TEMP_EN },
-		{ 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                              | 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_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::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_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::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 },
 	};
 };
diff --git a/src/drivers/imu/invensense/icm42605/InvenSense_ICM42605_registers.hpp b/src/drivers/imu/invensense/icm42605/InvenSense_ICM42605_registers.hpp
index 4a4080feda..b0e8ed8bee 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
@@ -66,36 +66,38 @@ namespace Register
 {
 
 enum class BANK_0 : uint8_t {
-	DEVICE_CONFIG     = 0x11,
+	DEVICE_CONFIG      = 0x11,
 
-	INT_CONFIG        = 0x14,
+	INT_CONFIG         = 0x14,
 
-	FIFO_CONFIG       = 0x16,
+	FIFO_CONFIG        = 0x16,
 
-	TEMP_DATA1        = 0x1D,
-	TEMP_DATA0        = 0x1E,
+	TEMP_DATA1         = 0x1D,
+	TEMP_DATA0         = 0x1E,
 
-	INT_STATUS        = 0x2D,
-	FIFO_COUNTH       = 0x2E,
-	FIFO_COUNTL       = 0x2F,
-	FIFO_DATA         = 0x30,
+	INT_STATUS         = 0x2D,
+	FIFO_COUNTH        = 0x2E,
+	FIFO_COUNTL        = 0x2F,
+	FIFO_DATA          = 0x30,
 
-	SIGNAL_PATH_RESET = 0x4B,
+	SIGNAL_PATH_RESET  = 0x4B,
 
-	PWR_MGMT0         = 0x4E,
-	GYRO_CONFIG0      = 0x4F,
-	ACCEL_CONFIG0     = 0x50,
+	PWR_MGMT0          = 0x4E,
+	GYRO_CONFIG0       = 0x4F,
+	ACCEL_CONFIG0      = 0x50,
+	GYRO_CONFIG1       = 0x51,
+	ACCEL_CONFIG1      = 0x53,
 
-	FIFO_CONFIG1      = 0x5F,
-	FIFO_CONFIG2      = 0x60,
-	FIFO_CONFIG3      = 0x61,
+	FIFO_CONFIG1       = 0x5F,
+	FIFO_CONFIG2       = 0x60,
+	FIFO_CONFIG3       = 0x61,
 
-	INT_CONFIG0       = 0x63,
-	INT_CONFIG1       = 0x64,
-	INT_SOURCE0       = 0x65,
+	INT_CONFIG0        = 0x63,
+	INT_CONFIG1        = 0x64,
+	INT_SOURCE0        = 0x65,
 
-	WHO_AM_I          = 0x75,
-	REG_BANK_SEL      = 0x76,
+	WHO_AM_I           = 0x75,
+	REG_BANK_SEL       = 0x76,
 };
 
 };
@@ -104,7 +106,7 @@ enum class BANK_0 : uint8_t {
 
 // DEVICE_CONFIG
 enum DEVICE_CONFIG_BIT : uint8_t {
-	SOFT_RESET_CONFIG = Bit0, //
+	SOFT_RESET_CONFIG = Bit0,
 };
 
 // INT_CONFIG
@@ -123,15 +125,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
@@ -143,38 +144,46 @@ 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,         // 0b001 = ±1000 dps
-	GYRO_FS_SEL_500_DPS  = Bit6,         // 0b010 = ±500 dps
-	GYRO_FS_SEL_250_DPS  = Bit6 | Bit5,  // 0b011 = ±250 dps
-	GYRO_FS_SEL_125_DPS  = Bit7,         // 0b100 = ±125 dps
+	//  0b000: ±2000dps (default)
+	GYRO_FS_SEL_2000_DPS_CLEAR = Bit7 | Bit6 | Bit5,
 
 	// 3:0 GYRO_ODR
-	GYRO_ODR_8kHz        = Bit1 | Bit0,  // 0011: 8kHz
-	GYRO_ODR_4kHz        = Bit2,         // 0100: 4kHz
-	GYRO_ODR_2kHz        = Bit2 | Bit0,  // 0101: 2kHz
-	GYRO_ODR_1kHz        = Bit2 | Bit1,  // 0110: 1kHz (default)
+	//  0b0011: 8kHz (maximum)
+	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,        // 001: ±8g
-	ACCEL_FS_SEL_4G  = Bit6,        // 010: ±4g
-	ACCEL_FS_SEL_2G  = Bit6 | Bit5, // 011: ±2g
+	//  0b000: ±16g (default)
+	ACCEL_FS_SEL_16G_CLEAR     = Bit7 | Bit6 | Bit5,
 
 	// 3:0 ACCEL_ODR
-	ACCEL_ODR_8kHz   = Bit1 | Bit0, // 0011: 8kHz
-	ACCEL_ODR_4kHz   = Bit2,        // 0100: 4kHz
-	ACCEL_ODR_2kHz   = Bit2 | Bit0, // 0101: 2kHz
-	ACCEL_ODR_1kHz   = Bit2 | Bit1, // 0110: 1kHz (default)
+	//  0b0011: 8kHz (maximum)
+	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_1ST_CLEAR = Bit3 | Bit2, // 00: 1st Order UI filter
+};
+
+// ACCEL_CONFIG1
+enum ACCEL_CONFIG1_BIT : uint8_t {
+	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_TEMP_EN           = Bit2,
 	FIFO_GYRO_EN           = Bit1,
@@ -184,23 +193,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, // 0: Interrupt pulse duration is 100μs
+	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
@@ -232,9 +239,9 @@ struct DATA {
 	uint8_t GYRO_DATA_Y0;
 	uint8_t GYRO_DATA_Z1;
 	uint8_t GYRO_DATA_Z0;
-	uint8_t temperature;  // Temperature[7:0]
-	uint8_t timestamp_l;
-	uint8_t timestamp_h;
+	uint8_t FIFO_TEMP_DATA; // Temperature[7:0]
+	uint8_t TimeStamp_h;    // TimeStamp[15:8]
+	uint8_t TimeStamp_l;    // TimeStamp[7:0]
 };
 
 // With FIFO_ACCEL_EN and FIFO_GYRO_EN header should be 8’b_0110_10xx
@@ -242,7 +249,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/icm42605/icm42605_main.cpp b/src/drivers/imu/invensense/icm42605/icm42605_main.cpp
index 4dd6fa2e37..88707c855a 100644
--- a/src/drivers/imu/invensense/icm42605/icm42605_main.cpp
+++ b/src/drivers/imu/invensense/icm42605/icm42605_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 icm42605_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/icm42688p/CMakeLists.txt b/src/drivers/imu/invensense/icm42688p/CMakeLists.txt
index 8ffd2f7328..8281d63156 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
diff --git a/src/drivers/imu/invensense/icm42688p/ICM42688P.cpp b/src/drivers/imu/invensense/icm42688p/ICM42688P.cpp
index 32513feeea..af2f7b6b6f 100644
--- a/src/drivers/imu/invensense/icm42688p/ICM42688P.cpp
+++ b/src/drivers/imu/invensense/icm42688p/ICM42688P.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
@@ -275,19 +275,21 @@ void ICM42688P::RunImpl()
 			}
 
 			// check configuration registers periodically or immediately following any failure
-			if (RegisterCheck(_register_bank0_cfg[_checked_register_bank0])
-			    && RegisterCheck(_register_bank1_cfg[_checked_register_bank1])
-			    && RegisterCheck(_register_bank2_cfg[_checked_register_bank2])
-			   ) {
-				_last_config_check_timestamp = now;
-				_checked_register_bank0 = (_checked_register_bank0 + 1) % size_register_bank0_cfg;
-				_checked_register_bank1 = (_checked_register_bank1 + 1) % size_register_bank1_cfg;
-				_checked_register_bank2 = (_checked_register_bank2 + 1) % size_register_bank2_cfg;
+			if (!success || hrt_elapsed_time(&_last_config_check_timestamp) > 100_ms) {
+				if (RegisterCheck(_register_bank0_cfg[_checked_register_bank0])
+				    && RegisterCheck(_register_bank1_cfg[_checked_register_bank1])
+				    && RegisterCheck(_register_bank2_cfg[_checked_register_bank2])
+				   ) {
+					_last_config_check_timestamp = now;
+					_checked_register_bank0 = (_checked_register_bank0 + 1) % size_register_bank0_cfg;
+					_checked_register_bank1 = (_checked_register_bank1 + 1) % size_register_bank1_cfg;
+					_checked_register_bank2 = (_checked_register_bank2 + 1) % size_register_bank2_cfg;
 
-			} else {
-				// register check failed, force reset
-				perf_count(_bad_register_perf);
-				Reset();
+				} else {
+					// register check failed, force reset
+					perf_count(_bad_register_perf);
+					Reset();
+				}
 			}
 		}
 
@@ -378,7 +380,10 @@ 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_accel.set_scale(CONSTANTS_ONE_G / 2048.f);
+
 	_px4_gyro.set_range(math::radians(2000.f));
+	_px4_gyro.set_scale(math::radians(2000.f / 32768.f));
 
 	return success;
 }
@@ -668,8 +673,8 @@ void ICM42688P::ProcessAccel(const hrt_abstime &timestamp_sample, const FIFO::DA
 		// 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];
+		accel.y[i] = math::negate(accel.y[i]);
+		accel.z[i] = math::negate(accel.z[i]);
 	}
 
 	_px4_accel.set_error_count(perf_event_count(_bad_register_perf) + perf_event_count(_bad_transfer_perf) +
@@ -740,8 +745,8 @@ void ICM42688P::ProcessGyro(const hrt_abstime &timestamp_sample, const FIFO::DAT
 		// 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];
+		gyro.y[i] = math::negate(gyro.y[i]);
+		gyro.z[i] = math::negate(gyro.z[i]);
 	}
 
 	_px4_gyro.set_error_count(perf_event_count(_bad_register_perf) + perf_event_count(_bad_transfer_perf) +
diff --git a/src/drivers/imu/invensense/icm42688p/ICM42688P.hpp b/src/drivers/imu/invensense/icm42688p/ICM42688P.hpp
index 96f1d1d8f0..14bf8cb681 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
@@ -70,7 +70,7 @@ private:
 	void exit_and_cleanup() override;
 
 	// Sensor Configuration
-	static constexpr float FIFO_SAMPLE_DT{1e6f / 8000.f};     // 8000 Hz accel & gyro ODR configured
+	static constexpr float FIFO_SAMPLE_DT{1e6f / 8000.f};     // 8000 Hz accel & gyro ODR configured (must match GYRO_CONFIG0/ACCEL_CONFIG0)
 	static constexpr float GYRO_RATE{1e6f / FIFO_SAMPLE_DT};
 	static constexpr float ACCEL_RATE{1e6f / FIFO_SAMPLE_DT};
 
@@ -172,22 +172,23 @@ private:
 	int32_t _fifo_gyro_samples{static_cast<int32_t>(_fifo_empty_interval_us / (1000000 / GYRO_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 bf44666a01..d1b34475c8 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
@@ -81,8 +81,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,
@@ -95,7 +94,7 @@ enum class BANK_0 : uint8_t {
 	FIFO_CONFIG3       = 0x61,
 
 	INT_CONFIG0        = 0x63,
-
+	INT_CONFIG1        = 0x64,
 	INT_SOURCE0        = 0x65,
 
 	SELF_TEST_CONFIG   = 0x70,
@@ -120,7 +119,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
@@ -139,15 +138,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
@@ -159,55 +157,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
@@ -221,13 +206,12 @@ 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,
@@ -237,18 +221,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
@@ -276,6 +263,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);
 	}