From 6ef3ae891821ea282289a7f3f980acc798eda13a Mon Sep 17 00:00:00 2001
From: Comma Device <device@comma.ai>
Date: Mon, 1 Aug 2022 19:25:35 -0700
Subject: [PATCH] update tici power monitor

old-commit-hash: 82fbd1414428f9c4cb9b7542a55858ea5cdcd3b0
---
 system/hardware/tici/power_monitor.py | 71 ++++++++++-----------------
 1 file changed, 27 insertions(+), 44 deletions(-)

diff --git a/system/hardware/tici/power_monitor.py b/system/hardware/tici/power_monitor.py
index 07362c8398..3e00376b23 100755
--- a/system/hardware/tici/power_monitor.py
+++ b/system/hardware/tici/power_monitor.py
@@ -1,15 +1,16 @@
 #!/usr/bin/env python3
 import sys
 import time
-import numpy as np
+import datetime
 from typing import List
 
 from common.realtime import Ratekeeper
+from common.filter_simple import FirstOrderFilter
 
-def average(avg, sample):
-  # Weighted avg between existing value and new sample
-  return ((avg[0] * avg[1] + sample) / (avg[1] + 1), avg[1] + 1)
 
+def read_power():
+  with open("/sys/bus/i2c/devices/0-0040/hwmon/hwmon1/power1_input") as f:
+    return int(f.read()) / 1e6
 
 def sample_power(seconds=5) -> List[float]:
   rate = 123
@@ -17,8 +18,7 @@ def sample_power(seconds=5) -> List[float]:
 
   pwrs = []
   for _ in range(rate*seconds):
-    with open("/sys/bus/i2c/devices/0-0040/hwmon/hwmon1/power1_input") as f:
-      pwrs.append(int(f.read()) / 1e6)
+    pwrs.append(read_power())
     rk.keep_time()
   return pwrs
 
@@ -27,47 +27,30 @@ def get_power(seconds=5):
   return np.mean(pwrs)
 
 
-if __name__ == '__main__':
-
-  sample_time = None
+if __name__ == "__main__":
+  duration = None
   if len(sys.argv) > 1:
-    sample_time = int(sys.argv[1])
-
-  start_time = time.monotonic()
-  try:
-    voltage_average = (0, 0)  # average, count
-    current_average = (0, 0)
-    power_average = (0, 0)
-    while sample_time is None or time.monotonic() - start_time < sample_time:
-      with open("/sys/bus/i2c/devices/0-0040/hwmon/hwmon1/in1_input") as f:
-        voltage_total = int(f.read()) / 1000.
-
-      with open("/sys/bus/i2c/devices/0-0040/hwmon/hwmon1/curr1_input") as f:
-        current_total = int(f.read())
+    duration = int(sys.argv[1])
 
-      # SOM measurements are questionable
-      #with open("/sys/class/power_supply/bms/voltage_now") as f:
-      #  voltage = int(f.read()) / 1e6   # volts
-      #with open("/sys/class/power_supply/bms/current_now") as f:
-      #  current = int(f.read()) / 1e3   # ma
-
-      power_total = voltage_total*current_total
+  rate = 23
+  rk = Ratekeeper(rate, print_delay_threshold=None)
+  fltr = FirstOrderFilter(0, 5, 1. / rate, initialized=False)
 
-      # compute averages
-      voltage_average = average(voltage_average, voltage_total)
-      current_average = average(current_average, current_total)
-      power_average = average(power_average, power_total)
+  measurements = []
+  start_time = time.monotonic()
 
-      print(f"now: {power_total:.2f} mW, avg: {power_average[0]:.2f} mW")
-      time.sleep(0.25)
+  try:
+    while duration is None or time.monotonic() - start_time < duration:
+      fltr.update(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
-  finally:
-    stop_time = time.monotonic()
-    print("\n----------------------Average-----------------------------------")
-    voltage = voltage_average[0]
-    current = current_average[0]
-    power = power_average[0]
-    print(f"{voltage:.2f} volts {current:12.2f} ma {power:12.2f} mW {power_total:12.2f} mW")
-    print(f"  {stop_time - start_time:.2f} Seconds     {voltage_average[1]} samples")
-    print("----------------------------------------------------------------")
+
+  t = datetime.timedelta(seconds=time.monotonic() - start_time)
+  avg = sum(measurements) / len(measurements)
+  print(f"\nAverage power: {avg:.2f}W over {t}")