MPU9250: Separate mpu and mag resets

src/drivers/mpu9250/mpu9250.cpp

@@ -292,7 +292,7 @@ MPU9250::init()
 		goto out;
 	}
 
-	if (reset() != OK) {
+	if (reset_mpu() != OK) {
 		PX4_ERR("Exiting! Device failed to take initialization");
 		goto out;
 	}
@@ -340,6 +340,17 @@ MPU9250::init()
 		return ret;
 	}
 
+
+	if (_whoami == MPU_WHOAMI_9250) {
+		ret = _mag->ak8963_reset();
+	}
+
+	if (ret != OK) {
+		DEVICE_DEBUG("mag reset failed");
+		return ret;
+	}
+
+
 	_accel_class_instance = register_class_devname(ACCEL_BASE_DEVICE_PATH);
 
 	measure();
@@ -378,13 +389,31 @@ int MPU9250::reset()
 	// Hold off sampling until done (100 MS will be shortened)
 	state = px4_enter_critical_section();
 	_reset_wait = hrt_absolute_time() + 100000;
+	px4_leave_critical_section(state);
 
+	int ret;
+
+	ret = reset_mpu();
+
+	if (ret == OK && _whoami == MPU_WHOAMI_9250) {
+		ret = _mag->ak8963_reset();
+	}
+
+
+	state = px4_enter_critical_section();
+	_reset_wait = hrt_absolute_time() + 10;
+	px4_leave_critical_section(state);
+
+	return ret;
+}
+
+int MPU9250::reset_mpu()
+{
 	write_reg(MPUREG_PWR_MGMT_1, BIT_H_RESET);
 
 	write_checked_reg(MPUREG_PWR_MGMT_1, MPU_CLK_SEL_AUTO);
 	write_checked_reg(MPUREG_PWR_MGMT_2, 0);
 
-	px4_leave_critical_section(state);
 
 	usleep(1000);
 
@@ -453,15 +482,6 @@ int MPU9250::reset()
 		}
 	}
 
-
-	if (all_ok && _whoami == MPU_WHOAMI_9250) {
-		all_ok = _mag->ak8963_reset() == OK;
-	}
-
-	state = px4_enter_critical_section();
-	_reset_wait = hrt_absolute_time() + 10;
-	px4_leave_critical_section(state);
-
 	return all_ok ? OK : -EIO;
 }
 

src/drivers/mpu9250/mpu9250.h

@@ -375,6 +375,11 @@ private:
 	int			reset();
 
 
+	/**
+	 * Resets the main chip (excluding the magnetometer if any).
+	 */
+	int			reset_mpu();
+
 
 #if defined(USE_I2C)
 	/**