car: Check params before car state is updated (commaai#364)

* car: Check params before car state is updated

* read params in a thread instead

* initialize list

* Do this instead

* type hint

* ParamManager

* make them internal

Author: sunnyhaibin

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.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 @@ def __init__(self, CI=None):
 
     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 @@ def state_update(self) -> car.CarState:
 
     # 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 @@ def step(self):
     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():

selfdrive/car/chrysler/interface.py

@@ -91,7 +91,6 @@ def _get_params(ret, candidate, fingerprint, car_fw, experimental_long, docs):
 
   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,

selfdrive/car/ford/interface.py

@@ -72,7 +72,6 @@ def _get_params(ret, candidate, fingerprint, car_fw, experimental_long, docs):
 
   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,

selfdrive/car/gm/interface.py

@@ -204,7 +204,6 @@ def _get_params(ret, candidate, fingerprint, car_fw, experimental_long, docs):
   # 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
 

selfdrive/car/honda/interface.py

@@ -266,7 +266,6 @@ def init(CP, logcan, sendcan):
   # 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),

selfdrive/car/hyundai/interface.py

@@ -203,7 +203,6 @@ def init(CP, logcan, sendcan):
 
   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),

selfdrive/car/interfaces.py

@@ -7,6 +7,7 @@
 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.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 @@ def __init__(self, CP, CarController, CarState):
     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 @@ def sp_configure_custom_torque_tune(ret, params):
   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 @@ def create_common_events(self, cs_out, c, extra_gears=None, pcm_enable=True, all
       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 @@ def get_sp_common_state(self, cs_out, CS, min_enable_speed_pcm=False, gear_allow
     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 @@ def toggle_exp_mode(self, gap_pressed):
         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 @@ def create_sp_events(self, CS, cs_out, events, main_enabled=False, allow_enable=
 
     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 @@ def __init__(self, CP):
     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

selfdrive/car/mazda/interface.py

@@ -37,7 +37,6 @@ def _get_params(ret, candidate, fingerprint, car_fw, experimental_long, docs):
   # 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 = [

selfdrive/car/nissan/interface.py

@@ -32,7 +32,6 @@ def _get_params(ret, candidate, fingerprint, car_fw, experimental_long, docs):
   # 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})
 

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"),
+    })

selfdrive/car/subaru/interface.py

@@ -116,7 +116,6 @@ def _get_params(ret, candidate: CAR, fingerprint, car_fw, experimental_long, doc
   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)
 

selfdrive/car/toyota/interface.py

@@ -207,7 +207,6 @@ def init(CP, logcan, sendcan):
   # returns a car.CarState
   def _update(self, c):
     ret = self.CS.update(self.cp, self.cp_cam)
-    self.sp_update_params()
 
     distance_button = 0
 

selfdrive/car/volkswagen/interface.py

@@ -113,7 +113,6 @@ def _get_params(ret, candidate: CAR, fingerprint, car_fw, experimental_long, doc
   # 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)