diff --git a/libraries/AP_InertialSensor/AP_InertialSensor_MPU6000.cpp b/libraries/AP_InertialSensor/AP_InertialSensor_MPU6000.cpp
index aae7ffb31c..3e9b447a50 100644
--- a/libraries/AP_InertialSensor/AP_InertialSensor_MPU6000.cpp
+++ b/libraries/AP_InertialSensor/AP_InertialSensor_MPU6000.cpp
@@ -483,7 +483,7 @@ bool AP_InertialSensor_MPU6000::_poll_data()
     return true;
 }
 
-bool AP_InertialSensor_MPU6000::_accumulate(uint8_t *samples, uint8_t n_samples, int16_t raw_temp)
+bool AP_InertialSensor_MPU6000::_accumulate(uint8_t *samples, uint8_t n_samples)
 {
     for (uint8_t i = 0; i < n_samples; i++) {
         const uint8_t *data = samples + MPU_SAMPLE_SIZE * i;
@@ -500,8 +500,8 @@ bool AP_InertialSensor_MPU6000::_accumulate(uint8_t *samples, uint8_t n_samples,
         accel *= _accel_scale;
 
         int16_t t2 = int16_val(data, 3);
-        if (abs(t2 - raw_temp) > 400) {
-            debug("temp reset %d %d", raw_temp, t2);
+        if (!_check_raw_temp(t2)) {
+            debug("temp reset %d %d", _raw_temp, t2);
             _fifo_reset();
             return false;
         }
@@ -531,7 +531,7 @@ bool AP_InertialSensor_MPU6000::_accumulate(uint8_t *samples, uint8_t n_samples,
   gives very good aliasing rejection at frequencies well above what
   can be handled with 1kHz sample rates.
  */
-bool AP_InertialSensor_MPU6000::_accumulate_fast_sampling(uint8_t *samples, uint8_t n_samples, int16_t raw_temp)
+bool AP_InertialSensor_MPU6000::_accumulate_fast_sampling(uint8_t *samples, uint8_t n_samples)
 {
     int32_t tsum = 0;
     const int32_t clip_limit = AP_INERTIAL_SENSOR_ACCEL_CLIP_THRESH_MSS / _accel_scale;
@@ -543,8 +543,8 @@ bool AP_InertialSensor_MPU6000::_accumulate_fast_sampling(uint8_t *samples, uint
 
         // use temperatue to detect FIFO corruption
         int16_t t2 = int16_val(data, 3);
-        if (abs(t2 - raw_temp) > 400) {
-            debug("temp reset %d %d %d", raw_temp, t2, raw_temp - t2);
+        if (!_check_raw_temp(t2)) {
+            debug("temp reset %d %d", _raw_temp, t2);
             _fifo_reset();
             ret = false;
             break;
@@ -607,8 +607,6 @@ void AP_InertialSensor_MPU6000::_read_fifo()
     uint8_t n_samples;
     uint16_t bytes_read;
     uint8_t *rx = _fifo_buffer;
-    int16_t raw_temp;
-    uint8_t trx[2];
     bool need_reset = false;
 
     if (!_block_read(MPUREG_FIFO_COUNTH, rx, 2)) {
@@ -623,14 +621,6 @@ void AP_InertialSensor_MPU6000::_read_fifo()
         goto check_registers;
     }
 
-    /*
-      fetch temperature in order to detect FIFO sync errors
-     */
-    if (!_block_read(MPUREG_TEMP_OUT_H, trx, 2)) {
-        return;
-    }
-    raw_temp = int16_val(trx, 0);
-
     /*
       testing has shown that if we have more than 32 samples in the
       FIFO then some of those samples will be corrupt. It always is
@@ -651,12 +641,12 @@ void AP_InertialSensor_MPU6000::_read_fifo()
         }
 
         if (_fast_sampling) {
-            if (!_accumulate_fast_sampling(rx, n, raw_temp)) {
+            if (!_accumulate_fast_sampling(rx, n)) {
                 debug("stop at %u of %u", n_samples, bytes_read/MPU_SAMPLE_SIZE);
                 break;
             }
         } else {
-            if (!_accumulate(rx, n, raw_temp)) {
+            if (!_accumulate(rx, n)) {
                 break;
             }
         }
@@ -678,6 +668,22 @@ check_registers:
     _dev->set_speed(AP_HAL::Device::SPEED_HIGH);
 }
 
+/*
+  fetch temperature in order to detect FIFO sync errors
+*/
+bool AP_InertialSensor_MPU6000::_check_raw_temp(int16_t t2)
+{
+    if (abs(t2 - _raw_temp) < 400) {
+        // cached copy OK
+        return true;
+    }
+    uint8_t trx[2];
+    if (_block_read(MPUREG_TEMP_OUT_H, trx, 2)) {
+        _raw_temp = int16_val(trx, 0);
+    }
+    return (abs(t2 - _raw_temp) < 400);
+}
+
 bool AP_InertialSensor_MPU6000::_block_read(uint8_t reg, uint8_t *buf,
                                             uint32_t size)
 {
diff --git a/libraries/AP_InertialSensor/AP_InertialSensor_MPU6000.h b/libraries/AP_InertialSensor/AP_InertialSensor_MPU6000.h
index 07a4d55cfa..103abb5e3e 100644
--- a/libraries/AP_InertialSensor/AP_InertialSensor_MPU6000.h
+++ b/libraries/AP_InertialSensor/AP_InertialSensor_MPU6000.h
@@ -78,9 +78,13 @@ private:
     uint8_t _register_read(uint8_t reg);
     void _register_write(uint8_t reg, uint8_t val, bool checked=false);
 
-    bool _accumulate(uint8_t *samples, uint8_t n_samples, int16_t raw_temp);
-    bool _accumulate_fast_sampling(uint8_t *samples, uint8_t n_samples, int16_t raw_temp);
+    bool _accumulate(uint8_t *samples, uint8_t n_samples);
+    bool _accumulate_fast_sampling(uint8_t *samples, uint8_t n_samples);
 
+    bool _check_raw_temp(int16_t t2);
+
+    int16_t _raw_temp;
+    
     // instance numbers of accel and gyro data
     uint8_t _gyro_instance;
     uint8_t _accel_instance;
diff --git a/libraries/AP_InertialSensor/AP_InertialSensor_MPU9250.cpp b/libraries/AP_InertialSensor/AP_InertialSensor_MPU9250.cpp
index c6385176e4..f9449314d9 100644
--- a/libraries/AP_InertialSensor/AP_InertialSensor_MPU9250.cpp
+++ b/libraries/AP_InertialSensor/AP_InertialSensor_MPU9250.cpp
@@ -421,7 +421,7 @@ bool AP_InertialSensor_MPU9250::_data_ready(uint8_t int_status)
 }
 
 
-bool AP_InertialSensor_MPU9250::_accumulate(uint8_t *samples, uint8_t n_samples, int16_t raw_temp)
+bool AP_InertialSensor_MPU9250::_accumulate(uint8_t *samples, uint8_t n_samples)
 {
     for (uint8_t i = 0; i < n_samples; i++) {
         const uint8_t *data = samples + MPU_SAMPLE_SIZE * i;
@@ -433,8 +433,8 @@ bool AP_InertialSensor_MPU9250::_accumulate(uint8_t *samples, uint8_t n_samples,
         accel *= MPU9250_ACCEL_SCALE_1G;
 
         int16_t t2 = int16_val(data, 3);
-        if (abs(t2 - raw_temp) > 400) {
-            debug("temp reset %d %d %d", raw_temp, t2, raw_temp-t2);
+        if (!_check_raw_temp(t2)) {
+            debug("temp reset %d %d %d", _raw_temp, t2, _raw_temp-t2);
             _fifo_reset();
             return false;
         }
@@ -464,7 +464,7 @@ bool AP_InertialSensor_MPU9250::_accumulate(uint8_t *samples, uint8_t n_samples,
   gives very good aliasing rejection at frequencies well above what
   can be handled with 1kHz sample rates.
  */
-bool AP_InertialSensor_MPU9250::_accumulate_fast_sampling(uint8_t *samples, uint8_t n_samples, int16_t raw_temp)
+bool AP_InertialSensor_MPU9250::_accumulate_fast_sampling(uint8_t *samples, uint8_t n_samples)
 {
     int32_t tsum = 0;
     const int32_t clip_limit = AP_INERTIAL_SENSOR_ACCEL_CLIP_THRESH_MSS / MPU9250_ACCEL_SCALE_1G;
@@ -476,8 +476,8 @@ bool AP_InertialSensor_MPU9250::_accumulate_fast_sampling(uint8_t *samples, uint
 
         // use temperatue to detect FIFO corruption
         int16_t t2 = int16_val(data, 3);
-        if (abs(t2 - raw_temp) > 400) {
-            debug("temp reset %d %d %d", raw_temp, t2, raw_temp - t2);
+        if (!_check_raw_temp(t2)) {
+            debug("temp reset %d %d %d", _raw_temp, t2, _raw_temp - t2);
             _fifo_reset();
             ret = false;
             break;
@@ -535,6 +535,23 @@ bool AP_InertialSensor_MPU9250::_accumulate_fast_sampling(uint8_t *samples, uint
 }
 
 
+/*
+  fetch temperature in order to detect FIFO sync errors
+*/
+bool AP_InertialSensor_MPU9250::_check_raw_temp(int16_t t2)
+{
+    if (abs(t2 - _raw_temp) < 400) {
+        // cached copy OK
+        return true;
+    }
+    uint8_t trx[2];
+    if (_block_read(MPUREG_TEMP_OUT_H, trx, 2)) {
+        _raw_temp = int16_val(trx, 0);
+    }
+    return (abs(t2 - _raw_temp) < 400);
+}
+
+
 /*
  * read from the data registers and update filtered data
  */
@@ -543,8 +560,6 @@ bool AP_InertialSensor_MPU9250::_read_sample()
     uint8_t n_samples;
     uint16_t bytes_read;
     uint8_t *rx = _fifo_buffer;
-    int16_t raw_temp;
-    uint8_t trx[2];
     bool need_reset = false;
     
     if (!_block_read(MPUREG_FIFO_COUNTH, rx, 2)) {
@@ -559,14 +574,6 @@ bool AP_InertialSensor_MPU9250::_read_sample()
         goto check_registers;
     }
 
-    /*
-      fetch temperature in order to detect FIFO sync errors
-     */
-    if (!_block_read(MPUREG_TEMP_OUT_H, trx, 2)) {
-        return true;
-    }
-    raw_temp = int16_val(trx, 0);
-
     /*
       testing has shown that if we have more than 32 samples in the
       FIFO then some of those samples will be corrupt. It always is
@@ -587,12 +594,12 @@ bool AP_InertialSensor_MPU9250::_read_sample()
         }
 
         if (_fast_sampling) {
-            if (!_accumulate_fast_sampling(rx, n, raw_temp)) {
+            if (!_accumulate_fast_sampling(rx, n)) {
                 debug("stop at %u of %u", n_samples, bytes_read/MPU_SAMPLE_SIZE);
                 break;
             }
         } else {
-            if (!_accumulate(rx, n, raw_temp)) {
+            if (!_accumulate(rx, n)) {
                 break;
             }
         }
diff --git a/libraries/AP_InertialSensor/AP_InertialSensor_MPU9250.h b/libraries/AP_InertialSensor/AP_InertialSensor_MPU9250.h
index 4dc1d2e9fc..52c8537419 100644
--- a/libraries/AP_InertialSensor/AP_InertialSensor_MPU9250.h
+++ b/libraries/AP_InertialSensor/AP_InertialSensor_MPU9250.h
@@ -73,9 +73,11 @@ private:
     uint8_t _register_read(uint8_t reg);
     void _register_write(uint8_t reg, uint8_t val, bool checked=false);
 
-    bool _accumulate(uint8_t *samples, uint8_t n_samples, int16_t raw_temp);
-    bool _accumulate_fast_sampling(uint8_t *samples, uint8_t n_samples, int16_t raw_temp);
+    bool _accumulate(uint8_t *samples, uint8_t n_samples);
+    bool _accumulate_fast_sampling(uint8_t *samples, uint8_t n_samples);
 
+    bool _check_raw_temp(int16_t t2);
+    
     // instance numbers of accel and gyro data
     uint8_t _gyro_instance;
     uint8_t _accel_instance;
@@ -97,6 +99,8 @@ private:
     // buffer for fifo read
     uint8_t *_fifo_buffer;
 
+    int16_t _raw_temp;
+    
     /*
       accumulators for fast sampling
       See description in _accumulate_fast_sampling()