diff --git a/src/drivers/mpu6000/mpu6000.cpp b/src/drivers/mpu6000/mpu6000.cpp index ab2eede349..0c902e2739 100644 --- a/src/drivers/mpu6000/mpu6000.cpp +++ b/src/drivers/mpu6000/mpu6000.cpp @@ -149,6 +149,12 @@ #define BIT_INT_STATUS_DATA 0x01 #define MPU_WHOAMI_6000 0x68 +#define ICM_WHOAMI_20608 0xaf + +// Product ID Description for ICM2608 +// There is none + +#define ICM20608_REV_00 0 // Product ID Description for MPU6000 // high 4 bits low 4 bits @@ -208,7 +214,8 @@ class MPU6000_gyro; class MPU6000 : public device::SPI { public: - MPU6000(int bus, const char *path_accel, const char *path_gyro, spi_dev_e device, enum Rotation rotation); + MPU6000(int bus, const char *path_accel, const char *path_gyro, spi_dev_e device, enum Rotation rotation, + int device_type); virtual ~MPU6000(); virtual int init(); @@ -242,6 +249,7 @@ protected: virtual int gyro_ioctl(struct file *filp, int cmd, unsigned long arg); private: + int _device_type; MPU6000_gyro *_gyro; uint8_t _product; /** product code */ @@ -326,6 +334,15 @@ private: */ int reset(); + /** + * is_icm_device + */ + bool is_icm_device() { return _device_type == 20608;} + /** + * is_mpu_device + */ + bool is_mpu_device() { return _device_type == 6000;} + /** * Static trampoline from the hrt_call context; because we don't have a * generic hrt wrapper yet. @@ -507,8 +524,10 @@ private: /** driver 'main' command */ extern "C" { __EXPORT int mpu6000_main(int argc, char *argv[]); } -MPU6000::MPU6000(int bus, const char *path_accel, const char *path_gyro, spi_dev_e device, enum Rotation rotation) : +MPU6000::MPU6000(int bus, const char *path_accel, const char *path_gyro, spi_dev_e device, enum Rotation rotation, + int device_type) : SPI("MPU6000", path_accel, bus, device, SPIDEV_MODE3, MPU6000_LOW_BUS_SPEED), + _device_type(device_type), _gyro(new MPU6000_gyro(this, path_gyro)), _product(0), _call{}, @@ -780,16 +799,15 @@ int MPU6000::reset() int MPU6000::probe() { - uint8_t whoami; - whoami = read_reg(MPUREG_WHOAMI); + uint8_t whoami = read_reg(MPUREG_WHOAMI); + uint8_t expected = is_mpu_device() ? MPU_WHOAMI_6000 : ICM_WHOAMI_20608; - if (whoami != MPU_WHOAMI_6000) { + if (whoami != expected) { DEVICE_DEBUG("unexpected WHOAMI 0x%02x", whoami); return -EIO; - } - /* look for a product ID we recognise */ + /* look for a product ID we recognize */ _product = read_reg(MPUREG_PRODUCT_ID); // verify product revision @@ -806,6 +824,7 @@ MPU6000::probe() case MPU6000_REV_D8: case MPU6000_REV_D9: case MPU6000_REV_D10: + case ICM20608_REV_00: DEVICE_DEBUG("ID 0x%02x", _product); _checked_values[0] = _product; return OK; @@ -1485,6 +1504,13 @@ MPU6000::read_reg(unsigned reg, uint32_t speed) { uint8_t cmd[2] = { (uint8_t)(reg | DIR_READ), 0}; + // There is no MPUREG_PRODUCT_ID on the icm device + // so lets make dummy it up and allow the rest of the + // code to run as is + if (reg == MPUREG_PRODUCT_ID && is_icm_device()) { + return ICM20608_REV_00; + } + // general register transfer at low clock speed set_frequency(speed); @@ -2054,7 +2080,7 @@ namespace mpu6000 MPU6000 *g_dev_int; // on internal bus MPU6000 *g_dev_ext; // on external bus -void start(bool, enum Rotation, int range); +void start(bool, enum Rotation, int range, int device_type); void stop(bool); void test(bool); void reset(bool); @@ -2071,7 +2097,7 @@ void usage(); * or failed to detect the sensor. */ void -start(bool external_bus, enum Rotation rotation, int range) +start(bool external_bus, enum Rotation rotation, int range, int device_type) { int fd; MPU6000 **g_dev_ptr = external_bus ? &g_dev_ext : &g_dev_int; @@ -2087,13 +2113,23 @@ start(bool external_bus, enum Rotation rotation, int range) /* create the driver */ if (external_bus) { #ifdef PX4_SPI_BUS_EXT - *g_dev_ptr = new MPU6000(PX4_SPI_BUS_EXT, path_accel, path_gyro, (spi_dev_e)PX4_SPIDEV_EXT_MPU, rotation); +# if defined(PX4_SPIDEV_EXT_ICM) + spi_dev_e cs = (spi_dev_e)(device_type == 6000 ? PX4_SPIDEV_EXT_MPU : PX4_SPIDEV_EXT_ICM); +# else + spi_dev_e cs = (spi_dev_e) PX4_SPIDEV_EXT_MPU; +# endif + *g_dev_ptr = new MPU6000(PX4_SPI_BUS_EXT, path_accel, path_gyro, cs, rotation, device_type); #else errx(0, "External SPI not available"); #endif } else { - *g_dev_ptr = new MPU6000(PX4_SPI_BUS_SENSORS, path_accel, path_gyro, (spi_dev_e)PX4_SPIDEV_MPU, rotation); +#if defined(PX4_SPIDEV_ICM) + spi_dev_e cs = (spi_dev_e)(device_type == 6000 ? PX4_SPIDEV_MPU : PX4_SPIDEV_ICM); +#else + spi_dev_e cs = (spi_dev_e) PX4_SPIDEV_MPU; +#endif + *g_dev_ptr = new MPU6000(PX4_SPI_BUS_SENSORS, path_accel, path_gyro, cs, rotation, device_type); } if (*g_dev_ptr == nullptr) { @@ -2337,6 +2373,7 @@ usage() warnx("missing command: try 'start', 'info', 'test', 'stop',\n'reset', 'regdump', 'factorytest', 'testerror'"); warnx("options:"); warnx(" -X (external bus)"); + warnx(" -M 6000|20608 (default 6000)"); warnx(" -R rotation"); warnx(" -a accel range (in g)"); } @@ -2347,17 +2384,22 @@ int mpu6000_main(int argc, char *argv[]) { bool external_bus = false; + int device_type = 6000; int ch; enum Rotation rotation = ROTATION_NONE; int accel_range = 8; /* jump over start/off/etc and look at options first */ - while ((ch = getopt(argc, argv, "XR:a:")) != EOF) { + while ((ch = getopt(argc, argv, "T:XR:a:")) != EOF) { switch (ch) { case 'X': external_bus = true; break; + case 'T': + device_type = atoi(optarg); + break; + case 'R': rotation = (enum Rotation)atoi(optarg); break; @@ -2379,7 +2421,7 @@ mpu6000_main(int argc, char *argv[]) */ if (!strcmp(verb, "start")) { - mpu6000::start(external_bus, rotation, accel_range); + mpu6000::start(external_bus, rotation, accel_range, device_type); } if (!strcmp(verb, "stop")) {