From f310cacd411c743b0850b6bc26fcc3ada7078a1d Mon Sep 17 00:00:00 2001
From: anatoly-savchenkov
 <48646998+anatoly-savchenkov@users.noreply.github.com>
Date: Thu, 11 Nov 2021 00:01:47 +0300
Subject: [PATCH] [ms5611] Re-implement conversion from ADC readings to sensor
 values (#2665)

---
 esphome/components/ms5611/ms5611.cpp | 52 +++++++++++++++++++---------
 1 file changed, 35 insertions(+), 17 deletions(-)

diff --git a/esphome/components/ms5611/ms5611.cpp b/esphome/components/ms5611/ms5611.cpp
index 1d7516dbe8..4b34e1d71a 100644
--- a/esphome/components/ms5611/ms5611.cpp
+++ b/esphome/components/ms5611/ms5611.cpp
@@ -75,30 +75,48 @@ void MS5611Component::read_pressure_(uint32_t raw_temperature) {
   const uint32_t raw_pressure = (uint32_t(bytes[0]) << 16) | (uint32_t(bytes[1]) << 8) | (uint32_t(bytes[2]));
   this->calculate_values_(raw_temperature, raw_pressure);
 }
+
+// Calculations are taken from the datasheet which can be found here:
+// https://www.te.com/commerce/DocumentDelivery/DDEController?Action=showdoc&DocId=Data+Sheet%7FMS5611-01BA03%7FB3%7Fpdf%7FEnglish%7FENG_DS_MS5611-01BA03_B3.pdf%7FCAT-BLPS0036
+// Sections PRESSURE AND TEMPERATURE CALCULATION and SECOND ORDER TEMPERATURE COMPENSATION
+// Variable names below match variable names from the datasheet but lowercased
 void MS5611Component::calculate_values_(uint32_t raw_temperature, uint32_t raw_pressure) {
-  const int32_t d_t = int32_t(raw_temperature) - (uint32_t(this->prom_[4]) << 8);
-  float temperature = (2000 + (int64_t(d_t) * this->prom_[5]) / 8388608.0f) / 100.0f;
+  const uint32_t c1 = uint32_t(this->prom_[0]);
+  const uint32_t c2 = uint32_t(this->prom_[1]);
+  const uint16_t c3 = uint16_t(this->prom_[2]);
+  const uint16_t c4 = uint16_t(this->prom_[3]);
+  const int32_t c5 = int32_t(this->prom_[4]);
+  const uint16_t c6 = uint16_t(this->prom_[5]);
+  const uint32_t d1 = raw_pressure;
+  const int32_t d2 = raw_temperature;
 
-  float pressure_offset = (uint32_t(this->prom_[1]) << 16) + ((this->prom_[3] * d_t) >> 7);
-  float pressure_sensitivity = (uint32_t(this->prom_[0]) << 15) + ((this->prom_[2] * d_t) >> 8);
+  // Promote dt to 64 bit here to make the math below cleaner
+  const int64_t dt = d2 - (c5 << 8);
+  int32_t temp = (2000 + ((dt * c6) >> 23));
 
-  if (temperature < 20.0f) {
-    const float t2 = (d_t * d_t) / 2147483648.0f;
-    const float temp20 = (temperature - 20.0f) * 100.0f;
-    float pressure_offset_2 = 2.5f * temp20 * temp20;
-    float pressure_sensitivity_2 = 1.25f * temp20 * temp20;
-    if (temp20 < -15.0f) {
-      const float temp15 = (temperature + 15.0f) * 100.0f;
-      pressure_offset_2 += 7.0f * temp15;
-      pressure_sensitivity_2 += 5.5f * temp15;
+  int64_t off = (c2 << 16) + ((dt * c4) >> 7);
+  int64_t sens = (c1 << 15) + ((dt * c3) >> 8);
+
+  if (temp < 2000) {
+    const int32_t t2 = (dt * dt) >> 31;
+    int32_t off2 = ((5 * (temp - 2000) * (temp - 2000)) >> 1);
+    int32_t sens2 = ((5 * (temp - 2000) * (temp - 2000)) >> 2);
+    if (temp < -1500) {
+      off2 = (off2 + 7 * (temp + 1500) * (temp + 1500));
+      sens2 = sens2 + ((11 * (temp + 1500) * (temp + 1500)) >> 1);
     }
-    temperature -= t2;
-    pressure_offset -= pressure_offset_2;
-    pressure_sensitivity -= pressure_sensitivity_2;
+    temp = temp - t2;
+    off = off - off2;
+    sens = sens - sens2;
   }
 
-  const float pressure = ((raw_pressure * pressure_sensitivity) / 2097152.0f - pressure_offset) / 3276800.0f;
+  // Here we multiply unsigned 32-bit by signed 64-bit using signed 64-bit math.
+  // Possible ranges of D1 and SENS from the datasheet guarantee
+  // that this multiplication does not overflow
+  const int32_t p = ((((d1 * sens) >> 21) - off) >> 15);
 
+  const float temperature = temp / 100.0f;
+  const float pressure = p / 100.0f;
   ESP_LOGD(TAG, "Got temperature=%0.02f°C pressure=%0.01fhPa", temperature, pressure);
 
   if (this->temperature_sensor_ != nullptr)