diff --git a/common/params_keys.h b/common/params_keys.h index a28a02693b..6474fb5840 100644 --- a/common/params_keys.h +++ b/common/params_keys.h @@ -125,6 +125,8 @@ inline static std::unordered_map keys = { {"ApiCache_DriveStats", PERSISTENT}, {"AutoLaneChangeBsmDelay", PERSISTENT}, {"AutoLaneChangeTimer", PERSISTENT}, + {"BlinkerMinLateralControlSpeed", PERSISTENT | BACKUP}, + {"BlinkerPauseLateralControl", PERSISTENT | BACKUP}, {"CarParamsSP", CLEAR_ON_MANAGER_START | CLEAR_ON_ONROAD_TRANSITION}, {"CarParamsSPCache", CLEAR_ON_MANAGER_START}, {"CarParamsSPPersistent", PERSISTENT}, diff --git a/sunnypilot/selfdrive/controls/controlsd_ext.py b/sunnypilot/selfdrive/controls/controlsd_ext.py index 4978c01332..6be015a9f4 100644 --- a/sunnypilot/selfdrive/controls/controlsd_ext.py +++ b/sunnypilot/selfdrive/controls/controlsd_ext.py @@ -8,6 +8,7 @@ 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 @@ -20,6 +21,10 @@ class ControlsExt: 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") @@ -30,9 +35,19 @@ class ControlsExt: def get_params_sp(self) -> None: self.param_store.update(self.params) - @staticmethod - def get_lat_active(sm: messaging.SubMaster) -> bool: + 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: + return False if ss_sp.mads.available: return bool(ss_sp.mads.active) diff --git a/system/manager/manager.py b/system/manager/manager.py index 71f2bc525a..42a5baec5d 100755 --- a/system/manager/manager.py +++ b/system/manager/manager.py @@ -44,6 +44,8 @@ def manager_init() -> None: sunnypilot_default_params: list[tuple[str, str | bytes]] = [ ("AutoLaneChangeTimer", "0"), ("AutoLaneChangeBsmDelay", "0"), + ("BlinkerMinLateralControlSpeed", "5"), # MPH or km/h + ("BlinkerPauseLateralControl", "1"), ("DynamicExperimentalControl", "0"), ("HyundaiLongitudinalTuning", "0"), ("Mads", "1"),