From 6713a203f0b91daead897a2c39b2dcf190ae1b32 Mon Sep 17 00:00:00 2001
From: Andrew Tridgell <andrew@tridgell.net>
Date: Fri, 2 Feb 2024 16:36:11 +1100
Subject: [PATCH] AP_BattMonitor: added temperature reading to INA2xx driver

---
 .../AP_BattMonitor/AP_BattMonitor_INA2xx.cpp  | 28 ++++++++++++++++---
 .../AP_BattMonitor/AP_BattMonitor_INA2xx.h    |  7 ++++-
 2 files changed, 30 insertions(+), 5 deletions(-)

diff --git a/libraries/AP_BattMonitor/AP_BattMonitor_INA2xx.cpp b/libraries/AP_BattMonitor/AP_BattMonitor_INA2xx.cpp
index cab13b302f191..94be30abc7b9f 100644
--- a/libraries/AP_BattMonitor/AP_BattMonitor_INA2xx.cpp
+++ b/libraries/AP_BattMonitor/AP_BattMonitor_INA2xx.cpp
@@ -31,8 +31,10 @@ extern const AP_HAL::HAL& hal;
 #define REG_228_CURRENT       0x07
 #define REG_228_MANUFACT_ID   0x3e
 #define REG_228_DEVICE_ID     0x3f
+#define REG_228_DIETEMP       0x06
+#define INA_228_TEMP_C_LSB    7.8125e-3
 
-// INA238 specific registers
+// INA237/INA238 specific registers
 #define REG_238_CONFIG        0x00
 #define  REG_238_CONFIG_RESET   0x8000
 #define REG_238_ADC_CONFIG    0x01
@@ -41,6 +43,8 @@ extern const AP_HAL::HAL& hal;
 #define REG_238_CURRENT       0x07
 #define REG_238_MANUFACT_ID   0x3e
 #define REG_238_DEVICE_ID     0x3f
+#define REG_238_DIETEMP       0x06
+#define INA_238_TEMP_C_LSB    7.8125e-3 // need to mask bottom 4 bits
 
 #ifndef DEFAULT_BATTMON_INA2XX_MAX_AMPS
 #define DEFAULT_BATTMON_INA2XX_MAX_AMPS 90.0
@@ -262,10 +266,12 @@ bool AP_BattMonitor_INA2XX::detect_device(void)
 
     if (read_word16(REG_228_MANUFACT_ID, id) && id == 0x5449 &&
         read_word16(REG_228_DEVICE_ID, id) && (id&0xFFF0) == 0x2280) {
+        has_temp = true;
         return configure(DevType::INA228);
     }
     if (read_word16(REG_238_MANUFACT_ID, id) && id == 0x5449 &&
         read_word16(REG_238_DEVICE_ID, id) && (id&0xFFF0) == 0x2380) {
+        has_temp = true;
         return configure(DevType::INA238);
     }
     if (read_word16(REG_226_MANUFACT_ID, id) && id == 0x5449 &&
@@ -311,8 +317,10 @@ void AP_BattMonitor_INA2XX::timer(void)
 
     case DevType::INA228: {
         int32_t bus_voltage24, current24;
+        int16_t temp16;
         if (!read_word24(REG_228_VBUS, bus_voltage24) ||
-            !read_word24(REG_228_CURRENT, current24)) {
+            !read_word24(REG_228_CURRENT, current24) ||
+            !read_word16(REG_228_DIETEMP, temp16)) {
             failed_reads++;
             if (failed_reads > 10) {
                 // device has disconnected, we need to reconfigure it
@@ -322,13 +330,15 @@ void AP_BattMonitor_INA2XX::timer(void)
         }
         voltage = (bus_voltage24>>4) * voltage_LSB;
         current = (current24>>4) * current_LSB;
+        temperature = temp16 * INA_228_TEMP_C_LSB;
         break;
     }
 
     case DevType::INA238: {
-        int16_t bus_voltage16, current16;
+        int16_t bus_voltage16, current16, temp16;
         if (!read_word16(REG_238_VBUS, bus_voltage16) ||
-            !read_word16(REG_238_CURRENT, current16)) {
+            !read_word16(REG_238_CURRENT, current16) ||
+            !read_word16(REG_238_DIETEMP, temp16)) {
             failed_reads++;
             if (failed_reads > 10) {
                 // device has disconnected, we need to reconfigure it
@@ -338,6 +348,7 @@ void AP_BattMonitor_INA2XX::timer(void)
         }
         voltage = bus_voltage16 * voltage_LSB;
         current = current16 * current_LSB;
+        temperature = (temp16&0xFFF0) * INA_238_TEMP_C_LSB;
         break;
     }
     }
@@ -350,4 +361,13 @@ void AP_BattMonitor_INA2XX::timer(void)
     accumulate.count++;
 }
 
+/*
+  get last temperature
+ */
+bool AP_BattMonitor_INA2XX::get_temperature(float &temp) const
+{
+    temp = temperature;
+    return has_temp;
+}
+
 #endif // AP_BATTERY_INA2XX_ENABLED
diff --git a/libraries/AP_BattMonitor/AP_BattMonitor_INA2xx.h b/libraries/AP_BattMonitor/AP_BattMonitor_INA2xx.h
index d49e7c82746da..b23b4420eacfc 100644
--- a/libraries/AP_BattMonitor/AP_BattMonitor_INA2xx.h
+++ b/libraries/AP_BattMonitor/AP_BattMonitor_INA2xx.h
@@ -17,9 +17,10 @@ class AP_BattMonitor_INA2XX : public AP_BattMonitor_Backend
                           AP_BattMonitor_Params &params);
 
     bool has_cell_voltages() const override { return false; }
-    bool has_temperature() const override { return false; }
+    bool has_temperature() const override { return has_temp; }
     bool has_current() const override { return true; }
     bool get_cycle_count(uint16_t &cycles) const override { return false; }
+    bool get_temperature(float &temperature) const override;
 
     void init(void) override;
     void read() override;
@@ -63,6 +64,10 @@ class AP_BattMonitor_INA2XX : public AP_BattMonitor_Backend
     } accumulate;
     float current_LSB;
     float voltage_LSB;
+
+    float temperature;
+
+    bool has_temp;
 };
 
 #endif // AP_BATTERY_INA2XX_ENABLED