diff --git a/src/drivers/imu/invensense/icm42688p/CMakeLists.txt b/src/drivers/imu/invensense/icm42688p/CMakeLists.txt
index 42264f98d6..8ffd2f7328 100644
--- a/src/drivers/imu/invensense/icm42688p/CMakeLists.txt
+++ b/src/drivers/imu/invensense/icm42688p/CMakeLists.txt
@@ -34,6 +34,7 @@ px4_add_module(
 	MODULE drivers__imu__invensense__icm42688p
 	MAIN icm42688p
 	COMPILE_FLAGS
+		${MAX_CUSTOM_OPT_LEVEL}
 	SRCS
 		icm42688p_main.cpp
 		ICM42688P.cpp
diff --git a/src/drivers/imu/invensense/icm42688p/ICM42688P.cpp b/src/drivers/imu/invensense/icm42688p/ICM42688P.cpp
index 9a4714ac16..1d9916dad8 100644
--- a/src/drivers/imu/invensense/icm42688p/ICM42688P.cpp
+++ b/src/drivers/imu/invensense/icm42688p/ICM42688P.cpp
@@ -252,25 +252,20 @@ void ICM42688P::RunImpl()
 				}
 			}
 
-			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;
-					_checked_register_bank0 = (_checked_register_bank0 + 1) % size_register_bank0_cfg;
-
-				} else {
-					// register check failed, force reset
-					perf_count(_bad_register_perf);
-					Reset();
-				}
+			// check configuration registers periodically or immediately following any failure
+			if (RegisterCheck(_register_bank0_cfg[_checked_register_bank0])
+			    && RegisterCheck(_register_bank1_cfg[_checked_register_bank1])
+			    && RegisterCheck(_register_bank2_cfg[_checked_register_bank2])
+			   ) {
+				_last_config_check_timestamp = now;
+				_checked_register_bank0 = (_checked_register_bank0 + 1) % size_register_bank0_cfg;
+				_checked_register_bank1 = (_checked_register_bank1 + 1) % size_register_bank1_cfg;
+				_checked_register_bank2 = (_checked_register_bank2 + 1) % size_register_bank2_cfg;
 
 			} else {
-				// periodically update temperature (~1 Hz)
-				if (hrt_elapsed_time(&_temperature_update_timestamp) >= 1_s) {
-					UpdateTemperature();
-					_temperature_update_timestamp = now;
-				}
+				// register check failed, force reset
+				perf_count(_bad_register_perf);
+				Reset();
 			}
 		}
 
@@ -278,65 +273,6 @@ void ICM42688P::RunImpl()
 	}
 }
 
-void ICM42688P::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 ICM42688P::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 ICM42688P::ConfigureSampleRate(int sample_rate)
 {
 	if (sample_rate == 0) {
@@ -392,6 +328,14 @@ bool ICM42688P::Configure()
 		RegisterSetAndClearBits(reg_cfg.reg, reg_cfg.set_bits, reg_cfg.clear_bits);
 	}
 
+	for (const auto &reg_cfg : _register_bank1_cfg) {
+		RegisterSetAndClearBits(reg_cfg.reg, reg_cfg.set_bits, reg_cfg.clear_bits);
+	}
+
+	for (const auto &reg_cfg : _register_bank2_cfg) {
+		RegisterSetAndClearBits(reg_cfg.reg, reg_cfg.set_bits, reg_cfg.clear_bits);
+	}
+
 	// now check that all are configured
 	bool success = true;
 
@@ -401,8 +345,22 @@ bool ICM42688P::Configure()
 		}
 	}
 
-	ConfigureAccel();
-	ConfigureGyro();
+	for (const auto &reg_cfg : _register_bank1_cfg) {
+		if (!RegisterCheck(reg_cfg)) {
+			success = false;
+		}
+	}
+
+	for (const auto &reg_cfg : _register_bank2_cfg) {
+		if (!RegisterCheck(reg_cfg)) {
+			success = false;
+		}
+	}
+
+	// 20-bits data format used
+	//  the only FSR settings that are operational are ±2000dps for gyroscope and ±16g for accelerometer
+	_px4_accel.set_range(16.f * CONSTANTS_ONE_G);
+	_px4_gyro.set_range(math::radians(2000.f));
 
 	return success;
 }
