diff --git a/src/drivers/imu/mpu9250/mag.cpp b/src/drivers/imu/mpu9250/mag.cpp index 402bc69a68..dcf5aa9751 100644 --- a/src/drivers/imu/mpu9250/mag.cpp +++ b/src/drivers/imu/mpu9250/mag.cpp @@ -118,7 +118,7 @@ MPU9250_mag::init() /* if cdev init failed, bail now */ if (ret != OK) { - if (_parent->_device_type == MPU_DEVICE_TYPE_MPU9250) { DEVICE_DEBUG("MPU9250 mag init failed"); } + if (_parent->_whoami == MPU_WHOAMI_9250) { DEVICE_DEBUG("MPU9250 mag init failed"); } else { DEVICE_DEBUG("ICM20948 mag init failed"); } @@ -171,7 +171,7 @@ MPU9250_mag::measure() struct ak09916_regs ak09916_data; } raw_data; - if (_parent->_device_type == MPU_DEVICE_TYPE_MPU9250) { + if (_parent->_whoami == MPU_WHOAMI_9250) { ret = _interface->read(AK8963REG_ST1, &raw_data, sizeof(struct ak8963_regs)); } else { // ICM20948 --> AK09916 @@ -179,7 +179,7 @@ MPU9250_mag::measure() } if (ret == OK) { - if (_parent->_device_type == MPU_DEVICE_TYPE_ICM20948) { raw_data.ak8963_data.st2 = raw_data.ak09916_data.st2; } + if (_parent->_whoami == ICM_WHOAMI_20948) { raw_data.ak8963_data.st2 = raw_data.ak09916_data.st2; } _measure(raw_data.ak8963_data); } @@ -215,7 +215,7 @@ MPU9250_mag::_measure(struct ak8963_regs data) // need a better check here. Using _parent->is_external() for mpu9250 also sets the // internal magnetometers connected to the "external" spi bus as external, at least // on Pixhawk 2.1. For now assuming the ICM20948 is only used on Here GPS, hence external. - if (_parent->_device_type == MPU_DEVICE_TYPE_ICM20948) { + if (_parent->_whoami == ICM_WHOAMI_20948) { mrb.is_external = _parent->is_external(); } else { @@ -228,7 +228,7 @@ MPU9250_mag::_measure(struct ak8963_regs data) */ float xraw_f, yraw_f, zraw_f; - if (_parent->_device_type == MPU_DEVICE_TYPE_ICM20948) { + if (_parent->_whoami == ICM_WHOAMI_20948) { /* * Keeping consistent with the accel and gyro axes of the ICM20948 here, just aligning the magnetometer to them. */ @@ -253,7 +253,7 @@ MPU9250_mag::_measure(struct ak8963_regs data) /* apply user specified rotation */ rotate_3f(_parent->_rotation, xraw_f, yraw_f, zraw_f); - if (_parent->_device_type == MPU_DEVICE_TYPE_ICM20948) { + if (_parent->_whoami == ICM_WHOAMI_20948) { rotate_3f(ROTATION_YAW_270, xraw_f, yraw_f, zraw_f); //offset between accel/gyro and mag on icm20948 } @@ -464,7 +464,7 @@ MPU9250_mag::ak8963_setup_master_i2c(void) * in master mode (SPI to I2C bridge) */ if (_interface == nullptr) { - if (_parent->_device_type == MPU_DEVICE_TYPE_MPU9250) { + 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); @@ -503,7 +503,7 @@ MPU9250_mag::ak8963_setup(void) } while (retries > 0); /* No sensitivity adjustments available for AK09916/ICM20948 */ - if (_parent->_device_type == MPU_DEVICE_TYPE_MPU9250) { + if (_parent->_whoami == MPU_WHOAMI_9250) { if (retries > 0) { retries = 10; @@ -526,7 +526,7 @@ MPU9250_mag::ak8963_setup(void) return -EIO; } - if (_parent->_device_type == MPU_DEVICE_TYPE_MPU9250) { + if (_parent->_whoami == MPU_WHOAMI_9250) { write_reg(AK8963REG_CNTL1, AK8963_CONTINUOUS_MODE2 | AK8963_16BIT_ADC); } else { // ICM20948 -> AK09916 @@ -539,7 +539,7 @@ MPU9250_mag::ak8963_setup(void) /* Configure mpu' I2c Master interface to read ak8963 data * Into to fifo */ - if (_parent->_device_type == MPU_DEVICE_TYPE_MPU9250) { + if (_parent->_whoami == MPU_WHOAMI_9250) { set_passthrough(AK8963REG_ST1, sizeof(struct ak8963_regs)); } else { // ICM20948 -> AK09916 diff --git a/src/drivers/imu/mpu9250/mag.h b/src/drivers/imu/mpu9250/mag.h index 2bc1d02832..32908c2e45 100644 --- a/src/drivers/imu/mpu9250/mag.h +++ b/src/drivers/imu/mpu9250/mag.h @@ -97,7 +97,7 @@ #define AK09916_ST1_DOR 0x02 -#define AK_MPU_OR_ICM(m,i) ((_parent->_device_type==MPU_DEVICE_TYPE_MPU9250) ? (m) : (i)) +#define AK_MPU_OR_ICM(m,i) ((_parent->_whoami == MPU_WHOAMI_9250) ? (m) : (i)) diff --git a/src/drivers/imu/mpu9250/main.cpp b/src/drivers/imu/mpu9250/main.cpp index d5e05f2612..0d5704b2fa 100644 --- a/src/drivers/imu/mpu9250/main.cpp +++ b/src/drivers/imu/mpu9250/main.cpp @@ -139,7 +139,6 @@ namespace mpu9250 struct mpu9250_bus_option { enum MPU9250_BUS busid; - MPU_DEVICE_TYPE device_type; const char *accelpath; const char *gyropath; const char *magpath; @@ -151,15 +150,15 @@ struct mpu9250_bus_option { } bus_options[] = { #if defined (USE_I2C) # if defined(PX4_I2C_BUS_ONBOARD) && defined(PX4_I2C_OBDEV_MPU9250) - { MPU9250_BUS_I2C_INTERNAL, MPU_DEVICE_TYPE_MPU9250, MPU_DEVICE_PATH_ACCEL, MPU_DEVICE_PATH_GYRO, MPU_DEVICE_PATH_MAG, &MPU9250_I2C_interface, false, PX4_I2C_BUS_ONBOARD, PX4_I2C_OBDEV_MPU9250, NULL }, - { MPU9250_BUS_I2C_INTERNAL, MPU_DEVICE_TYPE_MPU6500, MPU_DEVICE_PATH_ACCEL, MPU_DEVICE_PATH_GYRO, MPU_DEVICE_PATH_MAG, &MPU9250_I2C_interface, false, PX4_I2C_BUS_ONBOARD, PX4_I2C_OBDEV_MPU9250, NULL }, + { MPU9250_BUS_I2C_INTERNAL, MPU_DEVICE_PATH_ACCEL, MPU_DEVICE_PATH_GYRO, MPU_DEVICE_PATH_MAG, &MPU9250_I2C_interface, false, PX4_I2C_BUS_ONBOARD, PX4_I2C_OBDEV_MPU9250, NULL }, + { MPU9250_BUS_I2C_INTERNAL, MPU_DEVICE_PATH_ACCEL, MPU_DEVICE_PATH_GYRO, MPU_DEVICE_PATH_MAG, &MPU9250_I2C_interface, false, PX4_I2C_BUS_ONBOARD, PX4_I2C_OBDEV_MPU9250, NULL }, # endif # if defined(PX4_I2C_BUS_EXPANSION) # if defined(PX4_I2C_OBDEV_MPU9250) - { MPU9250_BUS_I2C_EXTERNAL, MPU_DEVICE_TYPE_MPU9250, MPU_DEVICE_PATH_ACCEL_EXT, MPU_DEVICE_PATH_GYRO_EXT, MPU_DEVICE_PATH_MAG_EXT, &MPU9250_I2C_interface, false, PX4_I2C_BUS_EXPANSION, PX4_I2C_OBDEV_MPU9250, NULL }, - { MPU9250_BUS_I2C_EXTERNAL, MPU_DEVICE_TYPE_MPU6500, MPU_DEVICE_PATH_ACCEL_EXT, MPU_DEVICE_PATH_GYRO_EXT, MPU_DEVICE_PATH_MAG_EXT, &MPU9250_I2C_interface, false, PX4_I2C_BUS_EXPANSION, PX4_I2C_OBDEV_MPU9250, NULL }, + { MPU9250_BUS_I2C_EXTERNAL, MPU_DEVICE_PATH_ACCEL_EXT, MPU_DEVICE_PATH_GYRO_EXT, MPU_DEVICE_PATH_MAG_EXT, &MPU9250_I2C_interface, false, PX4_I2C_BUS_EXPANSION, PX4_I2C_OBDEV_MPU9250, NULL }, + { MPU9250_BUS_I2C_EXTERNAL, MPU_DEVICE_PATH_ACCEL_EXT, MPU_DEVICE_PATH_GYRO_EXT, MPU_DEVICE_PATH_MAG_EXT, &MPU9250_I2C_interface, false, PX4_I2C_BUS_EXPANSION, PX4_I2C_OBDEV_MPU9250, NULL }, # endif - { MPU9250_BUS_I2C_EXTERNAL, MPU_DEVICE_TYPE_ICM20948, MPU_DEVICE_PATH_ICM_ACCEL_EXT, MPU_DEVICE_PATH_ICM_GYRO_EXT, MPU_DEVICE_PATH_ICM_MAG_EXT, &MPU9250_I2C_interface, false, PX4_I2C_BUS_EXPANSION, PX4_I2C_EXT_ICM20948_1, NULL }, + { MPU9250_BUS_I2C_EXTERNAL, MPU_DEVICE_PATH_ICM_ACCEL_EXT, MPU_DEVICE_PATH_ICM_GYRO_EXT, MPU_DEVICE_PATH_ICM_MAG_EXT, &MPU9250_I2C_interface, false, PX4_I2C_BUS_EXPANSION, PX4_I2C_EXT_ICM20948_1, NULL }, #endif # if defined(PX4_I2C_BUS_EXPANSION1) && defined(PX4_I2C_OBDEV_MPU9250) { MPU9250_BUS_I2C_EXTERNAL, MPU_DEVICE_PATH_ACCEL_EXT1, MPU_DEVICE_PATH_GYRO_EXT1, MPU_DEVICE_PATH_MAG_EXT1, &MPU9250_I2C_interface, false, PX4_I2C_BUS_EXPANSION1, PX4_I2C_OBDEV_MPU9250, NULL }, @@ -169,16 +168,16 @@ struct mpu9250_bus_option { # endif #endif #ifdef PX4_SPIDEV_MPU - { MPU9250_BUS_SPI_INTERNAL, MPU_DEVICE_TYPE_MPU9250, MPU_DEVICE_PATH_ACCEL, MPU_DEVICE_PATH_GYRO, MPU_DEVICE_PATH_MAG, &MPU9250_SPI_interface, true, PX4_SPI_BUS_SENSORS, PX4_SPIDEV_MPU, NULL }, - { MPU9250_BUS_SPI_INTERNAL, MPU_DEVICE_TYPE_MPU6500, MPU_DEVICE_PATH_MPU6500_ACCEL, MPU_DEVICE_PATH_MPU6500_GYRO, MPU_DEVICE_PATH_MPU6500_MAG, &MPU9250_SPI_interface, true, PX4_SPI_BUS_SENSORS, PX4_SPIDEV_MPU, NULL }, + { MPU9250_BUS_SPI_INTERNAL, MPU_DEVICE_PATH_ACCEL, MPU_DEVICE_PATH_GYRO, MPU_DEVICE_PATH_MAG, &MPU9250_SPI_interface, true, PX4_SPI_BUS_SENSORS, PX4_SPIDEV_MPU, NULL }, + { MPU9250_BUS_SPI_INTERNAL, MPU_DEVICE_PATH_MPU6500_ACCEL, MPU_DEVICE_PATH_MPU6500_GYRO, MPU_DEVICE_PATH_MPU6500_MAG, &MPU9250_SPI_interface, true, PX4_SPI_BUS_SENSORS, PX4_SPIDEV_MPU, NULL }, #endif #ifdef PX4_SPIDEV_MPU2 - { MPU9250_BUS_SPI_INTERNAL2, MPU_DEVICE_TYPE_MPU9250, MPU_DEVICE_PATH_ACCEL_1, MPU_DEVICE_PATH_GYRO_1, MPU_DEVICE_PATH_MAG_1, &MPU9250_SPI_interface, true, PX4_SPI_BUS_SENSORS, PX4_SPIDEV_MPU2, NULL }, - { MPU9250_BUS_SPI_INTERNAL2, MPU_DEVICE_TYPE_MPU6500, MPU_DEVICE_PATH_MPU6500_ACCEL_1, MPU_DEVICE_PATH_MPU6500_GYRO_1, MPU_DEVICE_PATH_MPU6500_MAG_1, &MPU9250_SPI_interface, true, PX4_SPI_BUS_SENSORS, PX4_SPIDEV_MPU2, NULL }, + { MPU9250_BUS_SPI_INTERNAL2, MPU_DEVICE_PATH_ACCEL_1, MPU_DEVICE_PATH_GYRO_1, MPU_DEVICE_PATH_MAG_1, &MPU9250_SPI_interface, true, PX4_SPI_BUS_SENSORS, PX4_SPIDEV_MPU2, NULL }, + { MPU9250_BUS_SPI_INTERNAL2, MPU_DEVICE_PATH_MPU6500_ACCEL_1, MPU_DEVICE_PATH_MPU6500_GYRO_1, MPU_DEVICE_PATH_MPU6500_MAG_1, &MPU9250_SPI_interface, true, PX4_SPI_BUS_SENSORS, PX4_SPIDEV_MPU2, NULL }, #endif #if defined(PX4_SPI_BUS_EXT) && defined(PX4_SPIDEV_EXT_MPU) - { MPU9250_BUS_SPI_EXTERNAL, MPU_DEVICE_TYPE_MPU9250, MPU_DEVICE_PATH_ACCEL_EXT, MPU_DEVICE_PATH_GYRO_EXT, MPU_DEVICE_PATH_MAG_EXT, &MPU9250_SPI_interface, true, PX4_SPI_BUS_EXT, PX4_SPIDEV_EXT_MPU, NULL }, - { MPU9250_BUS_SPI_EXTERNAL, MPU_DEVICE_TYPE_MPU6500, MPU_DEVICE_PATH_MPU6500_ACCEL_EXT, MPU_DEVICE_PATH_MPU6500_GYRO_EXT, MPU_DEVICE_PATH_MPU6500_MAG_EXT, &MPU9250_SPI_interface, true, PX4_SPI_BUS_EXT, PX4_SPIDEV_EXT_MPU, NULL }, + { MPU9250_BUS_SPI_EXTERNAL, MPU_DEVICE_PATH_ACCEL_EXT, MPU_DEVICE_PATH_GYRO_EXT, MPU_DEVICE_PATH_MAG_EXT, &MPU9250_SPI_interface, true, PX4_SPI_BUS_EXT, PX4_SPIDEV_EXT_MPU, NULL }, + { MPU9250_BUS_SPI_EXTERNAL, MPU_DEVICE_PATH_MPU6500_ACCEL_EXT, MPU_DEVICE_PATH_MPU6500_GYRO_EXT, MPU_DEVICE_PATH_MPU6500_MAG_EXT, &MPU9250_SPI_interface, true, PX4_SPI_BUS_EXT, PX4_SPIDEV_EXT_MPU, NULL }, #endif }; @@ -223,7 +222,7 @@ start_bus(struct mpu9250_bus_option &bus, enum Rotation rotation, bool external, return false; } - device::Device *interface = bus.interface_constructor(bus.busnum, bus.device_type, bus.address, external); + device::Device *interface = bus.interface_constructor(bus.busnum, bus.address, external); if (interface == nullptr) { warnx("no device on bus %u", (unsigned)bus.busid); @@ -248,7 +247,7 @@ start_bus(struct mpu9250_bus_option &bus, enum Rotation rotation, bool external, #endif - bus.dev = new MPU9250(interface, mag_interface, bus.accelpath, bus.gyropath, bus.magpath, rotation, bus.device_type, + bus.dev = new MPU9250(interface, mag_interface, bus.accelpath, bus.gyropath, bus.magpath, rotation, magnetometer_only); if (bus.dev == nullptr) { @@ -265,19 +264,10 @@ start_bus(struct mpu9250_bus_option &bus, enum Rotation rotation, bool external, goto fail; } - /* - * Set the poll rate to default, starts automatic data collection. - * Doing this through the mag device for the time being - it's always there, even in magnetometer only mode. - * Using accel device for MPU6500. - */ - if (bus.device_type == MPU_DEVICE_TYPE_MPU6500) { - fd = open(bus.accelpath, O_RDONLY); - - } else { - fd = open(bus.magpath, O_RDONLY); - } + fd = open(bus.accelpath, O_RDONLY); if (fd < 0) { + PX4_INFO("ioctl failed"); goto fail; } @@ -327,11 +317,6 @@ start(enum MPU9250_BUS busid, enum Rotation rotation, bool external, bool magnet continue; } - if (bus_options[i].device_type == MPU_DEVICE_TYPE_MPU6500 && magnetometer_only) { - // prevent starting MPU6500 in magnetometer only mode - continue; - } - started |= start_bus(bus_options[i], rotation, external, magnetometer_only); if (started) { break; } diff --git a/src/drivers/imu/mpu9250/mpu9250.cpp b/src/drivers/imu/mpu9250/mpu9250.cpp index 4127f10977..8fbe7be283 100644 --- a/src/drivers/imu/mpu9250/mpu9250.cpp +++ b/src/drivers/imu/mpu9250/mpu9250.cpp @@ -112,14 +112,12 @@ const uint16_t MPU9250::_icm20948_checked_registers[ICM20948_NUM_CHECKED_REGISTE MPU9250::MPU9250(device::Device *interface, device::Device *mag_interface, const char *path_accel, const char *path_gyro, const char *path_mag, enum Rotation rotation, - int device_type, bool magnetometer_only) : _interface(interface), + _whoami(0), _accel(magnetometer_only ? nullptr : new MPU9250_accel(this, path_accel)), _gyro(magnetometer_only ? nullptr : new MPU9250_gyro(this, path_gyro)), _mag(new MPU9250_mag(this, mag_interface, path_mag)), - _whoami(0), - _device_type(device_type), _selected_bank(0xFF), // invalid/improbable bank value, will be set on first read/write _magnetometer_only(magnetometer_only), #if defined(USE_I2C) @@ -194,9 +192,6 @@ MPU9250::MPU9250(device::Device *interface, device::Device *mag_interface, const _mag->_device_id.devid_s.bus = _interface->get_device_bus(); _mag->_device_id.devid_s.address = _interface->get_device_address(); - /* For an independent mag, ensure that it is connected to the i2c bus */ - _interface->set_device_type(DRV_ACC_DEVTYPE_MPU9250); - // default accel scale factors _accel_scale.x_offset = 0; _accel_scale.x_scale = 1.0f; @@ -365,7 +360,7 @@ MPU9250::init() } /* Magnetometer setup */ - if (_device_type == MPU_DEVICE_TYPE_MPU9250 || _device_type == MPU_DEVICE_TYPE_ICM20948) { + if (_whoami == MPU_WHOAMI_9250 || _whoami == ICM_WHOAMI_20948) { #ifdef USE_I2C @@ -448,7 +443,7 @@ int MPU9250::reset() ret = reset_mpu(); - if (ret == OK && (_device_type == MPU_DEVICE_TYPE_MPU9250 || _device_type == MPU_DEVICE_TYPE_ICM20948)) { + if (ret == OK && (_whoami == MPU_WHOAMI_9250 || _whoami == ICM_WHOAMI_20948)) { ret = _mag->ak8963_reset(); } @@ -464,16 +459,16 @@ int MPU9250::reset_mpu() { uint8_t retries; - switch (_device_type) { - case MPU_DEVICE_TYPE_MPU9250: - case MPU_DEVICE_TYPE_MPU6500: + switch (_whoami) { + case MPU_WHOAMI_9250: + case MPU_WHOAMI_6500: write_reg(MPUREG_PWR_MGMT_1, BIT_H_RESET); write_checked_reg(MPUREG_PWR_MGMT_1, MPU_CLK_SEL_AUTO); write_checked_reg(MPUREG_PWR_MGMT_2, 0); usleep(1000); break; - case MPU_DEVICE_TYPE_ICM20948: + case ICM_WHOAMI_20948: write_reg(ICMREG_20948_PWR_MGMT_1, BIT_H_RESET); usleep(1000); @@ -495,13 +490,13 @@ int MPU9250::reset_mpu() _set_dlpf_filter(MPU9250_DEFAULT_ONCHIP_FILTER_FREQ); // Gyro scale 2000 deg/s () - switch (_device_type) { - case MPU_DEVICE_TYPE_MPU9250: - case MPU_DEVICE_TYPE_MPU6500: + switch (_whoami) { + case MPU_WHOAMI_9250: + case MPU_WHOAMI_6500: write_checked_reg(MPUREG_GYRO_CONFIG, BITS_FS_2000DPS); break; - case MPU_DEVICE_TYPE_ICM20948: + case ICM_WHOAMI_20948: modify_checked_reg(ICMREG_20948_GYRO_CONFIG_1, ICM_BITS_GYRO_FS_SEL_MASK, ICM_BITS_GYRO_FS_SEL_2000DPS); break; } @@ -554,7 +549,7 @@ int MPU9250::reset_mpu() for (uint8_t i = 0; i < _num_checked_registers; i++) { if ((reg = read_reg(_checked_registers[i])) != _checked_values[i]) { - if (_device_type == MPU_DEVICE_TYPE_ICM20948) { + if (_whoami == ICM_WHOAMI_20948) { _interface->read(MPU9250_LOW_SPEED_OP(ICMREG_20948_BANK_SEL), &bankcheck, 1); } @@ -572,55 +567,37 @@ int MPU9250::reset_mpu() int MPU9250::probe() { - int ret = -EIO; + int ret = PX4_ERROR; - /* look for device ID */ - switch (_device_type) { + // Try first for mpu9250/6500 + _whoami = read_reg(MPUREG_WHOAMI); - case MPU_DEVICE_TYPE_MPU9250: - _whoami = read_reg(MPUREG_WHOAMI); - - if (_whoami == MPU_WHOAMI_9250) { - _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 = OK; - } - - break; - - case MPU_DEVICE_TYPE_MPU6500: - _whoami = read_reg(MPUREG_WHOAMI); - - if (_whoami == MPU_WHOAMI_6500) { - _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 = OK; - } - - break; - - case MPU_DEVICE_TYPE_ICM20948: + // If it's not an MPU it must be an ICM + if ((_whoami != MPU_WHOAMI_9250) && (_whoami != MPU_WHOAMI_6500)) { _whoami = read_reg(ICMREG_20948_WHOAMI); + } - if (_whoami == ICM_WHOAMI_20948) { - _num_checked_registers = ICM20948_NUM_CHECKED_REGISTERS; - _checked_registers = _icm20948_checked_registers; - memset(_checked_values, 0, ICM20948_NUM_CHECKED_REGISTERS); - memset(_checked_bad, 0, ICM20948_NUM_CHECKED_REGISTERS); - ret = OK; - } + if (_whoami == MPU_WHOAMI_9250 || _whoami == MPU_WHOAMI_6500) { - break; + _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; + + } else if (_whoami == ICM_WHOAMI_20948) { + + _num_checked_registers = ICM20948_NUM_CHECKED_REGISTERS; + _checked_registers = _icm20948_checked_registers; + memset(_checked_values, 0, ICM20948_NUM_CHECKED_REGISTERS); + memset(_checked_bad, 0, ICM20948_NUM_CHECKED_REGISTERS); + ret = PX4_OK; } _checked_values[0] = _whoami; _checked_bad[0] = _whoami; - if (ret != OK) { + if (ret != PX4_OK) { PX4_DEBUG("unexpected whoami 0x%02x", _whoami); } @@ -640,13 +617,13 @@ MPU9250::_set_sample_rate(unsigned desired_sample_rate_hz) desired_sample_rate_hz = MPU9250_GYRO_DEFAULT_RATE; } - switch (_device_type) { - case MPU_DEVICE_TYPE_MPU9250: - case MPU_DEVICE_TYPE_MPU6500: + switch (_whoami) { + case MPU_WHOAMI_9250: + case MPU_WHOAMI_6500: div = 1000 / desired_sample_rate_hz; break; - case MPU_DEVICE_TYPE_ICM20948: + case ICM_WHOAMI_20948: div = 1100 / desired_sample_rate_hz; break; } @@ -656,14 +633,14 @@ MPU9250::_set_sample_rate(unsigned desired_sample_rate_hz) if (div < 1) { div = 1; } - switch (_device_type) { - case MPU_DEVICE_TYPE_MPU9250: - case MPU_DEVICE_TYPE_MPU6500: + switch (_whoami) { + case MPU_WHOAMI_9250: + case MPU_WHOAMI_6500: write_checked_reg(MPUREG_SMPLRT_DIV, div - 1); _sample_rate = 1000 / div; break; - case MPU_DEVICE_TYPE_ICM20948: + case ICM_WHOAMI_20948: write_checked_reg(ICMREG_20948_GYRO_SMPLRT_DIV, div - 1); // There's also an MSB for this allowing much higher dividers for the accelerometer. // For 1 < div < 200 the LSB is sufficient. @@ -736,9 +713,9 @@ MPU9250::_set_dlpf_filter(uint16_t frequency_hz) { uint8_t filter; - switch (_device_type) { - case MPU_DEVICE_TYPE_MPU9250: - case MPU_DEVICE_TYPE_MPU6500: + switch (_whoami) { + case MPU_WHOAMI_9250: + case MPU_WHOAMI_6500: /* choose next highest filter frequency available @@ -783,7 +760,7 @@ MPU9250::_set_dlpf_filter(uint16_t frequency_hz) write_checked_reg(MPUREG_CONFIG, filter); break; - case MPU_DEVICE_TYPE_ICM20948: + case ICM_WHOAMI_20948: /* choose next highest filter frequency available for gyroscope @@ -920,7 +897,7 @@ MPU9250::read_reg(unsigned reg, uint32_t speed) { uint8_t buf; - if (_device_type == MPU_DEVICE_TYPE_ICM20948) { + if (_whoami == ICM_WHOAMI_20948) { select_register_bank(REG_BANK(reg)); _interface->read(MPU9250_SET_SPEED(REG_ADDRESS(reg), speed), &buf, 1); @@ -938,7 +915,7 @@ MPU9250::read_reg_range(unsigned start_reg, uint32_t speed, uint8_t *buf, uint16 if (buf == NULL) { return PX4_ERROR; } - if (_device_type == MPU_DEVICE_TYPE_ICM20948) { + if (_whoami == ICM_WHOAMI_20948) { select_register_bank(REG_BANK(start_reg)); ret = _interface->read(MPU9250_SET_SPEED(REG_ADDRESS(start_reg), speed), buf, count); @@ -956,7 +933,7 @@ MPU9250::read_reg16(unsigned reg) // general register transfer at low clock speed - if (_device_type == MPU_DEVICE_TYPE_ICM20948) { + if (_whoami == ICM_WHOAMI_20948) { select_register_bank(REG_BANK(reg)); _interface->read(MPU9250_LOW_SPEED_OP(REG_ADDRESS(reg)), &buf, arraySize(buf)); @@ -972,7 +949,7 @@ MPU9250::write_reg(unsigned reg, uint8_t value) { // general register transfer at low clock speed - if (_device_type == MPU_DEVICE_TYPE_ICM20948) { + if (_whoami == ICM_WHOAMI_20948) { select_register_bank(REG_BANK(reg)); _interface->write(MPU9250_LOW_SPEED_OP(REG_ADDRESS(reg)), &value, 1); @@ -1045,13 +1022,13 @@ MPU9250::set_accel_range(unsigned max_g_in) max_accel_g = 2; } - switch (_device_type) { - case MPU_DEVICE_TYPE_MPU9250: - case MPU_DEVICE_TYPE_MPU6500: + switch (_whoami) { + case MPU_WHOAMI_9250: + case MPU_WHOAMI_6500: write_checked_reg(MPUREG_ACCEL_CONFIG, afs_sel << 3); break; - case MPU_DEVICE_TYPE_ICM20948: + case ICM_WHOAMI_20948: modify_checked_reg(ICMREG_20948_ACCEL_CONFIG, ICM_BITS_ACCEL_FS_SEL_MASK, afs_sel << 1); break; } @@ -1185,7 +1162,7 @@ MPU9250::check_registers(void) // if the product_id is wrong then reset the // sensor completely - if (_device_type == MPU_DEVICE_TYPE_ICM20948) { + if (_whoami == ICM_WHOAMI_20948) { // reset_mpu(); } else { write_reg(MPUREG_PWR_MGMT_1, BIT_H_RESET); @@ -1289,7 +1266,7 @@ MPU9250::measure() */ if ((!_magnetometer_only || _mag->is_passthrough()) && _register_wait == 0) { - if (_device_type == MPU_DEVICE_TYPE_MPU9250 || _device_type == MPU_DEVICE_TYPE_MPU6500) { + 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; @@ -1340,7 +1317,7 @@ MPU9250::measure() /* * Convert from big to little endian */ - if (_device_type == MPU_DEVICE_TYPE_ICM20948) { + if (_whoami == ICM_WHOAMI_20948) { report.accel_x = int16_t_from_bytes(icm_report.accel_x); report.accel_y = int16_t_from_bytes(icm_report.accel_y); report.accel_z = int16_t_from_bytes(icm_report.accel_z); @@ -1389,7 +1366,7 @@ MPU9250::measure() /* * Keeping the axes as they are for ICM20948 so orientation will match the actual chip orientation */ - if (_device_type != MPU_DEVICE_TYPE_ICM20948) { + if (_whoami != ICM_WHOAMI_20948) { /* * Swap axes and negate y */ @@ -1541,7 +1518,7 @@ MPU9250::measure() void MPU9250::print_info() { - ::printf("Device type:%d\n", _device_type); + ::printf("Device type:%d\n", _whoami); perf_print_counter(_sample_perf); perf_print_counter(_accel_reads); perf_print_counter(_gyro_reads); diff --git a/src/drivers/imu/mpu9250/mpu9250.h b/src/drivers/imu/mpu9250/mpu9250.h index 81c6f50f07..3cd1ab9f71 100644 --- a/src/drivers/imu/mpu9250/mpu9250.h +++ b/src/drivers/imu/mpu9250/mpu9250.h @@ -57,14 +57,6 @@ #include "gyro.h" -/* List of supported device types */ -enum MPU_DEVICE_TYPE { - MPU_DEVICE_TYPE_MPU9250 = 9250, - MPU_DEVICE_TYPE_MPU6500 = 6500, - MPU_DEVICE_TYPE_ICM20948 = 20948 -}; - - #if defined(PX4_I2C_OBDEV_MPU9250) || defined(PX4_I2C_BUS_EXPANSION) # define USE_I2C #endif @@ -311,7 +303,7 @@ enum MPU_DEVICE_TYPE { -#define MPU_OR_ICM(m,i) ((_device_type==MPU_DEVICE_TYPE_ICM20948) ? i : m) +#define MPU_OR_ICM(m,i) ((_whoami==ICM_WHOAMI_20948) ? i : m) #pragma pack(push, 1) @@ -373,11 +365,11 @@ struct MPUReport { # define MPU9250_LOW_SPEED_OP(r) ((r) &~MPU9250_HIGH_BUS_SPEED) /* interface factories */ -extern device::Device *MPU9250_SPI_interface(int bus, int device_type, uint32_t cs, bool external_bus); -extern device::Device *MPU9250_I2C_interface(int bus, int device_type, uint32_t address, bool external_bus); -extern int MPU9250_probe(device::Device *dev, int device_type); +extern device::Device *MPU9250_SPI_interface(int bus, uint32_t cs, bool external_bus); +extern device::Device *MPU9250_I2C_interface(int bus, uint32_t address, bool external_bus); +extern int MPU9250_probe(device::Device *dev); -typedef device::Device *(*MPU9250_constructor)(int, int, uint32_t, bool); +typedef device::Device *(*MPU9250_constructor)(int, uint32_t, bool); class MPU9250_mag; class MPU9250_accel; @@ -389,7 +381,6 @@ public: MPU9250(device::Device *interface, device::Device *mag_interface, const char *path_accel, const char *path_gyro, const char *path_mag, enum Rotation rotation, - int device_type, bool magnetometer_only); virtual ~MPU9250(); @@ -403,6 +394,7 @@ public: protected: device::Device *_interface; + uint8_t _whoami; /** whoami result */ virtual int probe(); @@ -414,8 +406,6 @@ private: MPU9250_accel *_accel; MPU9250_gyro *_gyro; MPU9250_mag *_mag; - uint8_t _whoami; /** whoami result */ - int _device_type; uint8_t _selected_bank; /* Remember selected memory bank to avoid polling / setting on each read/write */ bool _magnetometer_only; /* To disable accel and gyro reporting if only magnetometer is used (e.g. as external magnetometer) */ diff --git a/src/drivers/imu/mpu9250/mpu9250_i2c.cpp b/src/drivers/imu/mpu9250/mpu9250_i2c.cpp index f9ded07ddc..3e3a1fecd2 100644 --- a/src/drivers/imu/mpu9250/mpu9250_i2c.cpp +++ b/src/drivers/imu/mpu9250/mpu9250_i2c.cpp @@ -46,12 +46,12 @@ #ifdef USE_I2C -device::Device *MPU9250_I2C_interface(int bus, int device_type, uint32_t address, bool external_bus); +device::Device *MPU9250_I2C_interface(int bus, uint32_t address, bool external_bus); class MPU9250_I2C : public device::I2C { public: - MPU9250_I2C(int bus, int device_type, uint32_t address); + MPU9250_I2C(int bus, uint32_t address); ~MPU9250_I2C() override = default; int read(unsigned address, void *data, unsigned count) override; @@ -61,19 +61,17 @@ protected: virtual int probe(); private: - int _device_type; }; device::Device * -MPU9250_I2C_interface(int bus, int device_type, uint32_t address, bool external_bus) +MPU9250_I2C_interface(int bus, uint32_t address, bool external_bus) { - return new MPU9250_I2C(bus, device_type, address); + return new MPU9250_I2C(bus, address); } -MPU9250_I2C::MPU9250_I2C(int bus, int device_type, uint32_t address) : - I2C("MPU9250_I2C", nullptr, bus, address, 400000), - _device_type(device_type) +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; } @@ -109,35 +107,43 @@ MPU9250_I2C::read(unsigned reg_speed, void *data, unsigned count) int MPU9250_I2C::probe() { - uint8_t whoami = 0; - uint8_t reg_whoami = 0; - uint8_t expected = 0; - uint8_t register_select = REG_BANK(BANK0); // register bank containing WHOAMI for ICM20948 + // uint8_t whoami = 0; + // uint8_t reg_whoami = 0; + // uint8_t expected = 0; + // uint8_t register_select = REG_BANK(BANK0); // register bank containing WHOAMI for ICM20948 - switch (_device_type) { - case MPU_DEVICE_TYPE_MPU9250: - reg_whoami = MPUREG_WHOAMI; - expected = MPU_WHOAMI_9250; - break; + // switch (_whoami) { + // case MPU_WHOAMI_9250: + // reg_whoami = MPUREG_WHOAMI; + // expected = MPU_WHOAMI_9250; + // break; - case MPU_DEVICE_TYPE_MPU6500: - reg_whoami = MPUREG_WHOAMI; - expected = MPU_WHOAMI_6500; - break; + // case MPU_WHOAMI_6500: + // reg_whoami = MPUREG_WHOAMI; + // expected = MPU_WHOAMI_6500; + // break; - case MPU_DEVICE_TYPE_ICM20948: - reg_whoami = ICMREG_20948_WHOAMI; - expected = ICM_WHOAMI_20948; - /* - * make sure register bank 0 is selected - whoami is only present on bank 0, and that is - * not sure e.g. if the device has rebooted without repowering the sensor - */ - write(ICMREG_20948_BANK_SEL, ®ister_select, 1); + // case ICM_WHOAMI_20948: + // reg_whoami = ICMREG_20948_WHOAMI; + // expected = ICM_WHOAMI_20948; + // /* + // * make sure register bank 0 is selected - whoami is only present on bank 0, and that is + // * not sure e.g. if the device has rebooted without repowering the sensor + // */ + // write(ICMREG_20948_BANK_SEL, ®ister_select, 1); - break; - } + // break; + // } - return (read(reg_whoami, &whoami, 1) == OK && (whoami == expected)) ? 0 : -EIO; + // return (read(reg_whoami, &whoami, 1) == OK && (whoami == expected)) ? 0 : -EIO; + + + // // Try the mpu9250/6500 first + // read(MPUREG_WHOAMI, &whoami, 1); + // if (whoami == MPU_WHOAMI_9250) + + // this does not matter + return PX4_OK; } #endif /* USE_I2C */ diff --git a/src/drivers/imu/mpu9250/mpu9250_spi.cpp b/src/drivers/imu/mpu9250/mpu9250_spi.cpp index 8a30fff910..c02d6f2dc4 100644 --- a/src/drivers/imu/mpu9250/mpu9250_spi.cpp +++ b/src/drivers/imu/mpu9250/mpu9250_spi.cpp @@ -63,7 +63,7 @@ #define MPU9250_LOW_SPI_BUS_SPEED 1000*1000 #define MPU9250_HIGH_SPI_BUS_SPEED 20*1000*1000 -device::Device *MPU9250_SPI_interface(int bus, int device_type, uint32_t cs, bool external_bus); +device::Device *MPU9250_SPI_interface(int bus, uint32_t cs, bool external_bus); class MPU9250_SPI : public device::SPI { @@ -84,7 +84,7 @@ private: }; device::Device * -MPU9250_SPI_interface(int bus, int device_type, uint32_t cs, bool external_bus) +MPU9250_SPI_interface(int bus, uint32_t cs, bool external_bus) { device::Device *interface = nullptr;