Smarter thermal controls for tici (#21923)

* dumb controller 'tuned' on a desk

* reduce Ki

* fix prev_time bug

* reset integral on going onroad

* cleanup thermal detection logic

* more minor tuning

* use controls' PI controller

* get rid of bat_temp

* change if once more

* cleanup

* cleanup import order

* add back bat temp in this PR

* move to constructor

* consistency

* add bp

Co-authored-by: Comma Device <device@comma.ai>
Co-authored-by: Willem Melching <willem.melching@gmail.com>
Co-authored-by: Adeeb Shihadeh <adeebshihadeh@gmail.com>

selfdrive/controls/lib/pid.py

@@ -1,4 +1,6 @@
 import numpy as np
+from numbers import Number
+
 from common.numpy_fast import clip, interp
 
 def apply_deadzone(error, deadzone):
@@ -14,7 +16,11 @@ class PIController():
   def __init__(self, k_p, k_i, k_f=1., pos_limit=None, neg_limit=None, rate=100, sat_limit=0.8):
     self._k_p = k_p  # proportional gain
     self._k_i = k_i  # integral gain
-    self.k_f = k_f  # feedforward gain
+    self.k_f = k_f   # feedforward gain
+    if isinstance(self._k_p, Number):
+      self._k_p = [[0], [self._k_p]]
+    if isinstance(self._k_i, Number):
+      self._k_i = [[0], [self._k_i]]
 
     self.pos_limit = pos_limit
     self.neg_limit = neg_limit

selfdrive/thermald/thermald.py

@@ -16,7 +16,8 @@ from common.params import Params, ParamKeyType
 from common.realtime import DT_TRML, sec_since_boot
 from common.dict_helpers import strip_deprecated_keys
 from selfdrive.controls.lib.alertmanager import set_offroad_alert
-from selfdrive.hardware import EON, TICI, HARDWARE
+from selfdrive.controls.lib.pid import PIController
+from selfdrive.hardware import EON, TICI, PC, HARDWARE
 from selfdrive.loggerd.config import get_available_percent
 from selfdrive.pandad import get_expected_signature
 from selfdrive.swaglog import cloudlog
@@ -89,11 +90,9 @@ _TEMP_THRS_H = [50., 65., 80., 10000]
 _TEMP_THRS_L = [42.5, 57.5, 72.5, 10000]
 # fan speed options
 _FAN_SPEEDS = [0, 16384, 32768, 65535]
-# max fan speed only allowed if battery is hot
-_BAT_TEMP_THRESHOLD = 45.
 
 
-def handle_fan_eon(max_cpu_temp, bat_temp, fan_speed, ignition):
+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)
 
@@ -104,16 +103,12 @@ def handle_fan_eon(max_cpu_temp, bat_temp, fan_speed, ignition):
     # update speed if using the low thresholds results in fan speed decrement
     fan_speed = new_speed_l
 
-  if bat_temp < _BAT_TEMP_THRESHOLD:
-    # no max fan speed unless battery is hot
-    fan_speed = min(fan_speed, _FAN_SPEEDS[-2])
-
   set_eon_fan(fan_speed // 16384)
 
   return fan_speed
 
 
-def handle_fan_uno(max_cpu_temp, bat_temp, fan_speed, ignition):
+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:
@@ -122,6 +117,19 @@ def handle_fan_uno(max_cpu_temp, bat_temp, fan_speed, ignition):
   return new_speed
 
 
+def handle_fan_tici(controller, max_cpu_temp, fan_speed, ignition):
+  controller.neg_limit = -(80 if ignition else 30)
+  controller.pos_limit = -(30 if ignition else 0)
+
+  fan_pwr_out = -int(controller.update(
+                     setpoint=(75 if ignition else 68),
+                     measurement=max_cpu_temp,
+                     feedforward=interp(max_cpu_temp, [60.0, 100.0], [0, -80])
+                  ))
+
+  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
@@ -173,6 +181,9 @@ def thermald_thread():
   HARDWARE.initialize_hardware()
   thermal_config = HARDWARE.get_thermal_config()
 
+  # TODO: use PI controller for UNO
+  controller = PIController(k_p=0, k_i=2e-3, neg_limit=-80, pos_limit=0, rate=(1 / DT_TRML))
+
   if params.get_bool("IsOnroad"):
     cloudlog.event("onroad flag not cleared")
 
@@ -216,7 +227,10 @@ def thermald_thread():
       if handle_fan is None and pandaState.pandaState.pandaType != log.PandaState.PandaType.unknown:
         is_uno = pandaState.pandaState.pandaType == log.PandaState.PandaType.uno
 
-        if (not EON) or is_uno:
+        if TICI:
+          cloudlog.info("Setting up TICI fan handler")
+          handle_fan = handle_fan_tici
+        elif is_uno or PC:
           cloudlog.info("Setting up UNO fan handler")
           handle_fan = handle_fan_uno
         else:
@@ -286,7 +300,7 @@ def thermald_thread():
     bat_temp = msg.deviceState.batteryTempC
 
     if handle_fan is not None:
-      fan_speed = handle_fan(max_cpu_temp, bat_temp, fan_speed, startup_conditions["ignition"])
+      fan_speed = handle_fan(controller, max_cpu_temp, fan_speed, startup_conditions["ignition"])
       msg.deviceState.fanSpeedPercentDesired = fan_speed
 
     # If device is offroad we want to cool down before going onroad