From 3200b032c0fe693e93c145e6336819f3ec7fc06e Mon Sep 17 00:00:00 2001
From: Lucas De Marchi <lucas.demarchi@intel.com>
Date: Tue, 14 Mar 2017 23:12:43 -0700
Subject: [PATCH] mpu9250: add support to MPU6500

MPU9250 is mostly an MPU6500 with a mag (AK8963) in the same package.
Support driving MPU6500 with the MPU9250 driver. The id of the driver
isn't set differently since this way it allows to force a recalibration.

Ideally MPU9250 driver could even not exist and the support for these
sensors be merged back in the MPU6000 that's more complete. This is an
intermediate step in that direction.
---
 src/drivers/mpu9250/mpu9250.cpp     |  5 ++++-
 src/drivers/mpu9250/mpu9250.h       |  1 +
 src/drivers/mpu9250/mpu9250_spi.cpp | 21 +++++++++++++++++++--
 3 files changed, 24 insertions(+), 3 deletions(-)

diff --git a/src/drivers/mpu9250/mpu9250.cpp b/src/drivers/mpu9250/mpu9250.cpp
index aee16fe32e..d192ddaa1b 100644
--- a/src/drivers/mpu9250/mpu9250.cpp
+++ b/src/drivers/mpu9250/mpu9250.cpp
@@ -328,7 +328,9 @@ MPU9250::init()
 #endif
 
 	/* do CDev init for the mag device node, keep it optional */
-	ret = _mag->init();
+	if (_whoami == MPU_WHOAMI_9250) {
+		ret = _mag->init();
+	}
 
 	/* if probe/setup failed, bail now */
 	if (ret != OK) {
@@ -450,6 +452,7 @@ MPU9250::probe()
 	// 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;
diff --git a/src/drivers/mpu9250/mpu9250.h b/src/drivers/mpu9250/mpu9250.h
index 564a2c3bf8..da3daf8934 100644
--- a/src/drivers/mpu9250/mpu9250.h
+++ b/src/drivers/mpu9250/mpu9250.h
@@ -178,6 +178,7 @@
 #define BIT_I2C_SLV3_DLY_EN         0x08
 
 #define MPU_WHOAMI_9250			0x71
+#define MPU_WHOAMI_6500			0x70
 
 #define MPU9250_ACCEL_DEFAULT_RATE	1000
 #define MPU9250_ACCEL_MAX_OUTPUT_RATE			280
diff --git a/src/drivers/mpu9250/mpu9250_spi.cpp b/src/drivers/mpu9250/mpu9250_spi.cpp
index 5ce17e380c..39c4daf45a 100644
--- a/src/drivers/mpu9250/mpu9250_spi.cpp
+++ b/src/drivers/mpu9250/mpu9250_spi.cpp
@@ -265,8 +265,25 @@ int
 MPU9250_SPI::probe()
 {
 	uint8_t whoami = 0;
-	uint8_t expected = MPU_WHOAMI_9250;
-	return (read(MPUREG_WHOAMI, &whoami, 1) == OK && (whoami == expected)) ? 0 : -EIO;
+
+	int ret = read(MPUREG_WHOAMI, &whoami, 1);
+
+	if (ret != OK) {
+		return -EIO;
+	}
+
+	switch (whoami) {
+	case MPU_WHOAMI_9250:
+	case MPU_WHOAMI_6500:
+		ret = 0;
+		break;
+
+	default:
+		PX4_WARN("probe failed! %u", whoami);
+		ret = -EIO;
+	}
+
+	return ret;
 }
 
 #endif // PX4_SPIDEV_MPU