diff --git a/libraries/AP_Baro/AP_Baro_MS5611.cpp b/libraries/AP_Baro/AP_Baro_MS5611.cpp
index 87e39e40fd..f5f50e8ae4 100644
--- a/libraries/AP_Baro/AP_Baro_MS5611.cpp
+++ b/libraries/AP_Baro/AP_Baro_MS5611.cpp
@@ -282,9 +282,11 @@ void AP_Baro_MS56XX::_timer(void)
                 _s_D2 >>= 1;
                 _d2_count = 16;
             }
+
+            if (_serial->write(CMD_CONVERT_D1_OSR4096)) {      // Command to read pressure
+                _state++;
+            }
         }
-        _state++;
-        _serial->write(CMD_CONVERT_D1_OSR4096);      // Command to read pressure
     } else {
         uint32_t d1 = _serial->read_24bits(0);;
         if (d1 != 0) {
@@ -301,13 +303,16 @@ void AP_Baro_MS56XX::_timer(void)
             }
             // Now a new reading exists
             _updated = true;
-        }
-        _state++;
-        if (_state == 5) {
-            _serial->write(CMD_CONVERT_D2_OSR4096); // Command to read temperature
-            _state = 0;
-        } else {
-            _serial->write(CMD_CONVERT_D1_OSR4096); // Command to read pressure
+
+            if (_state == 4) {
+                if (_serial->write(CMD_CONVERT_D2_OSR4096)) { // Command to read temperature
+                    _state = 0;
+                }
+            } else {
+                if (_serial->write(CMD_CONVERT_D1_OSR4096)) { // Command to read pressure
+                    _state++;
+                }
+            }
         }
     }