From afe9b83b01e0189769fa5ec5c7d4dc3597d67559 Mon Sep 17 00:00:00 2001
From: dario <dario.vandenboom@arcor.de>
Date: Tue, 6 Aug 2024 11:44:34 +0200
Subject: [PATCH] Added simple barometric formula to MS5611 driver

---
 include/sta/drivers/MS56xx.hpp |  4 ++++
 src/MS56xx.cpp                 | 12 +++++++++++-
 src/mock.cpp                   | 24 +++++++++++++++++-------
 3 files changed, 32 insertions(+), 8 deletions(-)

diff --git a/include/sta/drivers/MS56xx.hpp b/include/sta/drivers/MS56xx.hpp
index 3452558..2ca87d4 100644
--- a/include/sta/drivers/MS56xx.hpp
+++ b/include/sta/drivers/MS56xx.hpp
@@ -169,6 +169,10 @@ namespace sta
          * @return float The altitude estimate in meters.
          */
         float getAltitudeEstimate();
+
+        static float getSeaLevel(float pressRef, float altRef);
+
+        static float getAltitudeEstimate(float pressure, float sealevel);
     private:
         /**
          * @brief Reset the sensor.
diff --git a/src/MS56xx.cpp b/src/MS56xx.cpp
index 17e8599..8e6882a 100644
--- a/src/MS56xx.cpp
+++ b/src/MS56xx.cpp
@@ -121,6 +121,16 @@ namespace sta
         return altitude;
     }
 
+    float MS56xx::getSeaLevel(float pressRef, float altRef)
+    {
+    	return pressRef / (std::pow((1 - altRef / 44330), 5.255));
+    }
+
+    float MS56xx::getAltitudeEstimate(float pressure, float sealevel)
+    {
+    	return 44330 * (1 - std::pow(pressure / sealevel, 1 / 5.255));
+    }
+
     void MS56xx::readPROM() {
         uint8_t buffer[2];
 
@@ -218,4 +228,4 @@ namespace sta
     }
 }
 
-#endif // STA_SPATZ_ENABLED
\ No newline at end of file
+#endif // STA_SPATZ_ENABLED
diff --git a/src/mock.cpp b/src/mock.cpp
index e274ed3..a66818c 100644
--- a/src/mock.cpp
+++ b/src/mock.cpp
@@ -93,15 +93,25 @@ namespace sta
         sealevel_ = pressRef / (std::pow((1 - altRef / 44330), 5.255));
     }
 
-    float MS56xx::getAltitudeEstimate()
-    {
-        float pressure = getPressure(Unit::hPa);
+	float MS56xx::getAltitudeEstimate()
+	{
+		float pressure = getPressure(Unit::hPa);
 
-        // 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));
+		// 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;
-    }
+		return altitude;
+	}
+
+	float MS56xx::getSeaLevel(float pressRef, float altRef)
+	{
+		return pressRef / (std::pow((1 - altRef / 44330), 5.255));
+	}
+
+	float MS56xx::getAltitudeEstimate(float pressure, float sealevel)
+	{
+		return 44330 * (1 - std::pow(pressure / sealevel, 1 / 5.255));
+	}
 
     void MS56xx::readPROM() {