Brian Hanson
4dae5804ab
Fan control rework (thermald → 4Hz):
- DT_TRML 0.5s → 0.25s (thermald loop + fan PID now at 4Hz)
- New clamp rules based on (gear, cruise_engaged, standstill):
parked → 0-100%
in drive + cruise engaged (any speed) → 30-100%
in drive + cruise off + standstill → 10-100%
in drive + cruise off + moving → 30-100%
- thermald now reads gearShifter (via carState) and controlsState.enabled,
passes them to fan_controller.update()
- Removed BENCH_MODE special case — new rules cover bench automatically
- Removed ignition-based branches — gear is the correct signal
System health overlay:
- Subscribed UI to peripheralState so we can read fanSpeedRpm
- Added FAN row: actual fan% (RPM / 65) to sit alongside LAG/DROP/TEMP/CPU/MEM.
Shows the real fan output vs. what the PID is asking for.
Migrate hot signals from paramsMemory to cereal (frogpilotCarControl):
- Added latRequested @3 and noLatLaneChange @4 to FrogPilotCarControl schema
- controlsd sets FPCC.latRequested / FPCC.noLatLaneChange (send-on-change
already gates the IPC)
- modeld reads from sm['frogpilotCarControl'] (added to its subscribers)
instead of paramsMemory (saves ~20 file-read syscalls/sec)
- carcontroller reads from frogpilot_variables (set in-process by controlsd)
instead of paramsMemory (saves ~100 file-read syscalls/sec in 100Hz path).
Dropped carcontroller's now-unused Params instance and import.
- UI (ui.cc, onroad.cc) reads from sm['frogpilotCarControl'].noLatLaneChange
- Removed LatRequested and no_lat_lane_change param registrations + defaults
Co-Authored-By: Claude Opus 4.6 (1M context) <noreply@anthropic.com>
98 lines
2.6 KiB
Python
Executable File
98 lines
2.6 KiB
Python
Executable File
"""Utilities for reading real time clocks and keeping soft real time constraints."""
|
|
import gc
|
|
import os
|
|
import time
|
|
from collections import deque
|
|
|
|
from setproctitle import getproctitle
|
|
|
|
from openpilot.system.hardware import PC
|
|
|
|
|
|
# time step for each process
|
|
DT_CTRL = 0.01 # controlsd
|
|
DT_MDL = 0.05 # model
|
|
DT_TRML = 0.25 # thermald and manager — 4 Hz
|
|
DT_DMON = 0.05 # driver monitoring
|
|
|
|
|
|
class Priority:
|
|
# CORE 2
|
|
# - modeld = 55
|
|
# - camerad = 54
|
|
CTRL_LOW = 51 # plannerd & radard
|
|
|
|
# CORE 3
|
|
# - boardd = 55
|
|
CTRL_HIGH = 53
|
|
|
|
|
|
def set_realtime_priority(level: int) -> None:
|
|
if not PC:
|
|
os.sched_setscheduler(0, os.SCHED_FIFO, os.sched_param(level))
|
|
|
|
|
|
def set_core_affinity(cores: list[int]) -> None:
|
|
if not PC:
|
|
try: # CLEARPILOT ....
|
|
os.sched_setaffinity(0, cores)
|
|
except:
|
|
nothing=0
|
|
|
|
|
|
def config_realtime_process(cores: int | list[int], priority: int) -> None:
|
|
gc.disable()
|
|
set_realtime_priority(priority)
|
|
c = cores if isinstance(cores, list) else [cores, ]
|
|
set_core_affinity(c)
|
|
|
|
|
|
class Ratekeeper:
|
|
def __init__(self, rate: float, print_delay_threshold: float | None = 0.0) -> None:
|
|
"""Rate in Hz for ratekeeping. print_delay_threshold must be nonnegative."""
|
|
self._interval = 1. / rate
|
|
self._next_frame_time = time.monotonic() + self._interval
|
|
self._print_delay_threshold = print_delay_threshold
|
|
self._frame = 0
|
|
self._remaining = 0.0
|
|
self._process_name = getproctitle()
|
|
self._dts = deque([self._interval], maxlen=100)
|
|
self._last_monitor_time = time.monotonic()
|
|
|
|
@property
|
|
def frame(self) -> int:
|
|
return self._frame
|
|
|
|
@property
|
|
def remaining(self) -> float:
|
|
return self._remaining
|
|
|
|
@property
|
|
def lagging(self) -> bool:
|
|
avg_dt = sum(self._dts) / len(self._dts)
|
|
expected_dt = self._interval * (1 / 0.9)
|
|
return avg_dt > expected_dt
|
|
|
|
# Maintain loop rate by calling this at the end of each loop
|
|
def keep_time(self) -> bool:
|
|
lagged = self.monitor_time()
|
|
if self._remaining > 0:
|
|
time.sleep(self._remaining)
|
|
return lagged
|
|
|
|
# Monitors the cumulative lag, but does not enforce a rate
|
|
def monitor_time(self) -> bool:
|
|
prev = self._last_monitor_time
|
|
self._last_monitor_time = time.monotonic()
|
|
self._dts.append(self._last_monitor_time - prev)
|
|
|
|
lagged = False
|
|
remaining = self._next_frame_time - time.monotonic()
|
|
self._next_frame_time += self._interval
|
|
if self._print_delay_threshold is not None and remaining < -self._print_delay_threshold:
|
|
print(f"{self._process_name} lagging by {-remaining * 1000:.2f} ms")
|
|
lagged = True
|
|
self._frame += 1
|
|
self._remaining = remaining
|
|
return lagged
|