Added second order temperature compensation for MS5611 and MS5637

titanproger · Apr 30, 2015 · b552cac · b552cac
1 parent 3e82584
commit b552cac
Show file tree

Hide file tree

Showing 3 changed files with 47 additions and 9 deletions.
diff --git a/README.md b/README.md
@@ -82,6 +82,10 @@ SyntroPiNav is available as part of the richards-tech SyntroPiApps repo (https:/
 
 ## Release history
 
+### April 30 2015 - 6.2.1
+
+Added second order temperature compensation for MS5611 and MS5637. MS5637 still seems to be affected by temperature - this is being investigated.
+
 ### April 24 2015 - 6.2.0
 
 Add support for Bosch BMX055 IMU and MS5637 pressure sensor. See notes above about auto-detection for the MS5637.

diff --git a/RTIMULib/IMUDrivers/RTPressureMS5611.cpp b/RTIMULib/IMUDrivers/RTPressureMS5611.cpp
@@ -123,14 +123,30 @@ void RTPressureMS5611::pressureBackground()
 
         //  now calculate the real values
 
-        int64_t deltaT = m_D2 - (((uint32_t)m_calData[4]) << 8);
+        int64_t deltaT = (int32_t)m_D2 - (((int32_t)m_calData[4]) << 8);
 
-        m_temperature = (RTFLOAT)(2000 + ((deltaT * (int64_t)m_calData[5]) >> 23)) / (RTFLOAT)100.0;
+        int32_t temperature = 2000 + ((deltaT * (int64_t)m_calData[5]) >> 23); // note - this needs to be divided by 100
 
         int64_t offset = ((int64_t)m_calData[1] << 16) + (((int64_t)m_calData[3] * deltaT) >> 7);
         int64_t sens = ((int64_t)m_calData[0] << 15) + (((int64_t)m_calData[2] * deltaT) >> 8);
 
+        //  do second order temperature compensation
+
+        if (temperature < 2000) {
+            int64_t T2 = (deltaT * deltaT) >> 31;
+            int64_t offset2 = 5 * ((temperature - 2000) * (temperature - 2000)) / 2;
+            int64_t sens2 = offset2 / 2;
+            if (temperature < -1500) {
+                offset2 += 7 * (temperature + 1500) * (temperature + 1500);
+                sens2 += 11 * ((temperature + 1500) * (temperature + 1500)) / 2;
+            }
+            temperature -= T2;
+            offset -= offset2;
+            sens -=sens2;
+        }
+
         m_pressure = (RTFLOAT)(((((int64_t)m_D1 * sens) >> 21) - offset) >> 15) / (RTFLOAT)100.0;
+        m_temperature = (RTFLOAT)temperature/(RTFLOAT)100;
 
         // printf("Temp: %f, pressure: %f\n", m_temperature, m_pressure);
 

diff --git a/RTIMULib/IMUDrivers/RTPressureMS5637.cpp b/RTIMULib/IMUDrivers/RTPressureMS5637.cpp
@@ -44,7 +44,7 @@ bool RTPressureMS5637::pressureInit()
     for (int i = 0; i < 6; i++) {
         if (!m_settings->HALRead(m_pressureAddr, cmd, 2, data, "Failed to read MS5611 calibration data"))
             return false;
-        m_calData[i] = (((uint16_t)data[0]) << 8) + (uint16_t)data[1];
+        m_calData[i] = (((uint16_t)data[0]) << 8) | ((uint16_t)data[1]);
         // printf("Cal index: %d, data: %d\n", i, m_calData[i]);
         cmd += 2;
     }
@@ -96,7 +96,7 @@ void RTPressureMS5637::pressureBackground()
         if (!m_settings->HALRead(m_pressureAddr, MS5611_CMD_ADC, 3, data, "Failed to read MS5611 pressure")) {
             break;
         }
-        m_D1 = (((uint32_t)data[0]) << 16) + (((uint32_t)data[1]) << 8) + (uint32_t)data[2];
+        m_D1 = (((uint32_t)data[0]) << 16) | (((uint32_t)data[1]) << 8) | ((uint32_t)data[2]);
 
         // start temperature conversion
 
@@ -114,7 +114,7 @@ void RTPressureMS5637::pressureBackground()
         if (!m_settings->HALRead(m_pressureAddr, MS5611_CMD_ADC, 3, data, "Failed to read MS5611 temperature")) {
             break;
         }
-        m_D2 = (((uint32_t)data[0]) << 16) + (((uint32_t)data[1]) << 8) + (uint32_t)data[2];
+        m_D2 = (((uint32_t)data[0]) << 16) | (((uint32_t)data[1]) << 8) | ((uint32_t)data[2]);
 
         //  call this function for testing only
         //  should give T = 2000 (20.00C) and pressure 110002 (1100.02hPa)
@@ -123,14 +123,32 @@ void RTPressureMS5637::pressureBackground()
 
         //  now calculate the real values
 
-        int64_t deltaT = m_D2 - (((uint32_t)m_calData[4]) << 8);
+        int64_t deltaT = (int32_t)m_D2 - (((int32_t)m_calData[4]) << 8);
 
-        m_temperature = (RTFLOAT)(2000 + ((deltaT * (int64_t)m_calData[5]) >> 23)) / (RTFLOAT)100.0;
+        int32_t temperature = 2000 + ((deltaT * (int64_t)m_calData[5]) >> 23); // note - this still needs to be divided by 100
 
-        int64_t offset = ((int64_t)m_calData[1] << 17) + (((int64_t)m_calData[3] * deltaT) >> 6);
-        int64_t sens = ((int64_t)m_calData[0] << 16) + (((int64_t)m_calData[2] * deltaT) >> 7);
+        int64_t offset = (((int64_t)m_calData[1]) << 17) + ((m_calData[3] * deltaT) >> 6);
+        int64_t sens = (((int64_t)m_calData[0]) << 16) + ((m_calData[2] * deltaT) >> 7);
+
+        //  do second order temperature compensation
+
+        if (temperature < 2000) {
+            int64_t T2 = (3 * (deltaT * deltaT)) >> 33;
+            int64_t offset2 = 61 * ((temperature - 2000) * (temperature - 2000)) / 16;
+            int64_t sens2 = 29 * ((temperature - 2000) * (temperature - 2000)) / 16;
+            if (temperature < -1500) {
+                offset2 += 17 * (temperature + 1500) * (temperature + 1500);
+                sens2 += 9 * ((temperature + 1500) * (temperature + 1500));
+            }
+            temperature -= T2;
+            offset -= offset2;
+            sens -=sens2;
+        } else {
+            temperature -= (5 * (deltaT * deltaT)) >> 38;
+        }
 
         m_pressure = (RTFLOAT)(((((int64_t)m_D1 * sens) >> 21) - offset) >> 15) / (RTFLOAT)100.0;
+        m_temperature = (RTFLOAT)temperature/(RTFLOAT)100;
 
         // printf("Temp: %f, pressure: %f\n", m_temperature, m_pressure);