From 0a3df91ed35e0edd05fa036c9e23c7554832f68f Mon Sep 17 00:00:00 2001
From: rob tillaart <rob.tillaart@gmail.com>
Date: Sun, 19 Oct 2014 13:02:42 +0200
Subject: [PATCH] + 0.1.04 + changed float to double (for platforms which
 support it) + changed divisions in multiplications + fixed uint32_t readADC()
 + reduced size of C array by 1 float + added second order temperature
 compensation

---
 libraries/MS5611/MS5611.cpp | 55 +++++++++++++++++++++++++++++--------
 libraries/MS5611/MS5611.h   |  9 +++---
 2 files changed, 49 insertions(+), 15 deletions(-)

diff --git a/libraries/MS5611/MS5611.cpp b/libraries/MS5611/MS5611.cpp
index 7cd3f479..156679da 100644
--- a/libraries/MS5611/MS5611.cpp
+++ b/libraries/MS5611/MS5611.cpp
@@ -2,11 +2,16 @@
 //    FILE: MS5611.cpp
 //  AUTHOR: Rob Tillaart
 //          Erni - testing/fixes
-// VERSION: 0.1.03
+// VERSION: 0.1.04
 // PURPOSE: MS5611 Temperature & Humidity library for Arduino
 //     URL:
 //
 // HISTORY:
+// 0.1.04 changed float to double (for platforms which support it)
+//        changed divisions in multiplications
+//        fixed uint32_t readADC()
+//        reduced size of C array by 1 float
+//        added second order temperature compensation
 // 0.1.03 changed math to float [test version]
 // 0.1.02 fixed bug return value read()
 //        fixed bug #bits D2
@@ -39,8 +44,10 @@ MS5611::MS5611(uint8_t address)
 void MS5611::init()
 {
     reset();
-    for (int reg = 0; reg < 8; reg++)
+    for (int reg = 0; reg < 7; reg++)
     {
+        // C[0] not used; this way indices match datasheet.
+        // C[7] == CRC skipped.
         C[reg] = readProm(reg);
     }
 }
@@ -50,21 +57,47 @@ int MS5611::read(uint8_t bits)
     // VARIABLES NAMES BASED ON DATASHEET
     convert(0x40, bits);
     if (_result) return _result;
-    int32_t D1 = readADC();
+    uint32_t D1 = readADC();
     if (_result) return _result;
 
     convert(0x50, bits);
     if (_result) return _result;
-    int32_t D2 = readADC();
+    uint32_t D2 = readADC();
     if (_result) return _result;
 
-    // PAGE 7/20 of the datasheet
-    float dT = D2 - (C[5] * 256L);
-    _temperature = 2000 + (dT * C[6])/8388608L;
+    // TODO the multiplications of these constants can be done in init()
+    // but first they need to be verified.
+    
+    // TEMP & PRESS MATH - PAGE 7/20
+    double dT = D2 - C[5] * 256L;
+    _temperature = 2000 + dT * C[6] * 1.1920928955E-7;
 
-    float offset =  (C[2] * 65536L) + (C[4] * dT ) / 128L;
-    float sens = C[1] * 32768L + (C[3] * dT ) / 256L;
-    _pressure = (((D1 * sens)/2097152L) - offset) / 32768L;
+    double offset =  C[2] * 65536L + dT * C[4] * 7.8125E-3;
+    double sens = C[1] * 32768L + dT * C[3] * 3.90625E-3;
+
+    // SECOND ORDER COMPENSATION - PAGE 8/20
+    // COMMENT OUT < 20 CORRECTION IF NOT NEEDED
+    if (_temperature < 20)
+    {
+        double T2 = dT * dT * 4.6566128731E-10;
+        double t = _temperature - 2000;
+        double offset2 = 2.5 * t * t;
+        double sens2 = 1.25 * t * t * t;
+        // COMMENT OUT < -15 CORRECTION IF NOT NEEDED
+        if (_temperature < -15)
+        {
+            t = _temperature + 1500;
+            t = t * t;
+            offset2 += 7 * t;
+            sens2 += 5.5 * t;
+        }
+        _temperature -= T2;
+        offset -= offset2;
+        sens -= sens2;
+    }
+    // END SECOND ORDER COMPENSATION
+
+    _pressure = (D1 * sens * 4.76837158205E-7 - offset) * 3.051757813E-5;
 
     return 0;
 }
@@ -108,7 +141,7 @@ uint16_t MS5611::readProm(uint8_t reg)
     return 0;
 }
 
-int32_t MS5611::readADC()
+uint32_t MS5611::readADC()
 {
     command(0x00);
     if (_result == 0)
diff --git a/libraries/MS5611/MS5611.h b/libraries/MS5611/MS5611.h
index b49efd84..f1513c2f 100644
--- a/libraries/MS5611/MS5611.h
+++ b/libraries/MS5611/MS5611.h
@@ -2,7 +2,7 @@
 //    FILE: MS5611.h
 //  AUTHOR: Rob Tillaart
 //          Erni - testing/fixes
-// VERSION: 0.1.03
+// VERSION: 0.1.04
 // PURPOSE: MS5611 Temperature & Pressure library for Arduino
 //     URL:
 //
@@ -20,7 +20,7 @@
 #include <Arduino.h>
 #endif
 
-#define MS5611_LIB_VERSION "0.1.03"
+#define MS5611_LIB_VERSION "0.1.04"
 
 #define MS5611_READ_OK  0
 
@@ -38,7 +38,7 @@ public:
 private:
     void reset();
     void convert(uint8_t ADDR, uint8_t bits);
-    int32_t  readADC();
+    uint32_t readADC();
     uint16_t readProm(uint8_t reg);
     void command(uint8_t command);
 
@@ -46,7 +46,8 @@ private:
     int32_t _temperature;
     int32_t _pressure;
     int _result;
-    float C[8];
+    double C[7];
+
 };
 #endif
 //