diff --git a/libraries/AP_InertialSensor/AP_InertialSensor_MPU6000.cpp b/libraries/AP_InertialSensor/AP_InertialSensor_MPU6000.cpp
index 52f376bcd3aa542622dae5ff41e78367951b2f87..d49460fad8a7c4a8de7e117eded3de2c74e8bba0 100644
--- a/libraries/AP_InertialSensor/AP_InertialSensor_MPU6000.cpp
+++ b/libraries/AP_InertialSensor/AP_InertialSensor_MPU6000.cpp
@@ -322,9 +322,10 @@ bool AP_InertialSensor_MPU6000::_init()
 
 void AP_InertialSensor_MPU6000::_fifo_reset()
 {
-    _register_write(MPUREG_USER_CTRL, 0);
-    _register_write(MPUREG_USER_CTRL, BIT_USER_CTRL_FIFO_RESET);
-    _register_write(MPUREG_USER_CTRL, BIT_USER_CTRL_FIFO_EN);
+    uint8_t user_ctrl = _master_i2c_enable?BIT_USER_CTRL_I2C_MST_EN:0;
+    _register_write(MPUREG_USER_CTRL, user_ctrl);
+    _register_write(MPUREG_USER_CTRL, user_ctrl | BIT_USER_CTRL_FIFO_RESET);
+    _register_write(MPUREG_USER_CTRL, user_ctrl | BIT_USER_CTRL_FIFO_EN);
 }
 
 void AP_InertialSensor_MPU6000::_fifo_enable()
@@ -907,6 +908,8 @@ void AP_MPU6000_AuxiliaryBus::_configure_slaves()
     uint8_t user_ctrl = backend._register_read(MPUREG_USER_CTRL);
     backend._register_write(MPUREG_USER_CTRL, user_ctrl | BIT_USER_CTRL_I2C_MST_EN);
 
+    backend._master_i2c_enable = true;
+    
     /* stop condition between reads; clock at 400kHz */
     backend._register_write(MPUREG_I2C_MST_CTRL,
                             BIT_I2C_MST_P_NSR | BIT_I2C_MST_CLK_400KHZ);
diff --git a/libraries/AP_InertialSensor/AP_InertialSensor_MPU6000.h b/libraries/AP_InertialSensor/AP_InertialSensor_MPU6000.h
index e187acbdd415370d20fbcf7ae1d704be9d645276..bdbf847324576834babafda97b387b91f4f0a745 100644
--- a/libraries/AP_InertialSensor/AP_InertialSensor_MPU6000.h
+++ b/libraries/AP_InertialSensor/AP_InertialSensor_MPU6000.h
@@ -115,6 +115,9 @@ private:
     float _last_temp;
     uint8_t _temp_counter;
 
+    // has master i2c been enabled?
+    bool _master_i2c_enable;    
+    
     // buffer for fifo read
     uint8_t *_fifo_buffer;
 };
diff --git a/libraries/AP_InertialSensor/AP_InertialSensor_MPU9250.cpp b/libraries/AP_InertialSensor/AP_InertialSensor_MPU9250.cpp
index c90dd22f8290fbfbed099dbdc03abfcf1aad732d..031ec62b91601eb07cc45840320ead2cf918b13c 100644
--- a/libraries/AP_InertialSensor/AP_InertialSensor_MPU9250.cpp
+++ b/libraries/AP_InertialSensor/AP_InertialSensor_MPU9250.cpp
@@ -276,9 +276,10 @@ bool AP_InertialSensor_MPU9250::_init()
 
 void AP_InertialSensor_MPU9250::_fifo_reset()
 {
-    _register_write(MPUREG_USER_CTRL, 0);
-    _register_write(MPUREG_USER_CTRL, BIT_USER_CTRL_FIFO_RESET);
-    _register_write(MPUREG_USER_CTRL, BIT_USER_CTRL_FIFO_EN);
+    uint8_t user_ctrl = _master_i2c_enable?BIT_USER_CTRL_I2C_MST_EN:0;
+    _register_write(MPUREG_USER_CTRL, user_ctrl);
+    _register_write(MPUREG_USER_CTRL, user_ctrl | BIT_USER_CTRL_FIFO_RESET);
+    _register_write(MPUREG_USER_CTRL, user_ctrl | BIT_USER_CTRL_FIFO_EN);
 }
 
 void AP_InertialSensor_MPU9250::_fifo_enable()
@@ -781,6 +782,8 @@ void AP_MPU9250_AuxiliaryBus::_configure_slaves()
     uint8_t user_ctrl = backend._register_read(MPUREG_USER_CTRL);
     backend._register_write(MPUREG_USER_CTRL, user_ctrl | BIT_USER_CTRL_I2C_MST_EN);
 
+    backend._master_i2c_enable = true;
+
     /* stop condition between reads; clock at 400kHz */
     backend._register_write(MPUREG_I2C_MST_CTRL,
                             I2C_MST_CLOCK_400KHZ | I2C_MST_P_NSR);
diff --git a/libraries/AP_InertialSensor/AP_InertialSensor_MPU9250.h b/libraries/AP_InertialSensor/AP_InertialSensor_MPU9250.h
index c27fa5790e4948d4f33579c5b763a045656f76e3..3e7eedcec4e125af3b4246453bf8011313cb716f 100644
--- a/libraries/AP_InertialSensor/AP_InertialSensor_MPU9250.h
+++ b/libraries/AP_InertialSensor/AP_InertialSensor_MPU9250.h
@@ -100,6 +100,9 @@ private:
 
     // are we doing more than 1kHz sampling?
     bool _fast_sampling;
+
+    // has master i2c been enabled?
+    bool _master_i2c_enable;
     
     // last temperature reading, used to detect FIFO errors
     float _last_temp;