wip

tools/zookeeper/__init__.py

@@ -0,0 +1,78 @@
+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,27 @@
+#!/usr/bin/env python3
+
+import sys
+import time
+from openpilot.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()
+
+  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,8 @@
+#!/usr/bin/env python3
+
+from openpilot.tools.zookeeper import Zookeeper
+
+if __name__ == "__main__":
+  z = Zookeeper()
+  z.set_device_power(False)
+

tools/zookeeper/enable_and_wait.py

@@ -0,0 +1,31 @@
+#!/usr/bin/env python3
+import os
+import sys
+import time
+from socket import gethostbyname, gaierror
+from openpilot.tools.zookeeper import Zookeeper
+
+def is_online(ip):
+  try:
+    addr = gethostbyname(ip)
+    return (os.system(f"ping -c 1 {addr} > /dev/null") == 0)
+  except gaierror:
+    return False
+
+if __name__ == "__main__":
+  z = Zookeeper()
+  z.set_device_power(True)
+
+
+  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,10 @@
+#!/usr/bin/env python3
+
+import sys
+from openpilot.tools.zookeeper import Zookeeper
+
+
+if __name__ == "__main__":
+  z = Zookeeper()
+  z.set_device_ignition(1 if int(sys.argv[1]) > 0 else 0)
+

tools/zookeeper/power_monitor.py

@@ -0,0 +1,40 @@
+#!/usr/bin/env python3
+import sys
+import time
+import datetime
+
+from openpilot.common.realtime import Ratekeeper
+from openpilot.common.filter_simple import FirstOrderFilter
+from openpilot.tools.zookeeper import Zookeeper
+
+if __name__ == "__main__":
+  z = Zookeeper()
+  z.set_device_power(True)
+  z.set_device_ignition(False)
+
+  duration = None
+  if len(sys.argv) > 1:
+    duration = int(sys.argv[1])
+
+  rate = 123
+  rk = Ratekeeper(rate, print_delay_threshold=None)
+  fltr = FirstOrderFilter(0, 5, 1. / rate, initialized=False)
+
+  measurements = []
+  start_time = time.monotonic()
+
+  try:
+    while duration is None or time.monotonic() - start_time < duration:
+      fltr.update(z.read_power())
+      if rk.frame % rate == 0:
+        measurements.append(fltr.x)
+        t = datetime.timedelta(seconds=time.monotonic() - start_time)
+        avg = sum(measurements) / len(measurements)
+        print(f"Now: {fltr.x:.2f} W, Avg: {avg:.2f} W over {t}")
+      rk.keep_time()
+  except KeyboardInterrupt:
+    pass
+
+  t = datetime.timedelta(seconds=time.monotonic() - start_time)
+  avg = sum(measurements) / len(measurements)
+  print(f"\nAverage power: {avg:.2f}W over {t}")

tools/zookeeper/test_zookeeper.py

@@ -0,0 +1,25 @@
+#!/usr/bin/env python3
+
+import time
+from openpilot.tools.zookeeper import Zookeeper
+
+
+if __name__ == "__main__":
+  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)