Offroad power monitoring (#1067)

* Untested implementation of offroad power monitoring

* Fixed some syntax errors

* Cast to int

* Fixed pylint

* Wrapped in class

* Put pulsed calc in own thread

* Longer timeout before starting pulse measurement

* Fudge factor + flake8

* Made integration thread-safe and catch charge disable exceptions

* Catch all calculation errors

* Fixed networkstrength removal

release/files_common

@@ -60,7 +60,6 @@ selfdrive/crash.py
 selfdrive/launcher.py
 selfdrive/manager.py
 selfdrive/swaglog.py
-selfdrive/thermald.py
 selfdrive/logmessaged.py
 selfdrive/tombstoned.py
 selfdrive/pandad.py
@@ -295,6 +294,9 @@ selfdrive/sensord/sensors.cc
 selfdrive/sensord/sensord
 selfdrive/sensord/gpsd
 
+selfdrive/thermald/thermald.py
+selfdrive/thermald/power_monitoring.py
+
 selfdrive/test/__init__.py
 selfdrive/test/longitudinal_maneuvers/*.py
 selfdrive/test/test_openpilot.py

selfdrive/manager.py

@@ -136,7 +136,7 @@ ThermalStatus = cereal.log.ThermalData.ThermalStatus
 
 # comment out anything you don't want to run
 managed_processes = {
-  "thermald": "selfdrive.thermald",
+  "thermald": "selfdrive.thermald.thermald",
   "uploader": "selfdrive.loggerd.uploader",
   "deleter": "selfdrive.loggerd.deleter",
   "controlsd": "selfdrive.controls.controlsd",

selfdrive/thermald/power_monitoring.py

@@ -0,0 +1,145 @@
+import time
+import datetime
+import threading
+import random
+from statistics import mean
+from cereal import log
+
+PANDA_OUTPUT_VOLTAGE = 5.28
+
+# Parameters
+def get_battery_capacity():
+  return _read_param("/sys/class/power_supply/battery/capacity", int)
+
+def get_battery_status():
+  # This does not correspond with actual charging or not. 
+  # If a USB cable is plugged in, it responds with 'Charging', even when charging is disabled
+  return _read_param("/sys/class/power_supply/battery/status", lambda x: x.strip(), '')
+
+def get_battery_current():
+  return _read_param("/sys/class/power_supply/battery/current_now", int)
+
+def get_battery_voltage():
+  return _read_param("/sys/class/power_supply/battery/voltage_now", int)
+
+def get_usb_present():
+  return _read_param("/sys/class/power_supply/usb/present", lambda x: bool(int(x)), False)
+
+def get_battery_charging():
+  # This does correspond with actually charging
+  return _read_param("/sys/class/power_supply/battery/charge_type", lambda x: x.strip() != "N/A", False)
+
+def set_battery_charging(on):
+  with open('/sys/class/power_supply/battery/charging_enabled', 'w') as f:
+    f.write(f"{1 if on else 0}\n")
+
+# Helpers
+def _read_param(path, parser, default=0):
+  try:
+    with open(path) as f:
+      return parser(f.read())
+  except Exception:
+    return default
+
+def panda_current_to_actual_current(panda_current):
+  # From white/grey panda schematic
+  return (3.3 - (panda_current * 3.3 / 4096)) / 8.25
+
+class PowerMonitoring:
+  def __init__(self):
+    self.last_measurement_time = None           # Used for integration delta
+    self.power_used_uWh = 0                     # Integrated power usage in uWh since going into offroad
+    self.next_pulsed_measurement_time = None
+    self.integration_lock = threading.Lock()
+
+  # Calculation tick
+  def calculate(self, health):
+    try:
+      now = time.time()
+
+      # Check that time is valid
+      if datetime.datetime.fromtimestamp(now).year < 2019:
+        return
+
+      # Only integrate when there is no ignition
+      # If health is None, we're probably not in a car, so we don't care
+      if health == None or (health.health.ignitionLine or health.health.ignitionCan):
+        self.last_measurement_time = None
+        self.power_used_uWh = 0
+        return
+
+      # First measurement, set integration time
+      if self.last_measurement_time == None:
+        self.last_measurement_time = now
+        return
+
+      # Get current power draw somehow
+      current_power = 0
+      if get_battery_status() == 'Discharging':
+        # If the battery is discharging, we can use this measurement
+        # On C2: this is low by about 10-15%, probably mostly due to UNO draw not being factored in
+        current_power = ((get_battery_voltage() / 1000000)  * (get_battery_current() / 1000000))
+      elif (health.health.hwType in [log.HealthData.HwType.whitePanda, log.HealthData.HwType.greyPanda]) and (health.health.current > 1):
+        # If white/grey panda, use the integrated current measurements if the measurement is not 0
+        # If the measurement is 0, the current is 400mA or greater, and out of the measurement range of the panda
+        # This seems to be accurate to about 5%
+        current_power = (PANDA_OUTPUT_VOLTAGE * panda_current_to_actual_current(health.health.current))
+      elif (self.next_pulsed_measurement_time != None) and (self.next_pulsed_measurement_time <= now):
+        # TODO: Figure out why this is off by a factor of 3/4???
+        FUDGE_FACTOR = 1.33
+
+        # Turn off charging for about 10 sec in a thread that does not get killed on SIGINT, and perform measurement here to avoid blocking thermal
+        def perform_pulse_measurement(now):
+          try:
+            set_battery_charging(False)
+            time.sleep(5)
+            
+            # Measure for a few sec to get a good average
+            voltages = []
+            currents = []
+            for i in range(6):
+              voltages.append(get_battery_voltage())
+              currents.append(get_battery_current())
+              time.sleep(1)
+            current_power = ((mean(voltages) / 1000000)  * (mean(currents) / 1000000))
+            self._perform_integration(now, current_power * FUDGE_FACTOR)
+
+            # Enable charging again
+            set_battery_charging(True)
+          except Exception as e:
+            print("Pulsed power measurement failed:", str(e))
+        
+        # Start pulsed measurement and return
+        threading.Thread(target=perform_pulse_measurement, args=(now,)).start()
+        self.next_pulsed_measurement_time = None
+        return
+        
+      elif self.next_pulsed_measurement_time == None:
+        # On a charging EON with black panda, or drawing more than 400mA out of a white/grey one
+        # Only way to get the power draw is to turn off charging for a few sec and check what the discharging rate is
+        # We shouldn't do this very often, so make sure it has been some long-ish random time interval
+        self.next_pulsed_measurement_time = now + random.randint(120, 180)
+        return
+      else:
+        # Do nothing
+        return
+
+      # Do the integration
+      self._perform_integration(now, current_power)
+    except Exception as e:
+      print("Power monitoring calculation failed:", str(e))
+
+  def _perform_integration(self, t, current_power):
+    self.integration_lock.acquire()
+    integration_time_h = (t - self.last_measurement_time) / 3600
+    self.power_used_uWh += (current_power * 1000000) * integration_time_h
+    self.last_measurement_time = t
+    self.integration_lock.release()
+
+  # Get the power usage
+  def get_power_used(self):
+    return int(self.power_used_uWh)
+
+
+
+

selfdrive/thermald/thermald.py

@@ -17,6 +17,7 @@ from selfdrive.swaglog import cloudlog
 import cereal.messaging as messaging
 from selfdrive.loggerd.config import get_available_percent
 from selfdrive.pandad import get_expected_signature
+from selfdrive.thermald.power_monitoring import PowerMonitoring, get_battery_capacity, get_battery_status, get_battery_current, get_battery_voltage, get_usb_present
 
 FW_SIGNATURE = get_expected_signature()
 
@@ -180,6 +181,7 @@ def thermald_thread():
     handle_fan = handle_fan_eon
 
   params = Params()
+  pm = PowerMonitoring()
 
   while 1:
     health = messaging.recv_sock(health_sock, wait=True)
@@ -208,20 +210,11 @@ def thermald_thread():
     msg.thermal.cpuPerc = int(round(psutil.cpu_percent()))
     msg.thermal.networkType = network_type
     msg.thermal.networkStrength = network_strength
-
-    try:
-      with open("/sys/class/power_supply/battery/capacity") as f:
-        msg.thermal.batteryPercent = int(f.read())
-      with open("/sys/class/power_supply/battery/status") as f:
-        msg.thermal.batteryStatus = f.read().strip()
-      with open("/sys/class/power_supply/battery/current_now") as f:
-        msg.thermal.batteryCurrent = int(f.read())
-      with open("/sys/class/power_supply/battery/voltage_now") as f:
-        msg.thermal.batteryVoltage = int(f.read())
-      with open("/sys/class/power_supply/usb/present") as f:
-        msg.thermal.usbOnline = bool(int(f.read()))
-    except FileNotFoundError:
-      pass
+    msg.thermal.batteryPercent = get_battery_capacity()
+    msg.thermal.batteryStatus = get_battery_status()
+    msg.thermal.batteryCurrent = get_battery_current()
+    msg.thermal.batteryVoltage = get_battery_voltage()
+    msg.thermal.usbOnline = get_usb_present()
 
     # Fake battery levels on uno for frame
     if is_uno:
@@ -368,6 +361,10 @@ def thermald_thread():
          started_seen and (sec_since_boot() - off_ts) > 60:
         os.system('LD_LIBRARY_PATH="" svc power shutdown')
 
+    # Offroad power monitoring
+    pm.calculate(health)
+    msg.thermal.offroadPowerUsage = pm.get_power_used()
+
     msg.thermal.chargingError = current_filter.x > 0. and msg.thermal.batteryPercent < 90  # if current is positive, then battery is being discharged
     msg.thermal.started = started_ts is not None
     msg.thermal.startedTs = int(1e9*(started_ts or 0))