Fan controller cleanup + testing (#23886)

* clean up fan controllers in preparation for testing

* add fan controller to release

* add some unit tests around the fan controller

* subclass ABC

release/files_common

@@ -336,6 +336,7 @@ selfdrive/sensord/sensord
 
 selfdrive/thermald/thermald.py
 selfdrive/thermald/power_monitoring.py
+selfdrive/thermald/fan_controller.py
 
 selfdrive/test/__init__.py
 selfdrive/test/helpers.py

selfdrive/thermald/fan_controller.py

@@ -0,0 +1,103 @@
+#!/usr/bin/env python3
+
+import os
+from smbus2 import SMBus
+from abc import ABC, abstractmethod
+from common.realtime import DT_TRML
+from common.numpy_fast import interp
+from selfdrive.swaglog import cloudlog
+from selfdrive.controls.lib.pid import PIController
+
+class BaseFanController(ABC):
+  @abstractmethod
+  def update(self, max_cpu_temp: float, ignition: bool) -> int:
+    pass
+
+
+class EonFanController(BaseFanController):
+  # Temp thresholds to control fan speed - high hysteresis
+  TEMP_THRS_H = [50., 65., 80., 10000]
+  # Temp thresholds to control fan speed - low hysteresis
+  TEMP_THRS_L = [42.5, 57.5, 72.5, 10000]
+  # Fan speed options
+  FAN_SPEEDS = [0, 16384, 32768, 65535]
+
+  def __init__(self) -> None:
+    super().__init__()
+    cloudlog.info("Setting up EON fan handler")
+
+    self.fan_speed = -1
+    self.setup_eon_fan()
+
+  def setup_eon_fan(self) -> None:
+    os.system("echo 2 > /sys/module/dwc3_msm/parameters/otg_switch")
+
+  def set_eon_fan(self, speed: int) -> None:
+    if self.fan_speed != speed:
+      # FIXME: this is such an ugly hack to get the right index
+      val = speed // 16384
+
+      bus = SMBus(7, force=True)
+      try:
+        i = [0x1, 0x3 | 0, 0x3 | 0x08, 0x3 | 0x10][val]
+        bus.write_i2c_block_data(0x3d, 0, [i])
+      except OSError:
+        # tusb320
+        if val == 0:
+          bus.write_i2c_block_data(0x67, 0xa, [0])
+        else:
+          bus.write_i2c_block_data(0x67, 0xa, [0x20])
+          bus.write_i2c_block_data(0x67, 0x8, [(val - 1) << 6])
+      bus.close()
+      self.fan_speed = speed
+
+  def update(self, max_cpu_temp: float, ignition: bool) -> int:
+    new_speed_h = next(speed for speed, temp_h in zip(self.FAN_SPEEDS, self.TEMP_THRS_H) if temp_h > max_cpu_temp)
+    new_speed_l = next(speed for speed, temp_l in zip(self.FAN_SPEEDS, self.TEMP_THRS_L) if temp_l > max_cpu_temp)
+
+    if new_speed_h > self.fan_speed:
+      self.set_eon_fan(new_speed_h)
+    elif new_speed_l < self.fan_speed:
+      self.set_eon_fan(new_speed_l)
+
+    return self.fan_speed
+
+
+class UnoFanController(BaseFanController):
+  def __init__(self) -> None:
+    super().__init__()
+    cloudlog.info("Setting up UNO fan handler")
+
+  def update(self, max_cpu_temp: float, ignition: bool) -> int:
+    new_speed = int(interp(max_cpu_temp, [40.0, 80.0], [0, 80]))
+
+    if not ignition:
+      new_speed = min(30, new_speed)
+
+    return new_speed
+
+
+class TiciFanController(BaseFanController):
+  def __init__(self) -> None:
+    super().__init__()
+    cloudlog.info("Setting up TICI fan handler")
+
+    self.last_ignition = False
+    self.controller = PIController(k_p=0, k_i=2e-3, k_f=1, neg_limit=-80, pos_limit=0, rate=(1 / DT_TRML))
+
+  def update(self, max_cpu_temp: float, ignition: bool) -> int:
+    self.controller.neg_limit = -(80 if ignition else 30)
+    self.controller.pos_limit = -(30 if ignition else 0)
+
+    if ignition != self.last_ignition:
+      self.controller.reset()
+
+    fan_pwr_out = -int(self.controller.update(
+                      setpoint=75,
+                      measurement=max_cpu_temp,
+                      feedforward=interp(max_cpu_temp, [60.0, 100.0], [0, -80])
+                    ))
+
+    self.last_ignition = ignition
+    return fan_pwr_out
+

selfdrive/thermald/tests/test_fan_controller.py

@@ -0,0 +1,58 @@
+#!/usr/bin/env python3
+import unittest
+from unittest.mock import Mock, MagicMock, patch
+from parameterized import parameterized
+
+with patch("smbus2.SMBus", new=MagicMock()):
+  from selfdrive.thermald.fan_controller import EonFanController, UnoFanController, TiciFanController
+
+ALL_CONTROLLERS = [(EonFanController, ), (UnoFanController,), (TiciFanController,)]
+GEN2_CONTROLLERS = [(UnoFanController,), (TiciFanController,)]
+
+def patched_controller(controller_class):
+  with patch("os.system", new=Mock()):
+    return controller_class()
+
+class TestFanController(unittest.TestCase):
+  def wind_up(self, controller, ignition=True):
+    for _ in range(1000):
+      controller.update(max_cpu_temp=100, ignition=ignition)
+
+  def wind_down(self, controller, ignition=False):
+    for _ in range(1000):
+      controller.update(max_cpu_temp=10, ignition=ignition)
+
+  @parameterized.expand(ALL_CONTROLLERS)
+  def test_hot_onroad(self, controller_class):
+    controller = patched_controller(controller_class)
+    self.wind_up(controller)
+    self.assertGreaterEqual(controller.update(max_cpu_temp=100, ignition=True), 70)
+
+  @parameterized.expand(GEN2_CONTROLLERS)
+  def test_offroad_limits(self, controller_class):
+    controller = patched_controller(controller_class)
+    self.wind_up(controller)
+    self.assertLessEqual(controller.update(max_cpu_temp=100, ignition=False), 30)
+
+  @parameterized.expand(ALL_CONTROLLERS)
+  def test_no_fan_wear(self, controller_class):
+    controller = patched_controller(controller_class)
+    self.wind_down(controller)
+    self.assertEqual(controller.update(max_cpu_temp=10, ignition=False), 0)
+
+  @parameterized.expand(GEN2_CONTROLLERS)
+  def test_limited(self, controller_class):
+    controller = patched_controller(controller_class)
+    self.wind_up(controller, ignition=True)
+    self.assertGreaterEqual(controller.update(max_cpu_temp=100, ignition=True), 80)
+
+  @parameterized.expand(ALL_CONTROLLERS)
+  def test_windup_speed(self, controller_class):
+    controller = patched_controller(controller_class)
+    self.wind_down(controller, ignition=True)
+    for _ in range(10):
+      controller.update(max_cpu_temp=90, ignition=True)
+    self.assertGreaterEqual(controller.update(max_cpu_temp=90, ignition=True), 60)
+
+if __name__ == "__main__":
+  unittest.main()

selfdrive/thermald/thermald.py

@@ -9,22 +9,20 @@ from pathlib import Path
 from typing import Dict, Optional, Tuple
 
 import psutil
-from smbus2 import SMBus
 
 import cereal.messaging as messaging
 from cereal import log
 from common.dict_helpers import strip_deprecated_keys
 from common.filter_simple import FirstOrderFilter
-from common.numpy_fast import interp
 from common.params import Params
 from common.realtime import DT_TRML, sec_since_boot
 from selfdrive.controls.lib.alertmanager import set_offroad_alert
-from selfdrive.controls.lib.pid import PIController
 from selfdrive.hardware import EON, HARDWARE, PC, TICI
 from selfdrive.loggerd.config import get_available_percent
 from selfdrive.statsd import statlog
 from selfdrive.swaglog import cloudlog
 from selfdrive.thermald.power_monitoring import PowerMonitoring
+from selfdrive.thermald.fan_controller import EonFanController, UnoFanController, TiciFanController
 from selfdrive.version import terms_version, training_version
 
 ThermalStatus = log.DeviceState.ThermalStatus
@@ -73,83 +71,6 @@ def read_thermal(thermal_config):
   return dat
 
 
-def setup_eon_fan():
-  os.system("echo 2 > /sys/module/dwc3_msm/parameters/otg_switch")
-
-
-last_eon_fan_val = None
-def set_eon_fan(val):
-  global last_eon_fan_val
-
-  if last_eon_fan_val is None or last_eon_fan_val != val:
-    bus = SMBus(7, force=True)
-    try:
-      i = [0x1, 0x3 | 0, 0x3 | 0x08, 0x3 | 0x10][val]
-      bus.write_i2c_block_data(0x3d, 0, [i])
-    except OSError:
-      # tusb320
-      if val == 0:
-        bus.write_i2c_block_data(0x67, 0xa, [0])
-      else:
-        bus.write_i2c_block_data(0x67, 0xa, [0x20])
-        bus.write_i2c_block_data(0x67, 0x8, [(val - 1) << 6])
-    bus.close()
-    last_eon_fan_val = val
-
-
-# temp thresholds to control fan speed - high hysteresis
-_TEMP_THRS_H = [50., 65., 80., 10000]
-# temp thresholds to control fan speed - low hysteresis
-_TEMP_THRS_L = [42.5, 57.5, 72.5, 10000]
-# fan speed options
-_FAN_SPEEDS = [0, 16384, 32768, 65535]
-
-
-def handle_fan_eon(controller, max_cpu_temp, fan_speed, ignition):
-  new_speed_h = next(speed for speed, temp_h in zip(_FAN_SPEEDS, _TEMP_THRS_H) if temp_h > max_cpu_temp)
-  new_speed_l = next(speed for speed, temp_l in zip(_FAN_SPEEDS, _TEMP_THRS_L) if temp_l > max_cpu_temp)
-
-  if new_speed_h > fan_speed:
-    # update speed if using the high thresholds results in fan speed increment
-    fan_speed = new_speed_h
-  elif new_speed_l < fan_speed:
-    # update speed if using the low thresholds results in fan speed decrement
-    fan_speed = new_speed_l
-
-  set_eon_fan(fan_speed // 16384)
-
-  return fan_speed
-
-
-def handle_fan_uno(controller, max_cpu_temp, fan_speed, ignition):
-  new_speed = int(interp(max_cpu_temp, [40.0, 80.0], [0, 80]))
-
-  if not ignition:
-    new_speed = min(30, new_speed)
-
-  return new_speed
-
-
-last_ignition = False
-def handle_fan_tici(controller, max_cpu_temp, fan_speed, ignition):
-  global last_ignition
-
-  controller.neg_limit = -(80 if ignition else 30)
-  controller.pos_limit = -(30 if ignition else 0)
-
-  if ignition != last_ignition:
-    controller.reset()
-
-  fan_pwr_out = -int(controller.update(
-                     setpoint=75,
-                     measurement=max_cpu_temp,
-                     feedforward=interp(max_cpu_temp, [60.0, 100.0], [0, -80])
-                  ))
-
-  last_ignition = ignition
-  return fan_pwr_out
-
-
 def set_offroad_alert_if_changed(offroad_alert: str, show_alert: bool, extra_text: Optional[str]=None):
   if prev_offroad_states.get(offroad_alert, None) == (show_alert, extra_text):
     return
@@ -202,7 +123,6 @@ def thermald_thread(end_event, hw_queue):
   pm = messaging.PubMaster(['deviceState'])
   sm = messaging.SubMaster(["peripheralState", "gpsLocationExternal", "controlsState", "pandaStates"], poll=["pandaStates"])
 
-  fan_speed = 0
   count = 0
 
   onroad_conditions: Dict[str, bool] = {
@@ -229,7 +149,6 @@ def thermald_thread(end_event, hw_queue):
   temp_filter = FirstOrderFilter(0., TEMP_TAU, DT_TRML)
   should_start_prev = False
   in_car = False
-  handle_fan = None
   is_uno = False
   engaged_prev = False
 
@@ -239,8 +158,7 @@ def thermald_thread(end_event, hw_queue):
   HARDWARE.initialize_hardware()
   thermal_config = HARDWARE.get_thermal_config()
 
-  # TODO: use PI controller for UNO
+  fan_controller = None
-  controller = PIController(k_p=0, k_i=2e-3, k_f=1, neg_limit=-80, pos_limit=0, rate=(1 / DT_TRML))
 
   while not end_event.is_set():
     sm.update(PANDA_STATES_TIMEOUT)
@@ -261,19 +179,15 @@ def thermald_thread(end_event, hw_queue):
       usb_power = peripheralState.usbPowerMode != log.PeripheralState.UsbPowerMode.client
 
       # Setup fan handler on first connect to panda
-      if handle_fan is None and peripheralState.pandaType != log.PandaState.PandaType.unknown:
+      if fan_controller is None and peripheralState.pandaType != log.PandaState.PandaType.unknown:
         is_uno = peripheralState.pandaType == log.PandaState.PandaType.uno
 
         if TICI:
-          cloudlog.info("Setting up TICI fan handler")
+          fan_controller = TiciFanController()
-          handle_fan = handle_fan_tici
         elif is_uno or PC:
-          cloudlog.info("Setting up UNO fan handler")
+          fan_controller = UnoFanController()
-          handle_fan = handle_fan_uno
         else:
-          cloudlog.info("Setting up EON fan handler")
+          fan_controller = EonFanController()
-          setup_eon_fan()
-          handle_fan = handle_fan_eon
 
     try:
       last_hw_state = hw_queue.get_nowait()
@@ -303,9 +217,8 @@ def thermald_thread(end_event, hw_queue):
       max(max(msg.deviceState.cpuTempC), msg.deviceState.memoryTempC, max(msg.deviceState.gpuTempC))
     )
 
-    if handle_fan is not None:
+    if fan_controller is not None:
-      fan_speed = handle_fan(controller, max_comp_temp, fan_speed, onroad_conditions["ignition"])
+      msg.deviceState.fanSpeedPercentDesired = fan_controller.update(max_comp_temp, onroad_conditions["ignition"])
-      msg.deviceState.fanSpeedPercentDesired = fan_speed
 
     is_offroad_for_5_min = (started_ts is None) and ((not started_seen) or (off_ts is None) or (sec_since_boot() - off_ts > 60 * 5))
     if is_offroad_for_5_min and max_comp_temp > OFFROAD_DANGER_TEMP: