diff --git a/selfdrive/car/card.py b/selfdrive/car/card.py index 98be4bf82f..fe6f67296c 100755 --- a/selfdrive/car/card.py +++ b/selfdrive/car/card.py @@ -1,6 +1,8 @@ #!/usr/bin/env python3 import os +import threading import time +from types import SimpleNamespace import cereal.messaging as messaging @@ -14,6 +16,7 @@ from openpilot.common.realtime import config_realtime_process, Priority, Ratekee from openpilot.selfdrive.pandad import can_list_to_can_capnp from openpilot.selfdrive.car.car_helpers import get_car, get_one_can from openpilot.selfdrive.car.interfaces import CarInterfaceBase +from openpilot.selfdrive.car.param_manager import ParamManager from openpilot.selfdrive.controls.lib.events import Events REPLAY = "REPLAY" in os.environ @@ -90,6 +93,10 @@ class Car: self.events = Events() + self.param_manager: ParamManager = ParamManager() + self.param_manager.update(self.params) + self._params_list: SimpleNamespace = self.param_manager.get_params() + # card is driven by can recv, expected at 100Hz self.rk = Ratekeeper(100, print_delay_threshold=None) @@ -98,7 +105,7 @@ class Car: # Update carState from CAN can_strs = messaging.drain_sock_raw(self.can_sock, wait_for_one=True) - CS = self.CI.update(self.CC_prev, can_strs) + CS = self.CI.update(self.CC_prev, can_strs, self._params_list) self.sm.update(0) @@ -186,10 +193,23 @@ class Car: self.initialized_prev = initialized self.CS_prev = CS.as_reader() + def sp_params_thread(self, event: threading.Event) -> None: + while not event.is_set(): + self.param_manager.update(self.params) + self._params_list = self.param_manager.get_params() + time.sleep(0.1) + def card_thread(self): - while True: - self.step() - self.rk.monitor_time() + event = threading.Event() + thread = threading.Thread(target=self.sp_params_thread, args=(event, )) + try: + thread.start() + while True: + self.step() + self.rk.monitor_time() + finally: + event.set() + thread.join() def main(): diff --git a/selfdrive/car/chrysler/interface.py b/selfdrive/car/chrysler/interface.py index 38d7cf0828..fbfce9d30d 100755 --- a/selfdrive/car/chrysler/interface.py +++ b/selfdrive/car/chrysler/interface.py @@ -91,7 +91,6 @@ class CarInterface(CarInterfaceBase): def _update(self, c): ret = self.CS.update(self.cp, self.cp_cam) - self.sp_update_params() self.CS.button_events = [ *self.CS.button_events, diff --git a/selfdrive/car/ford/interface.py b/selfdrive/car/ford/interface.py index 080663c9cf..a3bbd7ecf5 100644 --- a/selfdrive/car/ford/interface.py +++ b/selfdrive/car/ford/interface.py @@ -72,7 +72,6 @@ class CarInterface(CarInterfaceBase): def _update(self, c): ret = self.CS.update(self.cp, self.cp_cam) - self.sp_update_params() self.CS.button_events = [ *self.CS.button_events, diff --git a/selfdrive/car/gm/interface.py b/selfdrive/car/gm/interface.py index 726af2afac..e60039ec9a 100755 --- a/selfdrive/car/gm/interface.py +++ b/selfdrive/car/gm/interface.py @@ -204,7 +204,6 @@ class CarInterface(CarInterfaceBase): # returns a car.CarState def _update(self, c): ret = self.CS.update(self.cp, self.cp_cam, self.cp_loopback) - self.sp_update_params() distance_button = 0 diff --git a/selfdrive/car/honda/interface.py b/selfdrive/car/honda/interface.py index af294a63a7..70124a8892 100755 --- a/selfdrive/car/honda/interface.py +++ b/selfdrive/car/honda/interface.py @@ -266,7 +266,6 @@ class CarInterface(CarInterfaceBase): # returns a car.CarState def _update(self, c): ret = self.CS.update(self.cp, self.cp_cam, self.cp_body) - self.sp_update_params() self.CS.button_events = [ *create_button_events(self.CS.cruise_buttons, self.CS.prev_cruise_buttons, BUTTONS_DICT), diff --git a/selfdrive/car/hyundai/interface.py b/selfdrive/car/hyundai/interface.py index e2ad23a7b6..02897f9765 100644 --- a/selfdrive/car/hyundai/interface.py +++ b/selfdrive/car/hyundai/interface.py @@ -203,7 +203,6 @@ class CarInterface(CarInterfaceBase): def _update(self, c): ret = self.CS.update(self.cp, self.cp_cam) - self.sp_update_params() self.CS.button_events = [ *create_button_events(self.CS.cruise_buttons[-1], self.CS.prev_cruise_buttons, BUTTONS_DICT), diff --git a/selfdrive/car/interfaces.py b/selfdrive/car/interfaces.py index b8ca13ec86..bda574f54d 100644 --- a/selfdrive/car/interfaces.py +++ b/selfdrive/car/interfaces.py @@ -7,6 +7,7 @@ import tomllib from abc import abstractmethod, ABC from difflib import SequenceMatcher from enum import StrEnum +from types import SimpleNamespace from typing import Any, NamedTuple from collections.abc import Callable from functools import cache @@ -19,6 +20,7 @@ from openpilot.common.numpy_fast import clip from openpilot.common.params import Params from openpilot.common.realtime import DT_CTRL from openpilot.selfdrive.car import apply_hysteresis, gen_empty_fingerprint, scale_rot_inertia, scale_tire_stiffness, STD_CARGO_KG, ButtonEvents +from openpilot.selfdrive.car.param_manager import ParamManager from openpilot.selfdrive.car.values import PLATFORMS from openpilot.selfdrive.controls.lib.desire_helper import get_min_lateral_speed from openpilot.selfdrive.controls.lib.drive_helpers import V_CRUISE_MAX, V_CRUISE_UNSET, get_friction @@ -239,16 +241,10 @@ class CarInterfaceBase(ABC): self.gear_warning = 0 self.cruise_cancelled_btn = True self.acc_mads_combo = self.param_s.get_bool("AccMadsCombo") - self.is_metric = self.param_s.get_bool("IsMetric") - self.below_speed_pause = self.param_s.get_bool("BelowSpeedPause") - self.pause_lateral_speed = int(self.param_s.get("PauseLateralSpeed", encoding="utf8")) self.prev_acc_mads_combo = False self.mads_event_lock = True self.gap_button_counter = 0 self.experimental_mode_hold = False - self.experimental_mode = self.param_s.get_bool("ExperimentalMode") - self._frame = 0 - self.reverse_dm_cam = self.param_s.get_bool("ReverseDmCam") self.mads_main_toggle = self.param_s.get_bool("MadsCruiseMain") self.lkas_toggle = self.param_s.get_bool("LkasToggle") self.last_mads_init = 0. @@ -421,13 +417,14 @@ class CarInterfaceBase(ABC): def _update(self, c: car.CarControl) -> car.CarState: pass - def update(self, c: car.CarControl, can_strings: list[bytes]) -> car.CarState: + def update(self, c: car.CarControl, can_strings: list[bytes], params_list: SimpleNamespace) -> car.CarState: # parse can for cp in self.can_parsers: if cp is not None: cp.update_strings(can_strings) self.CS.button_events = [] + self.CS.params_list = params_list # get CarState ret = self._update(c) @@ -472,7 +469,7 @@ class CarInterfaceBase(ABC): else: events.add(EventName.wrongGear) if cs_out.gearShifter == GearShifter.reverse: - if not self.reverse_dm_cam and cs_out.vEgo < 5: + if not self.CS.params_list.reverse_dm_cam and cs_out.vEgo < 5: events.add(EventName.spReverseGear) elif cs_out.vEgo >= 5: events.add(EventName.reverseGear) @@ -634,9 +631,9 @@ class CarInterfaceBase(ABC): if self.CP.openpilotLongitudinalControl: self.toggle_exp_mode(gap_button) - lane_change_speed_min = get_min_lateral_speed(self.pause_lateral_speed, self.is_metric) + lane_change_speed_min = get_min_lateral_speed(self.CS.params_list.pause_lateral_speed, self.CS.params_list.is_metric) - cs_out.belowLaneChangeSpeed = cs_out.vEgo < lane_change_speed_min and self.below_speed_pause + cs_out.belowLaneChangeSpeed = cs_out.vEgo < lane_change_speed_min and self.CS.params_list.below_speed_pause if cs_out.gearShifter in [GearShifter.park, GearShifter.reverse] or cs_out.doorOpen or \ (cs_out.seatbeltUnlatched and cs_out.gearShifter != GearShifter.park): @@ -669,7 +666,7 @@ class CarInterfaceBase(ABC): if self.gap_button_counter > 50: self.gap_button_counter = 0 self.experimental_mode_hold = True - self.param_s.put_bool_nonblocking("ExperimentalMode", not self.experimental_mode) + self.param_s.put_bool_nonblocking("ExperimentalMode", not self.CS.params_list.experimental_mode) else: self.gap_button_counter = 0 self.experimental_mode_hold = False @@ -735,17 +732,6 @@ class CarInterfaceBase(ABC): return events, cs_out - def sp_update_params(self): - self.experimental_mode = self.param_s.get_bool("ExperimentalMode") - self._frame += 1 - if self._frame % 100 == 0: - self.is_metric = self.param_s.get_bool("IsMetric") - self.below_speed_pause = self.param_s.get_bool("BelowSpeedPause") - self.pause_lateral_speed = int(self.param_s.get("PauseLateralSpeed", encoding="utf8")) - if self._frame % 300 == 0: - self._frame = 0 - self.reverse_dm_cam = self.param_s.get_bool("ReverseDmCam") - class RadarInterfaceBase(ABC): def __init__(self, CP): self.rcp = None @@ -785,6 +771,7 @@ class CarStateBase(ABC): self.control_initialized = False self.button_events: list[capnp.lib.capnp._DynamicStructBuilder] = [] + self.params_list: SimpleNamespace = ParamManager().get_params() Q = [[0.0, 0.0], [0.0, 100.0]] R = 0.3 diff --git a/selfdrive/car/mazda/interface.py b/selfdrive/car/mazda/interface.py index 8b113cf4db..4c26a18a57 100755 --- a/selfdrive/car/mazda/interface.py +++ b/selfdrive/car/mazda/interface.py @@ -37,7 +37,6 @@ class CarInterface(CarInterfaceBase): # returns a car.CarState def _update(self, c): ret = self.CS.update(self.cp, self.cp_cam) - self.sp_update_params() # TODO: add button types for inc and dec self.CS.button_events = [ diff --git a/selfdrive/car/nissan/interface.py b/selfdrive/car/nissan/interface.py index 4a9e5a9125..661ac668a5 100644 --- a/selfdrive/car/nissan/interface.py +++ b/selfdrive/car/nissan/interface.py @@ -32,7 +32,6 @@ class CarInterface(CarInterfaceBase): # returns a car.CarState def _update(self, c): ret = self.CS.update(self.cp, self.cp_adas, self.cp_cam) - self.sp_update_params() self.CS.button_events = create_button_events(self.CS.distance_button, self.CS.prev_distance_button, {1: ButtonType.gapAdjustCruise}) diff --git a/selfdrive/car/param_manager.py b/selfdrive/car/param_manager.py new file mode 100644 index 0000000000..85018aaa02 --- /dev/null +++ b/selfdrive/car/param_manager.py @@ -0,0 +1,30 @@ +from types import SimpleNamespace + +from openpilot.common.params import Params + + +class ParamManager: + def __init__(self): + self._params_list: SimpleNamespace = self._create_namespace({ + "below_speed_pause": False, + "experimental_mode": False, + "is_metric": False, + "pause_lateral_speed": 0, + "reverse_dm_cam": False, + }) + + @staticmethod + def _create_namespace(data: dict) -> SimpleNamespace: + return SimpleNamespace(**data) + + def get_params(self) -> SimpleNamespace: + return self._params_list + + def update(self, params: Params) -> None: + self._params_list = self._create_namespace({ + "below_speed_pause": params.get_bool("BelowSpeedPause"), + "experimental_mode": params.get_bool("ExperimentalMode"), + "is_metric": params.get_bool("IsMetric"), + "pause_lateral_speed": int(params.get("PauseLateralSpeed", encoding="utf8")), + "reverse_dm_cam": params.get_bool("ReverseDmCam"), + }) diff --git a/selfdrive/car/subaru/interface.py b/selfdrive/car/subaru/interface.py index 7eefeb66eb..185ee8e54d 100644 --- a/selfdrive/car/subaru/interface.py +++ b/selfdrive/car/subaru/interface.py @@ -116,7 +116,6 @@ class CarInterface(CarInterfaceBase): def _update(self, c): ret = self.CS.update(self.cp, self.cp_cam, self.cp_body) - self.sp_update_params() self.CS.mads_enabled = self.get_sp_cruise_main_state(ret, self.CS) diff --git a/selfdrive/car/toyota/interface.py b/selfdrive/car/toyota/interface.py index 1b0528d12c..5214cc1265 100644 --- a/selfdrive/car/toyota/interface.py +++ b/selfdrive/car/toyota/interface.py @@ -207,7 +207,6 @@ class CarInterface(CarInterfaceBase): # returns a car.CarState def _update(self, c): ret = self.CS.update(self.cp, self.cp_cam) - self.sp_update_params() distance_button = 0 diff --git a/selfdrive/car/volkswagen/interface.py b/selfdrive/car/volkswagen/interface.py index d965cf2e8b..0d57539a1b 100644 --- a/selfdrive/car/volkswagen/interface.py +++ b/selfdrive/car/volkswagen/interface.py @@ -113,7 +113,6 @@ class CarInterface(CarInterfaceBase): # returns a car.CarState def _update(self, c): ret = self.CS.update(self.cp, self.cp_cam, self.cp_ext, self.CP.transmissionType) - self.sp_update_params() self.CS.mads_enabled = self.get_sp_cruise_main_state(ret, self.CS)