@@ -558,6 +516,18 @@ bool ICM42688P::FIFORead(const hrt_abstime &timestamp_sample, uint8_t samples)
 		} 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) {
@@ -570,9 +540,11 @@ bool ICM42688P::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;
@@ -589,6 +561,23 @@ void ICM42688P::FIFOReset()
 	_drdy_fifo_read_samples.store(0);
 }
 
+static constexpr int32_t reassemble_20bit(const uint32_t a, const uint32_t b, const uint32_t c)
+{
+	// 0xXXXAABBC
+	uint32_t high   = ((a << 12) & 0x000FF000);
+	uint32_t low    = ((b << 4)  & 0x00000FF0);
+	uint32_t lowest = (c         & 0x0000000F);
+
+	uint32_t x = high | low | lowest;
+
+	if (a & Bit7) {
+		// sign extend
+		x |= 0xFFF00000u;
+	}
+
+	return static_cast<int32_t>(x);
+}
+
 void ICM42688P::ProcessAccel(const hrt_abstime &timestamp_sample, const FIFO::DATA fifo[], const uint8_t samples)
 {
 	sensor_accel_fifo_s accel{};
@@ -596,17 +585,76 @@ void ICM42688P::ProcessAccel(const hrt_abstime &timestamp_sample, const FIFO::DA
 	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);
+	// 18-bits of accelerometer data
+	bool scale_20bit = false;
 
+	// first pass
+	for (int i = 0; i < samples; i++) {
+		// 20 bit hires mode
+		// Sign extension + Accel [19:12] + Accel [11:4] + Accel [3:2] (20 bit extension byte)
+		// Accel data is 18 bit ()
+		int32_t accel_x = reassemble_20bit(fifo[i].ACCEL_DATA_X1, fifo[i].ACCEL_DATA_X0,
+						   fifo[i].Ext_Accel_X_Gyro_X & 0xF0 >> 4);
+		int32_t accel_y = reassemble_20bit(fifo[i].ACCEL_DATA_Y1, fifo[i].ACCEL_DATA_Y0,
+						   fifo[i].Ext_Accel_Y_Gyro_Y & 0xF0 >> 4);
+		int32_t accel_z = reassemble_20bit(fifo[i].ACCEL_DATA_Z1, fifo[i].ACCEL_DATA_Z0,
+						   fifo[i].Ext_Accel_Z_Gyro_Z & 0xF0 >> 4);
+
+		// sample invalid if -524288
+		if (accel_x != -524288 && accel_y != -524288 && accel_z != -524288) {
+			// check if any values are going to exceed int16 limits
+			static constexpr int16_t max_accel = INT16_MAX;
+			static constexpr int16_t min_accel = INT16_MIN;
+
+			if (accel_x >= max_accel || accel_x <= min_accel) {
+				scale_20bit = true;
+			}
+
+			if (accel_y >= max_accel || accel_y <= min_accel) {
+				scale_20bit = true;
+			}
+
+			if (accel_z >= max_accel || accel_z <= min_accel) {
+				scale_20bit = true;
+			}
+
+			// shift by 2 (2 least significant bits are always 0)
+			accel.x[accel.samples] = accel_x / 4;
+			accel.y[accel.samples] = accel_y / 4;
+			accel.z[accel.samples] = accel_z / 4;
+			accel.samples++;
+		}
+	}
+
+	if (!scale_20bit) {
+		// if highres enabled accel data is always 8192 LSB/g
+		_px4_accel.set_scale(CONSTANTS_ONE_G / 8192.f);
+
+	} else {
+		// 20 bit data scaled to 16 bit (2^4)
+		for (int i = 0; i < samples; i++) {
+			// 20 bit hires mode
+			// Sign extension + Accel [19:12] + Accel [11:4] + Accel [3:2] (20 bit extension byte)
+			// Accel data is 18 bit ()
+			int16_t accel_x = combine(fifo[i].ACCEL_DATA_X1, fifo[i].ACCEL_DATA_X0);
+			int16_t accel_y = combine(fifo[i].ACCEL_DATA_Y1, fifo[i].ACCEL_DATA_Y0);
+			int16_t accel_z = combine(fifo[i].ACCEL_DATA_Z1, fifo[i].ACCEL_DATA_Z0);
+
+			accel.x[i] = accel_x;
+			accel.y[i] = accel_y;
+			accel.z[i] = accel_z;
+		}
+
+		_px4_accel.set_scale(CONSTANTS_ONE_G / 2048.f);
+	}
+
+	// correct frame for publication
+	for (int i = 0; i < accel.samples; i++) {
 		// sensor's frame is +x forward, +y left, +z up
 		//  flip y & z to publish right handed with z down (x forward, y right, z down)
-		accel.x[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[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];
 	}
 
 	_px4_accel.set_error_count(perf_event_count(_bad_register_perf) + perf_event_count(_bad_transfer_perf) +
@@ -621,46 +669,115 @@ void ICM42688P::ProcessGyro(const hrt_abstime &timestamp_sample, const FIFO::DAT
 {
 	sensor_gyro_fifo_s gyro{};
 	gyro.timestamp_sample = timestamp_sample;
-	gyro.samples = samples;
+	gyro.samples = 0;
 	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);
+	// 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;
-		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[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));
 
-	_px4_gyro.updateFIFO(gyro);
+	if (gyro.samples > 0) {
+		_px4_gyro.updateFIFO(gyro);
+	}
 }
 
-void ICM42688P::UpdateTemperature()
+bool ICM42688P::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);
+	int16_t temperature[FIFO_MAX_SAMPLES];
+	float temperature_sum{0};
 
-	if (transfer(temperature_buf, temperature_buf, sizeof(temperature_buf)) != PX4_OK) {
-		perf_count(_bad_transfer_perf);
-		return;
+	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++;
+		}
 	}
 
