From cc4110140cfaa34556035cea3d9b3663858d3699 Mon Sep 17 00:00:00 2001
From: Peter Barker <pbarker@barker.dropbear.id.au>
Date: Thu, 21 Nov 2024 23:06:35 -0600
Subject: [PATCH] Tools: add support for ina3221 triple-channel current/voltage
 sensor

---
 Tools/autotest/ardusub.py      | 46 ++++++++++++++++++++++++++++++++++
 Tools/scripts/build_options.py |  1 +
 2 files changed, 47 insertions(+)

diff --git a/Tools/autotest/ardusub.py b/Tools/autotest/ardusub.py
index 309c22b8c4..a0eb71b5da 100644
--- a/Tools/autotest/ardusub.py
+++ b/Tools/autotest/ardusub.py
@@ -947,6 +947,51 @@ class AutoTestSub(vehicle_test_suite.TestSuite):
         self.wait_ready_to_arm()
         self.assert_mag_fusion_selection(MagFuseSel.FUSE_MAG)
 
+    def INA3221(self):
+        '''test INA3221 driver'''
+        self.set_parameters({
+            "BATT2_MONITOR": 30,
+            "BATT3_MONITOR": 30,
+            "BATT4_MONITOR": 30,
+        })
+        self.reboot_sitl()
+        self.set_parameters({
+            "BATT2_I2C_ADDR": 0x42,
+            "BATT2_I2C_BUS": 1,
+            "BATT2_CHANNEL": 1,
+
+            "BATT3_I2C_ADDR": 0x42,
+            "BATT3_I2C_BUS": 1,
+            "BATT3_CHANNEL": 2,
+
+            "BATT4_I2C_ADDR": 0x42,
+            "BATT4_I2C_BUS": 1,
+            "BATT4_CHANNEL": 3,
+        })
+        self.reboot_sitl()
+
+        seen_1 = False
+        seen_3 = False
+        tstart = self.get_sim_time()
+        while not (seen_1 and seen_3):
+            m = self.assert_receive_message('BATTERY_STATUS')
+            print(self.dump_message_verbose(m))
+            if self.get_sim_time() - tstart > 1:
+                break
+            continue
+            if m.id == 1:
+                self.assert_message_field_values(m, {
+                    "current_battery": 7.28 * 100,
+                })
+                # "voltages[0]": 12 * 1000,
+                seen_1 = True
+            if m.id == 3:
+                self.assert_message_field_values(m, {
+                    "current_battery": 2.24 * 100,
+                })
+                # "voltages[0]": 3.14159 * 1000,
+                seen_3 = True
+
     def tests(self):
         '''return list of all tests'''
         ret = super(AutoTestSub, self).tests()
@@ -978,6 +1023,7 @@ class AutoTestSub(vehicle_test_suite.TestSuite):
             self.SetGlobalOrigin,
             self.BackupOrigin,
             self.FuseMag,
+            self.INA3221,
         ])
 
         return ret
diff --git a/Tools/scripts/build_options.py b/Tools/scripts/build_options.py
index 1c259adeae..b140fe125b 100644
--- a/Tools/scripts/build_options.py
+++ b/Tools/scripts/build_options.py
@@ -56,6 +56,7 @@ BUILD_OPTIONS = [
     Feature('Battery', 'BATTERY_FUELLEVEL_ANALOG', 'AP_BATTERY_FUELLEVEL_ANALOG_ENABLED', 'Enable Analog Fuel level battry monitor', 0, None),  # noqa: E501
     Feature('Battery', 'BATTERY_SMBUS', 'AP_BATTERY_SMBUS_ENABLED', 'Enable SMBUS battery monitor', 0, None),
     Feature('Battery', 'BATTERY_INA2XX', 'AP_BATTERY_INA2XX_ENABLED', 'Enable INA2XX battery monitor', 0, None),
+    Feature('Battery', 'BATTERY_INA3221', 'AP_BATTERY_INA3221_ENABLED', 'Enable INA3221 battery monitor', 0, None),
     Feature('Battery', 'BATTERY_SYNTHETIC_CURRENT', 'AP_BATTERY_SYNTHETIC_CURRENT_ENABLED', 'Enable Synthetic Current monitor', 0, None), # noqa: E501
     Feature('Battery', 'BATTERY_ESC_TELEM_OUT', 'AP_BATTERY_ESC_TELEM_OUTBOUND_ENABLED', 'Enable Ability to put battery monitor data into ESC telem stream', 0, None), # noqa: E501
     Feature('Battery', 'BATTERY_SUM', 'AP_BATTERY_SUM_ENABLED', 'Enable Synthetic sum-of-other-batteries backend', 0, None), # noqa: E501