From 091c111f4fffb1b46117744ba2a1960ebbcbe6e4 Mon Sep 17 00:00:00 2001 From: Daniel Agar Date: Sat, 4 Jan 2020 12:38:22 -0500 Subject: [PATCH] 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 | 56 ++++++------ src/drivers/imu/mpu9250/MPU9250_mag.h | 15 ++-- src/drivers/imu/mpu9250/mpu9250.cpp | 111 ++++++------------------ src/drivers/imu/mpu9250/mpu9250.h | 32 ++++--- src/drivers/imu/mpu9250/mpu9250_i2c.cpp | 4 +- src/drivers/imu/mpu9250/mpu9250_spi.cpp | 8 +- 6 files changed, 87 insertions(+), 139 deletions(-) diff --git a/src/drivers/imu/mpu9250/MPU9250_mag.cpp b/src/drivers/imu/mpu9250/MPU9250_mag.cpp index 7d982aad37..18dd96debc 100644 --- a/src/drivers/imu/mpu9250/MPU9250_mag.cpp +++ b/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 ×tamp_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. diff --git a/src/drivers/imu/mpu9250/MPU9250_mag.h b/src/drivers/imu/mpu9250/MPU9250_mag.h index 7ab330ab13..383ea24021 100644 --- a/src/drivers/imu/mpu9250/MPU9250_mag.h +++ b/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 ×tamp_sample, const ak8963_regs &data); uint8_t read_reg(unsigned reg); void write_reg(unsigned reg, uint8_t value); diff --git a/src/drivers/imu/mpu9250/mpu9250.cpp b/src/drivers/imu/mpu9250/mpu9250.cpp index d02ff7e2f7..39f89c009c 100644 --- a/src/drivers/imu/mpu9250/mpu9250.cpp +++ b/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(); diff --git a/src/drivers/imu/mpu9250/mpu9250.h b/src/drivers/imu/mpu9250/mpu9250.h index 59988af297..84a20157e5 100644 --- a/src/drivers/imu/mpu9250/mpu9250.h +++ b/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 * diff --git a/src/drivers/imu/mpu9250/mpu9250_i2c.cpp b/src/drivers/imu/mpu9250/mpu9250_i2c.cpp index b47419e8d3..fbd19c2e56 100644 --- a/src/drivers/imu/mpu9250/mpu9250_i2c.cpp +++ b/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); } diff --git a/src/drivers/imu/mpu9250/mpu9250_spi.cpp b/src/drivers/imu/mpu9250/mpu9250_spi.cpp index 1d0f49408b..38c25ffe2d 100644 --- a/src/drivers/imu/mpu9250/mpu9250_spi.cpp +++ b/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