diff --git a/sunnypilot/selfdrive/controls/controlsd_ext.py b/sunnypilot/selfdrive/controls/controlsd_ext.py index 6be015a9f4..efbc405bab 100644 --- a/sunnypilot/selfdrive/controls/controlsd_ext.py +++ b/sunnypilot/selfdrive/controls/controlsd_ext.py @@ -8,23 +8,20 @@ import cereal.messaging as messaging from cereal import custom from opendbc.car import structs -from openpilot.common.conversions import Conversions as CV from openpilot.common.params import Params from openpilot.common.swaglog import cloudlog from openpilot.sunnypilot.selfdrive.controls.lib.param_store import ParamStore +from openpilot.sunnypilot.selfdrive.controls.lib.blinker_pause_lateral import BlinkerPauseLateral class ControlsExt: def __init__(self, CP: structs.CarParams, params: Params): self.CP = CP self.params = params + self.blinker_pause_lateral = BlinkerPauseLateral() self.param_store = ParamStore(self.CP) self.get_params_sp() - self.is_metric = self.params.get_bool("IsMetric") - self.blinker_pause_lateral_control = self.params.get_bool("BlinkerPauseLateralControl") - self.blinker_min_lat_control_speed = int(self.params.get("BlinkerMinLateralControlSpeed", encoding='utf8')) - cloudlog.info("controlsd_ext is waiting for CarParamsSP") self.CP_SP = messaging.log_from_bytes(params.get("CarParamsSP", block=True), custom.CarParamsSP) cloudlog.info("controlsd_ext got CarParamsSP") @@ -35,20 +32,11 @@ class ControlsExt: def get_params_sp(self) -> None: self.param_store.update(self.params) - self.is_metric = self.params.get_bool("IsMetric") - self.blinker_pause_lateral_control = self.params.get_bool("BlinkerPauseLateralControl") - self.blinker_min_lat_control_speed = int(self.params.get("BlinkerMinLateralControlSpeed", encoding='utf8')) - def get_lat_active(self, sm: messaging.SubMaster) -> bool: - ss_sp = sm['selfdriveStateSP'] - car_state = sm['carState'] - - one_blinker = car_state.leftBlinker != car_state.rightBlinker - blinker_min_speed_ms = self.blinker_min_lat_control_speed * (CV.MPH_TO_MS if self.is_metric else CV.KPH_TO_MS) - - if self.blinker_pause_lateral_control and one_blinker and car_state.vEgo < blinker_min_speed_ms: + if self.blinker_pause_lateral.should_blinker_pause_lateral(sm['carState']): return False + ss_sp = sm['selfdriveStateSP'] if ss_sp.mads.available: return bool(ss_sp.mads.active) diff --git a/sunnypilot/selfdrive/controls/lib/blinker_pause_lateral.py b/sunnypilot/selfdrive/controls/lib/blinker_pause_lateral.py new file mode 100644 index 0000000000..a9da5ac2e4 --- /dev/null +++ b/sunnypilot/selfdrive/controls/lib/blinker_pause_lateral.py @@ -0,0 +1,34 @@ +""" +Copyright (c) 2021-, Haibin Wen, sunnypilot, and a number of other contributors. + +This file is part of sunnypilot and is licensed under the MIT License. +See the LICENSE.md file in the root directory for more details. +""" +from cereal import car + +from openpilot.common.conversions import Conversions as CV +from openpilot.common.params import Params + + +class BlinkerPauseLateral: + def __init__(self): + self.params = Params() + + self.enabled = self.params.get_bool("BlinkerPauseLateralControl") + self.is_metric = self.params.get_bool("IsMetric") + self.min_speed = 0 + + def get_params(self) -> None: + self.enabled = self.params.get_bool("BlinkerPauseLateralControl") + self.is_metric = self.params.get_bool("IsMetric") + self.min_speed = int(self.params.get("BlinkerMinLateralControlSpeed", encoding='utf8')) + + def should_blinker_pause_lateral(self, CS: car.CarState) -> bool: + if not self.enabled: + return False + + one_blinker = CS.leftBlinker != CS.rightBlinker + speed_factor = CV.KPH_TO_MS if self.is_metric else CV.MPH_TO_MS + min_speed_ms = self.min_speed * speed_factor + + return one_blinker and CS.vEgo < min_speed_ms