From f4e9fe46f6fd2ae8ebd35c53ee302786b599521f Mon Sep 17 00:00:00 2001
From: Shaikimram <shaikimran2723@gmail.com>
Date: Tue, 21 Jan 2025 10:12:27 +0000
Subject: [PATCH] AP_Baro: Add support for MS5837-02BA sensor

---
 libraries/AP_Baro/AP_Baro_MS5611.cpp | 51 +++++++++++++++++++++++++++-
 libraries/AP_Baro/AP_Baro_MS5611.h   | 14 +++++++-
 2 files changed, 63 insertions(+), 2 deletions(-)

diff --git a/libraries/AP_Baro/AP_Baro_MS5611.cpp b/libraries/AP_Baro/AP_Baro_MS5611.cpp
index 46e3d8d2c39f0d..4de5f57f3e47d9 100644
--- a/libraries/AP_Baro/AP_Baro_MS5611.cpp
+++ b/libraries/AP_Baro/AP_Baro_MS5611.cpp
@@ -114,6 +114,26 @@ bool AP_Baro_MS56XX::_init()
     case BARO_MS5837:
         name = "MS5837";
         prom_read_ok = _read_prom_5637(prom);
+        // Check for MS5837 product ID from PROM Word 0 (bits 11:5)
+        if (prom_read_ok) {
+            uint8_t product_id = (prom[0] >> 5) & 0x7F; // Extract bits 11-5
+            switch (product_id) {
+            case 0b0000000: // MS5837-02BA01
+                name = "MS5837-02BA01";
+                _ms56xx_type = BARO_MS5837_02BA;
+                break;
+            case 0b0010101: // MS5837-02BA21
+                name = "MS5837-02BA21";
+                _ms56xx_type = BARO_MS5837_02BA;
+                break;
+            case 0b0011010: // MS5837-30BA26
+                name = "MS5837-30BA26";
+                _ms56xx_type = BARO_MS5837;
+                break;
+            default:
+                prom_read_ok = false; // Unsupported sensor type
+            }
+        }
         break;
     case BARO_MS5637:
         name = "MS5637";
@@ -155,6 +175,9 @@ bool AP_Baro_MS56XX::_init()
     case BARO_MS5837:
         devtype = DEVTYPE_BARO_MS5837;
         break;
+    case BARO_MS5837_02BA:
+        devtype = DEVTYPE_BARO_MS5837; // Reuse the same dev type for both variants
+        break;
     case BARO_MS5637:
         devtype = DEVTYPE_BARO_MS5637;
         break;
@@ -163,7 +186,7 @@ bool AP_Baro_MS56XX::_init()
     _dev->set_device_type(devtype);
     set_bus_id(_instance, _dev->get_bus_id());
 
-    if (_ms56xx_type == BARO_MS5837) {
+    if (_ms56xx_type == BARO_MS5837 || _ms56xx_type == BARO_MS5837_02BA) {
         _frontend.set_type(_instance, AP_Baro::BARO_TYPE_WATER);
     }
 
@@ -516,5 +539,31 @@ void AP_Baro_MS56XX::_calculate_5837()
 
     _copy_to_frontend(_instance, (float)pressure, temperature);
 }
+// Calculate Temperature and compensated Pressure in real units (Celsius degrees*100, mbar*100).
+void AP_Baro_MS56XX::_calculate_5837_02ba() {
+    int32_t dT = _D2 - ((int32_t)_cal_reg.c5 << 8);
+    int32_t TEMP = 2000 + ((dT * _cal_reg.c6) >> 23);
+
+    int64_t OFF = ((int64_t)_cal_reg.c2 << 17) + (((int64_t)_cal_reg.c4 * dT) >> 6);
+    int64_t SENS = ((int64_t)_cal_reg.c1 << 16) + (((int64_t)_cal_reg.c3 * dT) >> 7);
+
+    if (TEMP < 2000) {
+        // Second-order compensation
+        int32_t T2 = (dT * dT) >> 31;
+        int64_t TEMP_low = TEMP - 2000;
+        int64_t OFF2 = 5 * (TEMP_low * TEMP_low) >> 1;
+        int64_t SENS2 = (TEMP_low * TEMP_low) >> 3;
+
+        TEMP -= T2;
+        OFF -= OFF2;
+        SENS -= SENS2;
+    }
+
+    int32_t pressure = (((_D1 * SENS) >> 21) - OFF) >> 15;
+
+    // Update frontend with calculated values
+    _copy_to_frontend(_instance, (float)pressure / 10, (float)TEMP / 100);
+}
+
 
 #endif  // AP_BARO_MS56XX_ENABLED
diff --git a/libraries/AP_Baro/AP_Baro_MS5611.h b/libraries/AP_Baro/AP_Baro_MS5611.h
index 2f124db2287dae..adc610eab8be53 100644
--- a/libraries/AP_Baro/AP_Baro_MS5611.h
+++ b/libraries/AP_Baro/AP_Baro_MS5611.h
@@ -24,6 +24,11 @@
 #define HAL_BARO_MS5837_I2C_ADDR 0x76
 #endif
 
+#ifndef HAL_BARO_MS5837_02BA_I2C_ADDR
+#define HAL_BARO_MS5837_02BA_I2C_ADDR 0x76
+#endif
+
+
 #ifndef HAL_BARO_MS5637_I2C_ADDR
 #define HAL_BARO_MS5637_I2C_ADDR 0x76
 #endif
@@ -37,7 +42,8 @@ class AP_Baro_MS56XX : public AP_Baro_Backend
         BARO_MS5611 = 0,
         BARO_MS5607 = 1,
         BARO_MS5637 = 2,
-        BARO_MS5837 = 3
+        BARO_MS5837 = 3,
+        BARO_MS5837_02BA = 4 // Add new type here
     };
 
     static AP_Baro_Backend *probe_5611(AP_Baro &baro, AP_HAL::OwnPtr<AP_HAL::Device> dev) {
@@ -52,6 +58,11 @@ class AP_Baro_MS56XX : public AP_Baro_Backend
     static AP_Baro_Backend *probe_5837(AP_Baro &baro, AP_HAL::OwnPtr<AP_HAL::Device> dev) {
         return probe(baro, std::move(dev), BARO_MS5837);
     }
+    static AP_Baro_Backend *probe_5837_02ba(AP_Baro &baro, AP_HAL::OwnPtr<AP_HAL::Device> dev) {
+    return probe(baro, std::move(dev), BARO_MS5837_02BA);
+}
+
+
 
     static AP_Baro_Backend *probe(AP_Baro &baro, AP_HAL::OwnPtr<AP_HAL::Device> dev, enum MS56XX_TYPE ms56xx_type=BARO_MS5611);
 
@@ -73,6 +84,7 @@ class AP_Baro_MS56XX : public AP_Baro_Backend
     void _calculate_5607();
     void _calculate_5637();
     void _calculate_5837();
+    void _calculate_5837_02ba();  // Calculate Temperature and Pressure for MS5837-02BA sensor
     bool _read_prom_5611(uint16_t prom[8]);
     bool _read_prom_5637(uint16_t prom[8]);