mpu9250: more small fixes and cleanup

- delete unused _checked_bad
 - delete unused "good transfers" perf counter
 - delete unused "bad transfers" perf counter
 - use proper register names in transfer structures
 - mpu9250 don't transfer unused status register
 - mpu9250 mag (ak8963) check overflow
 - mpu9250 mag (ak8963) pass data as const references
 - mpu9250 mag fix mag errors perf count
 - delete obsolete whoami checks

src/drivers/imu/mpu9250/MPU9250_mag.cpp

@@ -80,6 +80,7 @@ MPU9250_mag::measure()
 	int ret = _interface->read(AK8963REG_ST1, &st1, sizeof(st1));
 
 	if (ret != OK) {
+		perf_count(_mag_errors);
 		_px4_mag.set_error_count(perf_event_count(_mag_errors));
 		return;
 	}
@@ -107,15 +108,14 @@ MPU9250_mag::measure()
 	}
 
 	/* Monitor magnetic sensor overflow flag. */
-	if (data.st2 & 0x08) {
+	if (data.ST2 & 0x08) {
 		perf_count(_mag_overflows);
 	}
 
 	_measure(timestamp_sample, data);
 }
 
-void
-MPU9250_mag::_measure(hrt_abstime timestamp_sample, ak8963_regs data)
+bool MPU9250_mag::_measure(const hrt_abstime &timestamp_sample, const ak8963_regs &data)
 {
 	/* Check if data ready is set.
 	 * This is not described to be set in continuous mode according to the
@@ -128,8 +128,14 @@ MPU9250_mag::_measure(hrt_abstime timestamp_sample, ak8963_regs data)
 	 * is not set. This has lead to intermittent spikes when the data was
 	 * being updated while getting read.
 	 */
-	if (!(data.st1 & AK09916_ST1_DRDY)) {
-		return;
+	if (!(data.ST1 & AK09916_ST1_DRDY)) {
+		return false;
+	}
+
+	/* Monitor magnetic sensor overflow flag. */
+	if (data.ST2 & 0x08) {
+		perf_count(_mag_overflows);
+		return false;
 	}
 
 	_px4_mag.set_external(_parent->is_external());
@@ -139,7 +145,12 @@ MPU9250_mag::_measure(hrt_abstime timestamp_sample, ak8963_regs data)
 	 * Align axes - note the accel & gyro are also re-aligned so this
 	 *              doesn't look obvious with the datasheet
 	 */
-	_px4_mag.update(timestamp_sample, data.x, -data.y, -data.z);
+	int16_t x = combine(data.HXH, data.HXL);
+	int16_t y = -combine(data.HYH, data.HYL);
+	int16_t z = -combine(data.HZH, data.HZL);
+	_px4_mag.update(timestamp_sample, x, y, z);
+
+	return true;
 }
 
 void
@@ -217,8 +228,7 @@ MPU9250_mag::ak8963_reset()
 bool
 MPU9250_mag::ak8963_read_adjustments()
 {
-	uint8_t response[3];
-	float ak8963_ASA[3];
+	uint8_t response[3] {};
 
 	write_reg_through_mpu9250(AK8963REG_CNTL1, AK8963_FUZE_MODE | AK8963_16BIT_ADC);
 	px4_usleep(200);
@@ -234,6 +244,8 @@ MPU9250_mag::ak8963_read_adjustments()
 
 	write_reg_through_mpu9250(AK8963REG_CNTL1, AK8963_POWERDOWN_MODE);
 
+	float ak8963_ASA[3] {};
+
 	for (int i = 0; i < 3; i++) {
 		if (0 != response[i] && 0xff != response[i]) {
 			ak8963_ASA[i] = ((float)(response[i] - 128) / 256.0f) + 1.0f;
@@ -256,10 +268,8 @@ MPU9250_mag::ak8963_setup_master_i2c()
 	 * in master mode (SPI to I2C bridge)
 	 */
 	if (_interface == nullptr) {
-		if (_parent->_whoami == MPU_WHOAMI_9250) {
-			_parent->modify_checked_reg(MPUREG_USER_CTRL, 0, BIT_I2C_MST_EN);
-			_parent->write_reg(MPUREG_I2C_MST_CTRL, BIT_I2C_MST_P_NSR | BIT_I2C_MST_WAIT_FOR_ES | BITS_I2C_MST_CLOCK_400HZ);
-		}
+		_parent->modify_checked_reg(MPUREG_USER_CTRL, 0, BIT_I2C_MST_EN);
+		_parent->write_reg(MPUREG_I2C_MST_CTRL, BIT_I2C_MST_P_NSR | BIT_I2C_MST_WAIT_FOR_ES | BITS_I2C_MST_CLOCK_400HZ);
 
 	} else {
 		_parent->modify_checked_reg(MPUREG_USER_CTRL, BIT_I2C_MST_EN, 0);
@@ -312,15 +322,11 @@ MPU9250_mag::ak8963_setup()
 		return -EIO;
 	}
 
-	if (_parent->_whoami == MPU_WHOAMI_9250) {
-		write_reg(AK8963REG_CNTL1, AK8963_CONTINUOUS_MODE2 | AK8963_16BIT_ADC);
-	}
+	write_reg(AK8963REG_CNTL1, AK8963_CONTINUOUS_MODE2 | AK8963_16BIT_ADC);
 
 	if (_interface == nullptr) {
 		// Configure mpu' I2C Master interface to read ak8963 data into to fifo
-		if (_parent->_whoami == MPU_WHOAMI_9250) {
-			set_passthrough(AK8963REG_ST1, sizeof(ak8963_regs));
-		}
+		set_passthrough(AK8963REG_ST1, sizeof(ak8963_regs));
 	}
 
 	return OK;
@@ -349,17 +355,14 @@ void MPU9250_mag::write_imu_reg_verified(int reg, uint8_t val, uint8_t mask)
 
 void MPU9250_mag::read_reg_through_mpu9250(uint8_t reg, uint8_t *val)
 {
-	uint8_t b = 0;
-
 	// Read operation on the mag using the slave 4 registers.
-	write_imu_reg_verified(MPUREG_I2C_SLV4_ADDR,
-			       AK8963_I2C_ADDR | BIT_I2C_READ_FLAG, 0xff);
+	write_imu_reg_verified(MPUREG_I2C_SLV4_ADDR, AK8963_I2C_ADDR | BIT_I2C_READ_FLAG, 0xff);
 
 	// Set the mag register to read from.
 	write_imu_reg_verified(MPUREG_I2C_SLV4_REG, reg, 0xff);
 
 	// Read the existing value of the SLV4 control register.
-	b = _parent->read_reg(MPUREG_I2C_SLV4_CTRL);
+	uint8_t b = _parent->read_reg(MPUREG_I2C_SLV4_CTRL);
 
 	// Set the I2C_SLV4_EN bit in I2C_SL4_CTRL register without overwriting other
 	// bits. Enable data transfer, a read transfer as configured above.
@@ -390,11 +393,8 @@ void MPU9250_mag::read_reg_through_mpu9250(uint8_t reg, uint8_t *val)
 
 void MPU9250_mag::write_reg_through_mpu9250(uint8_t reg, uint8_t val)
 {
-	uint8_t b = 0;
-
 	// Configure a write operation to the mag using Slave 4.
-	write_imu_reg_verified(MPUREG_I2C_SLV4_ADDR,
-			       AK8963_I2C_ADDR, 0xff);
+	write_imu_reg_verified(MPUREG_I2C_SLV4_ADDR, AK8963_I2C_ADDR, 0xff);
 
 	// Set the mag register address to write to using Slave 4.
 	write_imu_reg_verified(MPUREG_I2C_SLV4_REG, reg, 0xff);
@@ -403,7 +403,7 @@ void MPU9250_mag::write_reg_through_mpu9250(uint8_t reg, uint8_t val)
 	write_imu_reg_verified(MPUREG_I2C_SLV4_DO, val, 0xff);
 
 	// Read the current value of the Slave 4 control register.
-	b = _parent->read_reg(MPUREG_I2C_SLV4_CTRL);
+	uint8_t b = _parent->read_reg(MPUREG_I2C_SLV4_CTRL);
 
 	// Set I2C_SLV4_EN bit in I2C_SL4_CTRL register without overwriting other
 	// bits.

src/drivers/imu/mpu9250/MPU9250_mag.h

@@ -94,11 +94,14 @@ class MPU9250;
 
 #pragma pack(push, 1)
 struct ak8963_regs {
-	uint8_t st1;
-	int16_t x;
-	int16_t y;
-	int16_t z;
-	uint8_t st2;
+	uint8_t ST1;
+	uint8_t HXL;
+	uint8_t HXH;
+	uint8_t HYL;
+	uint8_t HYH;
+	uint8_t HZL;
+	uint8_t HZH;
+	uint8_t ST2;
 };
 #pragma pack(pop)
 
@@ -131,7 +134,7 @@ protected:
 	friend class MPU9250;
 
 	void measure();
-	void _measure(hrt_abstime timestamp_sample, ak8963_regs data);
+	bool _measure(const hrt_abstime &timestamp_sample, const ak8963_regs &data);
 
 	uint8_t read_reg(unsigned reg);
 	void write_reg(unsigned reg, uint8_t value);

src/drivers/imu/mpu9250/mpu9250.cpp

@@ -78,9 +78,7 @@ MPU9250::MPU9250(device::Device *interface, device::Device *mag_interface, enum
 	_mag(this, mag_interface, rotation),
 	_dlpf_freq(MPU9250_DEFAULT_ONCHIP_FILTER_FREQ),
 	_sample_perf(perf_alloc(PC_ELAPSED, MODULE_NAME": read")),
-	_bad_transfers(perf_alloc(PC_COUNT, MODULE_NAME": bad_trans")),
 	_bad_registers(perf_alloc(PC_COUNT, MODULE_NAME": bad_reg")),
-	_good_transfers(perf_alloc(PC_COUNT, MODULE_NAME": good_trans")),
 	_duplicates(perf_alloc(PC_COUNT, MODULE_NAME": dupe"))
 {
 	_px4_accel.set_device_type(DRV_ACC_DEVTYPE_MPU9250);
@@ -94,9 +92,7 @@ MPU9250::~MPU9250()
 
 	// delete the perf counter
 	perf_free(_sample_perf);
-	perf_free(_bad_transfers);
 	perf_free(_bad_registers);
-	perf_free(_good_transfers);
 	perf_free(_duplicates);
 }
 
@@ -274,12 +270,10 @@ MPU9250::probe()
 		_num_checked_registers = MPU9250_NUM_CHECKED_REGISTERS;
 		_checked_registers = _mpu9250_checked_registers;
 		memset(_checked_values, 0, MPU9250_NUM_CHECKED_REGISTERS);
-		memset(_checked_bad, 0, MPU9250_NUM_CHECKED_REGISTERS);
 		ret = PX4_OK;
 	}
 
 	_checked_values[0] = _whoami;
-	_checked_bad[0] = _whoami;
 
 	if (ret != PX4_OK) {
 		PX4_DEBUG("unexpected whoami 0x%02x", _whoami);
@@ -431,7 +425,6 @@ MPU9250::write_checked_reg(unsigned reg, uint8_t value)
 	for (uint8_t i = 0; i < _num_checked_registers; i++) {
 		if (reg == _checked_registers[i]) {
 			_checked_values[i] = value;
-			_checked_bad[i] = value;
 			break;
 		}
 	}
@@ -515,7 +508,8 @@ MPU9250::check_registers()
 
 	if ((v = read_reg(_checked_registers[_checked_next], MPU9250_HIGH_BUS_SPEED)) != _checked_values[_checked_next]) {
 
-		_checked_bad[_checked_next] = v;
+		PX4_DEBUG("reg: %d = %d (should be %d) _reset_wait: %llu", _checked_registers[_checked_next], v,
+			  _checked_values[_checked_next], _reset_wait);
 
 		/*
 		  if we get the wrong value then we know the SPI bus
@@ -559,26 +553,6 @@ MPU9250::check_registers()
 	_checked_next = (_checked_next + 1) % _num_checked_registers;
 }
 
-bool
-MPU9250::check_null_data(uint16_t *data, uint8_t size)
-{
-	while (size--) {
-		if (*data++) {
-			perf_count(_good_transfers);
-			return false;
-		}
-	}
-
-	// all zero data - probably a SPI bus error
-	perf_count(_bad_transfers);
-	perf_end(_sample_perf);
-	// note that we don't call reset() here as a reset()
-	// costs 20ms with interrupts disabled. That means if
-	// the mpu9250 does go bad it would cause a FMU failure,
-	// regardless of whether another sensor is available,
-	return true;
-}
-
 bool
 MPU9250::check_duplicate(uint8_t *accel_data)
 {
@@ -592,8 +566,6 @@ MPU9250::check_duplicate(uint8_t *accel_data)
 	*/
 	if (!_got_duplicate && memcmp(accel_data, &_last_accel_data, sizeof(_last_accel_data)) == 0) {
 		// it isn't new data - wait for next timer
-		perf_end(_sample_perf);
-		perf_count(_duplicates);
 		_got_duplicate = true;
 
 	} else {
@@ -617,30 +589,20 @@ MPU9250::measure()
 
 	MPUReport mpu_report{};
 
-	struct Report {
-		int16_t		accel_x;
-		int16_t		accel_y;
-		int16_t		accel_z;
-		int16_t		temp;
-		int16_t		gyro_x;
-		int16_t		gyro_y;
-		int16_t		gyro_z;
-	} report{};
-
 	const hrt_abstime timestamp_sample = hrt_absolute_time();
 
 	// Fetch the full set of measurements from the ICM20948 in one pass
 	if (_mag.is_passthrough() && _register_wait == 0) {
-		if (_whoami == MPU_WHOAMI_9250 || _whoami == MPU_WHOAMI_6500) {
-			if (OK != read_reg_range(MPUREG_INT_STATUS, MPU9250_HIGH_BUS_SPEED, (uint8_t *)&mpu_report, sizeof(mpu_report))) {
-				perf_end(_sample_perf);
-				return;
-			}
+		if (OK != read_reg_range(MPUREG_ACCEL_XOUT_H, MPU9250_HIGH_BUS_SPEED, (uint8_t *)&mpu_report, sizeof(mpu_report))) {
+			perf_end(_sample_perf);
+			return;
 		}
 
 		check_registers();
 
-		if (check_duplicate(&mpu_report.accel_x[0])) {
+		if (check_duplicate(&mpu_report.ACCEL_XOUT_H)) {
+			perf_end(_sample_perf);
+			perf_count(_duplicates);
 			return;
 		}
 	}
@@ -667,62 +629,47 @@ MPU9250::measure()
 
 #   endif
 
-	// Continue evaluating gyro and accelerometer results
-	if (_register_wait == 0) {
-		// Convert from big to little endian
-		report.accel_x = int16_t_from_bytes(mpu_report.accel_x);
-		report.accel_y = int16_t_from_bytes(mpu_report.accel_y);
-		report.accel_z = int16_t_from_bytes(mpu_report.accel_z);
-		report.temp    = int16_t_from_bytes(mpu_report.temp);
-		report.gyro_x  = int16_t_from_bytes(mpu_report.gyro_x);
-		report.gyro_y  = int16_t_from_bytes(mpu_report.gyro_y);
-		report.gyro_z  = int16_t_from_bytes(mpu_report.gyro_z);
-
-		if (check_null_data((uint16_t *)&report, sizeof(report) / 2)) {
-			return;
-		}
-	}
-
 	if (_register_wait != 0) {
-		/*
-		 * We are waiting for some good transfers before using the sensor again.
-		 * We still increment _good_transfers, but don't return any data yet.
-		*/
+		// We are waiting for some good transfers before using the sensor again
 		_register_wait--;
+
+		perf_end(_sample_perf);
 		return;
 	}
 
+	// Convert from big to little endian
+	int16_t accel_x = combine(mpu_report.ACCEL_XOUT_H, mpu_report.ACCEL_XOUT_L);
+	int16_t accel_y = combine(mpu_report.ACCEL_YOUT_H, mpu_report.ACCEL_YOUT_L);
+	int16_t accel_z = combine(mpu_report.ACCEL_ZOUT_H, mpu_report.ACCEL_ZOUT_L);
+	int16_t temp    = combine(mpu_report.TEMP_OUT_H, mpu_report.TEMP_OUT_L);
+	int16_t gyro_x  = combine(mpu_report.GYRO_XOUT_H, mpu_report.GYRO_XOUT_L);
+	int16_t gyro_y  = combine(mpu_report.GYRO_YOUT_H, mpu_report.GYRO_YOUT_L);
+	int16_t gyro_z  = combine(mpu_report.GYRO_ZOUT_H, mpu_report.GYRO_ZOUT_L);
+
 	// Get sensor temperature
-	_last_temperature = (report.temp) / 333.87f + 21.0f;
+	_last_temperature = temp / 333.87f + 21.0f;
 
 	_px4_accel.set_temperature(_last_temperature);
 	_px4_gyro.set_temperature(_last_temperature);
 
-
 	// Swap axes and negate y
-	int16_t accel_xt = report.accel_y;
-	int16_t accel_yt = ((report.accel_x == -32768) ? 32767 : -report.accel_x);
+	int16_t accel_xt = accel_y;
+	int16_t accel_yt = ((accel_x == -32768) ? 32767 : -accel_x);
 
-	int16_t gyro_xt = report.gyro_y;
-	int16_t gyro_yt = ((report.gyro_x == -32768) ? 32767 : -report.gyro_x);
-
-	// Apply the swap
-	report.accel_x = accel_xt;
-	report.accel_y = accel_yt;
-	report.gyro_x = gyro_xt;
-	report.gyro_y = gyro_yt;
+	int16_t gyro_xt = gyro_y;
+	int16_t gyro_yt = ((gyro_x == -32768) ? 32767 : -gyro_x);
 
 	// report the error count as the sum of the number of bad
 	// transfers and bad register reads. This allows the higher
 	// level code to decide if it should use this sensor based on
 	// whether it has had failures
-	const uint64_t error_count = perf_event_count(_bad_transfers) + perf_event_count(_bad_registers);
+	const uint64_t error_count = perf_event_count(_bad_registers);
 	_px4_accel.set_error_count(error_count);
 	_px4_gyro.set_error_count(error_count);
 
 	/* NOTE: Axes have been swapped to match the board a few lines above. */
-	_px4_accel.update(timestamp_sample, report.accel_x, report.accel_y, report.accel_z);
-	_px4_gyro.update(timestamp_sample, report.gyro_x, report.gyro_y, report.gyro_z);
+	_px4_accel.update(timestamp_sample, accel_xt, accel_yt, accel_z);
+	_px4_gyro.update(timestamp_sample, gyro_xt, gyro_yt, gyro_z);
 
 	/* stop measuring */
 	perf_end(_sample_perf);
@@ -732,9 +679,7 @@ void
 MPU9250::print_info()
 {
 	perf_print_counter(_sample_perf);
-	perf_print_counter(_bad_transfers);
 	perf_print_counter(_bad_registers);
-	perf_print_counter(_good_transfers);
 	perf_print_counter(_duplicates);
 
 	_px4_accel.print_status();

src/drivers/imu/mpu9250/mpu9250.h

@@ -185,15 +185,22 @@
  * interrupt status.
  */
 struct MPUReport {
-	uint8_t		cmd;
-	uint8_t		status;
-	uint8_t		accel_x[2];
-	uint8_t		accel_y[2];
-	uint8_t		accel_z[2];
-	uint8_t		temp[2];
-	uint8_t		gyro_x[2];
-	uint8_t		gyro_y[2];
-	uint8_t		gyro_z[2];
+	uint8_t cmd;
+	uint8_t ACCEL_XOUT_H;
+	uint8_t ACCEL_XOUT_L;
+	uint8_t ACCEL_YOUT_H;
+	uint8_t ACCEL_YOUT_L;
+	uint8_t ACCEL_ZOUT_H;
+	uint8_t ACCEL_ZOUT_L;
+	uint8_t TEMP_OUT_H;
+	uint8_t TEMP_OUT_L;
+	uint8_t GYRO_XOUT_H;
+	uint8_t GYRO_XOUT_L;
+	uint8_t GYRO_YOUT_H;
+	uint8_t GYRO_YOUT_L;
+	uint8_t GYRO_ZOUT_H;
+	uint8_t GYRO_ZOUT_L;
+
 	struct ak8963_regs mag;
 };
 #pragma pack(pop)
@@ -214,6 +221,8 @@ struct MPUReport {
 #  define MPU9250_HIGH_SPEED_OP(r) 			MPU9250_SET_SPEED((r), MPU9250_HIGH_BUS_SPEED)
 #  define MPU9250_LOW_SPEED_OP(r)			((r) &~MPU9250_HIGH_BUS_SPEED)
 
+static constexpr int16_t combine(uint8_t msb, uint8_t lsb) { return (msb << 8u) | lsb; }
+
 /* interface factories */
 extern device::Device *MPU9250_SPI_interface(int bus, uint32_t cs);
 extern device::Device *MPU9250_I2C_interface(int bus, uint32_t address);
@@ -261,9 +270,7 @@ private:
 	unsigned		_sample_rate{1000};
 
 	perf_counter_t		_sample_perf;
-	perf_counter_t		_bad_transfers;
 	perf_counter_t		_bad_registers;
-	perf_counter_t		_good_transfers;
 	perf_counter_t		_duplicates;
 
 	uint8_t			_register_wait{0};
@@ -279,7 +286,6 @@ private:
 	const uint16_t			*_checked_registers{nullptr};
 
 	uint8_t					_checked_values[MPU9250_NUM_CHECKED_REGISTERS] {};
-	uint8_t					_checked_bad[MPU9250_NUM_CHECKED_REGISTERS] {};
 	unsigned				_checked_next{0};
 	unsigned				_num_checked_registers{0};
 
@@ -287,7 +293,6 @@ private:
 	// last temperature reading for print_info()
 	float			_last_temperature{0.0f};
 
-	bool check_null_data(uint16_t *data, uint8_t size);
 	bool check_duplicate(uint8_t *accel_data);
 
 	// keep last accel reading for duplicate detection
@@ -317,7 +322,6 @@ private:
 	 */
 	uint8_t			read_reg(unsigned reg, uint32_t speed = MPU9250_LOW_BUS_SPEED);
 
-
 	/**
 	 * Read a register range from the mpu
 	 *

src/drivers/imu/mpu9250/mpu9250_i2c.cpp

@@ -70,7 +70,7 @@ MPU9250_I2C_interface(int bus, uint32_t address)
 MPU9250_I2C::MPU9250_I2C(int bus, uint32_t address) :
 	I2C("MPU9250_I2C", nullptr, bus, address, 400000)
 {
-	_device_id.devid_s.devtype = DRV_ACC_DEVTYPE_MPU9250;
+	set_device_type(DRV_ACC_DEVTYPE_MPU9250);
 }
 
 int
@@ -96,7 +96,7 @@ MPU9250_I2C::read(unsigned reg_speed, void *data, unsigned count)
 	 * Since MPUReport has a cmd at front, we must return the data
 	 * after that. Foe anthing else we must return it
 	 */
-	uint32_t offset = count < sizeof(MPUReport) ? 0 : offsetof(MPUReport, status);
+	uint32_t offset = count < sizeof(MPUReport) ? 0 : offsetof(MPUReport, ACCEL_XOUT_H);
 	uint8_t cmd = MPU9250_REG(reg_speed);
 	return transfer(&cmd, 1, &((uint8_t *)data)[offset], count);
 }

src/drivers/imu/mpu9250/mpu9250_spi.cpp

@@ -82,17 +82,13 @@ private:
 device::Device *
 MPU9250_SPI_interface(int bus, uint32_t cs)
 {
-	device::Device *interface = nullptr;
-
-	interface = new MPU9250_SPI(bus, cs);
-
-	return interface;
+	return new MPU9250_SPI(bus, cs);
 }
 
 MPU9250_SPI::MPU9250_SPI(int bus, uint32_t device) :
 	SPI("MPU9250", nullptr, bus, device, SPIDEV_MODE3, MPU9250_LOW_SPI_BUS_SPEED)
 {
-	_device_id.devid_s.devtype = DRV_ACC_DEVTYPE_MPU9250;
+	set_device_type(DRV_ACC_DEVTYPE_MPU9250);
 }
 
 void