dark0707
/
px4-firmware
forked from Archive/PX4-Autopilot
1
0
Fork 0
Code Pull Requests Activity

MPU9250 driver fix to preserve bus+device plug-and-play capabilities (#11020)

This commit is contained in:
Jacob Dahl 2019-01-03 07:10:23 -06:00 committed by Daniel Agar
parent 3c246ce170
commit a492d1cfdb
7 changed files with 130 additions and 172 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

20
src/drivers/imu/mpu9250/mag.cpp
View File

@ -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

2
src/drivers/imu/mpu9250/mag.h
View File

@ -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))

45
src/drivers/imu/mpu9250/main.cpp
View File

@ -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; }

139
src/drivers/imu/mpu9250/mpu9250.cpp
View File

@ -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);

22
src/drivers/imu/mpu9250/mpu9250.h
View File

@ -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) */

70
src/drivers/imu/mpu9250/mpu9250_i2c.cpp
View File

@ -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, &register_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, &register_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 */

4
src/drivers/imu/mpu9250/mpu9250_spi.cpp
View File

@ -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;