diff --git a/release/files_common b/release/files_common index d390ab742..7bfb0f62a 100644 --- a/release/files_common +++ b/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 diff --git a/selfdrive/thermald/fan_controller.py b/selfdrive/thermald/fan_controller.py new file mode 100644 index 000000000..2fe52d0cb --- /dev/null +++ b/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 + diff --git a/selfdrive/thermald/tests/test_fan_controller.py b/selfdrive/thermald/tests/test_fan_controller.py new file mode 100755 index 000000000..8865b1f98 --- /dev/null +++ b/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() diff --git a/selfdrive/thermald/thermald.py b/selfdrive/thermald/thermald.py index 13fd25535..8fd0db971 100755 --- a/selfdrive/thermald/thermald.py +++ b/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 - controller = PIController(k_p=0, k_i=2e-3, k_f=1, neg_limit=-80, pos_limit=0, rate=(1 / DT_TRML)) + fan_controller = None 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") - handle_fan = handle_fan_tici + fan_controller = TiciFanController() elif is_uno or PC: - cloudlog.info("Setting up UNO fan handler") - handle_fan = handle_fan_uno + fan_controller = UnoFanController() else: - cloudlog.info("Setting up EON fan handler") - setup_eon_fan() - handle_fan = handle_fan_eon + fan_controller = EonFanController() 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: - fan_speed = handle_fan(controller, max_comp_temp, fan_speed, onroad_conditions["ignition"]) - msg.deviceState.fanSpeedPercentDesired = fan_speed + if fan_controller is not None: + msg.deviceState.fanSpeedPercentDesired = fan_controller.update(max_comp_temp, onroad_conditions["ignition"]) 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: