Updated driver code; tested on MS5611

include/sta/drivers/MS56xx.hpp

@@ -12,13 +12,13 @@
  * @brief MS56xx address when the CS pin is connected to GND.
  * 
  */
-#define MS56XX_ADDRESS_CS_LOW       0x1110111
+#define MS56XX_ADDRESS_CS_LOW       (uint8_t)0x77
 
 /**
  * @brief MS56xx address when the CS pin is connected to VDD.
  * 
  */
-#define MS56XX_ADDRESS_CS_HIGH      0x1110110
+#define MS56XX_ADDRESS_CS_HIGH      (uint8_t)0x76
 
 
 namespace sta 
@@ -73,7 +73,7 @@ namespace sta
          */
         enum Operations {
             RESET = 0x1E,
-            READ_PROM = 0xA2,
+            READ_PROM = 0xA0,
             D1_CONVERSION = 0x40,
             D2_CONVERSION = 0x50,
             ADC_RESULT = 0x00
@@ -159,6 +159,14 @@ namespace sta
          */
         void readPROM();
 
+        /**
+         * @brief Initialize the constants used for conversion.
+         * 
+         * @note This code was taken from https://github.com/RobTillaart/MS5611
+         * 
+         */
+        void initConstants(bool mathMode = 0);
+
         /**
          * @brief Convert the pressure value given in Pa to a different unit.
          * 
@@ -201,16 +209,6 @@ namespace sta
          * @return true if successful, false otherwise.
          */
         bool busRead(uint8_t reg, uint8_t * buffer, size_t length);
-
-        /**
-         * @brief Write data to one of the sensor's registers.
-         * 
-         * @param reg The register to write to.
-         * @param buffer The buffer of data to write to the register.
-         * @param length The number of bytes to write.
-         * @return true if successful, false otherwise.
-         */
-        bool busWrite(uint8_t reg, uint8_t * buffer, size_t length);
     private:
         // STA internal object for SPi abstraction
         Device * device_;
@@ -221,13 +219,8 @@ namespace sta
         // Pressure at sealevel. Use the standard atmosphere per default.
         float sealevel_ = 1013.25;
 
-        // 6 Different constants; Includes Offsets, references etc.
+        // The different constants; Includes Offsets, references etc.
-        uint16_t sensibility_;
+        float C_[8];  
-        uint16_t offset_;
-        uint16_t tempSensCoeff_;
-        uint16_t tempOffsetCoeff_;
-        uint16_t refTemp_;
-        uint16_t tempCoeff_;      
 
         // Constants for waiting
         const uint32_t RESET_DELAY = 2800; // in uS

src/MS56xx.cpp

@@ -1,6 +1,7 @@
 #include <sta/drivers/MS56xx.hpp>
 
 #include <sta/debug/assert.hpp>
+#include <sta/debug/debug.hpp>
 #include <sta/lang.hpp>
 
 #include <cmath>
@@ -12,7 +13,8 @@ namespace sta
         : device_{device},
           delay_{delay},
           osr_{level},
-          intf_{Intf::SPI}
+          intf_{Intf::SPI},
+          C_{}
     {
         STA_ASSERT(device != nullptr);
     }
@@ -21,7 +23,8 @@ namespace sta
         : device_{device},
           delay_{delay},
           osr_{osr},
-          intf_{Intf::I2C}
+          intf_{Intf::I2C},
+          C_{}
     {
         STA_ASSERT(device != nullptr);
     }
@@ -31,8 +34,12 @@ namespace sta
         // Reset device on start-up
         this->reset();
 
+        this->initConstants();
+
         // Read the PROM e.g. constants
         this->readPROM();
+
+        return true;
     }
 
     void MS56xx::setOsr(OsrLevel osr)
@@ -49,7 +56,7 @@ namespace sta
     float MS56xx::getPressure(Unit unit /* = Unit::hPa */)
     {
         // Request the ADC to read temperature values.
-        busCommand(MS56xx::Operations::D2_CONVERSION + 2*this->osr_);
+        busCommand(MS56xx::Operations::D1_CONVERSION + 2*this->osr_);
 
         // 8.22 ms conversion according to the datasheet.
         delay_(osrDelay());
@@ -58,45 +65,51 @@ namespace sta
         busRead(MS56xx::Operations::ADC_RESULT, buffer, 3);
 
         // Difference between actual and reference temperature.
-        uint32_t rawTemperature = buffer[0] << 16 | buffer[1] << 8 | buffer[2];
+        uint32_t D1 = buffer[0] << 16 | buffer[1] << 8 | buffer[2];
-        uint32_t dT = rawTemperature - refTemp_ * (0x01<<8);
 
         // Request the ADC to read pressure values.
-        busCommand(MS56xx::Operations::D1_CONVERSION + 2*this->osr_);
-
-        // 8.22 ms conversion according to the datasheet.
-        delay_(osrDelay());
-
-        busRead(MS56xx::Operations::ADC_RESULT, buffer, 3);
-
-        uint32_t rawPressure = buffer[0] << 16 | buffer[1] << 8 | buffer[2];
-
-        // offset and sensitivity at actual temperature.
-        uint64_t offset = offset_ * (0x01<<16) + (tempOffsetCoeff_ * dT) / (0x01<<7);
-        uint64_t sensibility = sensibility_ * (0x01<<15) + (tempSensCoeff_ * dT) / (0x01<<8); 
-
-        // The pressure in Pa.
-        float pressure = (rawPressure * (sensibility / 0x01<<21) - offset) / (0x01<<15);
-        pressure = convertPressure(pressure, unit);
-
-        return pressure;
-    }
-
-    int32_t MS56xx::getTemperature() {
         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 rawTemperature = buffer[0] | buffer[1] << 8 | buffer[2] << 16;
 
-        // Convert the raw values to actual temperature values.
+        uint32_t D2 = buffer[0] << 16 | buffer[1] << 8 | buffer[2];
-        uint32_t dT = rawTemperature - refTemp_ * (0x01<<8);
-        uint32_t temperature = 20 * dT * tempCoeff_ / (0x01<<23);
 
-        return temperature;
+        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;
+            float t = (temperature - 2000) * (temperature - 2000);
+            float offset2 = 2.5 * t;
+            float sens2 = 1.25 * t;
+            //  COMMENT OUT < -1500 CORRECTION IF NOT NEEDED
+            if (temperature < -1500)
+            {
+                t = (temperature + 1500) * (temperature + 1500);
+                offset2 += 7 * t;
+                sens2 += 5.5 * t;
+            }
+            temperature -= T2;
+            offset -= offset2;
+            sens -= sens2;
+        }
+
+        // The pressure in Pa.
+        float pressure = (D1 * sens * 4.76837158205E-7 - offset) * 3.051757813E-5;
+        // pressure = convertPressure(pressure, unit);
+
+        return pressure * 0.01;
+    }
+
+    int32_t MS56xx::getTemperature() {
+       return 21.0f;
     }
 
     void MS56xx::setPressureReference(float pressRef, float altRef, Unit unit /* = Unit::hPa */)
@@ -111,38 +124,38 @@ namespace sta
 
         // Taken from: https://cdn-shop.adafruit.com/datasheets/BST-BMP180-DS000-09.pdf, page 16
         float altitude = 44330 * (1 - std::pow(pressure / sealevel_, 1 / 5.255));
+
+        return altitude;
     }
 
     void MS56xx::readPROM() {
         uint8_t buffer[2];
 
-        busRead(Operations::READ_PROM, buffer, 2);
+        for (size_t i = 0; i < 7; i++)
-        sensibility_ = uint_8BufferTouint16_t(buffer);
+        {
+            busRead(Operations::READ_PROM + i*2, buffer, 2);
 
-        delay_(1000);
+            C_[i] *= uint_8BufferTouint16_t(buffer);
+        }
+    }
 
-        busRead(Operations::READ_PROM+2, buffer, 2);
+    void MS56xx::initConstants(bool mathMode /* = 0 */)
-        offset_ = uint_8BufferTouint16_t(buffer);
+    {
+        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
 
-        delay_(1000);
+        if (mathMode == 1)  //  Appnote version for pressure.
-
+        {
-        busRead(Operations::READ_PROM+4, buffer, 2);
+            C_[1] = 65536L;          //  SENSt1
-        tempSensCoeff_ = uint_8BufferTouint16_t(buffer);
+            C_[2] = 131072L;         //  OFFt1
-
+            C_[3] = 7.8125E-3;       //  TCS
-        delay_(1000);
+            C_[4] = 1.5625e-2;       //  TCO
-
+        }
-        busRead(Operations::READ_PROM+6, buffer, 2);
-        tempOffsetCoeff_ = uint_8BufferTouint16_t(buffer);
-
-        delay_(1000);
-
-        busRead(Operations::READ_PROM+8, buffer, 2);
-        refTemp_ = uint_8BufferTouint16_t(buffer);
-
-        delay_(1000);
-
-        busRead(Operations::READ_PROM+10, buffer, 2);
-        tempCoeff_ = uint_8BufferTouint16_t(buffer);
     }
 
     inline float MS56xx::convertPressure(float pressure, Unit unit)
@@ -179,15 +192,17 @@ namespace sta
         // https://www.amsys-sensor.com/downloads/notes/MS5XXX-C-code-example-for-MS56xx-MS57xx-MS58xx-AMSYS-an520e.pdf
         switch (osr_) {
         case _256:
-            return 900;
+            return 600;
         case _512:
-            return 3000;
+            return 1200;
         case _1024:
-            return 4000;
+            return 2300;
         case _2048:
-            return 6000;
+            return 4600;
         case _4096:
-            return 10000;
+            return 9100;
+        default:
+            STA_UNREACHABLE();
         }
     }
 
@@ -200,9 +215,4 @@ namespace sta
 
         return true;
     }
-
-    bool MS56xx::busWrite(uint8_t reg, uint8_t * buffer, size_t length)
-    {
-        return true;
-    }
 }