From fbf6532c25a261c00b69b89dd67e4714983218bb Mon Sep 17 00:00:00 2001
From: David Sidrane <david_s5@nscdg.com>
Date: Fri, 7 Jul 2017 16:32:18 -1000
Subject: [PATCH] mpu9250:reset the mag on the reset operation

   This commit fixed a bug were the mag was orphened on a reset.
   That resulted in MAG timeouts on reset or test operations and
   left the mag in a broken state.
---
 src/drivers/mpu9250/mag.cpp     | 23 ++++++++++++++++-------
 src/drivers/mpu9250/mpu9250.cpp | 11 +++++++++--
 2 files changed, 25 insertions(+), 9 deletions(-)

diff --git a/src/drivers/mpu9250/mag.cpp b/src/drivers/mpu9250/mag.cpp
index fe43891f95..1026429927 100644
--- a/src/drivers/mpu9250/mag.cpp
+++ b/src/drivers/mpu9250/mag.cpp
@@ -297,11 +297,7 @@ MPU9250_mag::ioctl(struct file *filp, int cmd, unsigned long arg)
 	switch (cmd) {
 
 	case SENSORIOCRESET:
-		/*
-		 * TODO: we could implement a reset of the AK8963 registers
-		 */
-		//return reset();
-		return _parent->ioctl(filp, cmd, arg);
+		return ak8963_reset();
 
 	case SENSORIOCSPOLLRATE: {
 			switch (arg) {
@@ -508,10 +504,23 @@ MPU9250_mag::write_reg(unsigned reg, uint8_t value)
 
 
 
-void
+int
 MPU9250_mag::ak8963_reset(void)
 {
-	write_reg(AK8963REG_CNTL2, AK8963_RESET);
+	// First initialize it to use the bus
+
+	int rv = ak8963_setup();
+
+	if (rv == OK) {
+
+		// Now reset the mag
+		write_reg(AK8963REG_CNTL2, AK8963_RESET);
+		// Then re-initialize the bus/mag
+		rv = ak8963_setup();
+	}
+
+	return rv;
+
 }
 
 bool
diff --git a/src/drivers/mpu9250/mpu9250.cpp b/src/drivers/mpu9250/mpu9250.cpp
index 3a09df1de6..f52e3c8f44 100644
--- a/src/drivers/mpu9250/mpu9250.cpp
+++ b/src/drivers/mpu9250/mpu9250.cpp
@@ -720,8 +720,15 @@ MPU9250::ioctl(struct file *filp, int cmd, unsigned long arg)
 {
 	switch (cmd) {
 
-	case SENSORIOCRESET:
-		return reset();
+	case SENSORIOCRESET: {
+			int ret = reset();
+
+			if (_whoami == MPU_WHOAMI_9250) {
+				return (ret == OK ? _mag->ioctl(filp, cmd, arg) : ret);
+			}
+
+			return ret;
+		}
 
 	case SENSORIOCSPOLLRATE: {
 			switch (arg) {