mawei
/
openpilot
mirror of https://github.com/1okko/openpilot.git
1
0
Fork 0
Code Issues Packages Projects Releases Wiki Activity

car: Check params before car state is updated (#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
This commit is contained in:
Jason Wen 2024-08-01 14:12:27 -08:00 committed by GitHub
parent f311c8b1d7
commit 08afbd7cb5
No known key found for this signature in database
GPG Key ID: B5690EEEBB952194
13 changed files with 63 additions and 36 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

28
selfdrive/car/card.py
View File

@ -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():

1
selfdrive/car/chrysler/interface.py
View File

@ -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,

1
selfdrive/car/ford/interface.py
View File

@ -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,

1
selfdrive/car/gm/interface.py
View File

@ -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

1
selfdrive/car/honda/interface.py
View File

@ -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),

1
selfdrive/car/hyundai/interface.py
View File

@ -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),

31
selfdrive/car/interfaces.py
View File

@ -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

1
selfdrive/car/mazda/interface.py
View File

@ -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 = [

1
selfdrive/car/nissan/interface.py
View File

@ -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})

30
selfdrive/car/param_manager.py Normal file
View File

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

1
selfdrive/car/subaru/interface.py
View File

@ -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)

1
selfdrive/car/toyota/interface.py
View File

@ -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

1
selfdrive/car/volkswagen/interface.py
View File

@ -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)