mpu9250: accumulated minor improvements and cleanup

- perform reset as per the datasheet (disable I2C immediately, set power mode, wait for appropriate time, etc)
 - remove interrupt perf counter and instead only count misses
 - minor style changes to stay in sync with the other Invensense drivers

src/drivers/imu/invensense/mpu9250/CMakeLists.txt

@@ -35,9 +35,6 @@ px4_add_module(
 	MODULE drivers__imu__invensense__mpu9250
 	MAIN mpu9250
 	COMPILE_FLAGS
-		-O0
-		-DDEBUG_BUILD
-		-Wno-error
 	SRCS
 		AKM_AK8963_registers.hpp
 		InvenSense_MPU9250_registers.hpp

src/drivers/imu/invensense/mpu9250/InvenSense_MPU9250_registers.hpp

@@ -213,12 +213,10 @@ enum PWR_MGMT_1_BIT : uint8_t {
 	H_RESET    = Bit7,
 	SLEEP      = Bit6,
 
-	CLKSEL_2   = Bit2,
-	CLKSEL_1   = Bit1,
-	CLKSEL_0   = Bit0,
+	// CLKSEL[2:0]
+	CLKSEL_0     = Bit0, // It is required that CLKSEL[2:0] be set to 001 to achieve full gyroscope performance.
 };
 
-
 namespace FIFO
 {
 static constexpr size_t SIZE = 512;

src/drivers/imu/invensense/mpu9250/MPU9250.cpp

@@ -51,7 +51,7 @@ MPU9250::MPU9250(I2CSPIBusOption bus_option, int bus, uint32_t device, enum Rota
 	_px4_gyro(get_device_id(), ORB_PRIO_HIGH, rotation)
 {
 	if (drdy_gpio != 0) {
-		_drdy_interval_perf = perf_alloc(PC_INTERVAL, MODULE_NAME": DRDY interval");
+		_drdy_missed_perf = perf_alloc(PC_COUNT, MODULE_NAME": DRDY missed");
 	}
 
 	ConfigureSampleRate(_px4_gyro.get_max_rate_hz());
@@ -82,7 +82,7 @@ MPU9250::~MPU9250()
 	perf_free(_fifo_empty_perf);
 	perf_free(_fifo_overflow_perf);
 	perf_free(_fifo_reset_perf);
-	perf_free(_drdy_interval_perf);
+	perf_free(_drdy_missed_perf);
 
 	delete _slave_ak8963_magnetometer;
 }
@@ -118,14 +118,14 @@ void MPU9250::print_status()
 {
 	I2CSPIDriverBase::print_status();
 
-	PX4_INFO("FIFO empty interval: %d us (%.3f Hz)", _fifo_empty_interval_us, 1e6 / _fifo_empty_interval_us);
+	PX4_INFO("FIFO empty interval: %d us (%.1f Hz)", _fifo_empty_interval_us, 1e6 / _fifo_empty_interval_us);
 
 	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);
+	perf_print_counter(_drdy_missed_perf);
 
 
 	if (_slave_ak8963_magnetometer) {
@@ -154,7 +154,7 @@ void MPU9250::RunImpl()
 		// PWR_MGMT_1: Device Reset
 		RegisterWrite(Register::PWR_MGMT_1, PWR_MGMT_1_BIT::H_RESET);
 		_reset_timestamp = now;
-		_consecutive_failures = 0;
+		_failure_count = 0;
 		_state = STATE::WAIT_FOR_RESET;
 		ScheduleDelayed(100_ms);
 		break;
@@ -167,11 +167,11 @@ void MPU9250::RunImpl()
 		    && (RegisterRead(Register::PWR_MGMT_1) == 0x01)) {
 
 			// Wakeup and reset digital signal path
-			RegisterWrite(Register::PWR_MGMT_1, 0);
+			RegisterWrite(Register::PWR_MGMT_1, PWR_MGMT_1_BIT::CLKSEL_0);
 			RegisterWrite(Register::SIGNAL_PATH_RESET,
 				      SIGNAL_PATH_RESET_BIT::GYRO_RESET | SIGNAL_PATH_RESET_BIT::ACCEL_RESET | SIGNAL_PATH_RESET_BIT::TEMP_RESET);
-			RegisterWrite(Register::USER_CTRL, USER_CTRL_BIT::I2C_MST_EN | USER_CTRL_BIT::I2C_IF_DIS | USER_CTRL_BIT::I2C_MST_RST |
-				      USER_CTRL_BIT::SIG_COND_RST);
+			RegisterWrite(Register::USER_CTRL, USER_CTRL_BIT::I2C_MST_EN | USER_CTRL_BIT::SIG_COND_RST | USER_CTRL_BIT::I2C_IF_DIS |
+				      USER_CTRL_BIT::I2C_MST_RST);
 
 			// if reset succeeded then configure
 			_state = STATE::CONFIGURE;
@@ -233,8 +233,8 @@ void MPU9250::RunImpl()
 	case STATE::FIFO_READ: {
 			if (_data_ready_interrupt_enabled) {
 				// scheduled from interrupt if _drdy_fifo_read_samples was set
-				if (_drdy_fifo_read_samples.fetch_and(0) == _fifo_gyro_samples) {
-					perf_count_interval(_drdy_interval_perf, now);
+				if (_drdy_fifo_read_samples.fetch_and(0) != _fifo_gyro_samples) {
+					perf_count(_drdy_missed_perf);
 				}
 
 				// push backup schedule back
@@ -265,22 +265,25 @@ void MPU9250::RunImpl()
 				} else if (samples >= 1) {
 					if (FIFORead(now, samples)) {
 						success = true;
-						_consecutive_failures = 0;
+
+						if (_failure_count > 0) {
+							_failure_count--;
+						}
 					}
 				}
 			}
 
 			if (!success) {
-				_consecutive_failures++;
+				_failure_count++;
 
 				// full reset if things are failing consistently
-				if (_consecutive_failures > 10) {
+				if (_failure_count > 10) {
 					Reset();
 					return;
 				}
 			}
 
-			if (!success || hrt_elapsed_time(&_last_config_check_timestamp) > 10_ms) {
+			if (!success || hrt_elapsed_time(&_last_config_check_timestamp) > 100_ms) {
 				// check configuration registers periodically or immediately following any failure
 				if (RegisterCheck(_register_cfg[_checked_register])) {
 					_last_config_check_timestamp = now;
@@ -406,12 +409,12 @@ int MPU9250::DataReadyInterruptCallback(int irq, void *context, void *arg)
 
 void MPU9250::DataReady()
 {
-	const uint8_t count = _drdy_count.fetch_add(1) + 1;
-
-	uint8_t expected = 0;
+	uint32_t expected = 0;
 
 	// at least the required number of samples in the FIFO
-	if ((count >= _fifo_gyro_samples) && _drdy_fifo_read_samples.compare_exchange(&expected, _fifo_gyro_samples)) {
+	if (((_drdy_count.fetch_add(1) + 1) >= _fifo_gyro_samples)
+	    && _drdy_fifo_read_samples.compare_exchange(&expected, _fifo_gyro_samples)) {
+
 		_drdy_count.store(0);
 		ScheduleNow();
 	}

src/drivers/imu/invensense/mpu9250/MPU9250.hpp

@@ -145,15 +145,15 @@ private:
 	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{nullptr};
+	perf_counter_t _drdy_missed_perf{nullptr};
 
 	hrt_abstime _reset_timestamp{0};
 	hrt_abstime _last_config_check_timestamp{0};
 	hrt_abstime _temperature_update_timestamp{0};
-	unsigned _consecutive_failures{0};
+	int _failure_count{0};
 
-	px4::atomic<uint8_t> _drdy_fifo_read_samples{0};
-	px4::atomic<uint8_t> _drdy_count{0};
+	px4::atomic<uint32_t> _drdy_fifo_read_samples{0};
+	px4::atomic<uint32_t> _drdy_count{0};
 	bool _data_ready_interrupt_enabled{false};
 
 	enum class STATE : uint8_t {
@@ -166,7 +166,7 @@ private:
 	STATE _state{STATE::RESET};
 
 	uint16_t _fifo_empty_interval_us{1250}; // default 1250 us / 800 Hz transfer interval
-	uint8_t _fifo_gyro_samples{static_cast<uint8_t>(_fifo_empty_interval_us / (1000000 / GYRO_RATE))};
+	uint32_t _fifo_gyro_samples{static_cast<uint32_t>(_fifo_empty_interval_us / (1000000 / GYRO_RATE))};
 
 	uint8_t _checked_register{0};
 	static constexpr uint8_t size_register_cfg{12};
@@ -183,6 +183,6 @@ private:
 		{ Register::INT_ENABLE,         INT_ENABLE_BIT::RAW_RDY_EN, 0 },
 		{ Register::I2C_MST_DELAY_CTRL, I2C_MST_DELAY_CTRL_BIT::I2C_SLVX_DLY_EN, 0 },
 		{ Register::USER_CTRL,          USER_CTRL_BIT::FIFO_EN | USER_CTRL_BIT::I2C_MST_EN | USER_CTRL_BIT::I2C_IF_DIS, 0 },
-		{ Register::PWR_MGMT_1,         PWR_MGMT_1_BIT::CLKSEL_0, 0 },
+		{ Register::PWR_MGMT_1,         PWR_MGMT_1_BIT::CLKSEL_0, PWR_MGMT_1_BIT::SLEEP },
 	};
 };

src/drivers/imu/invensense/mpu9250/MPU9250_AK8963.cpp

@@ -86,7 +86,7 @@ void MPU9250_AK8963::Run()
 		// CNTL2 SRST: Soft reset
 		_mpu9250.I2CSlaveRegisterWrite(I2C_ADDRESS_DEFAULT, (uint8_t)Register::CNTL2, CNTL2_BIT::SRST);
 		_reset_timestamp = hrt_absolute_time();
-		_consecutive_failures = 0;
+		_failure_count = 0;
 		_state = STATE::READ_WHO_AM_I;
 		ScheduleDelayed(100_ms);
 		break;
@@ -190,7 +190,9 @@ void MPU9250_AK8963::Run()
 
 					success = true;
 
-					_consecutive_failures = 0;
+					if (_failure_count > 0) {
+						_failure_count--;
+					}
 
 					ScheduleDelayed(20_ms); // ~50 Hz
 					return;
@@ -199,9 +201,9 @@ void MPU9250_AK8963::Run()
 
 			if (!success) {
 				perf_count(_bad_transfer_perf);
-				_consecutive_failures++;
+				_failure_count++;
 
-				if (_consecutive_failures > 100) {
+				if (_failure_count > 10) {
 					Reset();
 					return;
 				}

src/drivers/imu/invensense/mpu9250/MPU9250_AK8963.hpp

@@ -93,7 +93,7 @@ private:
 
 	hrt_abstime _reset_timestamp{0};
 	hrt_abstime _last_config_check_timestamp{0};
-	unsigned _consecutive_failures{0};
+	int _failure_count{0};
 
 	bool _sensitivity_adjustments_loaded{false};
 	float _sensitivity[3] {1.f, 1.f, 1.f};