invensense/icm42688p: add additional state FIFO_RESET

This state is added to give extra time between  FIFO flush command. Some icm42688p IMUs need more time between config -> FIFO reset -> FIFO read. More about the issue  #20181

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
@@ -171,21 +171,9 @@ void ICM42688P::RunImpl()
 
 	case STATE::CONFIGURE:
 		if (Configure()) {
-			// if configure succeeded then start reading from FIFO
+			// if configure succeeded then reset the FIFO
-			_state = STATE::FIFO_READ;
+			_state = STATE::FIFO_RESET;
-
+			ScheduleDelayed(1_ms);
-			if (DataReadyInterruptConfigure()) {
-				_data_ready_interrupt_enabled = true;
-
-				// backup schedule as a watchdog timeout
-				ScheduleDelayed(100_ms);
-
-			} else {
-				_data_ready_interrupt_enabled = false;
-				ScheduleOnInterval(_fifo_empty_interval_us, _fifo_empty_interval_us);
-			}
-
-			FIFOReset();
 
 		} else {
 			// CONFIGURE not complete
@@ -202,6 +190,24 @@ void ICM42688P::RunImpl()
 
 		break;
 
+	case STATE::FIFO_RESET:
+
+		_state = STATE::FIFO_READ;
+		FIFOReset();
+
+		if (DataReadyInterruptConfigure()) {
+			_data_ready_interrupt_enabled = true;
+
+			// backup schedule as a watchdog timeout
+			ScheduleDelayed(100_ms);
+
+		} else {
+			_data_ready_interrupt_enabled = false;
+			ScheduleOnInterval(_fifo_empty_interval_us, _fifo_empty_interval_us);
+		}
+
+		break;
+
 	case STATE::FIFO_READ: {
 			hrt_abstime timestamp_sample = now;
 			uint8_t samples = 0;

src/drivers/imu/invensense/icm42688p/ICM42688P.hpp

@@ -165,6 +165,7 @@ private:
 		RESET,
 		WAIT_FOR_RESET,
 		CONFIGURE,
+		FIFO_RESET,
 		FIFO_READ,
 	} _state{STATE::RESET};