Zookeeper support (#20000)

* zookeeper lib

* add ft4222 to dev pip packages and fix include error

* started on CI

* it's a file

* now it should be happy

* use docker for all on-device tests

* test scripts

* does this work?

* access to devices

* too broad. only usb enough?

* permissions for zookeeper usb

* as env var maybe?

* this?

* try this for now

* all devices

* move to correct location for impoerts

* right paths

* not running in the right agent?

* ofc not

* fix broken merge

* add ft4222 package again

* add timeout

* power monitor

* cleanup

Co-authored-by: Batman <batman@openpilot-ci.internal>
Co-authored-by: Adeeb Shihadeh <adeebshihadeh@gmail.com>
old-commit-hash: 6a824d765137e5141115e1727bcd553bd17eaaef

tools/zookeeper/__init__.py

@@ -0,0 +1,82 @@
+#!/usr/bin/env python3
+
+# Python library to control Zookeeper
+
+import ft4222
+import ft4222.I2CMaster
+
+DEBUG = False
+
+INA231_ADDR = 0x40
+INA231_REG_CONFIG = 0x00
+INA231_REG_SHUNT_VOLTAGE = 0x01
+INA231_REG_BUS_VOLTAGE = 0x02
+INA231_REG_POWER = 0x03
+INA231_REG_CURRENT = 0x04
+INA231_REG_CALIBRATION = 0x05
+
+INA231_BUS_LSB = 1.25e-3
+INA231_SHUNT_LSB = 2.5e-6
+SHUNT_RESISTOR = 30e-3
+CURRENT_LSB = 1e-5
+
+class Zookeeper:
+  def __init__(self):
+    if ft4222.createDeviceInfoList() < 2:
+      raise Exception("No connected zookeeper found!")
+    self.dev_a = ft4222.openByDescription("FT4222 A")
+    self.dev_b = ft4222.openByDescription("FT4222 B")
+
+    if DEBUG:
+      for i in range(ft4222.createDeviceInfoList()):
+        print(f"Device {i}: {ft4222.getDeviceInfoDetail(i, False)}")
+
+    # Setup GPIO
+    self.dev_b.gpio_Init(gpio2=ft4222.Dir.OUTPUT, gpio3=ft4222.Dir.OUTPUT)
+    self.dev_b.setSuspendOut(False)
+    self.dev_b.setWakeUpInterrut(False)
+
+    # Setup I2C
+    self.dev_a.i2cMaster_Init(kbps=400)
+    self._initialize_ina()
+
+  # Helper functions
+  def _read_ina_register(self, register, length):
+    self.dev_a.i2cMaster_WriteEx(INA231_ADDR, data=register, flag=ft4222.I2CMaster.Flag.REPEATED_START)
+    return self.dev_a.i2cMaster_Read(INA231_ADDR, bytesToRead=length)
+
+  def _write_ina_register(self, register, data):
+    msg = register.to_bytes(1, byteorder="big") + data.to_bytes(2, byteorder="big")
+    self.dev_a.i2cMaster_Write(INA231_ADDR, data=msg)
+
+  def _initialize_ina(self):
+    # Config
+    self._write_ina_register(INA231_REG_CONFIG, 0x4127)
+
+    # Calibration
+    CAL_VALUE = int(0.00512 / (CURRENT_LSB * SHUNT_RESISTOR))
+    if DEBUG:
+      print(f"Calibration value: {hex(CAL_VALUE)}")
+    self._write_ina_register(INA231_REG_CALIBRATION, CAL_VALUE)
+
+  def _set_gpio(self, number, enabled):
+    self.dev_b.gpio_Write(portNum=number, value=enabled)
+
+  # Public API functions
+  def set_device_power(self, enabled):
+    self._set_gpio(2, enabled)
+
+  def set_device_ignition(self, enabled):
+    self._set_gpio(3, enabled)
+
+  def read_current(self):
+    # Returns in A
+    return int.from_bytes(self._read_ina_register(INA231_REG_CURRENT, 2), byteorder="big") * CURRENT_LSB
+
+  def read_power(self):
+    # Returns in W
+    return int.from_bytes(self._read_ina_register(INA231_REG_POWER, 2), byteorder="big") * CURRENT_LSB * 25
+
+  def read_voltage(self):
+    # Returns in V
+    return int.from_bytes(self._read_ina_register(INA231_REG_BUS_VOLTAGE, 2), byteorder="big") * INA231_BUS_LSB

tools/zookeeper/check_consumption.py

@@ -0,0 +1,30 @@
+#!/usr/bin/env python
+
+import sys
+import time
+from tools.zookeeper import Zookeeper
+
+# Usage: check_consumption.py <averaging_time_sec> <max_average_power_W>
+# Exit code: 0 -> passed
+#	           1 -> failed
+
+z = Zookeeper()
+
+averaging_time_s = int(sys.argv[1])
+max_average_power = float(sys.argv[2])
+
+start_time = time.time()
+measurements = []
+while time.time() - start_time < averaging_time_s:
+  measurements.append(z.read_power())
+  time.sleep(0.1)
+
+average_power = sum(measurements)/len(measurements)
+print(f"Average power: {round(average_power, 4)}W")
+
+if average_power > max_average_power:
+  exit(1)
+
+
+
+

tools/zookeeper/disable.py

@@ -0,0 +1,7 @@
+#!/usr/bin/env python
+
+from tools.zookeeper import Zookeeper
+
+z = Zookeeper()
+z.set_device_power(False)
+

tools/zookeeper/enable_and_wait.py

@@ -0,0 +1,25 @@
+#!/usr/bin/env python
+
+import os
+import sys
+import time
+from tools.zookeeper import Zookeeper
+
+z = Zookeeper()
+z.set_device_power(True)
+
+def is_online(ip):
+  return (os.system(f"ping -c 1 {ip} > /dev/null") == 0)
+
+ip = str(sys.argv[1])
+timeout = int(sys.argv[2])
+start_time = time.time()
+while not is_online(ip):
+  print(f"{ip} not online yet!")
+
+  if time.time() - start_time > timeout:
+    print("Timed out!")
+    raise TimeoutError()
+
+  time.sleep(1)
+

tools/zookeeper/ignition.py

@@ -0,0 +1,8 @@
+#!/usr/bin/env python
+
+import sys
+from tools.zookeeper import Zookeeper
+
+z = Zookeeper()
+z.set_device_ignition(1 if int(sys.argv[1]) > 0 else 0)
+

tools/zookeeper/power_monitor.py

@@ -0,0 +1,30 @@
+#!/usr/bin/env python
+
+import sys
+import time
+from tools.zookeeper import Zookeeper
+
+# Usage: check_consumption.py <averaging_time_sec> <max_average_power_W>
+# Exit code: 0 -> passed
+#	           1 -> failed
+
+if __name__ == "__main__":
+  z = Zookeeper()
+
+  duration = None
+  if len(sys.argv) > 1:
+    duration = int(sys.argv[1])
+
+  try:
+    start_time = time.monotonic()
+    measurements = []
+    while duration is None or time.monotonic() - start_time < duration:
+      p = z.read_power()
+      print(round(p, 3), "W")
+      measurements.append(p)
+      time.sleep(0.25)
+  except KeyboardInterrupt:
+    pass
+  finally:
+    average_power = sum(measurements)/len(measurements)
+    print(f"Average power: {round(average_power, 4)}W")

tools/zookeeper/requirements.txt

@@ -0,0 +1 @@
+ft4222==1.2.1

tools/zookeeper/test_zookeeper.py

@@ -0,0 +1,23 @@
+#!/usr/bin/env python3
+
+import time
+from tools.zookeeper import Zookeeper
+
+z = Zookeeper()
+z.set_device_power(True)
+
+i = 0
+ign = False
+while 1:
+  voltage = round(z.read_voltage(), 2)
+  current = round(z.read_current(), 3)
+  power = round(z.read_power(), 2)
+  z.set_device_ignition(ign)
+  print(f"Voltage: {voltage}V, Current: {current}A, Power: {power}W, Ignition: {ign}")
+
+  if i > 200:
+    ign = not ign
+    i = 0
+  
+  i += 1
+  time.sleep(0.1)