Files
StarPilot/openpilot/system/hardware/hardwared.py
T
2026-11-02 12:56:09 -06:00

574 lines
24 KiB
Python
Executable File

#!/usr/bin/env python3
from openpilot.starpilot.galaxy.companion import phone_uplink_active
import fcntl
import os
import queue
import struct
import subprocess
import sys
import threading
import time
from openpilot.starpilot.drive_state.owner import DriveStateOwner
from openpilot.starpilot.drive_state.resolver import Mode, effective_onroad, should_start as resolve_start
from openpilot.starpilot.storage import starpilot_storage_root
from collections import OrderedDict, namedtuple
import openpilot.cereal.messaging as messaging
from openpilot.cereal import log
from openpilot.cereal.services import SERVICE_LIST
from openpilot.common.utils import strip_deprecated_keys
from openpilot.common.filter_simple import FirstOrderFilter
from openpilot.common.params import Params, UnknownKeyName
from openpilot.common.realtime import DT_HW
from openpilot.selfdrive.selfdrived.alertmanager import set_offroad_alert
from openpilot.common.hardware import HARDWARE, COMMA_HARDWARE, PC
from openpilot.common.basedir import BASEDIR
from openpilot.common.git import get_short_branch
from openpilot.common.hardware.usb import CHESTNUT_FW_VERSION, CHESTNUT_USB_PRODUCT, get_usb_state, get_usb_topology, is_chestnut_usb_id, set_usb_state
from openpilot.common.linux import LinuxSystemStats
from openpilot.system.loggerd.config import get_available_percent
from openpilot.common.swaglog import cloudlog
from openpilot.system.hardware.power_monitoring import PowerMonitoring
from openpilot.system.hardware.fan_controller import FanController
from openpilot.system.hardware.chestnut.monitoring import chestnut_state_thread
from openpilot.system.hardware.chestnut.status import ChestnutStatus
from openpilot.common.version import terms_version, training_version
from openpilot.system.athena.registration import UNREGISTERED_DONGLE_ID
from openpilot.starpilot.models import jetlink_adapter
ThermalStatus = log.DeviceState.ThermalStatus
NetworkType = log.DeviceState.NetworkType
NetworkStrength = log.DeviceState.NetworkStrength
TEMP_TAU = 5. # 5s time constant
DISCONNECT_TIMEOUT = 5. # wait 5 seconds before going offroad after disconnect so you get an alert
PANDA_STATES_TIMEOUT = round(1000 / SERVICE_LIST['pandaStates'].frequency * 1.5) # 1.5x the expected pandaState frequency
ONROAD_CYCLE_TIME = 1 # seconds to wait offroad after requesting an onroad cycle
class Chestnut:
# flash before going onroad, modeld ignores chestnut until the product string matches
MAX_ATTEMPTS = 3
RETRY_INTERVAL = 20.
STARTUP_TIMEOUT = 30.
def __init__(self):
self.thread: threading.Thread | None = None
self.attempts = 0
self.last_attempt = 0.
self.flashed = False
self.mismatch = False
@property
def flashing(self) -> bool:
return self.thread is not None and self.thread.is_alive() and time.monotonic() - self.last_attempt < self.STARTUP_TIMEOUT
@property
def failed(self) -> bool:
return self.mismatch and self.attempts >= self.MAX_ATTEMPTS and self.thread is not None and not self.thread.is_alive() and not self.flashed
def flash(self) -> None:
ret = subprocess.run(["sudo", sys.executable, os.path.join(BASEDIR, "openpilot/system/hardware/chestnut/flash.py"), CHESTNUT_FW_VERSION],
stdout=subprocess.PIPE, stderr=subprocess.STDOUT, text=True, check=False)
cloudlog.event("chestnut flash done", returncode=ret.returncode, output=ret.stdout[-1000:], error=ret.returncode != 0)
self.flashed = ret.returncode == 0
def update(self, offroad: bool, usb_state: list[dict]) -> None:
self.mismatch = any(is_chestnut_usb_id(d["vendorId"], d["productId"], include_bootloader=True) and
d["product"] != CHESTNUT_USB_PRODUCT for d in usb_state)
if not self.mismatch:
self.flashed = False
return
if not offroad or self.flashed or self.attempts >= self.MAX_ATTEMPTS:
return
if self.thread is not None and self.thread.is_alive():
return
if self.attempts > 0 and time.monotonic() - self.last_attempt < self.RETRY_INTERVAL:
return
self.attempts += 1
self.last_attempt = time.monotonic()
cloudlog.warning(f"chestnut firmware out of date, flashing (attempt {self.attempts})")
self.thread = threading.Thread(target=self.flash, daemon=True)
self.thread.start()
ThermalBand = namedtuple("ThermalBand", ['min_temp', 'max_temp'])
HardwareState = namedtuple("HardwareState", ['network_type', 'network_info', 'network_strength', 'network_stats',
'network_metered', 'modem_temps', 'usb_state'])
# List of thermal bands. We will stay within this region as long as we are within the bounds.
# When exiting the bounds, we'll jump to the lower or higher band. Bands are ordered in the dict.
if HARDWARE.get_device_type() == "mici":
THERMAL_BANDS = OrderedDict({
ThermalStatus.ok: ThermalBand(None, 100.0),
ThermalStatus.overheated: ThermalBand(92.0, 107.),
ThermalStatus.critical: ThermalBand(98.0, None),
})
else:
THERMAL_BANDS = OrderedDict({
ThermalStatus.ok: ThermalBand(None, 96.0),
ThermalStatus.overheated: ThermalBand(88.0, 107.),
ThermalStatus.critical: ThermalBand(94.0, None),
})
# Override to highest thermal band when offroad and above this temp
OFFROAD_DANGER_TEMP = 85 if HARDWARE.get_device_type() == "mici" else 75
prev_offroad_states: dict[str, tuple[bool, str | None]] = {}
def set_offroad_alert_if_changed(offroad_alert: str, show_alert: bool, extra_text: str | None=None):
if prev_offroad_states.get(offroad_alert, None) == (show_alert, extra_text):
return
prev_offroad_states[offroad_alert] = (show_alert, extra_text)
set_offroad_alert(offroad_alert, show_alert, extra_text)
# A prebuilt params library not yet regenerated lacks Offroad_JetlinkUnavailable: the alert is skipped rather than
# taking hardwared down. The guard can go once the library knows the key.
jetlink_alert_usable = True
def update_jetlink_alert() -> None:
"""Why the Jetlink the user turned on cannot run. Reads files only; with the link off, jetlink binds nothing."""
global jetlink_alert_usable
if not jetlink_alert_usable:
return
reason = jetlink_adapter.hooks().reason()
try:
set_offroad_alert_if_changed("Offroad_JetlinkUnavailable", reason is not None, extra_text=reason)
except UnknownKeyName:
jetlink_alert_usable = False
cloudlog.warning("Offroad_JetlinkUnavailable is not in this params library: Jetlink's offroad alert is off")
def touch_thread(end_event):
count = 0
pm = messaging.PubMaster(["touch"])
event_format = "llHHi"
event_size = struct.calcsize(event_format)
event_frame = []
with open("/dev/input/by-path/platform-894000.i2c-event", "rb") as event_file:
fcntl.fcntl(event_file, fcntl.F_SETFL, os.O_NONBLOCK)
while not end_event.is_set():
if (count % int(1. / DT_HW)) == 0:
event = event_file.read(event_size)
if event:
(sec, usec, etype, code, value) = struct.unpack(event_format, event)
if etype != 0 or code != 0 or value != 0:
touch = log.Touch.new_message()
touch.sec = sec
touch.usec = usec
touch.type = etype
touch.code = code
touch.value = value
event_frame.append(touch)
else: # end of frame, push new log
msg = messaging.new_message('touch', len(event_frame), valid=True)
msg.touch = event_frame
pm.send('touch', msg)
event_frame = []
continue
count += 1
time.sleep(DT_HW)
def hw_state_thread(end_event, hw_queue):
"""Handles non critical hardware state, and sends over queue"""
count = 0
prev_hw_state = None
prev_usb_topology = set()
while not end_event.is_set():
usb_topology = get_usb_topology()
usb_changed = usb_topology != prev_usb_topology
# these are expensive calls. update every 10s or when USB devices change
if (count % int(10. / DT_HW)) == 0 or usb_changed:
prev_usb_topology = usb_topology
try:
phone_uplink = phone_uplink_active()
network_type = NetworkType.cell4G if phone_uplink else HARDWARE.get_network_type()
modem_temps = HARDWARE.get_modem_temperatures()
if len(modem_temps) == 0 and prev_hw_state is not None:
modem_temps = prev_hw_state.modem_temps
tx, rx = HARDWARE.get_modem_data_usage()
hw_state = HardwareState(
network_type=network_type,
network_info=HARDWARE.get_network_info(),
network_strength=HARDWARE.get_network_strength(network_type),
network_stats={'wwanTx': tx, 'wwanRx': rx},
network_metered=phone_uplink or HARDWARE.get_network_metered(network_type),
modem_temps=modem_temps,
usb_state=get_usb_state(),
)
try:
hw_queue.put_nowait(hw_state)
except queue.Full:
pass
prev_hw_state = hw_state
except Exception:
cloudlog.exception("Error getting hardware state")
count += 1
time.sleep(DT_HW)
def hardware_thread(end_event, hw_queue) -> None:
system_stats = LinuxSystemStats() if sys.platform == "linux" else None
pm = messaging.PubMaster(['deviceState'])
sm = messaging.SubMaster(["peripheralState", "gpsLocationExternal", "selfdriveState", "pandaStates", "chestnutState"], poll="pandaStates")
count = 0
onroad_conditions: dict[str, bool] = {
"ignition": False,
"not_onroad_cycle": True,
"device_temp_good": True,
}
startup_conditions: dict[str, bool] = {}
startup_conditions_prev: dict[str, bool] = {}
off_ts: float | None = None
physical_off_ts: float | None = None
forced_power_episode = False
started_ts: float | None = None
started_seen = False
startup_blocked_ts: float | None = None
thermal_status = ThermalStatus.ok
last_hw_state = HardwareState(
network_type=NetworkType.none,
network_info=None,
network_metered=False,
network_strength=NetworkStrength.unknown,
network_stats={'wwanTx': -1, 'wwanRx': -1},
modem_temps=[],
usb_state=get_usb_state(),
)
all_temp_filter = FirstOrderFilter(0., TEMP_TAU, DT_HW, initialized=False)
offroad_temp_filter = FirstOrderFilter(0., TEMP_TAU, DT_HW, initialized=False)
should_start_prev = False
in_car = False
engaged_prev = False
pwrsave = False
offroad_cycle_count = 0
params = Params()
drive_state = DriveStateOwner(params, starpilot_storage_root() / "drive-state")
drive_mode = Mode.AUTO
physical_ignition_prev = None
requested_onroad_prev = None
power_monitor = PowerMonitoring()
uptime_offroad: float = params.get("UptimeOffroad", return_default=True)
uptime_onroad: float = params.get("UptimeOnroad", return_default=True)
last_uptime_ts: float = time.monotonic()
HARDWARE.initialize_hardware()
thermal_config = HARDWARE.get_thermal_config()
fan_controller = FanController(int(1./DT_HW))
chestnut = Chestnut()
chestnut_status = ChestnutStatus()
branch = get_short_branch()
while not end_event.is_set():
sm.update(PANDA_STATES_TIMEOUT)
pandaStates = sm['pandaStates']
peripheralState = sm['peripheralState']
# handle requests to cycle system started state
if params.get_bool("OnroadCycleRequested"):
params.put_bool("OnroadCycleRequested", False, block=True)
offroad_cycle_count = sm.frame
onroad_conditions["not_onroad_cycle"] = (sm.frame - offroad_cycle_count) >= ONROAD_CYCLE_TIME * SERVICE_LIST['pandaStates'].frequency
if sm.updated['pandaStates'] and len(pandaStates) > 0:
# Set ignition based on any panda connected
onroad_conditions["ignition"] = any(ps.ignitionLine or ps.ignitionCan for ps in pandaStates if ps.pandaType != log.PandaState.PandaType.unknown)
pandaState = pandaStates[0]
in_car = pandaState.harnessStatus != log.PandaState.HarnessStatus.notConnected
elif (time.monotonic() - sm.recv_time['pandaStates']) > DISCONNECT_TIMEOUT:
if onroad_conditions["ignition"]:
onroad_conditions["ignition"] = False
cloudlog.error("panda timed out onroad")
# Run at 2Hz, plus effective or observed physical ignition edges.
physical_ignition = onroad_conditions["ignition"]
physical_edge = physical_ignition_prev is not None and physical_ignition != physical_ignition_prev
if physical_ignition_prev is not None or (sm.updated["pandaStates"] and len(pandaStates) > 0):
physical_ignition_prev = physical_ignition
if sm.frame % round(SERVICE_LIST['pandaStates'].frequency * DT_HW) == 0:
drive_mode = drive_state.snapshot().mode
requested_onroad = effective_onroad(drive_mode, onroad_conditions)
request_edge = requested_onroad_prev is not None and requested_onroad != requested_onroad_prev
requested_onroad_prev = requested_onroad
# Preserve Auto's startup retries without treating a blocked forced request as a new edge.
ign_edge = ((started_ts is not None) != requested_onroad if drive_mode == Mode.AUTO else
request_edge or physical_edge)
if (sm.frame % round(SERVICE_LIST['pandaStates'].frequency * DT_HW) != 0) and not ign_edge:
continue
msg = messaging.new_message('deviceState', valid=True)
msg.deviceState = thermal_config.get_msg()
msg.deviceState.deviceType = HARDWARE.get_device_type()
try:
last_hw_state = hw_queue.get_nowait()
except queue.Empty:
pass
memory_usage = system_stats.memory_usage_percent() if system_stats is not None else 0.
cpu_usage = system_stats.cpu_usage_percent() if system_stats is not None else []
msg.deviceState.freeSpacePercent = get_available_percent(default=100.0)
msg.deviceState.memoryUsagePercent = int(round(memory_usage))
msg.deviceState.gpuUsagePercent = int(round(HARDWARE.get_gpu_usage_percent()))
online_cpu_usage = [int(round(n)) for n in cpu_usage]
offline_cpu_usage = [0., ] * (len(msg.deviceState.cpuTempC) - len(online_cpu_usage))
msg.deviceState.cpuUsagePercent = online_cpu_usage + offline_cpu_usage
msg.deviceState.networkType = last_hw_state.network_type
msg.deviceState.networkMetered = last_hw_state.network_metered
msg.deviceState.networkStrength = last_hw_state.network_strength
msg.deviceState.networkStats = last_hw_state.network_stats
if last_hw_state.network_info is not None:
msg.deviceState.networkInfo = last_hw_state.network_info
msg.deviceState.modemTempC = last_hw_state.modem_temps
msg.deviceState.screenBrightnessPercent = HARDWARE.get_screen_brightness()
set_usb_state(msg.deviceState, last_hw_state.usb_state)
chestnut.update(started_ts is None, last_hw_state.usb_state)
chestnut_state = sm["chestnutState"]
chestnut_valid = sm.alive["chestnutState"] and sm.valid["chestnutState"]
chestnut_status.update(started_ts is None or not onroad_conditions["ignition"], branch, last_hw_state.usb_state, chestnut.failed,
params.get_bool("ChestnutLoading"), params.get("ChestnutActive"),
chestnut_state if chestnut_valid else None, set_offroad_alert_if_changed)
# this subset is only used for offroad
temp_sources = [
msg.deviceState.memoryTempC,
max(msg.deviceState.cpuTempC, default=0.),
max(msg.deviceState.gpuTempC, default=0.),
]
offroad_comp_temp = offroad_temp_filter.update(max(temp_sources))
# this drives the thermal status while onroad
temp_sources.append(max(msg.deviceState.pmicTempC, default=0.))
all_comp_temp = all_temp_filter.update(max(temp_sources))
msg.deviceState.maxTempC = all_comp_temp
msg.deviceState.fanSpeedPercentDesired = fan_controller.update(all_comp_temp, onroad_conditions["ignition"])
is_offroad_for_5_min = (started_ts is None) and ((not started_seen) or (off_ts is None) or (time.monotonic() - off_ts > 60 * 5))
if is_offroad_for_5_min and offroad_comp_temp > OFFROAD_DANGER_TEMP:
# if device is offroad and already hot without the extra onroad load,
# we want to cool down first before increasing load
thermal_status = ThermalStatus.critical
else:
current_band = THERMAL_BANDS[thermal_status]
band_idx = list(THERMAL_BANDS.keys()).index(thermal_status)
if current_band.min_temp is not None and all_comp_temp < current_band.min_temp:
thermal_status = list(THERMAL_BANDS.keys())[band_idx - 1]
elif current_band.max_temp is not None and all_comp_temp > current_band.max_temp:
thermal_status = list(THERMAL_BANDS.keys())[band_idx + 1]
# **** starting logic ****
startup_conditions["up_to_date"] = params.get("Offroad_ConnectivityNeeded") is None or params.get_bool("DisableUpdates") or params.get_bool("SnoozeUpdate")
startup_conditions["no_excessive_actuation"] = params.get("Offroad_ExcessiveActuation") is None
startup_conditions["not_uninstalling"] = not params.get_bool("DoUninstall")
startup_conditions["accepted_terms"] = params.get("HasAcceptedTerms") == terms_version
# with 2% left, we killall, otherwise the phone will take a long time to boot
startup_conditions["free_space"] = msg.deviceState.freeSpacePercent > 2
startup_conditions["completed_training"] = params.get("CompletedTrainingVersion") == training_version
startup_conditions["not_driver_view"] = not params.get_bool("IsDriverViewEnabled")
# must be at an engageable thermal band to go onroad
startup_conditions["device_temp_engageable"] = thermal_status < ThermalStatus.overheated
startup_conditions["not_flashing_chestnut"] = not chestnut.flashing
# ensure device is fully booted
startup_conditions["device_booted"] = startup_conditions.get("device_booted", False) or HARDWARE.booted()
# if the temperature enters the danger zone, go offroad to cool down
onroad_conditions["device_temp_good"] = thermal_status < ThermalStatus.critical
extra_text = f"{offroad_comp_temp:.1f}C"
show_alert = (not onroad_conditions["device_temp_good"] or not startup_conditions["device_temp_engageable"]) and onroad_conditions["ignition"]
set_offroad_alert_if_changed("Offroad_TemperatureTooHigh", show_alert, extra_text=extra_text)
update_jetlink_alert()
if show_alert:
msg.deviceState.fanSpeedPercentDesired = 100
# *** registration check ***
if not PC:
# we enforce this for our software, but you are welcome
# to make a different decision in your software
startup_conditions["registered_device"] = params.get("DongleId") != UNREGISTERED_DONGLE_ID
# Handle offroad/onroad transition
should_start = resolve_start(drive_mode, onroad_conditions, startup_conditions, already_started=started_ts is not None)
if should_start != should_start_prev or (count == 0):
params.put_bool("IsEngaged", False, block=True)
engaged_prev = False
if sm.updated['selfdriveState']:
engaged = sm['selfdriveState'].enabled
if engaged != engaged_prev:
params.put_bool("IsEngaged", engaged, block=True)
engaged_prev = engaged
try:
with open('/dev/kmsg', 'w') as kmsg:
kmsg.write(f"<3>[hardware] engaged: {engaged}\n")
except Exception:
pass
should_pwrsave = not (onroad_conditions["ignition"] or should_start) and msg.deviceState.screenBrightnessPercent < 1e-3
if should_pwrsave != pwrsave or (count == 0):
HARDWARE.set_power_save(should_pwrsave)
pwrsave = should_pwrsave
if onroad_conditions["ignition"]:
physical_off_ts = None
forced_power_episode = False
elif drive_mode != Mode.AUTO or forced_power_episode:
if physical_off_ts is None:
physical_off_ts = off_ts if not physical_edge and off_ts is not None else time.monotonic()
forced_power_episode |= drive_mode != Mode.AUTO
if should_start:
off_ts = None
if started_ts is None:
started_ts = time.monotonic()
started_seen = True
if startup_blocked_ts is not None:
cloudlog.event("Startup after block", block_duration=(time.monotonic() - startup_blocked_ts),
startup_conditions=startup_conditions, onroad_conditions=onroad_conditions,
startup_conditions_prev=startup_conditions_prev, error=True)
startup_blocked_ts = None
else:
if onroad_conditions["ignition"] and (startup_conditions != startup_conditions_prev):
cloudlog.event("Startup blocked", startup_conditions=startup_conditions, onroad_conditions=onroad_conditions, error=True)
startup_conditions_prev = startup_conditions.copy()
startup_blocked_ts = time.monotonic()
started_ts = None
if off_ts is None:
off_ts = time.monotonic()
# Offroad power monitoring
voltage = None if peripheralState.pandaType == log.PandaState.PandaType.unknown else peripheralState.voltage
power_monitor.calculate(voltage, onroad_conditions["ignition"])
msg.deviceState.offroadPowerUsageUwh = power_monitor.get_power_used()
msg.deviceState.carBatteryCapacityUwh = max(0, power_monitor.get_car_battery_capacity())
current_power_draw = HARDWARE.get_current_power_draw()
msg.deviceState.powerDrawW = current_power_draw
som_power_draw = HARDWARE.get_som_power_draw()
msg.deviceState.somPowerDrawW = som_power_draw
# Check if we need to shut down
shutdown_off_ts = physical_off_ts if forced_power_episode else off_ts
if power_monitor.should_shutdown(onroad_conditions["ignition"], in_car, shutdown_off_ts, started_seen):
cloudlog.warning(f"shutting device down, offroad since {shutdown_off_ts}")
params.put_bool("DoShutdown", True, block=True)
msg.deviceState.started = started_ts is not None
msg.deviceState.startedMonoTime = int(1e9*(started_ts or 0))
last_ping = params.get("LastAthenaPingTime")
if last_ping is not None:
msg.deviceState.lastAthenaPingTime = last_ping
msg.deviceState.thermalStatus = thermal_status
pm.send("deviceState", msg)
# report to server once every 10 minutes, or every 1s when thermally blocked
rising_edge_started = should_start and not should_start_prev
status_packet_interval = 1. if show_alert else 600.
if rising_edge_started or (count % int(status_packet_interval / DT_HW)) == 0:
dat = {
'count': count,
'pandaStates': [strip_deprecated_keys(p.to_dict()) for p in pandaStates],
'peripheralState': strip_deprecated_keys(peripheralState.to_dict()),
'location': (strip_deprecated_keys(sm["gpsLocationExternal"].to_dict()) if sm.alive["gpsLocationExternal"] else None),
'deviceState': strip_deprecated_keys(msg.to_dict())
}
cloudlog.event("STATUS_PACKET", **dat)
# save last one before going onroad
if rising_edge_started:
try:
params.put("LastOffroadStatusPacket", dat, block=True)
except Exception:
cloudlog.exception("failed to save offroad status")
params.put_bool("NetworkMetered", msg.deviceState.networkMetered)
now_ts = time.monotonic()
if off_ts:
uptime_offroad += now_ts - max(last_uptime_ts, off_ts)
elif started_ts:
uptime_onroad += now_ts - max(last_uptime_ts, started_ts)
last_uptime_ts = now_ts
if (count % int(60. / DT_HW)) == 0:
params.put("UptimeOffroad", uptime_offroad, block=True)
params.put("UptimeOnroad", uptime_onroad, block=True)
count += 1
should_start_prev = should_start
def main():
hw_queue = queue.Queue(maxsize=1)
end_event = threading.Event()
threads = [
threading.Thread(target=hw_state_thread, args=(end_event, hw_queue)),
threading.Thread(target=hardware_thread, args=(end_event, hw_queue)),
]
if COMMA_HARDWARE:
threads.append(threading.Thread(target=touch_thread, args=(end_event,)))
threads.append(threading.Thread(target=chestnut_state_thread, args=(end_event,)))
for t in threads:
t.start()
try:
while True:
time.sleep(1)
if not all(t.is_alive() for t in threads):
break
finally:
end_event.set()
for t in threads:
t.join()
if __name__ == "__main__":
main()