diff --git a/libraries/SITL/SIM_MS5611.cpp b/libraries/SITL/SIM_MS5611.cpp
index bc7f91a6f7..3a56217584 100644
--- a/libraries/SITL/SIM_MS5611.cpp
+++ b/libraries/SITL/SIM_MS5611.cpp
@@ -59,13 +59,13 @@ void MS5611::convert(float P_Pa, float Temp_C, uint32_t &D1, uint32_t &D2)
     // second order temperature compensation when under 20 degrees C
     if (TEMP < 2000) {
         // Solve the quadratic equation for D2 when TEMP < 2000
-        D2 = 128 * (2 * prom[5] - sqrt(sq(prom[6]) - 131072 * (TEMP - 2000)) + prom[6]);
+        D2 = 128 * (2 * int64_t(prom[5]) - sqrt(sq(int64_t(prom[6])) - 131072 * (TEMP - 2000)) + int64_t(prom[6]));
 
         // Must compute the pressure compensation values using D2
-        const float dT = float(D2) - (prom[5] << Q5);
-        float TEMP_forward = 2000 + (dT * prom[6]) / (1L << Q6);
-        float OFF  = prom[2] * (1L << Q2) + (prom[4] * dT) / (1L << Q4);
-        float SENS = prom[1] * (1L << Q1) + (prom[3] * dT) / (1L << Q3);
+        const float dT = float(D2) - (int64_t(prom[5]) << Q5);
+        float TEMP_forward = 2000 + (dT * int64_t(prom[6])) / (1L << Q6);
+        float OFF  = int64_t(prom[2]) * (1L << Q2) + (int64_t(prom[4]) * dT) / (1L << Q4);
+        float SENS = int64_t(prom[1]) * (1L << Q1) + (int64_t(prom[3]) * dT) / (1L << Q3);
 
         const float Aux = sq(TEMP_forward - 2000);
         float OFF2  = 2.5 * Aux;
@@ -81,12 +81,12 @@ void MS5611::convert(float P_Pa, float Temp_C, uint32_t &D1, uint32_t &D2)
 
         D1 = ((P_Pa * float(1L << 15) + OFF) * float(1L << 21)) / SENS;
     } else {
-        const float dT = (TEMP - 2000) * (1L << Q6) / prom[6];
-        const float OFF  = prom[2] * (1L << Q2) + (prom[4] * dT) / (1L << Q4);
-        const float SENS = prom[1] * (1L << Q1) + (prom[3] * dT) / (1L << Q3);
+        const float dT = (TEMP - 2000) * (1L << Q6) / int64_t(prom[6]);
+        const float OFF  = int64_t(prom[2]) * (1L << Q2) + (int64_t(prom[4]) * dT) / (1L << Q4);
+        const float SENS = int64_t(prom[1]) * (1L << Q1) + (int64_t(prom[3]) * dT) / (1L << Q3);
 
         D1 = ((P_Pa * float(1L << 15) + OFF) * float(1L << 21)) / SENS;
-        D2 = dT + (prom[5] << Q5);
+        D2 = dT + (int64_t(prom[5]) << Q5);
     }
 }
 
diff --git a/libraries/SITL/tests/test_sim_ms5611.cpp b/libraries/SITL/tests/test_sim_ms5611.cpp
index d36271b295..a05570d8ce 100644
--- a/libraries/SITL/tests/test_sim_ms5611.cpp
+++ b/libraries/SITL/tests/test_sim_ms5611.cpp
@@ -63,8 +63,10 @@ TEST(MS5611, convert)
         uint32_t D1;
         uint32_t D2;
         ms5611.convert_pub(elem.P_Pa, elem.Temp_C, D1, D2);
-        EXPECT_EQ(D1, elem.D1);
-        EXPECT_EQ(D2, elem.D2);
+
+        // Expect NEAR here instead of EQ because in 32bit they are off by 1
+        EXPECT_NEAR(D1, elem.D1, 1);
+        EXPECT_NEAR(D2, elem.D2, 1);
     }
 }