diff --git a/boards/px4/fmu-v3/init/rc.board b/boards/px4/fmu-v3/init/rc.board
index 0e168c04e0..d1404187d2 100644
--- a/boards/px4/fmu-v3/init/rc.board
+++ b/boards/px4/fmu-v3/init/rc.board
@@ -116,7 +116,7 @@ then
 	if [ $BOARD_FMUV3 = 21 ]
 	then
 		# v2.1 internal MPU9250 is rotated 180 deg roll, 270 deg yaw
-		mpu9250 -R 14 start
+		mpu9250 -s -R 14 start
 	fi
 
 else
diff --git a/src/drivers/imu/mpu9250/main.cpp b/src/drivers/imu/mpu9250/main.cpp
index 0d5704b2fa..f508b2fb74 100644
--- a/src/drivers/imu/mpu9250/main.cpp
+++ b/src/drivers/imu/mpu9250/main.cpp
@@ -264,7 +264,17 @@ start_bus(struct mpu9250_bus_option &bus, enum Rotation rotation, bool external,
 		goto fail;
 	}
 
-	fd = open(bus.accelpath, O_RDONLY);
+	/*
+	 * Set the poll rate to default, starts automatic data collection.
+	 * Doing this through the mag device for the time being - it's always there, even in magnetometer only mode.
+	 * Using accel device for MPU6500.
+	 */
+	if (bus.dev->get_whoami() == MPU_WHOAMI_6500) {
+		fd = open(bus.accelpath, O_RDONLY);
+
+	} else {
+		fd = open(bus.magpath, O_RDONLY);
+	}
 
 	if (fd < 0) {
 		PX4_INFO("ioctl failed");
diff --git a/src/drivers/imu/mpu9250/mpu9250.cpp b/src/drivers/imu/mpu9250/mpu9250.cpp
index 8fbe7be283..eee5b57e52 100644
--- a/src/drivers/imu/mpu9250/mpu9250.cpp
+++ b/src/drivers/imu/mpu9250/mpu9250.cpp
@@ -421,6 +421,11 @@ MPU9250::init()
 	return ret;
 }
 
+uint8_t MPU9250::get_whoami()
+{
+	return _whoami;
+}
+
 int MPU9250::reset()
 {
 	irqstate_t state;
@@ -574,6 +579,8 @@ MPU9250::probe()
 
 	// If it's not an MPU it must be an ICM
 	if ((_whoami != MPU_WHOAMI_9250) && (_whoami != MPU_WHOAMI_6500)) {
+		// Make sure selected register bank is bank 0 (which contains WHOAMI)
+		select_register_bank(REG_BANK(ICMREG_20948_WHOAMI));
 		_whoami = read_reg(ICMREG_20948_WHOAMI);
 	}
 
diff --git a/src/drivers/imu/mpu9250/mpu9250.h b/src/drivers/imu/mpu9250/mpu9250.h
index 3cd1ab9f71..718729758c 100644
--- a/src/drivers/imu/mpu9250/mpu9250.h
+++ b/src/drivers/imu/mpu9250/mpu9250.h
@@ -386,6 +386,7 @@ public:
 	virtual ~MPU9250();
 
 	virtual int		init();
+	uint8_t			get_whoami();
 
 	/**
 	 * Diagnostics - print some basic information about the driver.