diff --git a/libraries/AP_InertialSensor/AP_InertialSensor.cpp b/libraries/AP_InertialSensor/AP_InertialSensor.cpp
index 50592952543cf42152f10d7f650cb0efec2fc1ca..d2635efea499e3e5d1529baa8c534a59acd44f97 100644
--- a/libraries/AP_InertialSensor/AP_InertialSensor.cpp
+++ b/libraries/AP_InertialSensor/AP_InertialSensor.cpp
@@ -664,8 +664,14 @@ AP_InertialSensor::detect_backends(void)
     _add_backend(AP_InertialSensor_SITL::detect(*this));
 #elif HAL_INS_DEFAULT == HAL_INS_HIL
     _add_backend(AP_InertialSensor_HIL::detect(*this));
+#elif HAL_INS_DEFAULT == HAL_INS_MPU60XX_SPI && defined(HAL_INS_DEFAULT_ROTATION)
+    _add_backend(AP_InertialSensor_MPU6000::probe(*this, hal.spi->get_device(HAL_INS_MPU60x0_NAME),
+                                                  HAL_INS_DEFAULT_ROTATION);
 #elif HAL_INS_DEFAULT == HAL_INS_MPU60XX_SPI
     _add_backend(AP_InertialSensor_MPU6000::probe(*this, hal.spi->get_device(HAL_INS_MPU60x0_NAME)));
+#elif HAL_INS_DEFAULT == HAL_INS_MPU60XX_I2C && defined(HAL_INS_DEFAULT_ROTATION)
+    _add_backend(AP_InertialSensor_MPU6000::probe(*this, hal.i2c_mgr->get_device(HAL_INS_MPU60x0_I2C_BUS, HAL_INS_MPU60x0_I2C_ADDR),
+                                                  HAL_INS_DEFAULT_ROTATION);
 #elif HAL_INS_DEFAULT == HAL_INS_MPU60XX_I2C
     _add_backend(AP_InertialSensor_MPU6000::probe(*this, hal.i2c_mgr->get_device(HAL_INS_MPU60x0_I2C_BUS, HAL_INS_MPU60x0_I2C_ADDR)));
 #elif HAL_INS_DEFAULT == HAL_INS_BH
diff --git a/libraries/AP_InertialSensor/AP_InertialSensor_MPU6000.cpp b/libraries/AP_InertialSensor/AP_InertialSensor_MPU6000.cpp
index 5d80c792dc5cee74ffc57ecf1bcce301e2aeeeda..ef2ed8a8c52d61b180c3ea5bc37722a7e02fd4cd 100644
--- a/libraries/AP_InertialSensor/AP_InertialSensor_MPU6000.cpp
+++ b/libraries/AP_InertialSensor/AP_InertialSensor_MPU6000.cpp
@@ -499,20 +499,6 @@ void AP_InertialSensor_MPU6000::_accumulate(uint8_t *samples, uint8_t n_samples)
                         -int16_val(data, 5));
         gyro *= GYRO_SCALE;
 
-#if CONFIG_HAL_BOARD_SUBTYPE == HAL_BOARD_SUBTYPE_LINUX_PXF
-        accel.rotate(ROTATION_PITCH_180_YAW_90);
-        gyro.rotate(ROTATION_PITCH_180_YAW_90);
-#elif CONFIG_HAL_BOARD_SUBTYPE == HAL_BOARD_SUBTYPE_LINUX_BEBOP
-        accel.rotate(ROTATION_YAW_270);
-        gyro.rotate(ROTATION_YAW_270);
-#elif CONFIG_HAL_BOARD_SUBTYPE == HAL_BOARD_SUBTYPE_LINUX_DISCO
-        accel.rotate(ROTATION_PITCH_180_YAW_90);
-        gyro.rotate(ROTATION_PITCH_180_YAW_90);
-#elif CONFIG_HAL_BOARD_SUBTYPE == HAL_BOARD_SUBTYPE_LINUX_MINLURE
-        accel.rotate(ROTATION_YAW_90);
-        gyro.rotate(ROTATION_YAW_90);
-#endif
-
         _rotate_and_correct_accel(_accel_instance, accel);
         _rotate_and_correct_gyro(_gyro_instance, gyro);
 
@@ -541,20 +527,6 @@ void AP_InertialSensor_MPU6000::_accumulate_fast_sampling(uint8_t *samples, uint
     float gscale = GYRO_SCALE / n_samples;
     Vector3f gyro(gsum.x*gscale, gsum.y*gscale, gsum.z*gscale);
     
-#if CONFIG_HAL_BOARD_SUBTYPE == HAL_BOARD_SUBTYPE_LINUX_PXF
-    accel.rotate(ROTATION_PITCH_180_YAW_90);
-    gyro.rotate(ROTATION_PITCH_180_YAW_90);
-#elif CONFIG_HAL_BOARD_SUBTYPE == HAL_BOARD_SUBTYPE_LINUX_BEBOP
-    accel.rotate(ROTATION_YAW_270);
-    gyro.rotate(ROTATION_YAW_270);
-#elif CONFIG_HAL_BOARD_SUBTYPE == HAL_BOARD_SUBTYPE_LINUX_DISCO
-    accel.rotate(ROTATION_PITCH_180_YAW_90);
-    gyro.rotate(ROTATION_PITCH_180_YAW_90);
-#elif CONFIG_HAL_BOARD_SUBTYPE == HAL_BOARD_SUBTYPE_LINUX_MINLURE
-    accel.rotate(ROTATION_YAW_90);
-    gyro.rotate(ROTATION_YAW_90);
-#endif
-    
     _rotate_and_correct_accel(_accel_instance, accel);
     _rotate_and_correct_gyro(_gyro_instance, gyro);