From cecfae4e80a28bd2d9b03e12e581dab97b6b7215 Mon Sep 17 00:00:00 2001 From: Florian Olbrich Date: Wed, 1 Aug 2018 23:47:51 +0200 Subject: [PATCH 1/8] Integrated preliminary ICM20948 support into MPU9250 driver. Fixed temperature conversion for MPU9250/ICM20948. Included missing check for PX4_I2C_OBDEV_MPU9250 in main.cpp. Added explicit bus for internal MPU9250 on Pixhawk 2.1 to avoid implicit start of an externally attached device with wrong orientation. Took out printing checked registers from print_info, as it interferes with a driver running icm20948 in parallel by causing unexpected register bank switches. --- ROMFS/px4fmu_common/init.d/rc.sensors | 3 + src/drivers/imu/mpu9250/mag.cpp | 148 ++-- src/drivers/imu/mpu9250/mag.h | 44 ++ src/drivers/imu/mpu9250/mag_i2c.cpp | 2 +- src/drivers/imu/mpu9250/main.cpp | 83 ++- src/drivers/imu/mpu9250/mpu9250.cpp | 931 +++++++++++++++++------- src/drivers/imu/mpu9250/mpu9250.h | 214 +++++- src/drivers/imu/mpu9250/mpu9250_i2c.cpp | 48 +- src/drivers/imu/mpu9250/mpu9250_spi.cpp | 4 +- 9 files changed, 1115 insertions(+), 362 deletions(-) diff --git a/ROMFS/px4fmu_common/init.d/rc.sensors b/ROMFS/px4fmu_common/init.d/rc.sensors index 6a53ee9c9159..064fd7022512 100644 --- a/ROMFS/px4fmu_common/init.d/rc.sensors +++ b/ROMFS/px4fmu_common/init.d/rc.sensors @@ -100,6 +100,9 @@ then teraranger start -a fi +# ICM20948 as external magnetometer on I2C (e.g. Here GPS) +mpu9250 -X -R 6 start + ############################################################################### # End Optional drivers # ############################################################################### diff --git a/src/drivers/imu/mpu9250/mag.cpp b/src/drivers/imu/mpu9250/mag.cpp index 8e08c9dcaeb1..177da019cb3a 100644 --- a/src/drivers/imu/mpu9250/mag.cpp +++ b/src/drivers/imu/mpu9250/mag.cpp @@ -129,14 +129,17 @@ MPU9250_mag::init() /* if cdev init failed, bail now */ if (ret != OK) { - DEVICE_DEBUG("MPU9250 mag init failed"); + if (_parent->_device_type == MPU_DEVICE_TYPE_MPU9250) { DEVICE_DEBUG("MPU9250 mag init failed"); } + + else { DEVICE_DEBUG("ICM20948 mag init failed"); } + return ret; } _mag_reports = new ringbuffer::RingBuffer(2, sizeof(mag_report)); if (_mag_reports == nullptr) { - return -ENOMEM;; + return -ENOMEM; } _mag_class_instance = register_class_devname(MAG_BASE_DEVICE_PATH); @@ -147,8 +150,8 @@ MPU9250_mag::init() _mag_reports->get(&mrp); _mag_topic = orb_advertise_multi(ORB_ID(sensor_mag), &mrp, - &_mag_orb_class_instance, ORB_PRIO_LOW); - // &_mag_orb_class_instance, (is_external()) ? ORB_PRIO_MAX - 1 : ORB_PRIO_HIGH - 1); + &_mag_orb_class_instance, (_parent->is_external()) ? ORB_PRIO_VERY_HIGH : ORB_PRIO_DEFAULT); +// &_mag_orb_class_instance, ORB_PRIO_LOW); if (_mag_topic == nullptr) { PX4_ERR("ADVERT FAIL"); @@ -173,10 +176,23 @@ bool MPU9250_mag::check_duplicate(uint8_t *mag_data) void MPU9250_mag::measure() { - struct ak8963_regs data; + uint8_t ret; + union raw_data_t { + struct ak8963_regs ak8963_data; + struct ak09916_regs ak09916_data; + } raw_data; + + if (_parent->_device_type == MPU_DEVICE_TYPE_MPU9250) { + ret = _interface->read(AK8963REG_ST1, &raw_data, sizeof(struct ak8963_regs)); + + } else { // ICM20948 --> AK09916 + ret = _interface->read(AK09916REG_ST1, &raw_data, sizeof(struct ak09916_regs)); + } - if (OK == _interface->read(AK8963REG_ST1, &data, sizeof(struct ak8963_regs))) { - _measure(data); + if (ret == OK) { + if (_parent->_device_type == MPU_DEVICE_TYPE_ICM20948) { raw_data.ak8963_data.st2 = raw_data.ak09916_data.st2; } + + _measure(raw_data.ak8963_data); } } @@ -205,19 +221,45 @@ MPU9250_mag::_measure(struct ak8963_regs data) mag_report mrb; mrb.timestamp = hrt_absolute_time(); - mrb.is_external = false; +// mrb.is_external = false; + + // 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) { + mrb.is_external = _parent->is_external(); + + } else { + mrb.is_external = false; + } /* * Align axes - note the accel & gryo are also re-aligned so this * doesn't look obvious with the datasheet */ - mrb.x_raw = data.x; - mrb.y_raw = -data.y; - mrb.z_raw = -data.z; + float xraw_f, yraw_f, zraw_f; + + if (_parent->_device_type == MPU_DEVICE_TYPE_ICM20948) { + /* + * Keeping consistent with the accel and gyro axes of the ICM20948 here, just aligning the magnetometer to them. + */ + mrb.x_raw = data.y; + mrb.y_raw = data.x; + mrb.z_raw = -data.z; + + xraw_f = data.y; + yraw_f = data.x; + zraw_f = -data.z; + + } else { + mrb.x_raw = data.x; + mrb.y_raw = -data.y; + mrb.z_raw = -data.z; - float xraw_f = data.x; - float yraw_f = -data.y; - float zraw_f = -data.z; + xraw_f = data.x; + yraw_f = -data.y; + zraw_f = -data.z; + } /* apply user specified rotation */ rotate_3f(_parent->_rotation, xraw_f, yraw_f, zraw_f); @@ -337,19 +379,20 @@ MPU9250_mag::set_passthrough(uint8_t reg, uint8_t size, uint8_t *out) { uint8_t addr; - _parent->write_reg(MPUREG_I2C_SLV0_CTRL, 0); // ensure slave r/w is disabled before changing the registers + _parent->write_reg(AK_MPU_OR_ICM(MPUREG_I2C_SLV0_CTRL, ICMREG_20948_I2C_SLV0_CTRL), + 0); // ensure slave r/w is disabled before changing the registers if (out) { - _parent->write_reg(MPUREG_I2C_SLV0_D0, *out); + _parent->write_reg(AK_MPU_OR_ICM(MPUREG_I2C_SLV0_D0, ICMREG_20948_I2C_SLV0_DO), *out); addr = AK8963_I2C_ADDR; } else { addr = AK8963_I2C_ADDR | BIT_I2C_READ_FLAG; } - _parent->write_reg(MPUREG_I2C_SLV0_ADDR, addr); - _parent->write_reg(MPUREG_I2C_SLV0_REG, reg); - _parent->write_reg(MPUREG_I2C_SLV0_CTRL, size | BIT_I2C_SLV0_EN); + _parent->write_reg(AK_MPU_OR_ICM(MPUREG_I2C_SLV0_ADDR, ICMREG_20948_I2C_SLV0_ADDR), addr); + _parent->write_reg(AK_MPU_OR_ICM(MPUREG_I2C_SLV0_REG, ICMREG_20948_I2C_SLV0_REG), reg); + _parent->write_reg(AK_MPU_OR_ICM(MPUREG_I2C_SLV0_CTRL, ICMREG_20948_I2C_SLV0_CTRL), size | BIT_I2C_SLV0_EN); } void @@ -363,8 +406,8 @@ MPU9250_mag::passthrough_read(uint8_t reg, uint8_t *buf, uint8_t size) { set_passthrough(reg, size); usleep(25 + 25 * size); // wait for the value to be read from slave - read_block(MPUREG_EXT_SENS_DATA_00, buf, size); - _parent->write_reg(MPUREG_I2C_SLV0_CTRL, 0); // disable new reads + read_block(AK_MPU_OR_ICM(MPUREG_EXT_SENS_DATA_00, ICMREG_20948_EXT_SLV_SENS_DATA_00), buf, size); + _parent->write_reg(AK_MPU_OR_ICM(MPUREG_I2C_SLV0_CTRL, ICMREG_20948_I2C_SLV0_CTRL), 0); // disable new reads } uint8_t @@ -399,7 +442,7 @@ MPU9250_mag::passthrough_write(uint8_t reg, uint8_t val) { set_passthrough(reg, 1, &val); usleep(50); // wait for the value to be written to slave - _parent->write_reg(MPUREG_I2C_SLV0_CTRL, 0); // disable new writes + _parent->write_reg(AK_MPU_OR_ICM(MPUREG_I2C_SLV0_CTRL, ICMREG_20948_I2C_SLV0_CTRL), 0); // disable new writes } @@ -429,7 +472,7 @@ MPU9250_mag::ak8963_reset(void) if (rv == OK) { // Now reset the mag - write_reg(AK8963REG_CNTL2, AK8963_RESET); + write_reg(AK_MPU_OR_ICM(AK8963REG_CNTL2, AK09916REG_CNTL3), AK8963_RESET); // Then re-initialize the bus/mag rv = ak8963_setup(); } @@ -480,11 +523,18 @@ MPU9250_mag::ak8963_setup_master_i2c(void) * in master mode (SPI to I2C bridge) */ if (_interface == nullptr) { - _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); + if (_parent->_device_type == MPU_DEVICE_TYPE_MPU9250) { + _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 { // ICM20948 -> AK09916 + _parent->modify_checked_reg(ICMREG_20948_USER_CTRL, 0, BIT_I2C_MST_EN); + // WAIT_FOR_ES does not exist for ICM20948. Not sure how to replace this (or if that is needed) + _parent->write_reg(ICMREG_20948_I2C_MST_CTRL, BIT_I2C_MST_P_NSR | ICM_BITS_I2C_MST_CLOCK_400HZ); + } } else { - _parent->modify_checked_reg(MPUREG_USER_CTRL, BIT_I2C_MST_EN, 0); + _parent->modify_checked_reg(AK_MPU_OR_ICM(MPUREG_USER_CTRL, ICMREG_20948_USER_CTRL), BIT_I2C_MST_EN, 0); } return OK; @@ -497,7 +547,7 @@ MPU9250_mag::ak8963_setup(void) do { ak8963_setup_master_i2c(); - write_reg(AK8963REG_CNTL2, AK8963_RESET); + write_reg(AK_MPU_OR_ICM(AK8963REG_CNTL2, AK09916REG_CNTL3), AK8963_RESET); uint8_t id = 0; @@ -506,33 +556,41 @@ MPU9250_mag::ak8963_setup(void) } retries--; - PX4_ERR("AK8963: bad id %d retries %d", id, retries); - _parent->modify_reg(MPUREG_USER_CTRL, 0, BIT_I2C_MST_RST); + PX4_WARN("AK8963: bad id %d retries %d", id, retries); + _parent->modify_reg(AK_MPU_OR_ICM(MPUREG_USER_CTRL, ICMREG_20948_USER_CTRL), 0, BIT_I2C_MST_RST); up_udelay(100); } while (retries > 0); - if (retries > 0) { - retries = 10; + /* No sensitivity adjustments available for AK09916/ICM20948 */ + if (_parent->_device_type == MPU_DEVICE_TYPE_MPU9250) { + if (retries > 0) { + retries = 10; - while (!ak8963_read_adjustments() && retries) { - retries--; - PX4_ERR("AK8963: failed to read adjustment data. Retries %d", retries); + while (!ak8963_read_adjustments() && retries) { + retries--; + PX4_ERR("AK8963: failed to read adjustment data. Retries %d", retries); - _parent->modify_reg(MPUREG_USER_CTRL, 0, BIT_I2C_MST_RST); - up_udelay(100); - ak8963_setup_master_i2c(); - write_reg(AK8963REG_CNTL2, AK8963_RESET); + _parent->modify_reg(AK_MPU_OR_ICM(MPUREG_USER_CTRL, ICMREG_20948_USER_CTRL), 0, BIT_I2C_MST_RST); + up_udelay(100); + ak8963_setup_master_i2c(); + write_reg(AK_MPU_OR_ICM(AK8963REG_CNTL2, AK09916REG_CNTL3), AK8963_RESET); + } } } if (retries == 0) { PX4_ERR("AK8963: failed to initialize, disabled!"); - _parent->modify_checked_reg(MPUREG_USER_CTRL, BIT_I2C_MST_EN, 0); - _parent->write_reg(MPUREG_I2C_MST_CTRL, 0); + _parent->modify_checked_reg(AK_MPU_OR_ICM(MPUREG_USER_CTRL, ICMREG_20948_USER_CTRL), BIT_I2C_MST_EN, 0); + _parent->write_reg(AK_MPU_OR_ICM(MPUREG_I2C_MST_CTRL, ICMREG_20948_I2C_MST_CTRL), 0); return -EIO; } - write_reg(AK8963REG_CNTL1, AK8963_CONTINUOUS_MODE2 | AK8963_16BIT_ADC); + if (_parent->_device_type == MPU_DEVICE_TYPE_MPU9250) { + write_reg(AK8963REG_CNTL1, AK8963_CONTINUOUS_MODE2 | AK8963_16BIT_ADC); + + } else { // ICM20948 -> AK09916 + write_reg(AK09916REG_CNTL2, AK09916_CNTL2_CONTINOUS_MODE_100HZ); + } if (_interface == NULL) { @@ -540,7 +598,13 @@ MPU9250_mag::ak8963_setup(void) /* Configure mpu' I2c Master interface to read ak8963 data * Into to fifo */ - set_passthrough(AK8963REG_ST1, sizeof(struct ak8963_regs)); + if (_parent->_device_type == MPU_DEVICE_TYPE_MPU9250) { + set_passthrough(AK8963REG_ST1, sizeof(struct ak8963_regs)); + + } else { // ICM20948 -> AK09916 + set_passthrough(AK09916REG_ST1, sizeof(struct ak09916_regs)); + } + } return OK; diff --git a/src/drivers/imu/mpu9250/mag.h b/src/drivers/imu/mpu9250/mag.h index d734fa2efe29..45f3e3d56237 100644 --- a/src/drivers/imu/mpu9250/mag.h +++ b/src/drivers/imu/mpu9250/mag.h @@ -66,6 +66,38 @@ #define AK8963_16BIT_ADC 0x10 #define AK8963_14BIT_ADC 0x00 #define AK8963_RESET 0x01 +#define AK8963_HOFL 0x08 + +/* ak09916 deviating register addresses and bit definitions */ + +#define AK09916_DEVICE_ID_A 0x48 // same as AK8963 +#define AK09916_DEVICE_ID_B 0x09 // additional ID byte ("INFO" on AK9063 without content specification.) + +#define AK09916REG_HXL 0x11 +#define AK09916REG_HXH 0x12 +#define AK09916REG_HYL 0x13 +#define AK09916REG_HYH 0x14 +#define AK09916REG_HZL 0x15 +#define AK09916REG_HZH 0x16 +#define AK09916REG_ST1 0x10 +#define AK09916REG_ST2 0x18 +#define AK09916REG_CNTL2 0x31 +#define AK09916REG_CNTL3 0x32 + + +#define AK09916_CNTL2_POWERDOWN_MODE 0x00 +#define AK09916_CNTL2_SINGLE_MODE 0x01 /* default */ +#define AK09916_CNTL2_CONTINOUS_MODE_10HZ 0x02 +#define AK09916_CNTL2_CONTINOUS_MODE_20HZ 0x04 +#define AK09916_CNTL2_CONTINOUS_MODE_50HZ 0x06 +#define AK09916_CNTL2_CONTINOUS_MODE_100HZ 0x08 +#define AK09916_CNTL2_SELFTEST_MODE 0x10 +#define AK09916_CNTL3_SRST 0x01 +#define AK09916_ST1_DRDY 0x01 +#define AK09916_ST1_DOR 0x02 + + +#define AK_MPU_OR_ICM(m,i) ((_parent->_device_type==MPU_DEVICE_TYPE_MPU9250) ? m : i) @@ -81,6 +113,18 @@ struct ak8963_regs { }; #pragma pack(pop) +#pragma pack(push, 1) +struct ak09916_regs { + uint8_t st1; + int16_t x; + int16_t y; + int16_t z; + uint8_t tmps; + uint8_t st2; +}; +#pragma pack(pop) + + extern device::Device *AK8963_I2C_interface(int bus, bool external_bus); typedef device::Device *(*MPU9250_mag_constructor)(int, bool); diff --git a/src/drivers/imu/mpu9250/mag_i2c.cpp b/src/drivers/imu/mpu9250/mag_i2c.cpp index 23a4ba8b74e0..91e74bf0a833 100644 --- a/src/drivers/imu/mpu9250/mag_i2c.cpp +++ b/src/drivers/imu/mpu9250/mag_i2c.cpp @@ -102,7 +102,7 @@ AK8963_I2C::probe() uint8_t whoami = 0; uint8_t expected = AK8963_DEVICE_ID; - if (read(AK8963REG_WIA, &whoami, 1)) { + if (PX4_OK != read(AK8963REG_WIA, &whoami, 1)) { return -EIO; } diff --git a/src/drivers/imu/mpu9250/main.cpp b/src/drivers/imu/mpu9250/main.cpp index 294082966180..3adfd367989e 100644 --- a/src/drivers/imu/mpu9250/main.cpp +++ b/src/drivers/imu/mpu9250/main.cpp @@ -91,6 +91,30 @@ #define MPU_DEVICE_PATH_GYRO_EXT2 "/dev/mpu9250_gyro_ext2" #define MPU_DEVICE_PATH_MAG_EXT2 "/dev/mpu9250_mag_ext2" +#define MPU_DEVICE_PATH_MPU6500_ACCEL "/dev/mpu6500_accel" +#define MPU_DEVICE_PATH_MPU6500_GYRO "/dev/mpu6500_gyro" +#define MPU_DEVICE_PATH_MPU6500_MAG "/dev/mpu6500_mag" + +#define MPU_DEVICE_PATH_MPU6500_ACCEL_1 "/dev/mpu6500_accel1" +#define MPU_DEVICE_PATH_MPU6500_GYRO_1 "/dev/mpu6500_gyro1" +#define MPU_DEVICE_PATH_MPU6500_MAG_1 "/dev/mpu6500_mag1" + +#define MPU_DEVICE_PATH_MPU6500_ACCEL_EXT "/dev/mpu6500_accel_ext" +#define MPU_DEVICE_PATH_MPU6500_GYRO_EXT "/dev/mpu6500_gyro_ext" +#define MPU_DEVICE_PATH_MPU6500_MAG_EXT "/dev/mpu6500_mag_ext" + +#define MPU_DEVICE_PATH_ICM_ACCEL_EXT "/dev/mpu9250_icm_accel_ext" +#define MPU_DEVICE_PATH_ICM_GYRO_EXT "/dev/mpu9250_icm_gyro_ext" +#define MPU_DEVICE_PATH_ICM_MAG_EXT "/dev/mpu9250_icm_mag_ext" + +#define MPU_DEVICE_PATH_ICM_ACCEL_EXT1 "/dev/mpu9250_icm_accel_ext1" +#define MPU_DEVICE_PATH_ICM_GYRO_EXT1 "/dev/mpu9250_icm_gyro_ext1" +#define MPU_DEVICE_PATH_ICM_MAG_EXT1 "/dev/mpu9250_icm_mag_ext1" + +#define MPU_DEVICE_PATH_ICM_ACCEL_EXT2 "/dev/mpu9250_icm_accel_ext2" +#define MPU_DEVICE_PATH_ICM_GYRO_EXT2 "/dev/mpu9250_icm_gyro_ext2" +#define MPU_DEVICE_PATH_ICM_MAG_EXT2 "/dev/mpu9250_icm_mag_ext2" + /** driver 'main' command */ extern "C" { __EXPORT int mpu9250_main(int argc, char *argv[]); } @@ -117,6 +141,7 @@ namespace mpu9250 struct mpu9250_bus_option { enum MPU9250_BUS busid; + MPU_DEVICE_TYPE device_type; const char *accelpath; const char *gyropath; const char *magpath; @@ -127,35 +152,43 @@ struct mpu9250_bus_option { MPU9250 *dev; } bus_options[] = { #if defined (USE_I2C) -# if defined(PX4_I2C_BUS_ONBOARD) - { 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 }, +# 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 }, # endif # if defined(PX4_I2C_BUS_EXPANSION) - { 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 }, +# 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 }, # endif -# if defined(PX4_I2C_BUS_EXPANSION1) + { 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 }, +#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 }, # endif -# if defined(PX4_I2C_BUS_EXPANSION2) +# if defined(PX4_I2C_BUS_EXPANSION2) && defined(PX4_I2C_OBDEV_MPU9250) { MPU9250_BUS_I2C_EXTERNAL, MPU_DEVICE_PATH_ACCEL_EXT2, MPU_DEVICE_PATH_GYRO_EXT2, MPU_DEVICE_PATH_MAG_EXT2, &MPU9250_I2C_interface, false, PX4_I2C_BUS_EXPANSION2, PX4_I2C_OBDEV_MPU9250, NULL }, # endif #endif #ifdef PX4_SPIDEV_MPU - { 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_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 }, #endif #ifdef PX4_SPIDEV_MPU2 - { 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_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 }, #endif #if defined(PX4_SPI_BUS_EXT) && defined(PX4_SPIDEV_EXT_MPU) - { 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_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 }, #endif }; #define NUM_BUS_OPTIONS (sizeof(bus_options)/sizeof(bus_options[0])) -void start(enum MPU9250_BUS busid, enum Rotation rotation, bool external_bus); -bool start_bus(struct mpu9250_bus_option &bus, enum Rotation rotation, bool external_bus); +void start(enum MPU9250_BUS busid, enum Rotation rotation, bool external_bus, bool magnetometer_only); +bool start_bus(struct mpu9250_bus_option &bus, enum Rotation rotation, bool external_bus, bool magnetometer_only); struct mpu9250_bus_option &find_bus(enum MPU9250_BUS busid); void stop(enum MPU9250_BUS busid); void test(enum MPU9250_BUS busid); @@ -184,16 +217,18 @@ struct mpu9250_bus_option &find_bus(enum MPU9250_BUS busid) * start driver for a specific bus option */ bool -start_bus(struct mpu9250_bus_option &bus, enum Rotation rotation, bool external) +start_bus(struct mpu9250_bus_option &bus, enum Rotation rotation, bool external, bool magnetometer_only) { int fd = -1; + PX4_INFO("Bus probed: %d", bus.busid); + if (bus.dev != nullptr) { warnx("%s SPI not available", external ? "External" : "Internal"); return false; } - device::Device *interface = bus.interface_constructor(bus.busnum, bus.address, external); + device::Device *interface = bus.interface_constructor(bus.busnum, bus.device_type, bus.address, external); if (interface == nullptr) { warnx("no device on bus %u", (unsigned)bus.busid); @@ -218,10 +253,16 @@ 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.dev = new MPU9250(interface, mag_interface, bus.accelpath, bus.gyropath, bus.magpath, rotation, bus.device_type, + magnetometer_only); if (bus.dev == nullptr) { delete interface; + + if (mag_interface != nullptr) { + delete mag_interface; + } + return false; } @@ -266,7 +307,7 @@ start_bus(struct mpu9250_bus_option &bus, enum Rotation rotation, bool external) * or failed to detect the sensor. */ void -start(enum MPU9250_BUS busid, enum Rotation rotation, bool external) +start(enum MPU9250_BUS busid, enum Rotation rotation, bool external, bool magnetometer_only) { bool started = false; @@ -282,7 +323,9 @@ start(enum MPU9250_BUS busid, enum Rotation rotation, bool external) continue; } - started |= start_bus(bus_options[i], rotation, external); + started |= start_bus(bus_options[i], rotation, external, magnetometer_only); + + if (started) { break; } } exit(started ? 0 : 1); @@ -480,6 +523,7 @@ usage() PX4_INFO(" -S (spi external bus)"); PX4_INFO(" -t (spi internal bus, 2nd instance)"); PX4_INFO(" -R rotation"); + PX4_INFO(" -M only enable magnetometer, accel/gyro disabled"); } @@ -494,8 +538,9 @@ mpu9250_main(int argc, char *argv[]) enum MPU9250_BUS busid = MPU9250_BUS_ALL; enum Rotation rotation = ROTATION_NONE; + bool magnetometer_only = false; - while ((ch = px4_getopt(argc, argv, "XISstR:", &myoptind, &myoptarg)) != EOF) { + while ((ch = px4_getopt(argc, argv, "XISstMR:", &myoptind, &myoptarg)) != EOF) { switch (ch) { case 'X': busid = MPU9250_BUS_I2C_EXTERNAL; @@ -521,6 +566,10 @@ mpu9250_main(int argc, char *argv[]) rotation = (enum Rotation)atoi(myoptarg); break; + case 'M': + magnetometer_only = true; + break; + default: mpu9250::usage(); return 0; @@ -539,7 +588,7 @@ mpu9250_main(int argc, char *argv[]) * Start/load the driver. */ if (!strcmp(verb, "start")) { - mpu9250::start(busid, rotation, external); + mpu9250::start(busid, rotation, external, magnetometer_only); } if (!strcmp(verb, "stop")) { diff --git a/src/drivers/imu/mpu9250/mpu9250.cpp b/src/drivers/imu/mpu9250/mpu9250.cpp index bd48a61b18c5..cd9ccd607d8d 100644 --- a/src/drivers/imu/mpu9250/mpu9250.cpp +++ b/src/drivers/imu/mpu9250/mpu9250.cpp @@ -96,28 +96,52 @@ list of registers that will be checked in check_registers(). Note that MPUREG_PRODUCT_ID must be first in the list. */ -const uint8_t MPU9250::_checked_registers[MPU9250_NUM_CHECKED_REGISTERS] = { MPUREG_WHOAMI, - MPUREG_PWR_MGMT_1, - MPUREG_PWR_MGMT_2, - MPUREG_USER_CTRL, - MPUREG_SMPLRT_DIV, - MPUREG_CONFIG, - MPUREG_GYRO_CONFIG, - MPUREG_ACCEL_CONFIG, - MPUREG_ACCEL_CONFIG2, - MPUREG_INT_ENABLE, - MPUREG_INT_PIN_CFG - }; +const uint16_t MPU9250::_mpu9250_checked_registers[MPU9250_NUM_CHECKED_REGISTERS] = { MPUREG_WHOAMI, + MPUREG_PWR_MGMT_1, + MPUREG_PWR_MGMT_2, + MPUREG_USER_CTRL, + MPUREG_SMPLRT_DIV, + MPUREG_CONFIG, + MPUREG_GYRO_CONFIG, + MPUREG_ACCEL_CONFIG, + MPUREG_ACCEL_CONFIG2, + MPUREG_INT_ENABLE, + MPUREG_INT_PIN_CFG + }; + + +const uint16_t MPU9250::_icm20948_checked_registers[ICM20948_NUM_CHECKED_REGISTERS] = { + ICMREG_20948_USER_CTRL, + ICMREG_20948_PWR_MGMT_1, + ICMREG_20948_PWR_MGMT_2, + ICMREG_20948_INT_PIN_CFG, + ICMREG_20948_INT_ENABLE, + ICMREG_20948_INT_ENABLE_1, + ICMREG_20948_INT_ENABLE_2, + ICMREG_20948_INT_ENABLE_3, + ICMREG_20948_GYRO_SMPLRT_DIV, + ICMREG_20948_GYRO_CONFIG_1, + ICMREG_20948_GYRO_CONFIG_2, + ICMREG_20948_ACCEL_SMPLRT_DIV_1, + ICMREG_20948_ACCEL_SMPLRT_DIV_2, + ICMREG_20948_ACCEL_CONFIG, + ICMREG_20948_ACCEL_CONFIG_2 +}; MPU9250::MPU9250(device::Device *interface, device::Device *mag_interface, const char *path_accel, const char *path_gyro, const char *path_mag, - enum Rotation rotation) : + enum Rotation rotation, + int device_type, + bool magnetometer_only) : CDev("MPU9250", path_accel), _interface(interface), _gyro(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) _work {}, _use_hrt(false), @@ -138,6 +162,8 @@ MPU9250::MPU9250(device::Device *interface, device::Device *mag_interface, const _gyro_range_scale(0.0f), _gyro_range_rad_s(0.0f), _dlpf_freq(MPU9250_DEFAULT_ONCHIP_FILTER_FREQ), + _dlpf_freq_icm_gyro(MPU9250_DEFAULT_ONCHIP_FILTER_FREQ), + _dlpf_freq_icm_accel(MPU9250_DEFAULT_ONCHIP_FILTER_FREQ), _sample_rate(1000), _accel_reads(perf_alloc(PC_COUNT, "mpu9250_acc_read")), _gyro_reads(perf_alloc(PC_COUNT, "mpu9250_gyro_read")), @@ -158,7 +184,9 @@ MPU9250::MPU9250(device::Device *interface, device::Device *mag_interface, const _accel_int(1000000 / MPU9250_ACCEL_MAX_OUTPUT_RATE), _gyro_int(1000000 / MPU9250_GYRO_MAX_OUTPUT_RATE, true), _rotation(rotation), + _checked_registers(nullptr), _checked_next(0), + _num_checked_registers(0), _last_temperature(0), _last_accel_data{}, _got_duplicate(false) @@ -253,9 +281,10 @@ MPU9250::~MPU9250() int MPU9250::init() { + irqstate_t state; #if defined(USE_I2C) - use_i2c(_interface->get_device_bus_type() == Device::DeviceBusType_I2C); + use_i2c(_interface->get_device_bus_type() == device::Device::DeviceBusType_I2C); #endif /* @@ -286,20 +315,26 @@ MPU9250::init() return ret; } - /* allocate basic report buffers */ - _accel_reports = new ringbuffer::RingBuffer(2, sizeof(sensor_accel_s)); - ret = -ENOMEM; + if (!_magnetometer_only) { + /* allocate basic report buffers */ + _accel_reports = new ringbuffer::RingBuffer(2, sizeof(sensor_accel_s)); + ret = -ENOMEM; - if (_accel_reports == nullptr) { - return ret; - } + if (_accel_reports == nullptr) { + return ret; + } - _gyro_reports = new ringbuffer::RingBuffer(2, sizeof(sensor_gyro_s)); + _gyro_reports = new ringbuffer::RingBuffer(2, sizeof(sensor_gyro_s)); - if (_gyro_reports == nullptr) { - return ret; + if (_gyro_reports == nullptr) { + return ret; + } } + state = px4_enter_critical_section(); + _reset_wait = hrt_absolute_time() + 100000; + px4_leave_critical_section(state); + if (reset_mpu() != OK) { PX4_ERR("Exiting! Device failed to take initialization"); return ret; @@ -354,63 +389,66 @@ MPU9250::init() return ret; } + /* Magnetometer setup */ + if (_device_type == MPU_DEVICE_TYPE_MPU9250 || _device_type == MPU_DEVICE_TYPE_ICM20948) { + #ifdef USE_I2C - if (!_mag->is_passthrough() && _mag->_interface->init() != PX4_OK) { - PX4_ERR("failed to setup ak8963 interface"); - } + up_udelay(100); + + if (!_mag->is_passthrough() && _mag->_interface->init() != PX4_OK) { + PX4_ERR("failed to setup ak8963 interface"); + } #endif /* USE_I2C */ - /* do CDev init for the mag device node, keep it optional */ - if (_whoami == MPU_WHOAMI_9250) { + /* do CDev init for the mag device node */ ret = _mag->init(); - } - /* if probe/setup failed, bail now */ - if (ret != OK) { - DEVICE_DEBUG("mag init failed"); - return ret; - } + /* if probe/setup failed, bail now */ + if (ret != OK) { + DEVICE_DEBUG("mag init failed"); + return ret; + } - if (_whoami == MPU_WHOAMI_9250) { ret = _mag->ak8963_reset(); - } - if (ret != OK) { - DEVICE_DEBUG("mag reset failed"); - return ret; + if (ret != OK) { + DEVICE_DEBUG("mag reset failed"); + return ret; + } } - _accel_class_instance = register_class_devname(ACCEL_BASE_DEVICE_PATH); measure(); - /* advertise sensor topic, measure manually to initialize valid report */ - sensor_accel_s arp; - _accel_reports->get(&arp); + if (!_magnetometer_only) { + /* advertise sensor topic, measure manually to initialize valid report */ + sensor_accel_s arp; + _accel_reports->get(&arp); - /* measurement will have generated a report, publish */ - _accel_topic = orb_advertise_multi(ORB_ID(sensor_accel), &arp, - &_accel_orb_class_instance, (is_external()) ? ORB_PRIO_MAX - 1 : ORB_PRIO_HIGH - 1); + /* measurement will have generated a report, publish */ + _accel_topic = orb_advertise_multi(ORB_ID(sensor_accel), &arp, + &_accel_orb_class_instance, (is_external()) ? ORB_PRIO_MAX - 1 : ORB_PRIO_HIGH - 1); - if (_accel_topic == nullptr) { - PX4_ERR("ADVERT FAIL"); - return ret; - } + if (_accel_topic == nullptr) { + PX4_ERR("ADVERT FAIL"); + return ret; + } - /* advertise sensor topic, measure manually to initialize valid report */ - sensor_gyro_s grp; - _gyro_reports->get(&grp); + /* advertise sensor topic, measure manually to initialize valid report */ + sensor_gyro_s grp; + _gyro_reports->get(&grp); - _gyro->_gyro_topic = orb_advertise_multi(ORB_ID(sensor_gyro), &grp, - &_gyro->_gyro_orb_class_instance, (is_external()) ? ORB_PRIO_MAX - 1 : ORB_PRIO_HIGH - 1); + _gyro->_gyro_topic = orb_advertise_multi(ORB_ID(sensor_gyro), &grp, + &_gyro->_gyro_orb_class_instance, (is_external()) ? ORB_PRIO_MAX - 1 : ORB_PRIO_HIGH - 1); - if (_gyro->_gyro_topic == nullptr) { - PX4_ERR("ADVERT FAIL"); - return ret; + if (_gyro->_gyro_topic == nullptr) { + PX4_ERR("ADVERT FAIL"); + return ret; + } } return ret; @@ -438,7 +476,7 @@ int MPU9250::reset() ret = reset_mpu(); - if (ret == OK && _whoami == MPU_WHOAMI_9250) { + if (ret == OK && (_device_type == MPU_DEVICE_TYPE_MPU9250 || _device_type == MPU_DEVICE_TYPE_ICM20948)) { ret = _mag->ak8963_reset(); } @@ -452,16 +490,32 @@ int MPU9250::reset() int MPU9250::reset_mpu() { - 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); + uint8_t retries; + + switch (_device_type) { + case MPU_DEVICE_TYPE_MPU9250: + case MPU_DEVICE_TYPE_MPU6500: + 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: + write_reg(ICMREG_20948_PWR_MGMT_1, BIT_H_RESET); + usleep(1000); + + write_checked_reg(ICMREG_20948_PWR_MGMT_1, MPU_CLK_SEL_AUTO); + usleep(200); + write_checked_reg(ICMREG_20948_PWR_MGMT_2, 0); + break; + } + // Enable I2C bus or Disable I2C bus (recommended on data sheet) - usleep(1000); - // Enable I2C bus or Disable I2C bus (recommended on data sheet) + write_checked_reg(MPU_OR_ICM(MPUREG_USER_CTRL, ICMREG_20948_USER_CTRL), is_i2c() ? 0 : BIT_I2C_IF_DIS); - write_checked_reg(MPUREG_USER_CTRL, is_i2c() ? 0 : BIT_I2C_IF_DIS); // SAMPLE RATE _set_sample_rate(_sample_rate); @@ -469,7 +523,17 @@ int MPU9250::reset_mpu() _set_dlpf_filter(MPU9250_DEFAULT_ONCHIP_FILTER_FREQ); // Gyro scale 2000 deg/s () - write_checked_reg(MPUREG_GYRO_CONFIG, BITS_FS_2000DPS); + switch (_device_type) { + case MPU_DEVICE_TYPE_MPU9250: + case MPU_DEVICE_TYPE_MPU6500: + write_checked_reg(MPUREG_GYRO_CONFIG, BITS_FS_2000DPS); + break; + + case MPU_DEVICE_TYPE_ICM20948: + modify_checked_reg(ICMREG_20948_GYRO_CONFIG_1, ICM_BITS_GYRO_FS_SEL_MASK, ICM_BITS_GYRO_FS_SEL_2000DPS); + break; + } + // correct gyro scale factors // scale to rad/s in SI units @@ -482,7 +546,8 @@ int MPU9250::reset_mpu() set_accel_range(ACCEL_RANGE_G); // INT CFG => Interrupt on Data Ready - write_checked_reg(MPUREG_INT_ENABLE, BIT_RAW_RDY_EN); // INT: Raw data ready + write_checked_reg(MPU_OR_ICM(MPUREG_INT_ENABLE, ICMREG_20948_INT_ENABLE_1), + BIT_RAW_RDY_EN); // INT: Raw data ready #ifdef USE_I2C bool bypass = !_mag->is_passthrough(); @@ -499,11 +564,13 @@ int MPU9250::reset_mpu() * so bypass is true if the mag has an i2c non null interfaces. */ - write_checked_reg(MPUREG_INT_PIN_CFG, BIT_INT_ANYRD_2CLEAR | (bypass ? BIT_INT_BYPASS_EN : 0)); + write_checked_reg(MPU_OR_ICM(MPUREG_INT_PIN_CFG, ICMREG_20948_INT_PIN_CFG), + BIT_INT_ANYRD_2CLEAR | (bypass ? BIT_INT_BYPASS_EN : 0)); - write_checked_reg(MPUREG_ACCEL_CONFIG2, BITS_ACCEL_CONFIG2_41HZ); + write_checked_reg(MPU_OR_ICM(MPUREG_ACCEL_CONFIG2, ICMREG_20948_ACCEL_CONFIG_2), + MPU_OR_ICM(BITS_ACCEL_CONFIG2_41HZ, ICM_BITS_DEC3_CFG_32)); - uint8_t retries = 3; + retries = 3; bool all_ok = false; while (!all_ok && retries--) { @@ -511,11 +578,17 @@ int MPU9250::reset_mpu() // Assume all checked values are as expected all_ok = true; uint8_t reg; + uint8_t bankcheck = 0; - for (uint8_t i = 0; i < MPU9250_NUM_CHECKED_REGISTERS; i++) { + 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) { + _interface->read(MPU9250_LOW_SPEED_OP(ICMREG_20948_BANK_SEL), &bankcheck, 1); + } + write_reg(_checked_registers[i], _checked_values[i]); - PX4_ERR("Reg %d is:%d s/b:%d Tries:%d", _checked_registers[i], reg, _checked_values[i], retries); + PX4_ERR("Reg %d is:%d s/b:%d Tries:%d - bank s/b %d, is %d", _checked_registers[i], reg, _checked_values[i], retries, + REG_BANK(_checked_registers[i]), bankcheck); all_ok = false; } } @@ -527,42 +600,105 @@ int MPU9250::reset_mpu() int MPU9250::probe() { + int ret = -EIO; + /* look for device ID */ - _whoami = read_reg(MPUREG_WHOAMI); - - // verify product revision - switch (_whoami) { - case MPU_WHOAMI_9250: - case MPU_WHOAMI_6500: - memset(_checked_values, 0, sizeof(_checked_values)); - memset(_checked_bad, 0, sizeof(_checked_bad)); - _checked_values[0] = _whoami; - _checked_bad[0] = _whoami; - return OK; + switch (_device_type) { + + 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: + _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; + } + + break; } - DEVICE_DEBUG("unexpected whoami 0x%02x", _whoami); - return -EIO; + _checked_values[0] = _whoami; + _checked_bad[0] = _whoami; + + if (ret != OK) { + DEVICE_DEBUG("unexpected whoami 0x%02x", _whoami); + } + + return ret; } /* set sample rate (approximate) - 1kHz to 5Hz, for both accel and gyro + For ICM20948 accel and gyro samplerates are both set to the same value. */ void MPU9250::_set_sample_rate(unsigned desired_sample_rate_hz) { + uint8_t div = 1; + if (desired_sample_rate_hz == 0) { desired_sample_rate_hz = MPU9250_GYRO_DEFAULT_RATE; } - uint8_t div = 1000 / desired_sample_rate_hz; + switch (_device_type) { + case MPU_DEVICE_TYPE_MPU9250: + case MPU_DEVICE_TYPE_MPU6500: + div = 1000 / desired_sample_rate_hz; + break; + + case MPU_DEVICE_TYPE_ICM20948: + div = 1100 / desired_sample_rate_hz; + break; + } if (div > 200) { div = 200; } if (div < 1) { div = 1; } - write_checked_reg(MPUREG_SMPLRT_DIV, div - 1); - _sample_rate = 1000 / div; + + switch (_device_type) { + case MPU_DEVICE_TYPE_MPU9250: + case MPU_DEVICE_TYPE_MPU6500: + write_checked_reg(MPUREG_SMPLRT_DIV, div - 1); + _sample_rate = 1000 / div; + break; + + case MPU_DEVICE_TYPE_ICM20948: + 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. + write_checked_reg(ICMREG_20948_ACCEL_SMPLRT_DIV_2, div - 1); + _sample_rate = 1100 / div; + break; + } } /* @@ -573,47 +709,137 @@ MPU9250::_set_dlpf_filter(uint16_t frequency_hz) { uint8_t filter; - /* - choose next highest filter frequency available - */ - if (frequency_hz == 0) { - _dlpf_freq = 0; - filter = BITS_DLPF_CFG_3600HZ; - } else if (frequency_hz <= 5) { - _dlpf_freq = 5; - filter = BITS_DLPF_CFG_5HZ; + switch (_device_type) { + case MPU_DEVICE_TYPE_MPU9250: + case MPU_DEVICE_TYPE_MPU6500: - } else if (frequency_hz <= 10) { - _dlpf_freq = 10; - filter = BITS_DLPF_CFG_10HZ; + /* + choose next highest filter frequency available + */ + if (frequency_hz == 0) { + _dlpf_freq = 0; + filter = BITS_DLPF_CFG_3600HZ; - } else if (frequency_hz <= 20) { - _dlpf_freq = 20; - filter = BITS_DLPF_CFG_20HZ; + } else if (frequency_hz <= 5) { + _dlpf_freq = 5; + filter = BITS_DLPF_CFG_5HZ; - } else if (frequency_hz <= 41) { - _dlpf_freq = 41; - filter = BITS_DLPF_CFG_41HZ; + } else if (frequency_hz <= 10) { + _dlpf_freq = 10; + filter = BITS_DLPF_CFG_10HZ; - } else if (frequency_hz <= 92) { - _dlpf_freq = 92; - filter = BITS_DLPF_CFG_92HZ; + } else if (frequency_hz <= 20) { + _dlpf_freq = 20; + filter = BITS_DLPF_CFG_20HZ; - } else if (frequency_hz <= 184) { - _dlpf_freq = 184; - filter = BITS_DLPF_CFG_184HZ; + } else if (frequency_hz <= 41) { + _dlpf_freq = 41; + filter = BITS_DLPF_CFG_41HZ; - } else if (frequency_hz <= 250) { - _dlpf_freq = 250; - filter = BITS_DLPF_CFG_250HZ; + } else if (frequency_hz <= 92) { + _dlpf_freq = 92; + filter = BITS_DLPF_CFG_92HZ; - } else { - _dlpf_freq = 0; - filter = BITS_DLPF_CFG_3600HZ; - } + } else if (frequency_hz <= 184) { + _dlpf_freq = 184; + filter = BITS_DLPF_CFG_184HZ; + + } else if (frequency_hz <= 250) { + _dlpf_freq = 250; + filter = BITS_DLPF_CFG_250HZ; + + } else { + _dlpf_freq = 0; + filter = BITS_DLPF_CFG_3600HZ; + } + + write_checked_reg(MPUREG_CONFIG, filter); + break; + + case MPU_DEVICE_TYPE_ICM20948: + + /* + choose next highest filter frequency available for gyroscope + */ + if (frequency_hz == 0) { + _dlpf_freq_icm_gyro = 0; + filter = ICM_BITS_GYRO_DLPF_CFG_361HZ; + + } else if (frequency_hz <= 5) { + _dlpf_freq_icm_gyro = 5; + filter = ICM_BITS_GYRO_DLPF_CFG_5HZ; + + } else if (frequency_hz <= 11) { + _dlpf_freq_icm_gyro = 11; + filter = ICM_BITS_GYRO_DLPF_CFG_11HZ; + + } else if (frequency_hz <= 23) { + _dlpf_freq_icm_gyro = 23; + filter = ICM_BITS_GYRO_DLPF_CFG_23HZ; + + } else if (frequency_hz <= 51) { + _dlpf_freq_icm_gyro = 51; + filter = ICM_BITS_GYRO_DLPF_CFG_51HZ; + + } else if (frequency_hz <= 119) { + _dlpf_freq_icm_gyro = 119; + filter = ICM_BITS_GYRO_DLPF_CFG_119HZ; - write_checked_reg(MPUREG_CONFIG, filter); + } else if (frequency_hz <= 151) { + _dlpf_freq_icm_gyro = 151; + filter = ICM_BITS_GYRO_DLPF_CFG_151HZ; + + } else if (frequency_hz <= 197) { + _dlpf_freq_icm_gyro = 197; + filter = ICM_BITS_GYRO_DLPF_CFG_197HZ; + + } else { + _dlpf_freq_icm_gyro = 0; + filter = ICM_BITS_GYRO_DLPF_CFG_361HZ; + } + + write_checked_reg(ICMREG_20948_GYRO_CONFIG_1, filter); + + /* + choose next highest filter frequency available for accelerometer + */ + if (frequency_hz == 0) { + _dlpf_freq_icm_accel = 0; + filter = ICM_BITS_ACCEL_DLPF_CFG_473HZ; + + } else if (frequency_hz <= 5) { + _dlpf_freq_icm_accel = 5; + filter = ICM_BITS_ACCEL_DLPF_CFG_5HZ; + + } else if (frequency_hz <= 11) { + _dlpf_freq_icm_accel = 11; + filter = ICM_BITS_ACCEL_DLPF_CFG_11HZ; + + } else if (frequency_hz <= 23) { + _dlpf_freq_icm_accel = 23; + filter = ICM_BITS_ACCEL_DLPF_CFG_23HZ; + + } else if (frequency_hz <= 50) { + _dlpf_freq_icm_accel = 50; + filter = ICM_BITS_ACCEL_DLPF_CFG_50HZ; + + } else if (frequency_hz <= 111) { + _dlpf_freq_icm_accel = 111; + filter = ICM_BITS_ACCEL_DLPF_CFG_111HZ; + + } else if (frequency_hz <= 246) { + _dlpf_freq_icm_accel = 246; + filter = ICM_BITS_ACCEL_DLPF_CFG_246HZ; + + } else { + _dlpf_freq_icm_accel = 0; + filter = ICM_BITS_ACCEL_DLPF_CFG_473HZ; + } + + write_checked_reg(ICMREG_20948_ACCEL_CONFIG, filter); + break; + } } ssize_t @@ -831,14 +1057,87 @@ MPU9250::gyro_ioctl(struct file *filp, int cmd, unsigned long arg) } } +int +MPU9250::select_register_bank(uint8_t bank) +{ + uint8_t ret; + uint8_t buf; + uint8_t retries = 3; + + if (_selected_bank != bank) { + ret = _interface->write(MPU9250_LOW_SPEED_OP(ICMREG_20948_BANK_SEL), &bank, 1); + + if (ret != OK) { + return ret; + } + } + + /* + * Making sure the right register bank is selected (even if it should be). Observed some + * unexpected changes to this, don't risk writing to the wrong register bank. + */ + _interface->read(MPU9250_LOW_SPEED_OP(ICMREG_20948_BANK_SEL), &buf, 1); + + while (bank != buf && retries > 0) { + //PX4_WARN("user bank: expected %d got %d",bank,buf); + ret = _interface->write(MPU9250_LOW_SPEED_OP(ICMREG_20948_BANK_SEL), &bank, 1); + + if (ret != OK) { + return ret; + } + + retries--; + //PX4_WARN("BANK retries: %d", 4-retries); + + _interface->read(MPU9250_LOW_SPEED_OP(ICMREG_20948_BANK_SEL), &buf, 1); + } + + + _selected_bank = bank; + + if (bank != buf) { + DEVICE_DEBUG("SELECT FAILED %d %d %d %d", retries, _selected_bank, bank, buf); + return PX4_ERROR; + + } else { + return PX4_OK; + } +} + uint8_t MPU9250::read_reg(unsigned reg, uint32_t speed) { uint8_t buf; - _interface->read(MPU9250_SET_SPEED(reg, speed), &buf, 1); + + if (_device_type == MPU_DEVICE_TYPE_ICM20948) { + select_register_bank(REG_BANK(reg)); + _interface->read(MPU9250_SET_SPEED(REG_ADDRESS(reg), speed), &buf, 1); + + } else { + _interface->read(MPU9250_SET_SPEED(REG_ADDRESS(reg), speed), &buf, 1); + } + return buf; } +uint8_t +MPU9250::read_reg_range(unsigned start_reg, uint32_t speed, uint8_t *buf, uint16_t count) +{ + uint8_t ret; + + if (buf == NULL) { return PX4_ERROR; } + + if (_device_type == MPU_DEVICE_TYPE_ICM20948) { + select_register_bank(REG_BANK(start_reg)); + ret = _interface->read(MPU9250_SET_SPEED(REG_ADDRESS(start_reg), speed), buf, count); + + } else { + ret = _interface->read(MPU9250_SET_SPEED(REG_ADDRESS(start_reg), speed), buf, count); + } + + return ret; +} + uint16_t MPU9250::read_reg16(unsigned reg) { @@ -846,7 +1145,14 @@ MPU9250::read_reg16(unsigned reg) // general register transfer at low clock speed - _interface->read(MPU9250_LOW_SPEED_OP(reg), &buf, arraySize(buf)); + if (_device_type == MPU_DEVICE_TYPE_ICM20948) { + select_register_bank(REG_BANK(reg)); + _interface->read(MPU9250_LOW_SPEED_OP(REG_ADDRESS(reg)), &buf, arraySize(buf)); + + } else { + _interface->read(MPU9250_LOW_SPEED_OP(reg), &buf, arraySize(buf)); + } + return (uint16_t)(buf[0] << 8) | buf[1]; } @@ -855,7 +1161,13 @@ MPU9250::write_reg(unsigned reg, uint8_t value) { // general register transfer at low clock speed - _interface->write(MPU9250_LOW_SPEED_OP(reg), &value, 1); + if (_device_type == MPU_DEVICE_TYPE_ICM20948) { + select_register_bank(REG_BANK(reg)); + _interface->write(MPU9250_LOW_SPEED_OP(REG_ADDRESS(reg)), &value, 1); + + } else { + _interface->write(MPU9250_LOW_SPEED_OP(reg), &value, 1); + } } void @@ -885,7 +1197,7 @@ MPU9250::write_checked_reg(unsigned reg, uint8_t value) { write_reg(reg, value); - for (uint8_t i = 0; i < MPU9250_NUM_CHECKED_REGISTERS; i++) { + for (uint8_t i = 0; i < _num_checked_registers; i++) { if (reg == _checked_registers[i]) { _checked_values[i] = value; _checked_bad[i] = value; @@ -922,7 +1234,17 @@ MPU9250::set_accel_range(unsigned max_g_in) max_accel_g = 2; } - write_checked_reg(MPUREG_ACCEL_CONFIG, afs_sel << 3); + switch (_device_type) { + case MPU_DEVICE_TYPE_MPU9250: + case MPU_DEVICE_TYPE_MPU6500: + write_checked_reg(MPUREG_ACCEL_CONFIG, afs_sel << 3); + break; + + case MPU_DEVICE_TYPE_ICM20948: + modify_checked_reg(ICMREG_20948_ACCEL_CONFIG, ICM_BITS_ACCEL_FS_SEL_MASK, afs_sel << 1); + break; + } + _accel_range_scale = (CONSTANTS_ONE_G / lsb_per_g); _accel_range_m_s2 = max_accel_g * CONSTANTS_ONE_G; @@ -1048,8 +1370,14 @@ MPU9250::check_registers(void) if (_register_wait == 0 || _checked_next == 0) { // if the product_id is wrong then reset the // sensor completely - write_reg(MPUREG_PWR_MGMT_1, BIT_H_RESET); - write_reg(MPUREG_PWR_MGMT_2, MPU_CLK_SEL_AUTO); + + if (_device_type == MPU_DEVICE_TYPE_ICM20948) { + // reset_mpu(); + } else { + write_reg(MPUREG_PWR_MGMT_1, BIT_H_RESET); + write_reg(MPUREG_PWR_MGMT_2, MPU_CLK_SEL_AUTO); + } + // after doing a reset we need to wait a long // time before we do any other register writes // or we will end up with the mpu9250 in a @@ -1070,10 +1398,10 @@ MPU9250::check_registers(void) _register_wait = 20; } - _checked_next = (_checked_next + 1) % MPU9250_NUM_CHECKED_REGISTERS; + _checked_next = (_checked_next + 1) % _num_checked_registers; } -bool MPU9250::check_null_data(uint32_t *data, uint8_t size) +bool MPU9250::check_null_data(uint16_t *data, uint8_t size) { while (size--) { if (*data++) { @@ -1126,6 +1454,8 @@ MPU9250::measure() struct MPUReport mpu_report; + struct ICMReport icm_report; + struct Report { int16_t accel_x; int16_t accel_y; @@ -1140,197 +1470,255 @@ MPU9250::measure() perf_begin(_sample_perf); /* - * Fetch the full set of measurements from the MPU9250 in one pass. + * Fetch the full set of measurements from the MPU9250 in one pass */ - if (OK != _interface->read(MPU9250_SET_SPEED(MPUREG_INT_STATUS, MPU9250_HIGH_BUS_SPEED), - (uint8_t *)&mpu_report, - sizeof(mpu_report))) { - perf_end(_sample_perf); - return; - } - check_registers(); + if ((!_magnetometer_only || _mag->is_passthrough()) && _register_wait == 0) { + if (_device_type == MPU_DEVICE_TYPE_MPU9250 || _device_type == MPU_DEVICE_TYPE_MPU6500) { + 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 (check_duplicate(&mpu_report.accel_x[0])) { - return; + } else { // ICM20948 + select_register_bank(REG_BANK(ICMREG_20948_ACCEL_XOUT_H)); + + if (OK != read_reg_range(ICMREG_20948_ACCEL_XOUT_H, MPU9250_HIGH_BUS_SPEED, (uint8_t *)&icm_report, + sizeof(icm_report))) { + perf_end(_sample_perf); + return; + } + } + + check_registers(); + + if (check_duplicate(MPU_OR_ICM(&mpu_report.accel_x[0], &icm_report.accel_x[0]))) { + return; + } } -#ifdef USE_I2C + /* + * In case of a mag passthrough read, hand the magnetometer data over to _mag. Else, + * try to read a magnetometer report. + */ + +# ifdef USE_I2C if (_mag->is_passthrough()) { -#endif +# endif + _mag->_measure(mpu_report.mag); -#ifdef USE_I2C + +# ifdef USE_I2C } else { _mag->measure(); } -#endif +# endif /* - * Convert from big to little endian + * Continue evaluating gyro and accelerometer results */ - 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((uint32_t *)&report, sizeof(report) / 4)) { - return; + if (!_magnetometer_only && _register_wait == 0) { + + /* + * Convert from big to little endian + */ + if (_device_type == MPU_DEVICE_TYPE_ICM20948) { + 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); + report.temp = int16_t_from_bytes(icm_report.temp); + report.gyro_x = int16_t_from_bytes(icm_report.gyro_x); + report.gyro_y = int16_t_from_bytes(icm_report.gyro_y); + report.gyro_z = int16_t_from_bytes(icm_report.gyro_z); + + } else { // MPU9250/MPU6500 + 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. + * We still increment _good_transfers, but don't return any data yet. + * + */ _register_wait--; return; } /* - * Swap axes and negate y + * Get sensor temperature */ - int16_t accel_xt = report.accel_y; - int16_t accel_yt = ((report.accel_x == -32768) ? 32767 : -report.accel_x); + _last_temperature = (report.temp) / 333.87f + 21.0f; - int16_t gyro_xt = report.gyro_y; - int16_t gyro_yt = ((report.gyro_x == -32768) ? 32767 : -report.gyro_x); /* - * Apply the swap + * Convert and publish accelerometer and gyrometer data. */ - report.accel_x = accel_xt; - report.accel_y = accel_yt; - report.gyro_x = gyro_xt; - report.gyro_y = gyro_yt; - /* - * Report buffers. - */ - sensor_accel_s arb{}; - sensor_gyro_s grb{}; + if (!_magnetometer_only) { - /* - * Adjust and scale results to m/s^2. - */ - grb.timestamp = arb.timestamp = hrt_absolute_time(); + /* + * Keeping the axes as they are for ICM20948 so orientation will match the actual chip orientation + */ + if (_device_type != MPU_DEVICE_TYPE_ICM20948) { + /* + * 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 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; + } - // 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 - grb.error_count = arb.error_count = perf_event_count(_bad_transfers) + perf_event_count(_bad_registers); + /* + * Report buffers. + */ + sensor_accel_s arb{}; + sensor_gyro_s grb{}; - /* - * 1) Scale raw value to SI units using scaling from datasheet. - * 2) Subtract static offset (in SI units) - * 3) Scale the statically calibrated values with a linear - * dynamically obtained factor - * - * Note: the static sensor offset is the number the sensor outputs - * at a nominally 'zero' input. Therefore the offset has to - * be subtracted. - * - * Example: A gyro outputs a value of 74 at zero angular rate - * the offset is 74 from the origin and subtracting - * 74 from all measurements centers them around zero. - */ + /* + * Adjust and scale results to m/s^2. + */ + grb.timestamp = arb.timestamp = hrt_absolute_time(); - /* NOTE: Axes have been swapped to match the board a few lines above. */ + // 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 + grb.error_count = arb.error_count = perf_event_count(_bad_transfers) + perf_event_count(_bad_registers); - arb.x_raw = report.accel_x; - arb.y_raw = report.accel_y; - arb.z_raw = report.accel_z; + /* + * 1) Scale raw value to SI units using scaling from datasheet. + * 2) Subtract static offset (in SI units) + * 3) Scale the statically calibrated values with a linear + * dynamically obtained factor + * + * Note: the static sensor offset is the number the sensor outputs + * at a nominally 'zero' input. Therefore the offset has to + * be subtracted. + * + * Example: A gyro outputs a value of 74 at zero angular rate + * the offset is 74 from the origin and subtracting + * 74 from all measurements centers them around zero. + */ - float xraw_f = report.accel_x; - float yraw_f = report.accel_y; - float zraw_f = report.accel_z; + /* NOTE: Axes have been swapped to match the board a few lines above. */ - // apply user specified rotation - rotate_3f(_rotation, xraw_f, yraw_f, zraw_f); + arb.x_raw = report.accel_x; + arb.y_raw = report.accel_y; + arb.z_raw = report.accel_z; - float x_in_new = ((xraw_f * _accel_range_scale) - _accel_scale.x_offset) * _accel_scale.x_scale; - float y_in_new = ((yraw_f * _accel_range_scale) - _accel_scale.y_offset) * _accel_scale.y_scale; - float z_in_new = ((zraw_f * _accel_range_scale) - _accel_scale.z_offset) * _accel_scale.z_scale; + float xraw_f = report.accel_x; + float yraw_f = report.accel_y; + float zraw_f = report.accel_z; - arb.x = _accel_filter_x.apply(x_in_new); - arb.y = _accel_filter_y.apply(y_in_new); - arb.z = _accel_filter_z.apply(z_in_new); + // apply user specified rotation + rotate_3f(_rotation, xraw_f, yraw_f, zraw_f); - matrix::Vector3f aval(x_in_new, y_in_new, z_in_new); - matrix::Vector3f aval_integrated; + float x_in_new = ((xraw_f * _accel_range_scale) - _accel_scale.x_offset) * _accel_scale.x_scale; + float y_in_new = ((yraw_f * _accel_range_scale) - _accel_scale.y_offset) * _accel_scale.y_scale; + float z_in_new = ((zraw_f * _accel_range_scale) - _accel_scale.z_offset) * _accel_scale.z_scale; - bool accel_notify = _accel_int.put(arb.timestamp, aval, aval_integrated, arb.integral_dt); - arb.x_integral = aval_integrated(0); - arb.y_integral = aval_integrated(1); - arb.z_integral = aval_integrated(2); + arb.x = _accel_filter_x.apply(x_in_new); + arb.y = _accel_filter_y.apply(y_in_new); + arb.z = _accel_filter_z.apply(z_in_new); - arb.scaling = _accel_range_scale; + matrix::Vector3f aval(x_in_new, y_in_new, z_in_new); + matrix::Vector3f aval_integrated; - _last_temperature = (report.temp) / 361.0f + 35.0f; + bool accel_notify = _accel_int.put(arb.timestamp, aval, aval_integrated, arb.integral_dt); + arb.x_integral = aval_integrated(0); + arb.y_integral = aval_integrated(1); + arb.z_integral = aval_integrated(2); - arb.temperature = _last_temperature; + arb.scaling = _accel_range_scale; - /* return device ID */ - arb.device_id = _device_id.devid; - grb.x_raw = report.gyro_x; - grb.y_raw = report.gyro_y; - grb.z_raw = report.gyro_z; - xraw_f = report.gyro_x; - yraw_f = report.gyro_y; - zraw_f = report.gyro_z; + arb.temperature = _last_temperature; - // apply user specified rotation - rotate_3f(_rotation, xraw_f, yraw_f, zraw_f); + /* return device ID */ + arb.device_id = _device_id.devid; - float x_gyro_in_new = ((xraw_f * _gyro_range_scale) - _gyro_scale.x_offset) * _gyro_scale.x_scale; - float y_gyro_in_new = ((yraw_f * _gyro_range_scale) - _gyro_scale.y_offset) * _gyro_scale.y_scale; - float z_gyro_in_new = ((zraw_f * _gyro_range_scale) - _gyro_scale.z_offset) * _gyro_scale.z_scale; + grb.x_raw = report.gyro_x; + grb.y_raw = report.gyro_y; + grb.z_raw = report.gyro_z; - grb.x = _gyro_filter_x.apply(x_gyro_in_new); - grb.y = _gyro_filter_y.apply(y_gyro_in_new); - grb.z = _gyro_filter_z.apply(z_gyro_in_new); + xraw_f = report.gyro_x; + yraw_f = report.gyro_y; + zraw_f = report.gyro_z; - matrix::Vector3f gval(x_gyro_in_new, y_gyro_in_new, z_gyro_in_new); - matrix::Vector3f gval_integrated; + // apply user specified rotation + rotate_3f(_rotation, xraw_f, yraw_f, zraw_f); - bool gyro_notify = _gyro_int.put(arb.timestamp, gval, gval_integrated, grb.integral_dt); - grb.x_integral = gval_integrated(0); - grb.y_integral = gval_integrated(1); - grb.z_integral = gval_integrated(2); + float x_gyro_in_new = ((xraw_f * _gyro_range_scale) - _gyro_scale.x_offset) * _gyro_scale.x_scale; + float y_gyro_in_new = ((yraw_f * _gyro_range_scale) - _gyro_scale.y_offset) * _gyro_scale.y_scale; + float z_gyro_in_new = ((zraw_f * _gyro_range_scale) - _gyro_scale.z_offset) * _gyro_scale.z_scale; - grb.scaling = _gyro_range_scale; + grb.x = _gyro_filter_x.apply(x_gyro_in_new); + grb.y = _gyro_filter_y.apply(y_gyro_in_new); + grb.z = _gyro_filter_z.apply(z_gyro_in_new); - grb.temperature = _last_temperature; + matrix::Vector3f gval(x_gyro_in_new, y_gyro_in_new, z_gyro_in_new); + matrix::Vector3f gval_integrated; - /* return device ID */ - grb.device_id = _gyro->_device_id.devid; + bool gyro_notify = _gyro_int.put(arb.timestamp, gval, gval_integrated, grb.integral_dt); + grb.x_integral = gval_integrated(0); + grb.y_integral = gval_integrated(1); + grb.z_integral = gval_integrated(2); - _accel_reports->force(&arb); - _gyro_reports->force(&grb); + grb.scaling = _gyro_range_scale; - /* notify anyone waiting for data */ - if (accel_notify) { - poll_notify(POLLIN); - } + grb.temperature = _last_temperature; - if (gyro_notify) { - _gyro->parent_poll_notify(); - } + /* return device ID */ + grb.device_id = _gyro->_device_id.devid; - if (accel_notify && !(_pub_blocked)) { - /* publish it */ - orb_publish(ORB_ID(sensor_accel), _accel_topic, &arb); - } + _accel_reports->force(&arb); + _gyro_reports->force(&grb); + + /* notify anyone waiting for data */ + if (accel_notify) { + poll_notify(POLLIN); + } + + if (gyro_notify) { + _gyro->parent_poll_notify(); + } + + if (accel_notify && !(_pub_blocked)) { + /* publish it */ + orb_publish(ORB_ID(sensor_accel), _accel_topic, &arb); + } - if (gyro_notify && !(_pub_blocked)) { - /* publish it */ - orb_publish(ORB_ID(sensor_gyro), _gyro->_gyro_topic, &grb); + if (gyro_notify && !(_pub_blocked)) { + /* publish it */ + orb_publish(ORB_ID(sensor_gyro), _gyro->_gyro_topic, &grb); + } } /* stop measuring */ @@ -1340,6 +1728,7 @@ MPU9250::measure() void MPU9250::print_info() { + ::printf("Device type:%d\n", _device_type); perf_print_counter(_sample_perf); perf_print_counter(_accel_reads); perf_print_counter(_gyro_reads); @@ -1353,23 +1742,11 @@ MPU9250::print_info() _mag->_mag_reports->print_info("mag queue"); ::printf("checked_next: %u\n", _checked_next); - for (uint8_t i = 0; i < MPU9250_NUM_CHECKED_REGISTERS; i++) { - uint8_t v = read_reg(_checked_registers[i], MPU9250_HIGH_BUS_SPEED); - - if (v != _checked_values[i]) { - ::printf("reg %02x:%02x should be %02x\n", - (unsigned)_checked_registers[i], - (unsigned)v, - (unsigned)_checked_values[i]); - } - - if (v != _checked_bad[i]) { - ::printf("reg %02x:%02x was bad %02x\n", - (unsigned)_checked_registers[i], - (unsigned)v, - (unsigned)_checked_bad[i]); - } - } + ::printf("temperature: %.1f\n", (double)_last_temperature); + float accel_cut = _accel_filter_x.get_cutoff_freq(); + ::printf("accel cutoff set to %10.2f Hz\n", double(accel_cut)); + float gyro_cut = _gyro_filter_x.get_cutoff_freq(); + ::printf("gyro cutoff set to %10.2f Hz\n", double(gyro_cut)); } diff --git a/src/drivers/imu/mpu9250/mpu9250.h b/src/drivers/imu/mpu9250/mpu9250.h index 3939e9527013..d0157ec5cdc9 100644 --- a/src/drivers/imu/mpu9250/mpu9250.h +++ b/src/drivers/imu/mpu9250/mpu9250.h @@ -49,12 +49,22 @@ #include #include +#include +#include + #include "mag.h" #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) +#if defined(PX4_I2C_OBDEV_MPU9250) || defined(PX4_I2C_BUS_EXPANSION) # define USE_I2C #endif @@ -177,8 +187,9 @@ #define BIT_I2C_SLV2_DLY_EN 0x04 #define BIT_I2C_SLV3_DLY_EN 0x08 -#define MPU_WHOAMI_9250 0x71 -#define MPU_WHOAMI_6500 0x70 +#define MPU_WHOAMI_9250 0x71 +#define MPU_WHOAMI_6500 0x70 +#define ICM_WHOAMI_20948 0xEA #define MPU9250_ACCEL_DEFAULT_RATE 1000 #define MPU9250_ACCEL_MAX_OUTPUT_RATE 280 @@ -193,6 +204,135 @@ #define MPUIOCGIS_I2C (unsigned)(DEVIOCGDEVICEID+100) + +// ICM20948 registers and data + +/* + * ICM20948 I2C address LSB can be switched by the chip's AD0 pin, thus is device dependent. + * Noting this down for now. Here GPS uses 0x69. To support a device implementing the second + * address, probably an additional MPU_DEVICE_TYPE is the way to go. + */ +#define PX4_I2C_EXT_ICM20948_0 0x68 +#define PX4_I2C_EXT_ICM20948_1 0x69 + +/* + * ICM20948 uses register banks. Register 127 (0x7F) is used to switch between 4 banks. + * There's room in the upper address byte below the port speed setting to code in the + * used bank. This is a bit more efficient, already in use for the speed setting and more + * in one place than a solution with a lookup table for address/bank pairs. + */ + +#define BANK0 0x0000 +#define BANK1 0x0100 +#define BANK2 0x0200 +#define BANK3 0x0300 + +#define BANK_REG_MASK 0x0300 +#define REG_BANK(r) (((r) & BANK_REG_MASK)>>4) +#define REG_ADDRESS(r) ((r) & ~BANK_REG_MASK) + +#define ICMREG_20948_BANK_SEL 0x7F + +#define ICMREG_20948_WHOAMI (0x00 | BANK0) +#define ICMREG_20948_USER_CTRL (0x03 | BANK0) +#define ICMREG_20948_PWR_MGMT_1 (0x06 | BANK0) +#define ICMREG_20948_PWR_MGMT_2 (0x07 | BANK0) +#define ICMREG_20948_INT_PIN_CFG (0x0F | BANK0) +#define ICMREG_20948_INT_ENABLE (0x10 | BANK0) +#define ICMREG_20948_INT_ENABLE_1 (0x11 | BANK0) +#define ICMREG_20948_ACCEL_XOUT_H (0x2D | BANK0) +#define ICMREG_20948_INT_ENABLE_2 (0x12 | BANK0) +#define ICMREG_20948_INT_ENABLE_3 (0x13 | BANK0) +#define ICMREG_20948_EXT_SLV_SENS_DATA_00 (0x3B | BANK0) +#define ICMREG_20948_GYRO_SMPLRT_DIV (0x00 | BANK2) +#define ICMREG_20948_GYRO_CONFIG_1 (0x01 | BANK2) +#define ICMREG_20948_GYRO_CONFIG_2 (0x02 | BANK2) +#define ICMREG_20948_ACCEL_SMPLRT_DIV_1 (0x10 | BANK2) +#define ICMREG_20948_ACCEL_SMPLRT_DIV_2 (0x11 | BANK2) +#define ICMREG_20948_ACCEL_CONFIG (0x14 | BANK2) +#define ICMREG_20948_ACCEL_CONFIG_2 (0x15 | BANK2) +#define ICMREG_20948_I2C_MST_CTRL (0x01 | BANK3) +#define ICMREG_20948_I2C_SLV0_ADDR (0x03 | BANK3) +#define ICMREG_20948_I2C_SLV0_REG (0x04 | BANK3) +#define ICMREG_20948_I2C_SLV0_CTRL (0x05 | BANK3) +#define ICMREG_20948_I2C_SLV0_DO (0x06 | BANK3) + + + +/* +* ICM20948 register bits +* Most of the regiser set values from MPU9250 have the same +* meaning on ICM20948. The exceptions and values not already +* defined for MPU9250 are defined below +*/ +#define ICM_BIT_PWR_MGMT_1_ENABLE 0x00 +#define ICM_BIT_USER_CTRL_I2C_MST_DISABLE 0x00 + +#define ICM_BITS_GYRO_DLPF_CFG_197HZ 0x01 +#define ICM_BITS_GYRO_DLPF_CFG_151HZ 0x09 +#define ICM_BITS_GYRO_DLPF_CFG_119HZ 0x11 +#define ICM_BITS_GYRO_DLPF_CFG_51HZ 0x19 +#define ICM_BITS_GYRO_DLPF_CFG_23HZ 0x21 +#define ICM_BITS_GYRO_DLPF_CFG_11HZ 0x29 +#define ICM_BITS_GYRO_DLPF_CFG_5HZ 0x31 +#define ICM_BITS_GYRO_DLPF_CFG_361HZ 0x39 +#define ICM_BITS_GYRO_DLPF_CFG_MASK 0x39 + +#define ICM_BITS_GYRO_FS_SEL_250DPS 0x00 +#define ICM_BITS_GYRO_FS_SEL_500DPS 0x02 +#define ICM_BITS_GYRO_FS_SEL_1000DPS 0x04 +#define ICM_BITS_GYRO_FS_SEL_2000DPS 0x06 +#define ICM_BITS_GYRO_FS_SEL_MASK 0x06 + +#define ICM_BITS_ACCEL_DLPF_CFG_246HZ 0x09 +#define ICM_BITS_ACCEL_DLPF_CFG_111HZ 0x11 +#define ICM_BITS_ACCEL_DLPF_CFG_50HZ 0x19 +#define ICM_BITS_ACCEL_DLPF_CFG_23HZ 0x21 +#define ICM_BITS_ACCEL_DLPF_CFG_11HZ 0x29 +#define ICM_BITS_ACCEL_DLPF_CFG_5HZ 0x31 +#define ICM_BITS_ACCEL_DLPF_CFG_473HZ 0x39 +#define ICM_BITS_ACCEL_DLPF_CFG_MASK 0x39 + +#define ICM_BITS_ACCEL_FS_SEL_250DPS 0x00 +#define ICM_BITS_ACCEL_FS_SEL_500DPS 0x02 +#define ICM_BITS_ACCEL_FS_SEL_1000DPS 0x04 +#define ICM_BITS_ACCEL_FS_SEL_2000DPS 0x06 +#define ICM_BITS_ACCEL_FS_SEL_MASK 0x06 + +#define ICM_BITS_DEC3_CFG_4 0x00 +#define ICM_BITS_DEC3_CFG_8 0x01 +#define ICM_BITS_DEC3_CFG_16 0x10 +#define ICM_BITS_DEC3_CFG_32 0x11 +#define ICM_BITS_DEC3_CFG_MASK 0x11 + +#define ICM_BITS_I2C_MST_CLOCK_370KHZ 0x00 +#define ICM_BITS_I2C_MST_CLOCK_400HZ 0x07 // recommended by datasheet for 400kHz target clock + + + +#define MPU_OR_ICM(m,i) ((_device_type==MPU_DEVICE_TYPE_ICM20948) ? i : m) + + +#pragma pack(push, 1) +/** + * Report conversation within the mpu, including command byte and + * interrupt status. + */ +struct ICMReport { + uint8_t accel_x[2]; + uint8_t accel_y[2]; + uint8_t accel_z[2]; + uint8_t gyro_x[2]; + uint8_t gyro_y[2]; + uint8_t gyro_z[2]; + uint8_t temp[2]; + struct ak8963_regs mag; +}; +#pragma pack(pop) + + + + #pragma pack(push, 1) /** * Report conversation within the mpu, including command byte and @@ -224,18 +364,19 @@ struct MPUReport { */ #define MPU9250_LOW_BUS_SPEED 0 #define MPU9250_HIGH_BUS_SPEED 0x8000 +#define MPU9250_REG_MASK 0x00FF # define MPU9250_IS_HIGH_SPEED(r) ((r) & MPU9250_HIGH_BUS_SPEED) -# define MPU9250_REG(r) ((r) &~MPU9250_HIGH_BUS_SPEED) +# define MPU9250_REG(r) ((r) & MPU9250_REG_MASK) # define MPU9250_SET_SPEED(r, s) ((r)|(s)) # define MPU9250_HIGH_SPEED_OP(r) MPU9250_SET_SPEED((r), MPU9250_HIGH_BUS_SPEED) -# define MPU9250_LOW_SPEED_OP(r) MPU9250_REG((r)) +# define MPU9250_LOW_SPEED_OP(r) ((r) &~MPU9250_HIGH_BUS_SPEED) /* interface factories */ -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 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); -typedef device::Device *(*MPU9250_constructor)(int, uint32_t, bool); +typedef device::Device *(*MPU9250_constructor)(int, int, uint32_t, bool); class MPU9250_mag; class MPU9250_gyro; @@ -245,7 +386,9 @@ class MPU9250 : public device::CDev public: MPU9250(device::Device *interface, device::Device *mag_interface, const char *path_accel, const char *path_gyro, const char *path_mag, - enum Rotation rotation); + enum Rotation rotation, + int device_type, + bool magnetometer_only); virtual ~MPU9250(); virtual int init(); @@ -278,6 +421,10 @@ class MPU9250 : public device::CDev 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) */ #if defined(USE_I2C) /* @@ -307,6 +454,8 @@ class MPU9250 : public device::CDev float _gyro_range_rad_s; unsigned _dlpf_freq; + unsigned _dlpf_freq_icm_gyro; + unsigned _dlpf_freq_icm_accel; unsigned _sample_rate; perf_counter_t _accel_reads; @@ -336,16 +485,28 @@ class MPU9250 : public device::CDev // this is used to support runtime checking of key // configuration registers to detect SPI bus errors and sensor // reset + +#ifndef MAX +#define MAX(X,Y) ((X) > (Y) ? (X) : (Y)) +#endif + #define MPU9250_NUM_CHECKED_REGISTERS 11 - static const uint8_t _checked_registers[MPU9250_NUM_CHECKED_REGISTERS]; - uint8_t _checked_values[MPU9250_NUM_CHECKED_REGISTERS]; - uint8_t _checked_bad[MPU9250_NUM_CHECKED_REGISTERS]; - uint8_t _checked_next; + static const uint16_t _mpu9250_checked_registers[MPU9250_NUM_CHECKED_REGISTERS]; +#define ICM20948_NUM_CHECKED_REGISTERS 15 + static const uint16_t _icm20948_checked_registers[ICM20948_NUM_CHECKED_REGISTERS]; + + const uint16_t *_checked_registers; + + uint8_t _checked_values[MAX(MPU9250_NUM_CHECKED_REGISTERS, ICM20948_NUM_CHECKED_REGISTERS)]; + uint8_t _checked_bad[MAX(MPU9250_NUM_CHECKED_REGISTERS, ICM20948_NUM_CHECKED_REGISTERS)]; + unsigned _checked_next; + unsigned _num_checked_registers; + // last temperature reading for print_info() float _last_temperature; - bool check_null_data(uint32_t *data, uint8_t size); + bool check_null_data(uint16_t *data, uint8_t size); bool check_duplicate(uint8_t *accel_data); // keep last accel reading for duplicate detection uint8_t _last_accel_data[6]; @@ -425,15 +586,40 @@ class MPU9250 : public device::CDev */ void measure(); + /** + * Select a register bank in ICM20948 + * + * Only actually switches if the remembered bank is different from the + * requested one + * + * @param The index of the register bank to switch to (0-3) + * @return Error code + */ + int select_register_bank(uint8_t bank); + + /** * Read a register from the mpu * * @param The register to read. + * @param The bus speed to read with. * @return The value that was read. */ uint8_t read_reg(unsigned reg, uint32_t speed = MPU9250_LOW_BUS_SPEED); uint16_t read_reg16(unsigned reg); + + /** + * Read a register range from the mpu + * + * @param The start address to read from. + * @param The bus speed to read with. + * @param The address of the target data buffer. + * @param The count of bytes to be read. + * @return The value that was read. + */ + uint8_t read_reg_range(unsigned start_reg, uint32_t speed, uint8_t *buf, uint16_t count); + /** * Write a register in the mpu * diff --git a/src/drivers/imu/mpu9250/mpu9250_i2c.cpp b/src/drivers/imu/mpu9250/mpu9250_i2c.cpp index 3893d74621af..f9ded07ddcad 100644 --- a/src/drivers/imu/mpu9250/mpu9250_i2c.cpp +++ b/src/drivers/imu/mpu9250/mpu9250_i2c.cpp @@ -46,30 +46,34 @@ #ifdef USE_I2C -device::Device *MPU9250_I2C_interface(int bus, uint32_t address, bool external_bus); +device::Device *MPU9250_I2C_interface(int bus, int device_type, uint32_t address, bool external_bus); class MPU9250_I2C : public device::I2C { public: - MPU9250_I2C(int bus, uint32_t address); + MPU9250_I2C(int bus, int device_type, uint32_t address); ~MPU9250_I2C() override = default; int read(unsigned address, void *data, unsigned count) override; int write(unsigned address, void *data, unsigned count) override; protected: - int probe() override; + virtual int probe(); + +private: + int _device_type; }; device::Device * -MPU9250_I2C_interface(int bus, uint32_t address, bool external_bus) +MPU9250_I2C_interface(int bus, int device_type, uint32_t address, bool external_bus) { - return new MPU9250_I2C(bus, address); + return new MPU9250_I2C(bus, device_type, address); } -MPU9250_I2C::MPU9250_I2C(int bus, uint32_t address) : - I2C("MPU9250_I2C", nullptr, bus, address, 400000) +MPU9250_I2C::MPU9250_I2C(int bus, int device_type, uint32_t address) : + I2C("MPU9250_I2C", nullptr, bus, address, 400000), + _device_type(device_type) { _device_id.devid_s.devtype = DRV_ACC_DEVTYPE_MPU9250; } @@ -106,8 +110,34 @@ int MPU9250_I2C::probe() { uint8_t whoami = 0; - uint8_t expected = MPU_WHOAMI_9250; - return (read(MPUREG_WHOAMI, &whoami, 1) == OK && (whoami == expected)) ? 0 : -EIO; + 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; + + case MPU_DEVICE_TYPE_MPU6500: + 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); + + break; + } + + return (read(reg_whoami, &whoami, 1) == OK && (whoami == expected)) ? 0 : -EIO; } #endif /* USE_I2C */ diff --git a/src/drivers/imu/mpu9250/mpu9250_spi.cpp b/src/drivers/imu/mpu9250/mpu9250_spi.cpp index c02d6f2dc4c6..8a30fff910ef 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, uint32_t cs, bool external_bus); +device::Device *MPU9250_SPI_interface(int bus, int device_type, uint32_t cs, bool external_bus); class MPU9250_SPI : public device::SPI { @@ -84,7 +84,7 @@ class MPU9250_SPI : public device::SPI }; device::Device * -MPU9250_SPI_interface(int bus, uint32_t cs, bool external_bus) +MPU9250_SPI_interface(int bus, int device_type, uint32_t cs, bool external_bus) { device::Device *interface = nullptr; From bdcc2e4e83f75e6df184129adf47780026a87dac Mon Sep 17 00:00:00 2001 From: DanielePettenuzzo Date: Thu, 9 Aug 2018 18:10:58 +0200 Subject: [PATCH 2/8] mpu9250: don't flush ring buffers that are not initialized mpu9250: added a few if statements to check the _magnetometer_only flag --- src/drivers/imu/mpu9250/mpu9250.cpp | 37 +++++++++++++++++++---------- 1 file changed, 24 insertions(+), 13 deletions(-) diff --git a/src/drivers/imu/mpu9250/mpu9250.cpp b/src/drivers/imu/mpu9250/mpu9250.cpp index cd9ccd607d8d..00191fab689b 100644 --- a/src/drivers/imu/mpu9250/mpu9250.cpp +++ b/src/drivers/imu/mpu9250/mpu9250.cpp @@ -292,7 +292,7 @@ MPU9250::init() * make the integration autoreset faster so that we integrate just one * sample since the sampling rate is already low. */ - if (is_i2c()) { + if (is_i2c() && !_magnetometer_only) { _sample_rate = 200; _accel_int.set_autoreset_interval(1000000 / 1000); _gyro_int.set_autoreset_interval(1000000 / 1000); @@ -306,7 +306,6 @@ MPU9250::init() } /* do init */ - ret = CDev::init(); /* if init failed, bail now */ @@ -360,6 +359,8 @@ MPU9250::init() float accel_cut = MPU9250_ACCEL_DEFAULT_DRIVER_FILTER_FREQ; if (accel_cut_ph != PARAM_INVALID && (param_get(accel_cut_ph, &accel_cut) == PX4_OK)) { + PX4_INFO("accel cutoff set to %.2f Hz", double(accel_cut)); + _accel_filter_x.set_cutoff_frequency(MPU9250_ACCEL_DEFAULT_RATE, accel_cut); _accel_filter_y.set_cutoff_frequency(MPU9250_ACCEL_DEFAULT_RATE, accel_cut); _accel_filter_z.set_cutoff_frequency(MPU9250_ACCEL_DEFAULT_RATE, accel_cut); @@ -372,6 +373,8 @@ MPU9250::init() float gyro_cut = MPU9250_GYRO_DEFAULT_DRIVER_FILTER_FREQ; if (gyro_cut_ph != PARAM_INVALID && (param_get(gyro_cut_ph, &gyro_cut) == PX4_OK)) { + PX4_INFO("gyro cutoff set to %.2f Hz", double(gyro_cut)); + _gyro_filter_x.set_cutoff_frequency(MPU9250_GYRO_DEFAULT_RATE, gyro_cut); _gyro_filter_y.set_cutoff_frequency(MPU9250_GYRO_DEFAULT_RATE, gyro_cut); _gyro_filter_z.set_cutoff_frequency(MPU9250_GYRO_DEFAULT_RATE, gyro_cut); @@ -381,12 +384,14 @@ MPU9250::init() } /* do CDev init for the gyro device node, keep it optional */ - ret = _gyro->init(); + if (!_magnetometer_only) { + ret = _gyro->init(); - /* if probe/setup failed, bail now */ - if (ret != OK) { - DEVICE_DEBUG("gyro init failed"); - return ret; + /* if probe/setup failed, bail now */ + if (ret != OK) { + DEVICE_DEBUG("gyro init failed"); + return ret; + } } /* Magnetometer setup */ @@ -420,7 +425,9 @@ MPU9250::init() } } - _accel_class_instance = register_class_devname(ACCEL_BASE_DEVICE_PATH); + if (!_magnetometer_only) { + _accel_class_instance = register_class_devname(ACCEL_BASE_DEVICE_PATH); + } measure(); @@ -1258,8 +1265,11 @@ MPU9250::start() stop(); /* discard any stale data in the buffers */ - _accel_reports->flush(); - _gyro_reports->flush(); + if (!_magnetometer_only) { + _accel_reports->flush(); + _gyro_reports->flush(); + } + _mag->_mag_reports->flush(); if (_use_hrt) { @@ -1447,6 +1457,7 @@ bool MPU9250::check_duplicate(uint8_t *accel_data) void MPU9250::measure() { + if (hrt_absolute_time() < _reset_wait) { // we're waiting for a reset to complete return; @@ -1597,8 +1608,8 @@ MPU9250::measure() /* * Report buffers. */ - sensor_accel_s arb{}; - sensor_gyro_s grb{}; + sensor_accel_s arb; + sensor_gyro_s grb; /* * Adjust and scale results to m/s^2. @@ -1740,7 +1751,6 @@ MPU9250::print_info() _accel_reports->print_info("accel queue"); _gyro_reports->print_info("gyro queue"); _mag->_mag_reports->print_info("mag queue"); - ::printf("checked_next: %u\n", _checked_next); ::printf("temperature: %.1f\n", (double)_last_temperature); float accel_cut = _accel_filter_x.get_cutoff_freq(); @@ -1748,6 +1758,7 @@ MPU9250::print_info() float gyro_cut = _gyro_filter_x.get_cutoff_freq(); ::printf("gyro cutoff set to %10.2f Hz\n", double(gyro_cut)); + } void From 2677cf8056f0e5c8ca4ae5e34651c700671a7da5 Mon Sep 17 00:00:00 2001 From: Florian Olbrich Date: Fri, 10 Aug 2018 14:37:48 +0200 Subject: [PATCH 3/8] Split off accelerometer CDev from main driver class, changed startup code to be able to not setup gyro/accel devices. --- src/drivers/imu/mpu9250/CMakeLists.txt | 1 + src/drivers/imu/mpu9250/accel.cpp | 131 ++++++++++++ src/drivers/imu/mpu9250/accel.h | 66 ++++++ src/drivers/imu/mpu9250/mag.cpp | 21 +- src/drivers/imu/mpu9250/main.cpp | 7 +- src/drivers/imu/mpu9250/mpu9250.cpp | 282 +++++++++++++------------ src/drivers/imu/mpu9250/mpu9250.h | 22 +- 7 files changed, 368 insertions(+), 162 deletions(-) create mode 100644 src/drivers/imu/mpu9250/accel.cpp create mode 100644 src/drivers/imu/mpu9250/accel.h diff --git a/src/drivers/imu/mpu9250/CMakeLists.txt b/src/drivers/imu/mpu9250/CMakeLists.txt index 05cb193c7d34..49f83a7939b1 100644 --- a/src/drivers/imu/mpu9250/CMakeLists.txt +++ b/src/drivers/imu/mpu9250/CMakeLists.txt @@ -40,6 +40,7 @@ px4_add_module( mpu9250_i2c.cpp mpu9250_spi.cpp main.cpp + accel.cpp gyro.cpp mag.cpp mag_i2c.cpp diff --git a/src/drivers/imu/mpu9250/accel.cpp b/src/drivers/imu/mpu9250/accel.cpp new file mode 100644 index 000000000000..fd43687c4a01 --- /dev/null +++ b/src/drivers/imu/mpu9250/accel.cpp @@ -0,0 +1,131 @@ +/**************************************************************************** + * + * Copyright (c) 2012-2016 PX4 Development Team. All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + +/** + * @file gyro.cpp + * + * Driver for the Invensense mpu9250 connected via SPI. + * + * @author Andrew Tridgell + * + * based on the mpu6000 driver + */ + +#include + +#include +#include +#include +#include +#include +#include +#include + +#include + +#include +#include + +#include +#include +#include +#include +#include +#include +#include +#include + +#include "mag.h" +#include "gyro.h" +#include "mpu9250.h" + +MPU9250_accel::MPU9250_accel(MPU9250 *parent, const char *path) : + CDev("MPU9250_accel", path), + _parent(parent), + _accel_topic(nullptr), + _accel_orb_class_instance(-1), + _accel_class_instance(-1) +{ +} + +MPU9250_accel::~MPU9250_accel() +{ + if (_accel_class_instance != -1) { + unregister_class_devname(ACCEL_BASE_DEVICE_PATH, _accel_class_instance); + } +} + +int +MPU9250_accel::init() +{ + int ret; + + // do base class init + ret = CDev::init(); + + /* if probe/setup failed, bail now */ + if (ret != OK) { + DEVICE_DEBUG("accel init failed"); + return ret; + } + + _accel_class_instance = register_class_devname(ACCEL_BASE_DEVICE_PATH); + + return ret; +} + +void +MPU9250_accel::parent_poll_notify() +{ + poll_notify(POLLIN); +} + +ssize_t +MPU9250_accel::read(struct file *filp, char *buffer, size_t buflen) +{ + return _parent->accel_read(filp, buffer, buflen); +} + +int +MPU9250_accel::ioctl(struct file *filp, int cmd, unsigned long arg) +{ + + switch (cmd) { + case DEVIOCGDEVICEID: + return (int)CDev::ioctl(filp, cmd, arg); + break; + + default: + return _parent->accel_ioctl(filp, cmd, arg); + } +} diff --git a/src/drivers/imu/mpu9250/accel.h b/src/drivers/imu/mpu9250/accel.h new file mode 100644 index 000000000000..ec79f1c56802 --- /dev/null +++ b/src/drivers/imu/mpu9250/accel.h @@ -0,0 +1,66 @@ +/**************************************************************************** + * + * Copyright (c) 2012-2016 PX4 Development Team. All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + +#pragma once + +class MPU9250; + +/** + * Helper class implementing the accel driver node. + */ +class MPU9250_accel : public device::CDev +{ +public: + MPU9250_accel(MPU9250 *parent, const char *path); + ~MPU9250_accel(); + + virtual ssize_t read(struct file *filp, char *buffer, size_t buflen); + virtual int ioctl(struct file *filp, int cmd, unsigned long arg); + + virtual int init(); + +protected: + friend class MPU9250; + + void parent_poll_notify(); + +private: + MPU9250 *_parent; + orb_advert_t _accel_topic; + int _accel_orb_class_instance; + int _accel_class_instance; + + /* do not allow to copy this class due to pointer data members */ + MPU9250_accel(const MPU9250_accel &); + MPU9250_accel operator=(const MPU9250_accel &); +}; diff --git a/src/drivers/imu/mpu9250/mag.cpp b/src/drivers/imu/mpu9250/mag.cpp index 177da019cb3a..d0efeb8a72d2 100644 --- a/src/drivers/imu/mpu9250/mag.cpp +++ b/src/drivers/imu/mpu9250/mag.cpp @@ -338,25 +338,8 @@ MPU9250_mag::ioctl(struct file *filp, int cmd, unsigned long arg) return ak8963_reset(); case SENSORIOCSPOLLRATE: { - switch (arg) { - - /* zero would be bad */ - case 0: - return -EINVAL; - - /* set default polling rate */ - case SENSOR_POLLRATE_DEFAULT: - return ioctl(filp, SENSORIOCSPOLLRATE, MPU9250_AK8963_SAMPLE_RATE); - - /* adjust to a legal polling interval in Hz */ - default: { - if (MPU9250_AK8963_SAMPLE_RATE != arg) { - return -EINVAL; - } - - return OK; - } - } + /* mag is polled through main driver only */ + return _parent->accel_ioctl(filp, cmd, arg); } case MAGIOCSSCALE: diff --git a/src/drivers/imu/mpu9250/main.cpp b/src/drivers/imu/mpu9250/main.cpp index 3adfd367989e..a90a782e563e 100644 --- a/src/drivers/imu/mpu9250/main.cpp +++ b/src/drivers/imu/mpu9250/main.cpp @@ -270,8 +270,11 @@ start_bus(struct mpu9250_bus_option &bus, enum Rotation rotation, bool external, goto fail; } - /* set the poll rate to default, starts automatic data collection */ - fd = open(bus.accelpath, O_RDONLY); + /* + * 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. + */ + fd = open(bus.magpath, O_RDONLY); if (fd < 0) { goto fail; diff --git a/src/drivers/imu/mpu9250/mpu9250.cpp b/src/drivers/imu/mpu9250/mpu9250.cpp index 00191fab689b..4a949858e5c6 100644 --- a/src/drivers/imu/mpu9250/mpu9250.cpp +++ b/src/drivers/imu/mpu9250/mpu9250.cpp @@ -79,6 +79,7 @@ #include #include "mag.h" +#include "accel.h" #include "gyro.h" #include "mpu9250.h" @@ -134,9 +135,10 @@ MPU9250::MPU9250(device::Device *interface, device::Device *mag_interface, const enum Rotation rotation, int device_type, bool magnetometer_only) : - CDev("MPU9250", path_accel), _interface(interface), - _gyro(new MPU9250_gyro(this, path_gyro)), + _name("MPU9250"), + _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), @@ -155,8 +157,6 @@ MPU9250::MPU9250(device::Device *interface, device::Device *mag_interface, const _accel_range_scale(0.0f), _accel_range_m_s2(0.0f), _accel_topic(nullptr), - _accel_orb_class_instance(-1), - _accel_class_instance(-1), _gyro_reports(nullptr), _gyro_scale{}, _gyro_range_scale(0.0f), @@ -194,29 +194,31 @@ MPU9250::MPU9250(device::Device *interface, device::Device *mag_interface, const // disable debug() calls _debug_enabled = false; - /* Set device parameters and make sure parameters of the bus device are adopted */ - _device_id.devid_s.devtype = DRV_ACC_DEVTYPE_MPU9250; - _device_id.devid_s.bus_type = (device::Device::DeviceBusType)_interface->get_device_bus_type(); - _device_id.devid_s.bus = _interface->get_device_bus(); - _device_id.devid_s.address = _interface->get_device_address(); + if (_accel != nullptr) { + /* Set device parameters and make sure parameters of the bus device are adopted */ + _accel->_device_id.devid_s.devtype = DRV_ACC_DEVTYPE_MPU9250; + _accel->_device_id.devid_s.bus_type = (device::Device::DeviceBusType)_interface->get_device_bus_type(); + _accel->_device_id.devid_s.bus = _interface->get_device_bus(); + _accel->_device_id.devid_s.address = _interface->get_device_address(); + } - /* Prime _gyro with parents devid. */ + /* Prime _gyro with common devid. */ /* Set device parameters and make sure parameters of the bus device are adopted */ - _gyro->_device_id.devid = _device_id.devid; + _gyro->_device_id.devid = _accel->_device_id.devid; _gyro->_device_id.devid_s.devtype = DRV_GYR_DEVTYPE_MPU9250; _gyro->_device_id.devid_s.bus_type = _interface->get_device_bus_type(); _gyro->_device_id.devid_s.bus = _interface->get_device_bus(); _gyro->_device_id.devid_s.address = _interface->get_device_address(); - /* Prime _mag with parents devid. */ - _mag->_device_id.devid = _device_id.devid; + /* Prime _mag with common devid. */ + _mag->_device_id.devid = _accel->_device_id.devid; _mag->_device_id.devid_s.devtype = DRV_MAG_DEVTYPE_MPU9250; _mag->_device_id.devid_s.bus_type = _interface->get_device_bus_type(); _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(_device_id.devid_s.devtype); + _interface->set_device_type(_accel->_device_id.devid_s.devtype); // default accel scale factors _accel_scale.x_offset = 0; @@ -248,6 +250,9 @@ MPU9250::~MPU9250() orb_unadvertise(_accel_topic); orb_unadvertise(_gyro->_gyro_topic); + /* delete the accel subdriver */ + delete _accel; + /* delete the gyro subdriver */ delete _gyro; @@ -263,10 +268,6 @@ MPU9250::~MPU9250() delete _gyro_reports; } - if (_accel_class_instance != -1) { - unregister_class_devname(ACCEL_BASE_DEVICE_PATH, _accel_class_instance); - } - /* delete the perf counter */ perf_free(_sample_perf); perf_free(_accel_reads); @@ -305,12 +306,12 @@ MPU9250::init() return ret; } - /* do init */ - ret = CDev::init(); + state = px4_enter_critical_section(); + _reset_wait = hrt_absolute_time() + 100000; + px4_leave_critical_section(state); - /* if init failed, bail now */ - if (ret != OK) { - DEVICE_DEBUG("CDev init failed"); + if (reset_mpu() != OK) { + PX4_ERR("Exiting! Device failed to take initialization"); return ret; } @@ -328,63 +329,55 @@ MPU9250::init() if (_gyro_reports == nullptr) { return ret; } - } - state = px4_enter_critical_section(); - _reset_wait = hrt_absolute_time() + 100000; - px4_leave_critical_section(state); - - if (reset_mpu() != OK) { - PX4_ERR("Exiting! Device failed to take initialization"); - return ret; - } + /* Initialize offsets and scales */ + _accel_scale.x_offset = 0; + _accel_scale.x_scale = 1.0f; + _accel_scale.y_offset = 0; + _accel_scale.y_scale = 1.0f; + _accel_scale.z_offset = 0; + _accel_scale.z_scale = 1.0f; - /* Initialize offsets and scales */ - _accel_scale.x_offset = 0; - _accel_scale.x_scale = 1.0f; - _accel_scale.y_offset = 0; - _accel_scale.y_scale = 1.0f; - _accel_scale.z_offset = 0; - _accel_scale.z_scale = 1.0f; + _gyro_scale.x_offset = 0; + _gyro_scale.x_scale = 1.0f; + _gyro_scale.y_offset = 0; + _gyro_scale.y_scale = 1.0f; + _gyro_scale.z_offset = 0; + _gyro_scale.z_scale = 1.0f; - _gyro_scale.x_offset = 0; - _gyro_scale.x_scale = 1.0f; - _gyro_scale.y_offset = 0; - _gyro_scale.y_scale = 1.0f; - _gyro_scale.z_offset = 0; - _gyro_scale.z_scale = 1.0f; + // set software low pass filter for controllers + param_t accel_cut_ph = param_find("IMU_ACCEL_CUTOFF"); + float accel_cut = MPU9250_ACCEL_DEFAULT_DRIVER_FILTER_FREQ; - // set software low pass filter for controllers - param_t accel_cut_ph = param_find("IMU_ACCEL_CUTOFF"); - float accel_cut = MPU9250_ACCEL_DEFAULT_DRIVER_FILTER_FREQ; + if (accel_cut_ph != PARAM_INVALID && (param_get(accel_cut_ph, &accel_cut) == PX4_OK)) { + PX4_INFO("accel cutoff set to %.2f Hz", double(accel_cut)); - if (accel_cut_ph != PARAM_INVALID && (param_get(accel_cut_ph, &accel_cut) == PX4_OK)) { - PX4_INFO("accel cutoff set to %.2f Hz", double(accel_cut)); + _accel_filter_x.set_cutoff_frequency(MPU9250_ACCEL_DEFAULT_RATE, accel_cut); + _accel_filter_y.set_cutoff_frequency(MPU9250_ACCEL_DEFAULT_RATE, accel_cut); + _accel_filter_z.set_cutoff_frequency(MPU9250_ACCEL_DEFAULT_RATE, accel_cut); - _accel_filter_x.set_cutoff_frequency(MPU9250_ACCEL_DEFAULT_RATE, accel_cut); - _accel_filter_y.set_cutoff_frequency(MPU9250_ACCEL_DEFAULT_RATE, accel_cut); - _accel_filter_z.set_cutoff_frequency(MPU9250_ACCEL_DEFAULT_RATE, accel_cut); + } else { + PX4_ERR("IMU_ACCEL_CUTOFF param invalid"); + } - } else { - PX4_ERR("IMU_ACCEL_CUTOFF param invalid"); - } + param_t gyro_cut_ph = param_find("IMU_GYRO_CUTOFF"); + float gyro_cut = MPU9250_GYRO_DEFAULT_DRIVER_FILTER_FREQ; - param_t gyro_cut_ph = param_find("IMU_GYRO_CUTOFF"); - float gyro_cut = MPU9250_GYRO_DEFAULT_DRIVER_FILTER_FREQ; + if (gyro_cut_ph != PARAM_INVALID && (param_get(gyro_cut_ph, &gyro_cut) == PX4_OK)) { + PX4_INFO("gyro cutoff set to %.2f Hz", double(gyro_cut)); - if (gyro_cut_ph != PARAM_INVALID && (param_get(gyro_cut_ph, &gyro_cut) == PX4_OK)) { - PX4_INFO("gyro cutoff set to %.2f Hz", double(gyro_cut)); + _gyro_filter_x.set_cutoff_frequency(MPU9250_GYRO_DEFAULT_RATE, gyro_cut); + _gyro_filter_y.set_cutoff_frequency(MPU9250_GYRO_DEFAULT_RATE, gyro_cut); + _gyro_filter_z.set_cutoff_frequency(MPU9250_GYRO_DEFAULT_RATE, gyro_cut); - _gyro_filter_x.set_cutoff_frequency(MPU9250_GYRO_DEFAULT_RATE, gyro_cut); - _gyro_filter_y.set_cutoff_frequency(MPU9250_GYRO_DEFAULT_RATE, gyro_cut); - _gyro_filter_z.set_cutoff_frequency(MPU9250_GYRO_DEFAULT_RATE, gyro_cut); + } else { + PX4_ERR("IMU_GYRO_CUTOFF param invalid"); + } - } else { - PX4_ERR("IMU_GYRO_CUTOFF param invalid"); - } + /* do CDev init for the accel device node */ + ret = _accel->init(); - /* do CDev init for the gyro device node, keep it optional */ - if (!_magnetometer_only) { + /* do CDev init for the gyro device node */ ret = _gyro->init(); /* if probe/setup failed, bail now */ @@ -425,10 +418,6 @@ MPU9250::init() } } - if (!_magnetometer_only) { - _accel_class_instance = register_class_devname(ACCEL_BASE_DEVICE_PATH); - } - measure(); if (!_magnetometer_only) { @@ -438,7 +427,7 @@ MPU9250::init() /* measurement will have generated a report, publish */ _accel_topic = orb_advertise_multi(ORB_ID(sensor_accel), &arp, - &_accel_orb_class_instance, (is_external()) ? ORB_PRIO_MAX - 1 : ORB_PRIO_HIGH - 1); + &_accel->_accel_orb_class_instance, (is_external()) ? ORB_PRIO_MAX - 1 : ORB_PRIO_HIGH - 1); if (_accel_topic == nullptr) { PX4_ERR("ADVERT FAIL"); @@ -706,6 +695,66 @@ MPU9250::_set_sample_rate(unsigned desired_sample_rate_hz) _sample_rate = 1100 / div; break; } + + /* + * Adjust pollrate to new sample rate. + */ + _set_pollrate(_sample_rate); +} + +/* + * Set poll rate + */ +int +MPU9250::_set_pollrate(unsigned long rate) +{ + if (rate == 0) { + return -EINVAL; + + } else { + /* do we need to start internal polling? */ + bool want_start = (_call_interval == 0); + + /* convert hz to hrt interval via microseconds */ + unsigned ticks = 1000000 / rate; + + /* check against maximum sane rate */ + if (ticks < 1000) { + return -EINVAL; + } + + // adjust filters + float cutoff_freq_hz = _accel_filter_x.get_cutoff_freq(); + float sample_rate = 1.0e6f / ticks; + _accel_filter_x.set_cutoff_frequency(sample_rate, cutoff_freq_hz); + _accel_filter_y.set_cutoff_frequency(sample_rate, cutoff_freq_hz); + _accel_filter_z.set_cutoff_frequency(sample_rate, cutoff_freq_hz); + + + float cutoff_freq_hz_gyro = _gyro_filter_x.get_cutoff_freq(); + _gyro_filter_x.set_cutoff_frequency(sample_rate, cutoff_freq_hz_gyro); + _gyro_filter_y.set_cutoff_frequency(sample_rate, cutoff_freq_hz_gyro); + _gyro_filter_z.set_cutoff_frequency(sample_rate, cutoff_freq_hz_gyro); + + /* update interval for next measurement */ + /* XXX this is a bit shady, but no other way to adjust... */ + _call_interval = ticks; + + /* + set call interval faster than the sample time. We + then detect when we have duplicate samples and reject + them. This prevents aliasing due to a beat between the + stm32 clock and the mpu9250 clock + */ + _call.period = _call_interval - MPU9250_TIMER_REDUCTION; + + /* if we need to start the poll state machine, do it */ + if (want_start) { + start(); + } + + return OK; + } } /* @@ -850,7 +899,7 @@ MPU9250::_set_dlpf_filter(uint16_t frequency_hz) } ssize_t -MPU9250::read(struct file *filp, char *buffer, size_t buflen) +MPU9250::accel_read(struct file *filp, char *buffer, size_t buflen) { unsigned count = buflen / sizeof(sensor_accel_s); @@ -956,7 +1005,7 @@ MPU9250::gyro_read(struct file *filp, char *buffer, size_t buflen) } int -MPU9250::ioctl(struct file *filp, int cmd, unsigned long arg) +MPU9250::accel_ioctl(struct file *filp, int cmd, unsigned long arg) { switch (cmd) { @@ -973,53 +1022,11 @@ MPU9250::ioctl(struct file *filp, int cmd, unsigned long arg) /* set default polling rate */ case SENSOR_POLLRATE_DEFAULT: - return ioctl(filp, SENSORIOCSPOLLRATE, MPU9250_ACCEL_DEFAULT_RATE); + return accel_ioctl(filp, SENSORIOCSPOLLRATE, MPU9250_ACCEL_DEFAULT_RATE); /* adjust to a legal polling interval in Hz */ - default: { - /* do we need to start internal polling? */ - bool want_start = (_call_interval == 0); - - /* convert hz to hrt interval via microseconds */ - unsigned ticks = 1000000 / arg; - - /* check against maximum sane rate */ - if (ticks < 1000) { - return -EINVAL; - } - - // adjust filters - float cutoff_freq_hz = _accel_filter_x.get_cutoff_freq(); - float sample_rate = 1.0e6f / ticks; - _accel_filter_x.set_cutoff_frequency(sample_rate, cutoff_freq_hz); - _accel_filter_y.set_cutoff_frequency(sample_rate, cutoff_freq_hz); - _accel_filter_z.set_cutoff_frequency(sample_rate, cutoff_freq_hz); - - - float cutoff_freq_hz_gyro = _gyro_filter_x.get_cutoff_freq(); - _gyro_filter_x.set_cutoff_frequency(sample_rate, cutoff_freq_hz_gyro); - _gyro_filter_y.set_cutoff_frequency(sample_rate, cutoff_freq_hz_gyro); - _gyro_filter_z.set_cutoff_frequency(sample_rate, cutoff_freq_hz_gyro); - - /* update interval for next measurement */ - /* XXX this is a bit shady, but no other way to adjust... */ - _call_interval = ticks; - - /* - set call interval faster than the sample time. We - then detect when we have duplicate samples and reject - them. This prevents aliasing due to a beat between the - stm32 clock and the mpu9250 clock - */ - _call.period = _call_interval - MPU9250_TIMER_REDUCTION; - - /* if we need to start the poll state machine, do it */ - if (want_start) { - start(); - } - - return OK; - } + default: + return _set_pollrate(arg); } } @@ -1039,7 +1046,7 @@ MPU9250::ioctl(struct file *filp, int cmd, unsigned long arg) default: /* give it to the superclass */ - return CDev::ioctl(filp, cmd, arg); + return _gyro->ioctl(filp, cmd, arg); } } @@ -1051,7 +1058,7 @@ MPU9250::gyro_ioctl(struct file *filp, int cmd, unsigned long arg) /* these are shared with the accel side */ case SENSORIOCSPOLLRATE: case SENSORIOCRESET: - return ioctl(filp, cmd, arg); + return accel_ioctl(filp, cmd, arg); case GYROIOCSSCALE: /* copy scale in */ @@ -1060,7 +1067,7 @@ MPU9250::gyro_ioctl(struct file *filp, int cmd, unsigned long arg) default: /* give it to the superclass */ - return CDev::ioctl(filp, cmd, arg); + return _accel->ioctl(filp, cmd, arg); } } @@ -1609,7 +1616,7 @@ MPU9250::measure() * Report buffers. */ sensor_accel_s arb; - sensor_gyro_s grb; + sensor_gyro_s grb; /* * Adjust and scale results to m/s^2. @@ -1673,7 +1680,7 @@ MPU9250::measure() arb.temperature = _last_temperature; /* return device ID */ - arb.device_id = _device_id.devid; + arb.device_id = _accel->_device_id.devid; grb.x_raw = report.gyro_x; grb.y_raw = report.gyro_y; @@ -1714,19 +1721,19 @@ MPU9250::measure() /* notify anyone waiting for data */ if (accel_notify) { - poll_notify(POLLIN); + _accel->poll_notify(POLLIN); } if (gyro_notify) { _gyro->parent_poll_notify(); } - if (accel_notify && !(_pub_blocked)) { + if (accel_notify && !(_accel->_pub_blocked)) { /* publish it */ orb_publish(ORB_ID(sensor_accel), _accel_topic, &arb); } - if (gyro_notify && !(_pub_blocked)) { + if (gyro_notify && !(_gyro->_pub_blocked)) { /* publish it */ orb_publish(ORB_ID(sensor_gyro), _gyro->_gyro_topic, &grb); } @@ -1748,15 +1755,20 @@ MPU9250::print_info() perf_print_counter(_good_transfers); perf_print_counter(_reset_retries); perf_print_counter(_duplicates); - _accel_reports->print_info("accel queue"); - _gyro_reports->print_info("gyro queue"); - _mag->_mag_reports->print_info("mag queue"); + + if (!_magnetometer_only) { + _accel_reports->print_info("accel queue"); + _gyro_reports->print_info("gyro queue"); + _mag->_mag_reports->print_info("mag queue"); + + float accel_cut = _accel_filter_x.get_cutoff_freq(); + ::printf("accel cutoff set to %10.2f Hz\n", double(accel_cut)); + float gyro_cut = _gyro_filter_x.get_cutoff_freq(); + ::printf("gyro cutoff set to %10.2f Hz\n", double(gyro_cut)); + + } ::printf("temperature: %.1f\n", (double)_last_temperature); - float accel_cut = _accel_filter_x.get_cutoff_freq(); - ::printf("accel cutoff set to %10.2f Hz\n", double(accel_cut)); - float gyro_cut = _gyro_filter_x.get_cutoff_freq(); - ::printf("gyro cutoff set to %10.2f Hz\n", double(gyro_cut)); } diff --git a/src/drivers/imu/mpu9250/mpu9250.h b/src/drivers/imu/mpu9250/mpu9250.h index d0157ec5cdc9..b57896d29226 100644 --- a/src/drivers/imu/mpu9250/mpu9250.h +++ b/src/drivers/imu/mpu9250/mpu9250.h @@ -53,6 +53,7 @@ #include #include "mag.h" +#include "accel.h" #include "gyro.h" @@ -379,9 +380,10 @@ extern int MPU9250_probe(device::Device *dev, int device_type); typedef device::Device *(*MPU9250_constructor)(int, int, uint32_t, bool); class MPU9250_mag; +class MPU9250_accel; class MPU9250_gyro; -class MPU9250 : public device::CDev +class MPU9250 { public: MPU9250(device::Device *interface, device::Device *mag_interface, const char *path_accel, const char *path_gyro, @@ -393,8 +395,8 @@ class MPU9250 : public device::CDev virtual int init(); - virtual ssize_t read(struct file *filp, char *buffer, size_t buflen); - virtual int ioctl(struct file *filp, int cmd, unsigned long arg); + virtual ssize_t accel_read(struct file *filp, char *buffer, size_t buflen); + virtual int accel_ioctl(struct file *filp, int cmd, unsigned long arg); /** * Diagnostics - print some basic information about the driver. @@ -407,10 +409,14 @@ class MPU9250 : public device::CDev void test_error(); protected: - Device *_interface; + device::Device *_interface; + + const char *_name; + bool _debug_enabled{false}; virtual int probe(); + friend class MPU9250_accel; friend class MPU9250_mag; friend class MPU9250_gyro; @@ -418,6 +424,7 @@ class MPU9250 : public device::CDev virtual int gyro_ioctl(struct file *filp, int cmd, unsigned long arg); private: + MPU9250_accel *_accel; MPU9250_gyro *_gyro; MPU9250_mag *_mag; uint8_t _whoami; /** whoami result */ @@ -444,8 +451,6 @@ class MPU9250 : public device::CDev float _accel_range_scale; float _accel_range_m_s2; orb_advert_t _accel_topic; - int _accel_orb_class_instance; - int _accel_class_instance; ringbuffer::RingBuffer *_gyro_reports; @@ -695,6 +700,11 @@ class MPU9250 : public device::CDev */ void _set_sample_rate(unsigned desired_sample_rate_hz); + /* + set poll rate + */ + int _set_pollrate(unsigned long rate); + /* check that key registers still have the right value */ From de8574233c9e7203239b9f8e6bfc673e51f467c3 Mon Sep 17 00:00:00 2001 From: DanielePettenuzzo Date: Wed, 19 Sep 2018 18:13:11 +0200 Subject: [PATCH 4/8] mpu9250: remove hardfault when running driver as mag_only mpu9250: don't call _set_pollrate twice because first time ring buffers are not initialized mpu9250: set _call_interval to zero when stopping driver (avoids one more work queue call after stop) mpu9250: move call_interval=0 to deconstructor --- src/drivers/imu/mpu9250/mpu9250.cpp | 32 ++++++++++++++--------------- 1 file changed, 16 insertions(+), 16 deletions(-) diff --git a/src/drivers/imu/mpu9250/mpu9250.cpp b/src/drivers/imu/mpu9250/mpu9250.cpp index 4a949858e5c6..2a21e3c521f4 100644 --- a/src/drivers/imu/mpu9250/mpu9250.cpp +++ b/src/drivers/imu/mpu9250/mpu9250.cpp @@ -202,23 +202,25 @@ MPU9250::MPU9250(device::Device *interface, device::Device *mag_interface, const _accel->_device_id.devid_s.address = _interface->get_device_address(); } - /* Prime _gyro with common devid. */ - /* Set device parameters and make sure parameters of the bus device are adopted */ - _gyro->_device_id.devid = _accel->_device_id.devid; - _gyro->_device_id.devid_s.devtype = DRV_GYR_DEVTYPE_MPU9250; - _gyro->_device_id.devid_s.bus_type = _interface->get_device_bus_type(); - _gyro->_device_id.devid_s.bus = _interface->get_device_bus(); - _gyro->_device_id.devid_s.address = _interface->get_device_address(); + if (_gyro != nullptr) { + /* Prime _gyro with common devid. */ + /* Set device parameters and make sure parameters of the bus device are adopted */ + _gyro->_device_id.devid = 0; + _gyro->_device_id.devid_s.devtype = DRV_GYR_DEVTYPE_MPU9250; + _gyro->_device_id.devid_s.bus_type = _interface->get_device_bus_type(); + _gyro->_device_id.devid_s.bus = _interface->get_device_bus(); + _gyro->_device_id.devid_s.address = _interface->get_device_address(); + } /* Prime _mag with common devid. */ - _mag->_device_id.devid = _accel->_device_id.devid; + _mag->_device_id.devid = 0; _mag->_device_id.devid_s.devtype = DRV_MAG_DEVTYPE_MPU9250; _mag->_device_id.devid_s.bus_type = _interface->get_device_bus_type(); _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(_accel->_device_id.devid_s.devtype); + _interface->set_device_type(DRV_ACC_DEVTYPE_MPU9250); // default accel scale factors _accel_scale.x_offset = 0; @@ -246,9 +248,12 @@ MPU9250::~MPU9250() { /* make sure we are truly inactive */ stop(); + _call_interval = 0; - orb_unadvertise(_accel_topic); - orb_unadvertise(_gyro->_gyro_topic); + if (!_magnetometer_only) { + orb_unadvertise(_accel_topic); + orb_unadvertise(_gyro->_gyro_topic); + } /* delete the accel subdriver */ delete _accel; @@ -695,11 +700,6 @@ MPU9250::_set_sample_rate(unsigned desired_sample_rate_hz) _sample_rate = 1100 / div; break; } - - /* - * Adjust pollrate to new sample rate. - */ - _set_pollrate(_sample_rate); } /* From d57fd645d994a6c1f88bd4b519d31d3a17ac2b8e Mon Sep 17 00:00:00 2001 From: Florian Olbrich Date: Tue, 9 Oct 2018 16:04:13 +0200 Subject: [PATCH 5/8] Added fix for mpu6500 startup and to prevent it starting in magnetometer only mode. mpu9250: fix mag rotation, other minor fixes, make format --- src/drivers/imu/mpu9250/CMakeLists.txt | 2 +- src/drivers/imu/mpu9250/mag.cpp | 4 ++++ src/drivers/imu/mpu9250/mag.h | 2 +- src/drivers/imu/mpu9250/main.cpp | 15 +++++++++++++-- 4 files changed, 19 insertions(+), 4 deletions(-) diff --git a/src/drivers/imu/mpu9250/CMakeLists.txt b/src/drivers/imu/mpu9250/CMakeLists.txt index 49f83a7939b1..bd40db5da786 100644 --- a/src/drivers/imu/mpu9250/CMakeLists.txt +++ b/src/drivers/imu/mpu9250/CMakeLists.txt @@ -40,7 +40,7 @@ px4_add_module( mpu9250_i2c.cpp mpu9250_spi.cpp main.cpp - accel.cpp + accel.cpp gyro.cpp mag.cpp mag_i2c.cpp diff --git a/src/drivers/imu/mpu9250/mag.cpp b/src/drivers/imu/mpu9250/mag.cpp index d0efeb8a72d2..e193942ffd94 100644 --- a/src/drivers/imu/mpu9250/mag.cpp +++ b/src/drivers/imu/mpu9250/mag.cpp @@ -264,6 +264,10 @@ 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) { + rotate_3f(ROTATION_YAW_270, xraw_f, yraw_f, zraw_f); //offset between accel/gyro and mag on icm20948 + } + mrb.x = ((xraw_f * _mag_range_scale * _mag_asa_x) - _mag_scale.x_offset) * _mag_scale.x_scale; mrb.y = ((yraw_f * _mag_range_scale * _mag_asa_y) - _mag_scale.y_offset) * _mag_scale.y_scale; mrb.z = ((zraw_f * _mag_range_scale * _mag_asa_z) - _mag_scale.z_offset) * _mag_scale.z_scale; diff --git a/src/drivers/imu/mpu9250/mag.h b/src/drivers/imu/mpu9250/mag.h index 45f3e3d56237..c6d84c593aee 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->_device_type==MPU_DEVICE_TYPE_MPU9250) ? (m) : (i)) diff --git a/src/drivers/imu/mpu9250/main.cpp b/src/drivers/imu/mpu9250/main.cpp index a90a782e563e..82b8e17b8b16 100644 --- a/src/drivers/imu/mpu9250/main.cpp +++ b/src/drivers/imu/mpu9250/main.cpp @@ -273,8 +273,14 @@ start_bus(struct mpu9250_bus_option &bus, enum Rotation rotation, bool external, /* * 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. */ - fd = open(bus.magpath, O_RDONLY); + if (bus.device_type == MPU_DEVICE_TYPE_MPU6500) { + fd = open(bus.accelpath, O_RDONLY); + + } else { + fd = open(bus.magpath, O_RDONLY); + } if (fd < 0) { goto fail; @@ -326,6 +332,11 @@ 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; } @@ -526,7 +537,7 @@ usage() PX4_INFO(" -S (spi external bus)"); PX4_INFO(" -t (spi internal bus, 2nd instance)"); PX4_INFO(" -R rotation"); - PX4_INFO(" -M only enable magnetometer, accel/gyro disabled"); + PX4_INFO(" -M only enable magnetometer, accel/gyro disabled - not av. on MPU6500"); } From 550c0b3cf21ac84c06edd926abd2948cb6d75ed8 Mon Sep 17 00:00:00 2001 From: Florian Olbrich Date: Thu, 11 Oct 2018 10:12:44 +0200 Subject: [PATCH 6/8] Removed verbose error messages, added missing init failure check. Fix in rc.sensors, uncommented hmc5883 driver start. Moved ioctl handlers to CDev-inheriting classes to get clean escalation of unhandled ioctls to superclass --- src/drivers/imu/mpu9250/accel.cpp | 45 ++++++++++++++-- src/drivers/imu/mpu9250/gyro.cpp | 15 ++++-- src/drivers/imu/mpu9250/mag.cpp | 24 +++++++-- src/drivers/imu/mpu9250/mpu9250.cpp | 79 +++-------------------------- src/drivers/imu/mpu9250/mpu9250.h | 2 - 5 files changed, 79 insertions(+), 86 deletions(-) diff --git a/src/drivers/imu/mpu9250/accel.cpp b/src/drivers/imu/mpu9250/accel.cpp index fd43687c4a01..58eab4b76086 100644 --- a/src/drivers/imu/mpu9250/accel.cpp +++ b/src/drivers/imu/mpu9250/accel.cpp @@ -42,6 +42,7 @@ */ #include +#include #include #include @@ -119,13 +120,49 @@ MPU9250_accel::read(struct file *filp, char *buffer, size_t buflen) int MPU9250_accel::ioctl(struct file *filp, int cmd, unsigned long arg) { + /* + * Repeated in MPU9250_mag::ioctl + * Both accel and mag CDev could be unused in case of magnetometer only mode or MPU6500 + */ switch (cmd) { - case DEVIOCGDEVICEID: - return (int)CDev::ioctl(filp, cmd, arg); - break; + + case SENSORIOCRESET: { + return _parent->reset(); + } + + case SENSORIOCSPOLLRATE: { + switch (arg) { + + /* zero would be bad */ + case 0: + return -EINVAL; + + case SENSOR_POLLRATE_DEFAULT: + return ioctl(filp, SENSORIOCSPOLLRATE, MPU9250_ACCEL_DEFAULT_RATE); + + /* adjust to a legal polling interval in Hz */ + default: + return _parent->_set_pollrate(arg); + } + } + + case ACCELIOCSSCALE: { + /* copy scale, but only if off by a few percent */ + DEVICE_DEBUG("I'm on bus %d, type %d", _parent->_interface->get_device_bus(), _parent->_device_type); + struct accel_calibration_s *s = (struct accel_calibration_s *) arg; + float sum = s->x_scale + s->y_scale + s->z_scale; + + if (sum > 2.0f && sum < 4.0f) { + memcpy(&_parent->_accel_scale, s, sizeof(_parent->_accel_scale)); + return OK; + + } else { + return -EINVAL; + } + } default: - return _parent->accel_ioctl(filp, cmd, arg); + return CDev::ioctl(filp, cmd, arg); } } diff --git a/src/drivers/imu/mpu9250/gyro.cpp b/src/drivers/imu/mpu9250/gyro.cpp index bb06a30de4fe..c5d69dd3cac5 100644 --- a/src/drivers/imu/mpu9250/gyro.cpp +++ b/src/drivers/imu/mpu9250/gyro.cpp @@ -121,11 +121,18 @@ MPU9250_gyro::ioctl(struct file *filp, int cmd, unsigned long arg) { switch (cmd) { - case DEVIOCGDEVICEID: - return (int)CDev::ioctl(filp, cmd, arg); - break; + + /* these are shared with the accel side */ + case SENSORIOCSPOLLRATE: + case SENSORIOCRESET: + return _parent->_accel->ioctl(filp, cmd, arg); + + case GYROIOCSSCALE: + /* copy scale in */ + memcpy(&_parent->_gyro_scale, (struct gyro_calibration_s *) arg, sizeof(_parent->_gyro_scale)); + return OK; default: - return _parent->gyro_ioctl(filp, cmd, arg); + return CDev::ioctl(filp, cmd, arg); } } diff --git a/src/drivers/imu/mpu9250/mag.cpp b/src/drivers/imu/mpu9250/mag.cpp index e193942ffd94..5f93d97a09b9 100644 --- a/src/drivers/imu/mpu9250/mag.cpp +++ b/src/drivers/imu/mpu9250/mag.cpp @@ -336,15 +336,31 @@ MPU9250_mag::read(struct file *filp, char *buffer, size_t buflen) int MPU9250_mag::ioctl(struct file *filp, int cmd, unsigned long arg) { + /* + * Repeated in MPU9250_accel::ioctl + * Both accel and mag CDev could be unused in case of magnetometer only mode or MPU6500 + */ + switch (cmd) { case SENSORIOCRESET: return ak8963_reset(); - case SENSORIOCSPOLLRATE: { - /* mag is polled through main driver only */ - return _parent->accel_ioctl(filp, cmd, arg); - } + case SENSORIOCSPOLLRATE: { + switch (arg) { + + /* zero would be bad */ + case 0: + return -EINVAL; + + case SENSOR_POLLRATE_DEFAULT: + return ioctl(filp, SENSORIOCSPOLLRATE, MPU9250_ACCEL_DEFAULT_RATE); + + /* adjust to a legal polling interval in Hz */ + default: + return _parent->_set_pollrate(arg); + } + } case MAGIOCSSCALE: /* copy scale in */ diff --git a/src/drivers/imu/mpu9250/mpu9250.cpp b/src/drivers/imu/mpu9250/mpu9250.cpp index 2a21e3c521f4..8b02a99b59a8 100644 --- a/src/drivers/imu/mpu9250/mpu9250.cpp +++ b/src/drivers/imu/mpu9250/mpu9250.cpp @@ -361,8 +361,6 @@ MPU9250::init() _accel_filter_y.set_cutoff_frequency(MPU9250_ACCEL_DEFAULT_RATE, accel_cut); _accel_filter_z.set_cutoff_frequency(MPU9250_ACCEL_DEFAULT_RATE, accel_cut); - } else { - PX4_ERR("IMU_ACCEL_CUTOFF param invalid"); } param_t gyro_cut_ph = param_find("IMU_GYRO_CUTOFF"); @@ -375,13 +373,17 @@ MPU9250::init() _gyro_filter_y.set_cutoff_frequency(MPU9250_GYRO_DEFAULT_RATE, gyro_cut); _gyro_filter_z.set_cutoff_frequency(MPU9250_GYRO_DEFAULT_RATE, gyro_cut); - } else { - PX4_ERR("IMU_GYRO_CUTOFF param invalid"); } /* do CDev init for the accel device node */ ret = _accel->init(); + /* if probe/setup failed, bail now */ + if (ret != OK) { + DEVICE_DEBUG("accel init failed"); + return ret; + } + /* do CDev init for the gyro device node */ ret = _gyro->init(); @@ -1004,73 +1006,6 @@ MPU9250::gyro_read(struct file *filp, char *buffer, size_t buflen) return (transferred * sizeof(sensor_gyro_s)); } -int -MPU9250::accel_ioctl(struct file *filp, int cmd, unsigned long arg) -{ - switch (cmd) { - - case SENSORIOCRESET: { - return reset(); - } - - case SENSORIOCSPOLLRATE: { - switch (arg) { - - /* zero would be bad */ - case 0: - return -EINVAL; - - /* set default polling rate */ - case SENSOR_POLLRATE_DEFAULT: - return accel_ioctl(filp, SENSORIOCSPOLLRATE, MPU9250_ACCEL_DEFAULT_RATE); - - /* adjust to a legal polling interval in Hz */ - default: - return _set_pollrate(arg); - } - } - - case ACCELIOCSSCALE: { - /* copy scale, but only if off by a few percent */ - struct accel_calibration_s *s = (struct accel_calibration_s *) arg; - float sum = s->x_scale + s->y_scale + s->z_scale; - - if (sum > 2.0f && sum < 4.0f) { - memcpy(&_accel_scale, s, sizeof(_accel_scale)); - return OK; - - } else { - return -EINVAL; - } - } - - default: - /* give it to the superclass */ - return _gyro->ioctl(filp, cmd, arg); - } -} - -int -MPU9250::gyro_ioctl(struct file *filp, int cmd, unsigned long arg) -{ - switch (cmd) { - - /* these are shared with the accel side */ - case SENSORIOCSPOLLRATE: - case SENSORIOCRESET: - return accel_ioctl(filp, cmd, arg); - - case GYROIOCSSCALE: - /* copy scale in */ - memcpy(&_gyro_scale, (struct gyro_calibration_s *) arg, sizeof(_gyro_scale)); - return OK; - - default: - /* give it to the superclass */ - return _accel->ioctl(filp, cmd, arg); - } -} - int MPU9250::select_register_bank(uint8_t bank) { @@ -1755,6 +1690,7 @@ MPU9250::print_info() perf_print_counter(_good_transfers); perf_print_counter(_reset_retries); perf_print_counter(_duplicates); + ::printf("temperature: %.1f\n", (double)_last_temperature); if (!_magnetometer_only) { _accel_reports->print_info("accel queue"); @@ -1768,7 +1704,6 @@ MPU9250::print_info() } - ::printf("temperature: %.1f\n", (double)_last_temperature); } diff --git a/src/drivers/imu/mpu9250/mpu9250.h b/src/drivers/imu/mpu9250/mpu9250.h index b57896d29226..949019c0f8e8 100644 --- a/src/drivers/imu/mpu9250/mpu9250.h +++ b/src/drivers/imu/mpu9250/mpu9250.h @@ -396,7 +396,6 @@ class MPU9250 virtual int init(); virtual ssize_t accel_read(struct file *filp, char *buffer, size_t buflen); - virtual int accel_ioctl(struct file *filp, int cmd, unsigned long arg); /** * Diagnostics - print some basic information about the driver. @@ -421,7 +420,6 @@ class MPU9250 friend class MPU9250_gyro; virtual ssize_t gyro_read(struct file *filp, char *buffer, size_t buflen); - virtual int gyro_ioctl(struct file *filp, int cmd, unsigned long arg); private: MPU9250_accel *_accel; From 0b03756f1964e76234524c070d29f8c25e697d4b Mon Sep 17 00:00:00 2001 From: Florian Olbrich Date: Wed, 14 Nov 2018 18:26:16 +0100 Subject: [PATCH 7/8] Moved start of mpu9250 as external mag to PX4FMU_V2 section. Ran code formatter. --- ROMFS/px4fmu_common/init.d/rc.sensors | 2 +- src/drivers/imu/mpu9250/mag.cpp | 24 ++++++++++++------------ 2 files changed, 13 insertions(+), 13 deletions(-) diff --git a/ROMFS/px4fmu_common/init.d/rc.sensors b/ROMFS/px4fmu_common/init.d/rc.sensors index 064fd7022512..d15365afd339 100644 --- a/ROMFS/px4fmu_common/init.d/rc.sensors +++ b/ROMFS/px4fmu_common/init.d/rc.sensors @@ -101,7 +101,7 @@ then fi # ICM20948 as external magnetometer on I2C (e.g. Here GPS) -mpu9250 -X -R 6 start +mpu9250 -X -M -R 6 start ############################################################################### # End Optional drivers # diff --git a/src/drivers/imu/mpu9250/mag.cpp b/src/drivers/imu/mpu9250/mag.cpp index 5f93d97a09b9..50112370f54e 100644 --- a/src/drivers/imu/mpu9250/mag.cpp +++ b/src/drivers/imu/mpu9250/mag.cpp @@ -346,21 +346,21 @@ MPU9250_mag::ioctl(struct file *filp, int cmd, unsigned long arg) case SENSORIOCRESET: return ak8963_reset(); - case SENSORIOCSPOLLRATE: { - switch (arg) { + case SENSORIOCSPOLLRATE: { + switch (arg) { - /* zero would be bad */ - case 0: - return -EINVAL; + /* zero would be bad */ + case 0: + return -EINVAL; - case SENSOR_POLLRATE_DEFAULT: - return ioctl(filp, SENSORIOCSPOLLRATE, MPU9250_ACCEL_DEFAULT_RATE); + case SENSOR_POLLRATE_DEFAULT: + return ioctl(filp, SENSORIOCSPOLLRATE, MPU9250_ACCEL_DEFAULT_RATE); - /* adjust to a legal polling interval in Hz */ - default: - return _parent->_set_pollrate(arg); - } - } + /* adjust to a legal polling interval in Hz */ + default: + return _parent->_set_pollrate(arg); + } + } case MAGIOCSSCALE: /* copy scale in */ From 651546be5cbb7ddb63ef84846ee7dc81bbc5844d Mon Sep 17 00:00:00 2001 From: Daniel Agar Date: Wed, 14 Nov 2018 10:58:47 -0500 Subject: [PATCH 8/8] mpu9250 cleanup to reduce code size --- src/drivers/imu/mpu9250/accel.cpp | 29 +---- src/drivers/imu/mpu9250/accel.h | 11 +- src/drivers/imu/mpu9250/gyro.cpp | 33 +---- src/drivers/imu/mpu9250/gyro.h | 10 +- src/drivers/imu/mpu9250/mag.cpp | 67 +--------- src/drivers/imu/mpu9250/mag.h | 5 - src/drivers/imu/mpu9250/main.cpp | 141 --------------------- src/drivers/imu/mpu9250/mpu9250.cpp | 187 ++-------------------------- src/drivers/imu/mpu9250/mpu9250.h | 24 +--- 9 files changed, 30 insertions(+), 477 deletions(-) diff --git a/src/drivers/imu/mpu9250/accel.cpp b/src/drivers/imu/mpu9250/accel.cpp index 58eab4b76086..230c5f006b30 100644 --- a/src/drivers/imu/mpu9250/accel.cpp +++ b/src/drivers/imu/mpu9250/accel.cpp @@ -72,10 +72,7 @@ MPU9250_accel::MPU9250_accel(MPU9250 *parent, const char *path) : CDev("MPU9250_accel", path), - _parent(parent), - _accel_topic(nullptr), - _accel_orb_class_instance(-1), - _accel_class_instance(-1) + _parent(parent) { } @@ -89,10 +86,8 @@ MPU9250_accel::~MPU9250_accel() int MPU9250_accel::init() { - int ret; - // do base class init - ret = CDev::init(); + int ret = CDev::init(); /* if probe/setup failed, bail now */ if (ret != OK) { @@ -111,12 +106,6 @@ MPU9250_accel::parent_poll_notify() poll_notify(POLLIN); } -ssize_t -MPU9250_accel::read(struct file *filp, char *buffer, size_t buflen) -{ - return _parent->accel_read(filp, buffer, buflen); -} - int MPU9250_accel::ioctl(struct file *filp, int cmd, unsigned long arg) { @@ -126,7 +115,6 @@ MPU9250_accel::ioctl(struct file *filp, int cmd, unsigned long arg) */ switch (cmd) { - case SENSORIOCRESET: { return _parent->reset(); } @@ -148,18 +136,9 @@ MPU9250_accel::ioctl(struct file *filp, int cmd, unsigned long arg) } case ACCELIOCSSCALE: { - /* copy scale, but only if off by a few percent */ - DEVICE_DEBUG("I'm on bus %d, type %d", _parent->_interface->get_device_bus(), _parent->_device_type); struct accel_calibration_s *s = (struct accel_calibration_s *) arg; - float sum = s->x_scale + s->y_scale + s->z_scale; - - if (sum > 2.0f && sum < 4.0f) { - memcpy(&_parent->_accel_scale, s, sizeof(_parent->_accel_scale)); - return OK; - - } else { - return -EINVAL; - } + memcpy(&_parent->_accel_scale, s, sizeof(_parent->_accel_scale)); + return OK; } default: diff --git a/src/drivers/imu/mpu9250/accel.h b/src/drivers/imu/mpu9250/accel.h index ec79f1c56802..fd9cc63eda62 100644 --- a/src/drivers/imu/mpu9250/accel.h +++ b/src/drivers/imu/mpu9250/accel.h @@ -44,7 +44,6 @@ class MPU9250_accel : public device::CDev MPU9250_accel(MPU9250 *parent, const char *path); ~MPU9250_accel(); - virtual ssize_t read(struct file *filp, char *buffer, size_t buflen); virtual int ioctl(struct file *filp, int cmd, unsigned long arg); virtual int init(); @@ -56,11 +55,9 @@ class MPU9250_accel : public device::CDev private: MPU9250 *_parent; - orb_advert_t _accel_topic; - int _accel_orb_class_instance; - int _accel_class_instance; - /* do not allow to copy this class due to pointer data members */ - MPU9250_accel(const MPU9250_accel &); - MPU9250_accel operator=(const MPU9250_accel &); + orb_advert_t _accel_topic{nullptr}; + int _accel_orb_class_instance{-1}; + int _accel_class_instance{-1}; + }; diff --git a/src/drivers/imu/mpu9250/gyro.cpp b/src/drivers/imu/mpu9250/gyro.cpp index c5d69dd3cac5..465bbae5aee6 100644 --- a/src/drivers/imu/mpu9250/gyro.cpp +++ b/src/drivers/imu/mpu9250/gyro.cpp @@ -42,27 +42,14 @@ */ #include - -#include -#include -#include -#include -#include -#include -#include - -#include - -#include -#include - +#include #include #include #include #include #include #include -#include +#include #include #include "mag.h" @@ -71,10 +58,7 @@ MPU9250_gyro::MPU9250_gyro(MPU9250 *parent, const char *path) : CDev("MPU9250_gyro", path), - _parent(parent), - _gyro_topic(nullptr), - _gyro_orb_class_instance(-1), - _gyro_class_instance(-1) + _parent(parent) { } @@ -88,10 +72,8 @@ MPU9250_gyro::~MPU9250_gyro() int MPU9250_gyro::init() { - int ret; - // do base class init - ret = CDev::init(); + int ret = CDev::init(); /* if probe/setup failed, bail now */ if (ret != OK) { @@ -110,16 +92,9 @@ MPU9250_gyro::parent_poll_notify() poll_notify(POLLIN); } -ssize_t -MPU9250_gyro::read(struct file *filp, char *buffer, size_t buflen) -{ - return _parent->gyro_read(filp, buffer, buflen); -} - int MPU9250_gyro::ioctl(struct file *filp, int cmd, unsigned long arg) { - switch (cmd) { /* these are shared with the accel side */ diff --git a/src/drivers/imu/mpu9250/gyro.h b/src/drivers/imu/mpu9250/gyro.h index d13304a3ed75..d24088d23f60 100644 --- a/src/drivers/imu/mpu9250/gyro.h +++ b/src/drivers/imu/mpu9250/gyro.h @@ -44,7 +44,6 @@ class MPU9250_gyro : public device::CDev MPU9250_gyro(MPU9250 *parent, const char *path); ~MPU9250_gyro(); - virtual ssize_t read(struct file *filp, char *buffer, size_t buflen); virtual int ioctl(struct file *filp, int cmd, unsigned long arg); virtual int init(); @@ -56,11 +55,8 @@ class MPU9250_gyro : public device::CDev private: MPU9250 *_parent; - orb_advert_t _gyro_topic; - int _gyro_orb_class_instance; - int _gyro_class_instance; - /* do not allow to copy this class due to pointer data members */ - MPU9250_gyro(const MPU9250_gyro &); - MPU9250_gyro operator=(const MPU9250_gyro &); + orb_advert_t _gyro_topic{nullptr}; + int _gyro_orb_class_instance{-1}; + int _gyro_class_instance{-1}; }; diff --git a/src/drivers/imu/mpu9250/mag.cpp b/src/drivers/imu/mpu9250/mag.cpp index 50112370f54e..cdbc35f3dadb 100644 --- a/src/drivers/imu/mpu9250/mag.cpp +++ b/src/drivers/imu/mpu9250/mag.cpp @@ -42,27 +42,15 @@ #include #include - -#include -#include -#include -#include -#include -#include -#include - -#include - -#include +#include #include - #include #include #include #include #include #include -#include +#include #include #include "mag.h" @@ -290,49 +278,6 @@ MPU9250_mag::_measure(struct ak8963_regs data) } } -ssize_t -MPU9250_mag::read(struct file *filp, char *buffer, size_t buflen) -{ - unsigned count = buflen / sizeof(mag_report); - - /* buffer must be large enough */ - if (count < 1) { - return -ENOSPC; - } - - /* if automatic measurement is not enabled, get a fresh measurement into the buffer */ - if (_parent->_call_interval == 0) { - _mag_reports->flush(); - /* TODO: this won't work as getting valid magnetometer - * data requires more than one measure cycle - */ - _parent->measure(); - } - - /* if no data, error (we could block here) */ - if (_mag_reports->empty()) { - return -EAGAIN; - } - - perf_count(_mag_reads); - - /* copy reports out of our buffer to the caller */ - mag_report *mrp = reinterpret_cast(buffer); - int transferred = 0; - - while (count--) { - if (!_mag_reports->get(mrp)) { - break; - } - - transferred++; - mrp++; - } - - /* return the number of bytes transferred */ - return (transferred * sizeof(mag_report)); -} - int MPU9250_mag::ioctl(struct file *filp, int cmd, unsigned long arg) { @@ -428,7 +373,6 @@ MPU9250_mag::read_reg(unsigned int reg) return buf; } - bool MPU9250_mag::ak8963_check_id(uint8_t &deviceid) { @@ -448,8 +392,6 @@ MPU9250_mag::passthrough_write(uint8_t reg, uint8_t val) _parent->write_reg(AK_MPU_OR_ICM(MPUREG_I2C_SLV0_CTRL, ICMREG_20948_I2C_SLV0_CTRL), 0); // disable new writes } - - void MPU9250_mag::write_reg(unsigned reg, uint8_t value) { @@ -462,14 +404,10 @@ MPU9250_mag::write_reg(unsigned reg, uint8_t value) } } - - - int MPU9250_mag::ak8963_reset(void) { // First initialize it to use the bus - int rv = ak8963_setup(); if (rv == OK) { @@ -481,7 +419,6 @@ MPU9250_mag::ak8963_reset(void) } return rv; - } bool diff --git a/src/drivers/imu/mpu9250/mag.h b/src/drivers/imu/mpu9250/mag.h index c6d84c593aee..2bc1d0283255 100644 --- a/src/drivers/imu/mpu9250/mag.h +++ b/src/drivers/imu/mpu9250/mag.h @@ -139,7 +139,6 @@ class MPU9250_mag : public device::CDev MPU9250_mag(MPU9250 *parent, device::Device *interface, const char *path); ~MPU9250_mag(); - virtual ssize_t read(struct file *filp, char *buffer, size_t buflen); virtual int ioctl(struct file *filp, int cmd, unsigned long arg); virtual int init(); @@ -165,15 +164,11 @@ class MPU9250_mag : public device::CDev /* Update the state with prefetched data (internally called by the regular measure() )*/ void _measure(struct ak8963_regs data); - uint8_t read_reg(unsigned reg); void write_reg(unsigned reg, uint8_t value); - bool is_passthrough() { return _interface == nullptr; } - int self_test(void); - private: MPU9250 *_parent; orb_advert_t _mag_topic; diff --git a/src/drivers/imu/mpu9250/main.cpp b/src/drivers/imu/mpu9250/main.cpp index 82b8e17b8b16..d5e05f2612f4 100644 --- a/src/drivers/imu/mpu9250/main.cpp +++ b/src/drivers/imu/mpu9250/main.cpp @@ -118,7 +118,6 @@ /** driver 'main' command */ extern "C" { __EXPORT int mpu9250_main(int argc, char *argv[]); } - enum MPU9250_BUS { MPU9250_BUS_ALL = 0, MPU9250_BUS_I2C_INTERNAL, @@ -128,7 +127,6 @@ enum MPU9250_BUS { MPU9250_BUS_SPI_EXTERNAL }; - /** * Local functions in support of the shell command. */ @@ -191,11 +189,8 @@ void start(enum MPU9250_BUS busid, enum Rotation rotation, bool external_bus, bo bool start_bus(struct mpu9250_bus_option &bus, enum Rotation rotation, bool external_bus, bool magnetometer_only); struct mpu9250_bus_option &find_bus(enum MPU9250_BUS busid); void stop(enum MPU9250_BUS busid); -void test(enum MPU9250_BUS busid); void reset(enum MPU9250_BUS busid); void info(enum MPU9250_BUS busid); -void regdump(enum MPU9250_BUS busid); -void testerror(enum MPU9250_BUS busid); void usage(); /** @@ -364,86 +359,6 @@ stop(enum MPU9250_BUS busid) exit(0); } -/** - * Perform some basic functional tests on the driver; - * make sure we can collect data from the sensor in polled - * and automatic modes. - */ -void -test(enum MPU9250_BUS busid) -{ - struct mpu9250_bus_option &bus = find_bus(busid); - sensor_accel_s a_report{}; - sensor_gyro_s g_report{}; - mag_report m_report; - ssize_t sz; - - /* get the driver */ - int fd = open(bus.accelpath, O_RDONLY); - - if (fd < 0) { - err(1, "%s open failed (try 'm start')", bus.accelpath); - } - - /* get the driver */ - int fd_gyro = open(bus.gyropath, O_RDONLY); - - if (fd_gyro < 0) { - err(1, "%s open failed", bus.gyropath); - } - - /* get the driver */ - int fd_mag = open(bus.magpath, O_RDONLY); - - if (fd_mag < 0) { - err(1, "%s open failed", bus.magpath); - } - - /* do a simple demand read */ - sz = read(fd, &a_report, sizeof(a_report)); - - if (sz != sizeof(a_report)) { - warnx("ret: %d, expected: %d", sz, sizeof(a_report)); - err(1, "immediate acc read failed"); - } - - print_message(a_report); - - /* do a simple demand read */ - sz = read(fd_gyro, &g_report, sizeof(g_report)); - - if (sz != sizeof(g_report)) { - warnx("ret: %d, expected: %d", sz, sizeof(g_report)); - err(1, "immediate gyro read failed"); - } - - print_message(g_report); - - /* do a simple demand read */ - sz = read(fd_mag, &m_report, sizeof(m_report)); - - if (sz != sizeof(m_report)) { - warnx("ret: %d, expected: %d", sz, sizeof(m_report)); - err(1, "immediate mag read failed"); - } - - print_message(m_report); - - /* reset to default polling */ - if (ioctl(fd, SENSORIOCSPOLLRATE, SENSOR_POLLRATE_DEFAULT) < 0) { - err(1, "reset to default polling"); - } - - close(fd); - close(fd_gyro); - close(fd_mag); - - /* XXX add poll-rate tests here too */ - - reset(busid); - errx(0, "PASS"); -} - /** * Reset the driver. */ @@ -489,43 +404,6 @@ info(enum MPU9250_BUS busid) exit(0); } -/** - * Dump the register information - */ -void -regdump(enum MPU9250_BUS busid) -{ - struct mpu9250_bus_option &bus = find_bus(busid); - - - if (bus.dev == nullptr) { - errx(1, "driver not running"); - } - - printf("regdump @ %p\n", bus.dev); - bus.dev->print_registers(); - - exit(0); -} - -/** - * deliberately produce an error to test recovery - */ -void -testerror(enum MPU9250_BUS busid) -{ - struct mpu9250_bus_option &bus = find_bus(busid); - - - if (bus.dev == nullptr) { - errx(1, "driver not running"); - } - - bus.dev->test_error(); - - exit(0); -} - void usage() { @@ -538,7 +416,6 @@ usage() PX4_INFO(" -t (spi internal bus, 2nd instance)"); PX4_INFO(" -R rotation"); PX4_INFO(" -M only enable magnetometer, accel/gyro disabled - not av. on MPU6500"); - } } // namespace @@ -609,13 +486,6 @@ mpu9250_main(int argc, char *argv[]) mpu9250::stop(busid); } - /* - * Test the driver/device. - */ - if (!strcmp(verb, "test")) { - mpu9250::test(busid); - } - /* * Reset the driver. */ @@ -630,17 +500,6 @@ mpu9250_main(int argc, char *argv[]) mpu9250::info(busid); } - /* - * Print register information. - */ - if (!strcmp(verb, "regdump")) { - mpu9250::regdump(busid); - } - - if (!strcmp(verb, "testerror")) { - mpu9250::testerror(busid); - } - mpu9250::usage(); return 0; } diff --git a/src/drivers/imu/mpu9250/mpu9250.cpp b/src/drivers/imu/mpu9250/mpu9250.cpp index 8b02a99b59a8..1c5792371e9b 100644 --- a/src/drivers/imu/mpu9250/mpu9250.cpp +++ b/src/drivers/imu/mpu9250/mpu9250.cpp @@ -42,40 +42,18 @@ */ #include -#include - -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include - -#include -#include +#include +#include #include #include - -#include -#include - -#include #include - #include #include #include #include #include #include -#include +#include #include #include "mag.h" @@ -136,7 +114,6 @@ MPU9250::MPU9250(device::Device *interface, device::Device *mag_interface, const int device_type, bool magnetometer_only) : _interface(interface), - _name("MPU9250"), _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)), @@ -191,9 +168,6 @@ MPU9250::MPU9250(device::Device *interface, device::Device *mag_interface, const _last_accel_data{}, _got_duplicate(false) { - // disable debug() calls - _debug_enabled = false; - if (_accel != nullptr) { /* Set device parameters and make sure parameters of the bus device are adopted */ _accel->_device_id.devid_s.devtype = DRV_ACC_DEVTYPE_MPU9250; @@ -237,11 +211,6 @@ MPU9250::MPU9250(device::Device *interface, device::Device *mag_interface, const _gyro_scale.y_scale = 1.0f; _gyro_scale.z_offset = 0; _gyro_scale.z_scale = 1.0f; - - memset(&_call, 0, sizeof(_call)); -#if defined(USE_I2C) - memset(&_work, 0, sizeof(_work)); -#endif } MPU9250::~MPU9250() @@ -307,7 +276,7 @@ MPU9250::init() int ret = probe(); if (ret != OK) { - DEVICE_DEBUG("MPU9250 probe failed"); + PX4_DEBUG("MPU9250 probe failed"); return ret; } @@ -380,7 +349,7 @@ MPU9250::init() /* if probe/setup failed, bail now */ if (ret != OK) { - DEVICE_DEBUG("accel init failed"); + PX4_DEBUG("accel init failed"); return ret; } @@ -389,7 +358,7 @@ MPU9250::init() /* if probe/setup failed, bail now */ if (ret != OK) { - DEVICE_DEBUG("gyro init failed"); + PX4_DEBUG("gyro init failed"); return ret; } } @@ -412,15 +381,14 @@ MPU9250::init() /* if probe/setup failed, bail now */ if (ret != OK) { - DEVICE_DEBUG("mag init failed"); + PX4_DEBUG("mag init failed"); return ret; } - ret = _mag->ak8963_reset(); if (ret != OK) { - DEVICE_DEBUG("mag reset failed"); + PX4_DEBUG("mag reset failed"); return ret; } } @@ -652,7 +620,7 @@ MPU9250::probe() _checked_bad[0] = _whoami; if (ret != OK) { - DEVICE_DEBUG("unexpected whoami 0x%02x", _whoami); + PX4_DEBUG("unexpected whoami 0x%02x", _whoami); } return ret; @@ -767,7 +735,6 @@ MPU9250::_set_dlpf_filter(uint16_t frequency_hz) { uint8_t filter; - switch (_device_type) { case MPU_DEVICE_TYPE_MPU9250: case MPU_DEVICE_TYPE_MPU6500: @@ -900,112 +867,6 @@ MPU9250::_set_dlpf_filter(uint16_t frequency_hz) } } -ssize_t -MPU9250::accel_read(struct file *filp, char *buffer, size_t buflen) -{ - unsigned count = buflen / sizeof(sensor_accel_s); - - /* buffer must be large enough */ - if (count < 1) { - return -ENOSPC; - } - - /* if automatic measurement is not enabled, get a fresh measurement into the buffer */ - if (_call_interval == 0) { - _accel_reports->flush(); - measure(); - } - - /* if no data, error (we could block here) */ - if (_accel_reports->empty()) { - return -EAGAIN; - } - - perf_count(_accel_reads); - - /* copy reports out of our buffer to the caller */ - sensor_accel_s *arp = reinterpret_cast(buffer); - int transferred = 0; - - while (count--) { - if (!_accel_reports->get(arp)) { - break; - } - - transferred++; - arp++; - } - - /* return the number of bytes transferred */ - return (transferred * sizeof(sensor_accel_s)); -} - -int -MPU9250::self_test() -{ - if (perf_event_count(_sample_perf) == 0) { - measure(); - } - - /* return 0 on success, 1 else */ - return (perf_event_count(_sample_perf) > 0) ? 0 : 1; -} - -/* - deliberately trigger an error in the sensor to trigger recovery - */ -void -MPU9250::test_error() -{ - // deliberately trigger an error. This was noticed during - // development as a handy way to test the reset logic - uint8_t data[16]; - memset(data, 0, sizeof(data)); - _interface->read(MPU9250_SET_SPEED(MPUREG_INT_STATUS, MPU9250_LOW_BUS_SPEED), data, sizeof(data)); - ::printf("error triggered\n"); - print_registers(); -} - -ssize_t -MPU9250::gyro_read(struct file *filp, char *buffer, size_t buflen) -{ - unsigned count = buflen / sizeof(sensor_gyro_s); - - /* buffer must be large enough */ - if (count < 1) { - return -ENOSPC; - } - - /* if automatic measurement is not enabled, get a fresh measurement into the buffer */ - if (_call_interval == 0) { - _gyro_reports->flush(); - measure(); - } - - /* if no data, error (we could block here) */ - if (_gyro_reports->empty()) { - return -EAGAIN; - } - - perf_count(_gyro_reads); - - /* copy reports out of our buffer to the caller */ - sensor_gyro_s *grp = reinterpret_cast(buffer); - int transferred = 0; - - while (count--) { - if (!_gyro_reports->get(grp)) { - break; - } - - transferred++; - grp++; - } - - /* return the number of bytes transferred */ - return (transferred * sizeof(sensor_gyro_s)); -} - int MPU9250::select_register_bank(uint8_t bank) { @@ -1045,7 +906,7 @@ MPU9250::select_register_bank(uint8_t bank) _selected_bank = bank; if (bank != buf) { - DEVICE_DEBUG("SELECT FAILED %d %d %d %d", retries, _selected_bank, bank, buf); + PX4_DEBUG("SELECT FAILED %d %d %d %d", retries, _selected_bank, bank, buf); return PX4_ERROR; } else { @@ -1610,8 +1471,6 @@ MPU9250::measure() arb.scaling = _accel_range_scale; - - arb.temperature = _last_temperature; /* return device ID */ @@ -1696,31 +1555,5 @@ MPU9250::print_info() _accel_reports->print_info("accel queue"); _gyro_reports->print_info("gyro queue"); _mag->_mag_reports->print_info("mag queue"); - - float accel_cut = _accel_filter_x.get_cutoff_freq(); - ::printf("accel cutoff set to %10.2f Hz\n", double(accel_cut)); - float gyro_cut = _gyro_filter_x.get_cutoff_freq(); - ::printf("gyro cutoff set to %10.2f Hz\n", double(gyro_cut)); - - } - - - -} - -void -MPU9250::print_registers() -{ - printf("MPU9250 registers\n"); - - for (uint8_t reg = 0; reg <= 126; reg++) { - uint8_t v = read_reg(reg); - printf("%02x:%02x ", (unsigned)reg, (unsigned)v); - - if (reg % 13 == 0) { - printf("\n"); - } } - - printf("\n"); } diff --git a/src/drivers/imu/mpu9250/mpu9250.h b/src/drivers/imu/mpu9250/mpu9250.h index 949019c0f8e8..81c6f50f0731 100644 --- a/src/drivers/imu/mpu9250/mpu9250.h +++ b/src/drivers/imu/mpu9250/mpu9250.h @@ -391,36 +391,25 @@ class MPU9250 enum Rotation rotation, int device_type, bool magnetometer_only); + virtual ~MPU9250(); virtual int init(); - virtual ssize_t accel_read(struct file *filp, char *buffer, size_t buflen); - /** * Diagnostics - print some basic information about the driver. */ void print_info(); - void print_registers(); - - // deliberately cause a sensor error - void test_error(); - protected: device::Device *_interface; - const char *_name; - bool _debug_enabled{false}; - virtual int probe(); friend class MPU9250_accel; friend class MPU9250_mag; friend class MPU9250_gyro; - virtual ssize_t gyro_read(struct file *filp, char *buffer, size_t buflen); - private: MPU9250_accel *_accel; MPU9250_gyro *_gyro; @@ -436,11 +425,11 @@ class MPU9250 * SPI bus based device use hrt * I2C bus needs to use work queue */ - work_s _work; + work_s _work{}; #endif bool _use_hrt; - struct hrt_call _call; + struct hrt_call _call {}; unsigned _call_interval; ringbuffer::RingBuffer *_accel_reports; @@ -681,13 +670,6 @@ class MPU9250 */ bool is_external() { return _interface->external(); } - /** - * Measurement self test - * - * @return 0 on success, 1 on failure - */ - int self_test(); - /* set low pass filter frequency */