-	const int16_t TEMP_DATA = combine(temperature_buf[1], temperature_buf[2]);
+	if (valid_samples > 0) {
+		const float temperature_avg = temperature_sum / valid_samples;
 
-	// Temperature in Degrees Centigrade
-	const float TEMP_degC = (TEMP_DATA / TEMPERATURE_SENSITIVITY) + TEMPERATURE_OFFSET;
+		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;
+			}
+		}
 
-	if (PX4_ISFINITE(TEMP_degC)) {
-		_px4_accel.set_temperature(TEMP_degC);
-		_px4_gyro.set_temperature(TEMP_degC);
+		// 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;
+
+		} else {
+			perf_count(_bad_transfer_perf);
+		}
 	}
+
+	return false;
 }
diff --git a/src/drivers/imu/invensense/icm42688p/ICM42688P.hpp b/src/drivers/imu/invensense/icm42688p/ICM42688P.hpp
index 953899dd47..c8e9aa51d3 100644
--- a/src/drivers/imu/invensense/icm42688p/ICM42688P.hpp
+++ b/src/drivers/imu/invensense/icm42688p/ICM42688P.hpp
@@ -73,7 +73,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 / 16000.f};     // 16000 Hz accel & gyro ODR configured
 	static constexpr float GYRO_RATE{1e6f / FIFO_SAMPLE_DT};
 	static constexpr float ACCEL_RATE{1e6f / FIFO_SAMPLE_DT};
 
@@ -97,18 +97,30 @@ private:
 		uint8_t clear_bits{0};
 	};
 
+	struct register_bank1_config_t {
+		Register::BANK_1 reg;
+		uint8_t set_bits{0};
+		uint8_t clear_bits{0};
+	};
+
+	struct register_bank2_config_t {
+		Register::BANK_2 reg;
+		uint8_t set_bits{0};
+		uint8_t clear_bits{0};
+	};
+
 	int probe() override;
 
 	bool Reset();
 
 	bool Configure();
