diff --git a/src/main/drivers/barometer_ms5611.c b/src/main/drivers/barometer_ms5611.c
index 897983db75..a8a48057a5 100644
--- a/src/main/drivers/barometer_ms5611.c
+++ b/src/main/drivers/barometer_ms5611.c
@@ -52,11 +52,11 @@ static void ms5611_start_ut(void);
 static void ms5611_get_ut(void);
 static void ms5611_start_up(void);
 static void ms5611_get_up(void);
-static void ms5611_calculate(int32_t *pressure, int32_t *temperature);
+STATIC_UNIT_TESTED void ms5611_calculate(int32_t *pressure, int32_t *temperature);
 
-static uint32_t ms5611_ut;  // static result of temperature measurement
-static uint32_t ms5611_up;  // static result of pressure measurement
-static uint16_t ms5611_c[PROM_NB];  // on-chip ROM
+STATIC_UNIT_TESTED uint32_t ms5611_ut;  // static result of temperature measurement
+STATIC_UNIT_TESTED uint32_t ms5611_up;  // static result of pressure measurement
+STATIC_UNIT_TESTED uint16_t ms5611_c[PROM_NB];  // on-chip ROM
 static uint8_t ms5611_osr = CMD_ADC_4096;
 
 bool ms5611Detect(baro_t *baro)
@@ -161,12 +161,12 @@ static void ms5611_get_up(void)
     ms5611_up = ms5611_read_adc();
 }
 
-static void ms5611_calculate(int32_t *pressure, int32_t *temperature)
+STATIC_UNIT_TESTED void ms5611_calculate(int32_t *pressure, int32_t *temperature)
 {
     uint32_t press;
     int64_t temp;
     int64_t delt;
-    int32_t dT = (int64_t)ms5611_ut - ((uint64_t)ms5611_c[5] * 256);
+    int64_t dT = (int64_t)ms5611_ut - ((uint64_t)ms5611_c[5] * 256);
     int64_t off = ((int64_t)ms5611_c[2] << 16) + (((int64_t)ms5611_c[4] * dT) >> 7);
     int64_t sens = ((int64_t)ms5611_c[1] << 15) + (((int64_t)ms5611_c[3] * dT) >> 8);
     temp = 2000 + ((dT * (int64_t)ms5611_c[6]) >> 23);
@@ -182,9 +182,11 @@ static void ms5611_calculate(int32_t *pressure, int32_t *temperature)
             off -= 7 * delt;
             sens -= (11 * delt) >> 1;
         }
+    temp -= ((dT * dT) >> 31);
     }
     press = ((((int64_t)ms5611_up * sens) >> 21) - off) >> 15;
 
+
     if (pressure)
         *pressure = press;
     if (temperature)
diff --git a/src/test/unit/baro_unittest.cc b/src/test/unit/baro_unittest.cc
index b84052c271..2d2b7726d5 100644
--- a/src/test/unit/baro_unittest.cc
+++ b/src/test/unit/baro_unittest.cc
@@ -19,6 +19,11 @@
 extern "C" {
 
 int8_t ms5611_crc(uint16_t *prom);
+void ms5611_calculate(int32_t *pressure, int32_t *temperature);
+
+extern uint16_t ms5611_c[8];
+extern uint32_t ms5611_up;
+extern uint32_t ms5611_ut;
 
 }
 
@@ -83,6 +88,66 @@ TEST(baroTest, TestMs5611AllOnesProm)
 
 }
 
+TEST(baroTest, TestMs5611CalculatePressureGT20Deg)
+{
+
+    // given
+    int32_t pressure, temperature;
+    uint16_t ms5611_c_test[] = {0x0000, 40127, 36924, 23317, 23282, 33464, 28312, 0x0000}; // calibration data from MS5611 datasheet 
+    memcpy(&ms5611_c, &ms5611_c_test, sizeof(ms5611_c_test));
+
+    ms5611_up = 9085466; // Digital pressure value from MS5611 datasheet
+    ms5611_ut = 8569150; // Digital temperature value from MS5611 datasheet
+
+    // when
+    ms5611_calculate(&pressure, &temperature);
+
+    // then
+    EXPECT_EQ(2007, temperature); // 20.07 deg C
+    EXPECT_EQ(100009, pressure);  // 1000.09 mbar
+
+}
+
+TEST(baroTest, TestMs5611CalculatePressureLT20Deg)
+{
+
+    // given
+    int32_t pressure, temperature;
+    uint16_t ms5611_c_test[] = {0x0000, 40127, 36924, 23317, 23282, 33464, 28312, 0x0000}; // calibration data from MS5611 datasheet 
+    memcpy(&ms5611_c, &ms5611_c_test, sizeof(ms5611_c_test));
+
+    ms5611_up = 9085466; // Digital pressure value from MS5611 datasheet
+    ms5611_ut = 8069150; // Digital temperature value
+
+    // when
+    ms5611_calculate(&pressure, &temperature);
+
+    // then
+    EXPECT_EQ(205, temperature); // 2.05 deg C
+    EXPECT_EQ(96512, pressure);  // 965.12 mbar
+
+}
+
+TEST(baroTest, TestMs5611CalculatePressureLTMinus15Deg)
+{
+
+    // given
+    int32_t pressure, temperature;
+    uint16_t ms5611_c_test[] = {0x0000, 40127, 36924, 23317, 23282, 33464, 28312, 0x0000}; // calibration data from MS5611 datasheet 
+    memcpy(&ms5611_c, &ms5611_c_test, sizeof(ms5611_c_test));
+
+    ms5611_up = 9085466; // Digital pressure value from MS5611 datasheet
+    ms5611_ut = 7369150; // Digital temperature value
+
+    // when
+    ms5611_calculate(&pressure, &temperature);
+
+    // then
+    EXPECT_EQ(-2710, temperature); // -27.10 deg C
+    EXPECT_EQ(90613, pressure);  // 906.13 mbar
+
+}
+
 // STUBS
 
 extern "C" {