Added support for ICM-20608-G to MPU6000 driver

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;
+	uint8_t whoami = read_reg(MPUREG_WHOAMI);
-	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")) {