-	void ConfigureAccel();
-	void ConfigureGyro();
 	void ConfigureSampleRate(int sample_rate);
 	void ConfigureFIFOWatermark(uint8_t samples);
 
 	void SelectRegisterBank(enum REG_BANK_SEL_BIT bank);
 	void SelectRegisterBank(Register::BANK_0 reg) { SelectRegisterBank(REG_BANK_SEL_BIT::USER_BANK_0); }
+	void SelectRegisterBank(Register::BANK_1 reg) { SelectRegisterBank(REG_BANK_SEL_BIT::USER_BANK_1); }
+	void SelectRegisterBank(Register::BANK_2 reg) { SelectRegisterBank(REG_BANK_SEL_BIT::USER_BANK_2); }
 
 	static int DataReadyInterruptCallback(int irq, void *context, void *arg);
 	void DataReady();
@@ -128,7 +140,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;
 
@@ -157,26 +169,41 @@ private:
 		WAIT_FOR_RESET,
 		CONFIGURE,
 		FIFO_READ,
-	};
-
-	STATE _state{STATE::RESET};
+	} _state{STATE::RESET};
 
 	uint16_t _fifo_empty_interval_us{1250}; // default 1250 us / 800 Hz transfer interval
 	uint32_t _fifo_gyro_samples{static_cast<uint32_t>(_fifo_empty_interval_us / (1000000 / GYRO_RATE))};
 
 	uint8_t _checked_register_bank0{0};
-	static constexpr uint8_t size_register_bank0_cfg{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_16kHz, Bit3 | Bit2 | Bit0 },
+		{ Register::BANK_0::ACCEL_CONFIG0,        ACCEL_CONFIG0_BIT::ACCEL_ODR_16kHz, Bit3 | Bit2 | Bit0 },
+		{ Register::BANK_0::GYRO_CONFIG1,         0, GYRO_CONFIG1_BIT::GYRO_UI_FILT_ORD },
+		{ Register::BANK_0::GYRO_ACCEL_CONFIG0,   0, GYRO_ACCEL_CONFIG0_BIT::ACCEL_UI_FILT_BW | GYRO_ACCEL_CONFIG0_BIT::GYRO_UI_FILT_BW },
+		{ Register::BANK_0::ACCEL_CONFIG1,        0, ACCEL_CONFIG1_BIT::ACCEL_UI_FILT_ORD },
+		{ Register::BANK_0::FIFO_CONFIG1,         FIFO_CONFIG1_BIT::FIFO_WM_GT_TH | FIFO_CONFIG1_BIT::FIFO_HIRES_EN | FIFO_CONFIG1_BIT::FIFO_TEMP_EN | FIFO_CONFIG1_BIT::FIFO_GYRO_EN | FIFO_CONFIG1_BIT::FIFO_ACCEL_EN, 0 },
+		{ Register::BANK_0::FIFO_CONFIG2,         0, 0 }, // FIFO_WM[7:0] set at runtime
+		{ Register::BANK_0::FIFO_CONFIG3,         0, 0 }, // FIFO_WM[11:8] set at runtime
+		{ Register::BANK_0::INT_CONFIG0,          INT_CONFIG0_BIT::CLEAR_ON_FIFO_READ, 0 },
+		{ Register::BANK_0::INT_SOURCE0,          INT_SOURCE0_BIT::FIFO_THS_INT1_EN, 0 },
+	};
+
+	uint8_t _checked_register_bank1{0};
+	static constexpr uint8_t size_register_bank1_cfg{1};
+	register_bank1_config_t _register_bank1_cfg[size_register_bank1_cfg] {
+		// Register                              | Set bits, Clear bits
+		{ Register::BANK_1::GYRO_CONFIG_STATIC2,  0, GYRO_CONFIG_STATIC2_BIT::GYRO_AAF_DIS | GYRO_CONFIG_STATIC2_BIT::GYRO_NF_DIS },
+	};
+
+	uint8_t _checked_register_bank2{0};
+	static constexpr uint8_t size_register_bank2_cfg{1};
+	register_bank2_config_t _register_bank2_cfg[size_register_bank2_cfg] {
+		// Register                              | Set bits, Clear bits
+		{ Register::BANK_2::ACCEL_CONFIG_STATIC2, 0, ACCEL_CONFIG_STATIC2_BIT::ACCEL_AAF_DIS },
 	};
 };
