diff --git a/include/sta/drivers/MS56xx.hpp b/include/sta/drivers/MS56xx.hpp
index e02521c..651ff73 100644
--- a/include/sta/drivers/MS56xx.hpp
+++ b/include/sta/drivers/MS56xx.hpp
@@ -165,7 +165,7 @@ namespace sta
          * @note This code was taken from https://github.com/RobTillaart/MS5611
          * 
          */
-        void initConstants(bool mathMode = 0);
+        void initConstants();
 
         /**
          * @brief Convert the pressure value given in Pa to a different unit.
diff --git a/src/MS56xx.cpp b/src/MS56xx.cpp
index d65ccae..0222708 100644
--- a/src/MS56xx.cpp
+++ b/src/MS56xx.cpp
@@ -55,7 +55,7 @@ namespace sta
 
     float MS56xx::getPressure(Unit unit /* = Unit::hPa */)
     {
-        // Request the ADC to read temperature values.
+        // Request the ADC to read pressure values.
         busCommand(MS56xx::Operations::D1_CONVERSION + 2*this->osr_);
 
         // 8.22 ms conversion according to the datasheet.
@@ -64,10 +64,9 @@ namespace sta
         uint8_t buffer[3] = { 0x00, 0x00, 0x00 };
         busRead(MS56xx::Operations::ADC_RESULT, buffer, 3);
 
-        // Difference between actual and reference temperature.
-        uint32_t D1 = buffer[0] << 16 | buffer[1] << 8 | buffer[2];
+        uint32_t D1 = 0x00 | buffer[0] << 16 | buffer[1] << 8 | buffer[2];
 
-        // Request the ADC to read pressure values.
+        // Request the ADC to read temperature values.
         busCommand(MS56xx::Operations::D2_CONVERSION + 2*this->osr_);
 
         // 8.22 ms conversion according to the datasheet.
@@ -78,11 +77,10 @@ namespace sta
         uint32_t D2 = buffer[0] << 16 | buffer[1] << 8 | buffer[2];
 
         float dT = D2 - C_[5];
-        float temperature = 2000 + dT * C_[6];
-
         float offset =  C_[2] + dT * C_[4];
         float sens = C_[1] + dT * C_[3];
 
+        /*
         if (temperature < 2000)
         {
             float T2 = dT * dT * 4.6566128731E-10;
@@ -100,6 +98,7 @@ namespace sta
             offset -= offset2;
             sens -= sens2;
         }
+        */
 
         // The pressure in Pa.
         float pressure = (D1 * sens * 4.76837158205E-7 - offset) * 3.051757813E-5;
@@ -108,8 +107,23 @@ namespace sta
         return pressure * 0.01;
     }
 
-    int32_t MS56xx::getTemperature() {
-       return 21.0f;
+    int32_t MS56xx::getTemperature()
+    {
+    	// Request the ADC to read temperature values.
+		busCommand(MS56xx::Operations::D2_CONVERSION + 2*this->osr_);
+
+		// 8.22 ms conversion according to the datasheet.
+		delay_(osrDelay());
+
+		uint8_t buffer[3] = { 0x00, 0x00, 0x00 };
+		busRead(MS56xx::Operations::ADC_RESULT, buffer, 3);
+
+		uint32_t D2 = buffer[0] << 16 | buffer[1] << 8 | buffer[2];
+
+    	float dT = D2 - C_[5];
+    	float temperature = 2000 + dT * C_[6];
+
+    	return temperature * 0.01;
     }
 
     void MS56xx::setPressureReference(float pressRef, float altRef, Unit unit /* = Unit::hPa */)
@@ -139,23 +153,15 @@ namespace sta
         }
     }
 
-    void MS56xx::initConstants(bool mathMode /* = 0 */)
+    void MS56xx::initConstants()
     {
         C_[0] = 1;
-        C_[1] = 32768L;          //  SENSt1   = C[1] * 2^15    |    * 2^16
-        C_[2] = 65536L;          //  OFFt1    = C[2] * 2^16    |    * 2^17
-        C_[3] = 3.90625E-3;      //  TCS      = C[3] / 2^8     |    / 2^7
-        C_[4] = 7.8125E-3;       //  TCO      = C[4] / 2^7     |    / 2^6
-        C_[5] = 256;             //  Tref     = C[5] * 2^8     |    * 2^8
-        C_[6] = 1.1920928955E-7; //  TEMPSENS = C[6] / 2^23    |    / 2^23
-
-        if (mathMode == 1)  //  Appnote version for pressure.
-        {
-            C_[1] = 65536L;          //  SENSt1
-            C_[2] = 131072L;         //  OFFt1
-            C_[3] = 7.8125E-3;       //  TCS
-            C_[4] = 1.5625e-2;       //  TCO
-        }
+        C_[1] = 65536L;
+        C_[2] = 131072;
+        C_[3] = 7.8125E-3;
+        C_[4] = 0.015625;
+        C_[5] = 256;
+        C_[6] = 1.1920928955E-7;
     }
 
     inline float MS56xx::convertPressure(float pressure, Unit unit)
@@ -215,4 +221,4 @@ namespace sta
 
         return true;
     }
-}
\ No newline at end of file
+}