diff --git a/src/drivers/mpu9250/mag.cpp b/src/drivers/mpu9250/mag.cpp
index ec35f5bde5..eacf37d99e 100644
--- a/src/drivers/mpu9250/mag.cpp
+++ b/src/drivers/mpu9250/mag.cpp
@@ -41,6 +41,7 @@
  */
 
 #include <px4_config.h>
+#include <px4_log.h>
 
 #include <sys/types.h>
 #include <stdint.h>
@@ -470,18 +471,11 @@ MPU9250_mag::read_reg(unsigned int reg)
 
 
 bool
-MPU9250_mag::ak8963_check_id(void)
+MPU9250_mag::ak8963_check_id(uint8_t &deviceid)
 {
-	for (int i = 0; i < 5; i++) {
+	deviceid = read_reg(AK8963REG_WIA);
 
-		uint8_t deviceid = read_reg(AK8963REG_WIA);
-
-		if (AK8963_DEVICE_ID == deviceid) {
-			return true;
-		}
-	}
-
-	return false;
+	return (AK8963_DEVICE_ID == deviceid);
 }
 
 /*
@@ -552,42 +546,71 @@ MPU9250_mag::ak8963_read_adjustments(void)
 	return true;
 }
 
-bool
+int
 MPU9250_mag::ak8963_setup(void)
 {
 	int retries = 10;
 
-	/* Configures the parent to act in master mode */
-	if (_interface == nullptr) {
-		uint8_t user_ctrl = _parent->read_reg(MPUREG_USER_CTRL);
-		_parent->write_checked_reg(MPUREG_USER_CTRL, user_ctrl | 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);
+	do {
 
-	} else {
-		//	uint8_t user_ctrl = _parent->read_reg(MPUREG_USER_CTRL);
+		/* When _interface is null we are using SPI and must
+		 * use the parent interface to configure the device to act
+		 * 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);
 
-		//	// Passthrough mode
-		//	_parent->write_checked_reg(MPUREG_USER_CTRL, user_ctrl & (~BIT_I2C_MST_EN));
-	}
+		} else {
+			_parent->modify_checked_reg(MPUREG_USER_CTRL, BIT_I2C_MST_EN, 0);
+		}
 
-	if (!ak8963_check_id()) {
-		::printf("AK8963: bad id\n");
-	}
+		uint8_t id = 0;
 
-	while (!ak8963_read_adjustments()) {
-		if (!retries--) {
-			::printf("AK8963: failed to read adjustment data\n");
+		if (ak8963_check_id(id)) {
 			break;
 		}
+
+		retries--;
+
+		if (retries < 8) {
+			PX4_ERR("AK8963: bad id %d retries %d", id, retries);
+			_parent->modify_checked_reg(MPUREG_USER_CTRL, 0, BIT_I2C_MST_RST);
+			up_udelay(100);
+		}
+	} while (retries > 0);
+
+	if (retries > 0) {
+		retries = 10;
+
+		while (!ak8963_read_adjustments() && retries) {
+			retries--;
+			PX4_ERR("AK8963: failed to read adjustment data. Retries %d", retries);
+
+			if (retries == 5) {
+				_parent->modify_checked_reg(MPUREG_USER_CTRL, 0, BIT_I2C_MST_RST);
+				up_udelay(100);
+			}
+		}
+	}
+
+	if (retries == 0) {
+		PX4_ERR("AK8963: failed to initialize, disabled!");
+		_parent->modify_checked_reg(MPUREG_USER_CTRL, 0, BIT_I2C_MST_RST);
+		_parent->write_reg(MPUREG_I2C_MST_CTRL, 0);
+		return -EIO;
 	}
 
 	write_reg(AK8963REG_CNTL1, AK8963_CONTINUOUS_MODE2 | AK8963_16BIT_ADC);
 
 
 	if (_interface == NULL) {
-		/* Configure mpu to internally read ak8963 data  */
+
+		/* Configure mpu' I2c Master interface to read ak8963 data
+		 * Into to fifo
+		 */
 		set_passthrough(AK8963REG_ST1, sizeof(struct ak8963_regs));
 	}
 
-	return true;
+	return OK;
 }
diff --git a/src/drivers/mpu9250/mag.h b/src/drivers/mpu9250/mag.h
index 8790ca1412..ea14db299b 100644
--- a/src/drivers/mpu9250/mag.h
+++ b/src/drivers/mpu9250/mag.h
@@ -104,9 +104,9 @@ public:
 	void passthrough_write(uint8_t reg, uint8_t val);
 	void read_block(uint8_t reg, uint8_t *val, uint8_t count);
 
-	void ak8963_reset(void);
-	bool ak8963_setup(void);
-	bool ak8963_check_id(void);
+	int ak8963_reset(void);
+	int ak8963_setup(void);
+	bool ak8963_check_id(uint8_t &id);
 	bool ak8963_read_adjustments(void);
 
 protected:
diff --git a/src/drivers/mpu9250/mpu9250.cpp b/src/drivers/mpu9250/mpu9250.cpp
index 58f98e01e3..3a09df1de6 100644
--- a/src/drivers/mpu9250/mpu9250.cpp
+++ b/src/drivers/mpu9250/mpu9250.cpp
@@ -388,6 +388,10 @@ int MPU9250::reset()
 
 	usleep(1000);
 
+	// Enable I2C bus or Disable I2C bus (recommended on data sheet)
+
+	write_checked_reg(MPUREG_USER_CTRL, is_i2c() ? 0 : BIT_I2C_IF_DIS);
+
 	// SAMPLE RATE
 	_set_sample_rate(_sample_rate);