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>

.pylintrc

@@ -161,7 +161,7 @@ contextmanager-decorators=contextlib.contextmanager
 # List of members which are set dynamically and missed by pylint inference
 # system, and so shouldn't trigger E1101 when accessed. Python regular
 # expressions are accepted.
-generated-members=capnp.* cereal.* pygame.* zmq.* setproctitle.* smbus2.* usb1.* serial.* cv2.*
+generated-members=capnp.* cereal.* pygame.* zmq.* setproctitle.* smbus2.* usb1.* serial.* cv2.* ft4222.*
 
 # Tells whether missing members accessed in mixin class should be ignored. A
 # mixin class is detected if its name ends with "mixin" (case insensitive).

Jenkinsfile

@@ -112,6 +112,10 @@ pipeline {
         stage('On-device Tests') {
           agent {
             docker {
+              /*
+              filename 'Dockerfile.ondevice_ci'
+              args "--privileged -v /dev:/dev --shm-size=1G --user=root"
+              */
               image 'python:3.7.3'
               args '--user=root'
             }
@@ -155,6 +159,27 @@ pipeline {
                   }
                 }
 
+                /*
+                stage('Power Consumption Tests') {
+                  steps {
+                    lock(resource: "", label: "c2-zookeeper", inversePrecedence: true, variable: 'device_ip', quantity: 1) {
+                      timeout(time: 60, unit: 'MINUTES') {
+                        sh script: "/home/batman/tools/zookeeper/enable_and_wait.py $device_ip 120", label: "turn on device"
+                        phone(device_ip, "git checkout", readFile("selfdrive/test/setup_device_ci.sh"),)
+                        phone(device_ip, "build", "SCONS_CACHE=1 scons -j4 && sync")
+                        sh script: "/home/batman/tools/zookeeper/disable.py $device_ip", label: "turn off device"
+                        sh script: "/home/batman/tools/zookeeper/enable_and_wait.py $device_ip 120", label: "turn on device"
+                        sh script: "/home/batman/tools/zookeeper/check_consumption.py 60 3", label: "idle power consumption after boot"
+                        sh script: "/home/batman/tools/zookeeper/ignition.py 1", label: "go onroad"
+                        sh script: "/home/batman/tools/zookeeper/check_consumption.py 60 10", label: "onroad power consumption"
+                        sh script: "/home/batman/tools/zookeeper/ignition.py 0", label: "go offroad"
+                        sh script: "/home/batman/tools/zookeeper/check_consumption.py 60 2", label: "idle power consumption offroad"
+                      }
+                    }
+                  }
+                }
+                */
+
                 stage('Tici Build') {
                   environment {
                     R3_PUSH = "${env.BRANCH_NAME == 'master' ? '1' : ' '}"

Pipfile

@@ -71,6 +71,7 @@ pyprof2calltree = "*"
 pre-commit = "*"
 mypy = "*"
 parameterized = "*"
+ft4222 = "*"
 
 [packages]
 atomicwrites = "*"

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)