mawei/sunnypilot
1
0
Fork 0
mirror of https://github.com/sunnypilot/sunnypilot.git synced 2026-11-03 00:53:44 +08:00
Code Issues Packages Projects Releases Wiki Activity

handle all params with exceptions

Browse Source
This commit is contained in:
Jason Wen
2025-06-05 01:49:28 -04:00
parent 06eaa56a9d
commit 1929a37b1f
1 changed files with 30 additions and 14 deletions
Download Patch File Download Diff File Expand all files Collapse all files
sunnypilot/selfdrive/controls/lib/speed_limit_controller/speed_limit_controller.py
+30 -14
View File
@@ -1,3 +1,4 @@
from collections.abc import Callable
import numpy as np
import time
@@ -48,12 +49,12 @@ class SpeedLimitController:
self._gas_pressed = False
self._pcm_cruise_op_long = CP.openpilotLongitudinalControl and CP.pcmCruise
self._offset_type = int(self._params.get("SpeedLimitOffsetType", encoding='utf8'))
self._offset_value = float(self._params.get("SpeedLimitValueOffset", encoding='utf8'))
self._warning_type = int(self._params.get("SpeedLimitWarningType", encoding='utf8'))
self._warning_offset_type = int(self._params.get("SpeedLimitWarningOffsetType", encoding='utf8'))
self._warning_offset_value = float(self._params.get("SpeedLimitWarningValueOffset", encoding='utf8'))
self._engage_type = np.clip(int(self._params.get("SpeedLimitEngageType", encoding='utf8')), Engage.auto, Engage.user_confirm)
self._offset_type = self._read_int_param("SpeedLimitOffsetType")
self._offset_value = self._read_int_param("SpeedLimitValueOffset")
self._warning_type = self._read_int_param("SpeedLimitWarningType")
self._warning_offset_type = self._read_int_param("SpeedLimitWarningOffsetType")
self._warning_offset_value = self._read_int_param("SpeedLimitWarningValueOffset")
self._engage_type = self._read_engage_type_param()
self._brake_pressed = False
self._brake_pressed_prev = False
self._current_time = 0.
@@ -150,18 +151,17 @@ class SpeedLimitController:
def _update_params(self) -> None:
if self._current_time > self._last_params_update + PARAMS_UPDATE_PERIOD:
self._is_enabled = self._params.get_bool("SpeedLimitControl")
self._offset_type = int(self._params.get("SpeedLimitOffsetType", encoding='utf8'))
self._offset_value = float(self._params.get("SpeedLimitValueOffset", encoding='utf8'))
self._warning_type = int(self._params.get("SpeedLimitWarningType", encoding='utf8'))
self._warning_offset_type = int(self._params.get("SpeedLimitWarningOffsetType", encoding='utf8'))
self._warning_offset_value = float(self._params.get("SpeedLimitWarningValueOffset", encoding='utf8'))
self._offset_type = self._read_int_param("SpeedLimitOffsetType")
self._offset_value = self._read_int_param("SpeedLimitValueOffset")
self._warning_type = self._read_int_param("SpeedLimitWarningType")
self._warning_offset_type = self._read_int_param("SpeedLimitWarningOffsetType")
self._warning_offset_value = self._read_int_param("SpeedLimitWarningValueOffset")
self._policy = self._read_policy_param()
self._is_metric = self._params.get_bool("IsMetric")
self._ms_to_local = CV.MS_TO_KPH if self._is_metric else CV.MS_TO_MPH
self._resolver.change_policy(self._policy)
self._engage_type = Engage.auto if self._pcm_cruise_op_long else \
np.clip(int(self._params.get("SpeedLimitEngageType", encoding='utf8')), Engage.auto, Engage.user_confirm)
debug(f'Updated Speed limit params. enabled: {self._is_enabled}, with offset: {self._offset_type}')
self._engage_type = self._read_engage_type_param()
self._last_params_update = self._current_time
def _read_policy_param(self) -> Policy:
@@ -170,6 +170,22 @@ class SpeedLimitController:
except (ValueError, TypeError):
return Policy.car_state_priority
def _read_engage_type_param(self) -> Engage:
if self._pcm_cruise_op_long:
return Engage.auto
return Engage(self._read_int_param("SpeedLimitEngageType", validator=lambda x: np.clip(x, Engage.auto, Engage.user_confirm)))
def _read_int_param(self, key: str, default: int = 0, validator: Callable[[int], int] = None) -> int:
try:
val = int(self._params.get(key, encoding='utf8'))
if validator is not None:
return validator(val)
return val
except (ValueError, TypeError):
return default
def _update_calculations(self) -> None:
# Update current velocity offset (error)
self._v_offset = self.speed_limit_offseted - self._v_ego