diff --git a/src/drivers/imu/invensense/icm42688p/InvenSense_ICM42688P_registers.hpp b/src/drivers/imu/invensense/icm42688p/InvenSense_ICM42688P_registers.hpp
index 872d88b52a..29c6632144 100644
--- a/src/drivers/imu/invensense/icm42688p/InvenSense_ICM42688P_registers.hpp
+++ b/src/drivers/imu/invensense/icm42688p/InvenSense_ICM42688P_registers.hpp
@@ -66,36 +66,52 @@ 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,
+	INTF_CONFIG0       = 0x4C,
+	INTF_CONFIG1       = 0x4D,
+	PWR_MGMT0          = 0x4E,
+	GYRO_CONFIG0       = 0x4F,
+	ACCEL_CONFIG0      = 0x50,
+	GYRO_CONFIG1       = 0x51,
+	GYRO_ACCEL_CONFIG0 = 0x52,
+	ACCEL_CONFIG1      = 0x53,
 
-	PWR_MGMT0         = 0x4E,
-	GYRO_CONFIG0      = 0x4F,
-	ACCEL_CONFIG0     = 0x50,
+	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,
+	SELF_TEST_CONFIG   = 0x70,
+
+	WHO_AM_I           = 0x75,
+	REG_BANK_SEL       = 0x76,
+};
+
+enum class BANK_1 : uint8_t {
+	GYRO_CONFIG_STATIC2 = 0x0B,
+
+	INTF_CONFIG5        = 0x7B,
+};
+
+enum class BANK_2 : uint8_t {
+	ACCEL_CONFIG_STATIC2 = 0x03,
 };
 
 };
@@ -144,10 +160,6 @@ enum PWR_MGMT0_BIT : uint8_t {
 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
 
 	// 3:0 GYRO_ODR
 	GYRO_ODR_32kHz       = Bit0,         // 0001: 32kHz
@@ -162,9 +174,6 @@ enum GYRO_CONFIG0_BIT : uint8_t {
 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
 
 	// 3:0 ACCEL_ODR
 	ACCEL_ODR_32kHz  = Bit0,        // 0001: 32kHz
@@ -175,11 +184,30 @@ enum ACCEL_CONFIG0_BIT : uint8_t {
 	ACCEL_ODR_1kHz   = Bit2 | Bit1, // 0110: 1kHz (default)
 };
 
+// GYRO_CONFIG1
+enum GYRO_CONFIG1_BIT : uint8_t {
+	GYRO_UI_FILT_ORD = Bit3 | Bit2, // 00: 1st Order
+};
+
+// GYRO_ACCEL_CONFIG0
+enum GYRO_ACCEL_CONFIG0_BIT : uint8_t {
+	// 7:4 ACCEL_UI_FILT_BW
+	ACCEL_UI_FILT_BW = Bit7 | Bit6 | Bit5 | Bit4, // 0: BW=ODR/2
+
+	// 3:0 GYRO_UI_FILT_BW
+	GYRO_UI_FILT_BW  = Bit3 | Bit2 | Bit1 | Bit0, // 0: BW=ODR/2
+};
+
+// ACCEL_CONFIG1
+enum ACCEL_CONFIG1_BIT : uint8_t {
+	ACCEL_UI_FILT_ORD = Bit4 | Bit3, // 00: 1st Order
+};
+
 // FIFO_CONFIG1
 enum FIFO_CONFIG1_BIT : uint8_t {
 	FIFO_RESUME_PARTIAL_RD = Bit6,
 	FIFO_WM_GT_TH          = Bit5,
-
+	FIFO_HIRES_EN          = Bit4,
 	FIFO_TEMP_EN           = Bit2,
 	FIFO_GYRO_EN           = Bit1,
 	FIFO_ACCEL_EN          = Bit0,
@@ -191,11 +219,6 @@ enum INT_CONFIG0_BIT : uint8_t {
 	CLEAR_ON_FIFO_READ = Bit3,
 };
 
-// INT_CONFIG1
-enum INT_CONFIG1_BIT : uint8_t {
-	INT_TPULSE_DURATION = Bit6, // 0: Interrupt pulse duration is 100μs
-};
-
 // INT_SOURCE0
 enum INT_SOURCE0_BIT : uint8_t {
 	UI_FSYNC_INT1_EN   = Bit6,
@@ -215,41 +238,62 @@ enum REG_BANK_SEL_BIT : uint8_t {
 	USER_BANK_3 = Bit5 | Bit4, // 3: Select USER BANK 3.
 };
 
+
+//---------------- BANK1 Register bits
+
+// GYRO_CONFIG_STATIC2
+enum GYRO_CONFIG_STATIC2_BIT : uint8_t {
+	GYRO_AAF_DIS = Bit1,
+	GYRO_NF_DIS  = Bit0,
+};
+
+
+//---------------- BANK2 Register bits
+
+// ACCEL_CONFIG_STATIC2
+enum ACCEL_CONFIG_STATIC2_BIT : uint8_t {
+	ACCEL_AAF_DIS = Bit0,
+};
+
 namespace FIFO
 {
 static constexpr size_t SIZE = 2048;
 
 // FIFO_DATA layout when FIFO_CONFIG1 has FIFO_GYRO_EN and FIFO_ACCEL_EN set
 
-// Packet 3
+// Packet 4
 struct DATA {
 	uint8_t FIFO_Header;
-	uint8_t ACCEL_DATA_X1;
-	uint8_t ACCEL_DATA_X0;
-	uint8_t ACCEL_DATA_Y1;
-	uint8_t ACCEL_DATA_Y0;
-	uint8_t ACCEL_DATA_Z1;
-	uint8_t ACCEL_DATA_Z0;
-	uint8_t GYRO_DATA_X1;
-	uint8_t GYRO_DATA_X0;
-	uint8_t GYRO_DATA_Y1;
-	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 ACCEL_DATA_X1; // Accel X [19:12]
+	uint8_t ACCEL_DATA_X0; // Accel X [11:4]
+	uint8_t ACCEL_DATA_Y1; // Accel Y [19:12]
+	uint8_t ACCEL_DATA_Y0; // Accel Y [11:4]
+	uint8_t ACCEL_DATA_Z1; // Accel Z [19:12]
+	uint8_t ACCEL_DATA_Z0; // Accel Z [11:4]
+	uint8_t GYRO_DATA_X1;  // Gyro X [19:12]
+	uint8_t GYRO_DATA_X0;  // Gyro X [11:4]
+	uint8_t GYRO_DATA_Y1;  // Gyro Y [19:12]
+	uint8_t GYRO_DATA_Y0;  // Gyro Y [11:4]
+	uint8_t GYRO_DATA_Z1;  // Gyro Z [19:12]
+	uint8_t GYRO_DATA_Z0;  // Gyro Z [11:4]
+	uint8_t TEMP_DATA1;    // Temperature[15:8]
+	uint8_t TEMP_DATA0;    // Temperature[7:0]
+	uint8_t TimeStamp_h;   // TimeStamp[15:8]
+	uint8_t TimeStamp_l;   // TimeStamp[7:0]
+	uint8_t Ext_Accel_X_Gyro_X; // Accel X [3:0] Gyro X [3:0]
+	uint8_t Ext_Accel_Y_Gyro_Y; // Accel Y [3:0] Gyro Y [3:0]
+	uint8_t Ext_Accel_Z_Gyro_Z; // Accel Z [3:0] Gyro Z [3:0]
 };
 
 // With FIFO_ACCEL_EN and FIFO_GYRO_EN header should be 8’b_0110_10xx
 enum FIFO_HEADER_BIT : uint8_t {
 	HEADER_MSG             = Bit7, // 1: FIFO is empty
-	HEADER_ACCEL           = Bit6,
-	HEADER_GYRO            = Bit5,
-	HEADER_20              = Bit4,
+	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,
-	HEADER_ODR_GYRO        = Bit0,
+	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
 };
 
 }