From 437726b3486c2e214205577ad009b11c79668cdc Mon Sep 17 00:00:00 2001 From: Jason Wen Date: Wed, 15 Oct 2025 18:05:50 -0400 Subject: [PATCH 1/9] Speed Limit Mode: only cleanup param if Assist was selected (#1393) Speed Limit Mode: only cleanup param if it was Assist --- .../settings/longitudinal/speed_limit/speed_limit_settings.cc | 4 +++- sunnypilot/selfdrive/car/interfaces.py | 4 +++- 2 files changed, 6 insertions(+), 2 deletions(-) diff --git a/selfdrive/ui/sunnypilot/qt/offroad/settings/longitudinal/speed_limit/speed_limit_settings.cc b/selfdrive/ui/sunnypilot/qt/offroad/settings/longitudinal/speed_limit/speed_limit_settings.cc index 95aa4ef26f..5c3b03d2af 100644 --- a/selfdrive/ui/sunnypilot/qt/offroad/settings/longitudinal/speed_limit/speed_limit_settings.cc +++ b/selfdrive/ui/sunnypilot/qt/offroad/settings/longitudinal/speed_limit/speed_limit_settings.cc @@ -124,7 +124,9 @@ void SpeedLimitSettings::refresh() { intelligent_cruise_button_management_available = CP_SP.getIntelligentCruiseButtonManagementAvailable(); if (!has_longitudinal_control && CP_SP.getPcmCruiseSpeed()) { - params.put("SpeedLimitMode", std::to_string(static_cast(SpeedLimitMode::WARNING))); + if (speed_limit_mode_param == SpeedLimitMode::ASSIST) { + params.put("SpeedLimitMode", std::to_string(static_cast(SpeedLimitMode::WARNING))); + } } } else { has_longitudinal_control = false; diff --git a/sunnypilot/selfdrive/car/interfaces.py b/sunnypilot/selfdrive/car/interfaces.py index 5467639a63..6a868ab85b 100644 --- a/sunnypilot/selfdrive/car/interfaces.py +++ b/sunnypilot/selfdrive/car/interfaces.py @@ -86,7 +86,9 @@ def _cleanup_unsupported_params(CP: structs.CarParams, CP_SP: structs.CarParamsS params.remove("CustomAccIncrementsEnabled") params.remove("SmartCruiseControlVision") params.remove("SmartCruiseControlMap") - params.put("SpeedLimitMode", int(SpeedLimitMode.warning)) + + if params.get("SpeedLimitMode", return_default=True) == SpeedLimitMode.assist: + params.put("SpeedLimitMode", int(SpeedLimitMode.warning)) def setup_interfaces(CI: CarInterfaceBase, params: Params = None) -> None: From 2d779f5db9d46816e580728af55d09931e9961c7 Mon Sep 17 00:00:00 2001 From: Jason Wen Date: Wed, 15 Oct 2025 22:38:57 -0400 Subject: [PATCH 2/9] E2E Helper: universal state machine --- .../controls/lib/e2e_alerts_helper.py | 100 +++++++++++++----- 1 file changed, 75 insertions(+), 25 deletions(-) diff --git a/sunnypilot/selfdrive/controls/lib/e2e_alerts_helper.py b/sunnypilot/selfdrive/controls/lib/e2e_alerts_helper.py index 5a92d878d6..df4c477e89 100644 --- a/sunnypilot/selfdrive/controls/lib/e2e_alerts_helper.py +++ b/sunnypilot/selfdrive/controls/lib/e2e_alerts_helper.py @@ -15,10 +15,20 @@ from openpilot.sunnypilot.selfdrive.selfdrived.events import EventsSP GREEN_LIGHT_X_THRESHOLD = 30 +class E2EStates: + INACTIVE = 0 + ARMED = 1 + CONSUMED = 2 + + class E2EAlertsHelper: def __init__(self): self._params = Params() self.frame = -1 + self.green_light_state = E2EStates.INACTIVE + self.prev_green_light_state = E2EStates.INACTIVE + self.lead_depart_state = E2EStates.INACTIVE + self.prev_lead_depart_state = E2EStates.INACTIVE self.green_light_alert = False self.green_light_alert_enabled = self._params.get_bool("GreenLightAlert") @@ -29,64 +39,104 @@ class E2EAlertsHelper: self.green_light_alert_count = 0 self.last_lead_distance = -1 self.last_moving_frame = -1 + self.last_standstill = False + + self.allowed = False + self.has_lead = False def _read_params(self) -> None: if self.frame % int(PARAMS_UPDATE_PERIOD / DT_MDL) == 0: self.green_light_alert_enabled = self._params.get_bool("GreenLightAlert") self.lead_depart_alert_enabled = self._params.get_bool("LeadDepartAlert") - def update(self, sm: messaging.SubMaster, events_sp: EventsSP) -> None: - self._read_params() - + def update_alert_trigger(self, sm: messaging.SubMaster): CS = sm['carState'] CC = sm['carControl'] model_x = sm['modelV2'].position.x max_idx = len(model_x) - 1 - has_lead = sm['radarState'].leadOne.status + self.has_lead = sm['radarState'].leadOne.status lead_dRel = sm['radarState'].leadOne.dRel + standstill = CS.standstill moving = not standstill and CS.vEgo > 0.1 - _allowed = standstill and not CS.gasPressed and not CC.enabled if moving: self.last_moving_frame = self.frame recent_moving = self.last_moving_frame == -1 or (self.frame - self.last_moving_frame) * DT_MDL < 2.0 - if standstill and not recent_moving: - self.alert_allowed = True - elif not standstill: - self.alert_allowed = False - self.green_light_alert_count = 0 - self.last_lead_distance = -1 + self.allowed = not moving and not CS.gasPressed and not CC.enabled and not recent_moving # Green Light Alert - _green_light_alert = False - if self.green_light_alert_enabled and _allowed and not has_lead and model_x[max_idx] > GREEN_LIGHT_X_THRESHOLD: - if self.alert_allowed: + green_light_trigger = False + if self.green_light_state == E2EStates.ARMED: + if model_x[max_idx] > GREEN_LIGHT_X_THRESHOLD: self.green_light_alert_count += 1 else: self.green_light_alert_count = 0 - if self.green_light_alert_count > 2 and self.alert_allowed: - _green_light_alert = True - self.alert_allowed = False - else: + if self.green_light_alert_count > 2: + green_light_trigger = True + elif self.green_light_state != E2EStates.ARMED: self.green_light_alert_count = 0 - self.green_light_alert = _green_light_alert - # Lead Departure Alert - _lead_depart_alert = False - if self.lead_depart_alert_enabled and _allowed and has_lead: + lead_depart_trigger = False + if self.lead_depart_state == E2EStates.ARMED: if self.last_lead_distance == -1 or lead_dRel < self.last_lead_distance: self.last_lead_distance = lead_dRel if self.last_lead_distance != -1 and (lead_dRel - self.last_lead_distance > 1.0) and self.alert_allowed: - _lead_depart_alert = True - self.alert_allowed = False + lead_depart_trigger = True + elif self.lead_depart_state != E2EStates.ARMED: + self.last_lead_distance = -1 - self.lead_depart_alert = _lead_depart_alert + return green_light_trigger, lead_depart_trigger + + @staticmethod + def update_state_machine(state: int, enabled: bool, allowed: bool, triggered: bool) -> tuple[int, bool]: + if state != E2EStates.INACTIVE: + if not allowed or not enabled: + state = E2EStates.INACTIVE + + else: + if state == E2EStates.ARMED: + if triggered: + state = E2EStates.CONSUMED + + elif not allowed: + state = E2EStates.INACTIVE + + elif state == E2EStates.CONSUMED: + pass + + elif state == E2EStates.INACTIVE: + if allowed and enabled: + state = E2EStates.ARMED + + return state, triggered + + def update(self, sm: messaging.SubMaster, events_sp: EventsSP) -> None: + self._read_params() + + green_light_trigger, lead_depart_trigger = self.update_alert_trigger(sm) + + self.prev_green_light_state = self.green_light_state + self.prev_lead_depart_state = self.lead_depart_state + + self.green_light_state, self.green_light_alert = self.update_state_machine( + self.green_light_state, + self.green_light_alert_enabled, + self.allowed and not self.has_lead, + green_light_trigger + ) + + self.lead_depart_state, self.lead_depart_alert = self.update_state_machine( + self.lead_depart_state, + self.lead_depart_alert_enabled, + self.allowed and self.has_lead, + lead_depart_trigger + ) if self.green_light_alert or self.lead_depart_alert: events_sp.add(custom.OnroadEventSP.EventName.e2eChime) From 7caf05dd5167733e9b65122ec0e574cf122397c1 Mon Sep 17 00:00:00 2001 From: Jason Wen Date: Wed, 15 Oct 2025 22:41:00 -0400 Subject: [PATCH 3/9] not used --- sunnypilot/selfdrive/controls/lib/e2e_alerts_helper.py | 1 - 1 file changed, 1 deletion(-) diff --git a/sunnypilot/selfdrive/controls/lib/e2e_alerts_helper.py b/sunnypilot/selfdrive/controls/lib/e2e_alerts_helper.py index df4c477e89..b3d0b9c2fb 100644 --- a/sunnypilot/selfdrive/controls/lib/e2e_alerts_helper.py +++ b/sunnypilot/selfdrive/controls/lib/e2e_alerts_helper.py @@ -39,7 +39,6 @@ class E2EAlertsHelper: self.green_light_alert_count = 0 self.last_lead_distance = -1 self.last_moving_frame = -1 - self.last_standstill = False self.allowed = False self.has_lead = False From 21aa7ff3672aa992416075047d4d6777c85808bc Mon Sep 17 00:00:00 2001 From: Jason Wen Date: Wed, 15 Oct 2025 22:47:08 -0400 Subject: [PATCH 4/9] rename --- .../selfdrive/controls/lib/e2e_alerts_helper.py | 13 ++++++------- 1 file changed, 6 insertions(+), 7 deletions(-) diff --git a/sunnypilot/selfdrive/controls/lib/e2e_alerts_helper.py b/sunnypilot/selfdrive/controls/lib/e2e_alerts_helper.py index b3d0b9c2fb..f0b0dcc5b9 100644 --- a/sunnypilot/selfdrive/controls/lib/e2e_alerts_helper.py +++ b/sunnypilot/selfdrive/controls/lib/e2e_alerts_helper.py @@ -35,8 +35,7 @@ class E2EAlertsHelper: self.lead_depart_alert = False self.lead_depart_alert_enabled = self._params.get_bool("LeadDepartAlert") - self.alert_allowed = False - self.green_light_alert_count = 0 + self.green_light_trigger_count = 0 self.last_lead_distance = -1 self.last_moving_frame = -1 @@ -70,14 +69,14 @@ class E2EAlertsHelper: green_light_trigger = False if self.green_light_state == E2EStates.ARMED: if model_x[max_idx] > GREEN_LIGHT_X_THRESHOLD: - self.green_light_alert_count += 1 + self.green_light_trigger_count += 1 else: - self.green_light_alert_count = 0 + self.green_light_trigger_count = 0 - if self.green_light_alert_count > 2: + if self.green_light_trigger_count > 2: green_light_trigger = True elif self.green_light_state != E2EStates.ARMED: - self.green_light_alert_count = 0 + self.green_light_trigger_count = 0 # Lead Departure Alert lead_depart_trigger = False @@ -85,7 +84,7 @@ class E2EAlertsHelper: if self.last_lead_distance == -1 or lead_dRel < self.last_lead_distance: self.last_lead_distance = lead_dRel - if self.last_lead_distance != -1 and (lead_dRel - self.last_lead_distance > 1.0) and self.alert_allowed: + if self.last_lead_distance != -1 and (lead_dRel - self.last_lead_distance > 1.0): lead_depart_trigger = True elif self.lead_depart_state != E2EStates.ARMED: self.last_lead_distance = -1 From 48cbe266fc39a0e7ef129d756558eed177099675 Mon Sep 17 00:00:00 2001 From: Jason Wen Date: Wed, 15 Oct 2025 22:50:22 -0400 Subject: [PATCH 5/9] 10 frames for both --- sunnypilot/selfdrive/controls/lib/e2e_alerts_helper.py | 10 +++++++++- 1 file changed, 9 insertions(+), 1 deletion(-) diff --git a/sunnypilot/selfdrive/controls/lib/e2e_alerts_helper.py b/sunnypilot/selfdrive/controls/lib/e2e_alerts_helper.py index f0b0dcc5b9..6a3175ec5e 100644 --- a/sunnypilot/selfdrive/controls/lib/e2e_alerts_helper.py +++ b/sunnypilot/selfdrive/controls/lib/e2e_alerts_helper.py @@ -13,6 +13,7 @@ from openpilot.sunnypilot import PARAMS_UPDATE_PERIOD from openpilot.sunnypilot.selfdrive.selfdrived.events import EventsSP GREEN_LIGHT_X_THRESHOLD = 30 +TRIGGER_TIMER_THRESHOLD = 10 class E2EStates: @@ -36,6 +37,7 @@ class E2EAlertsHelper: self.lead_depart_alert_enabled = self._params.get_bool("LeadDepartAlert") self.green_light_trigger_count = 0 + self.lead_depart_trigger_count = 0 self.last_lead_distance = -1 self.last_moving_frame = -1 @@ -73,7 +75,7 @@ class E2EAlertsHelper: else: self.green_light_trigger_count = 0 - if self.green_light_trigger_count > 2: + if self.green_light_trigger_count > TRIGGER_TIMER_THRESHOLD: green_light_trigger = True elif self.green_light_state != E2EStates.ARMED: self.green_light_trigger_count = 0 @@ -85,9 +87,15 @@ class E2EAlertsHelper: self.last_lead_distance = lead_dRel if self.last_lead_distance != -1 and (lead_dRel - self.last_lead_distance > 1.0): + self.lead_depart_trigger_count += 1 + else: + self.lead_depart_trigger_count = 0 + + if self.lead_depart_trigger_count > TRIGGER_TIMER_THRESHOLD: lead_depart_trigger = True elif self.lead_depart_state != E2EStates.ARMED: self.last_lead_distance = -1 + self.lead_depart_trigger_count = 0 return green_light_trigger, lead_depart_trigger From 4fb97045403d1dc9a5af3823ab0f91cc5ede056d Mon Sep 17 00:00:00 2001 From: Jason Wen Date: Wed, 15 Oct 2025 22:51:40 -0400 Subject: [PATCH 6/9] time based --- .../controls/lib/e2e_alerts_helper.py | 22 +++++++++---------- 1 file changed, 11 insertions(+), 11 deletions(-) diff --git a/sunnypilot/selfdrive/controls/lib/e2e_alerts_helper.py b/sunnypilot/selfdrive/controls/lib/e2e_alerts_helper.py index 6a3175ec5e..e528815083 100644 --- a/sunnypilot/selfdrive/controls/lib/e2e_alerts_helper.py +++ b/sunnypilot/selfdrive/controls/lib/e2e_alerts_helper.py @@ -13,7 +13,7 @@ from openpilot.sunnypilot import PARAMS_UPDATE_PERIOD from openpilot.sunnypilot.selfdrive.selfdrived.events import EventsSP GREEN_LIGHT_X_THRESHOLD = 30 -TRIGGER_TIMER_THRESHOLD = 10 +TRIGGER_TIMER_THRESHOLD = 0.5 class E2EStates: @@ -36,8 +36,8 @@ class E2EAlertsHelper: self.lead_depart_alert = False self.lead_depart_alert_enabled = self._params.get_bool("LeadDepartAlert") - self.green_light_trigger_count = 0 - self.lead_depart_trigger_count = 0 + self.green_light_trigger_timer = 0 + self.lead_depart_trigger_timer = 0 self.last_lead_distance = -1 self.last_moving_frame = -1 @@ -71,14 +71,14 @@ class E2EAlertsHelper: green_light_trigger = False if self.green_light_state == E2EStates.ARMED: if model_x[max_idx] > GREEN_LIGHT_X_THRESHOLD: - self.green_light_trigger_count += 1 + self.green_light_trigger_timer += 1 else: - self.green_light_trigger_count = 0 + self.green_light_trigger_timer = 0 - if self.green_light_trigger_count > TRIGGER_TIMER_THRESHOLD: + if self.green_light_trigger_timer * DT_MDL > TRIGGER_TIMER_THRESHOLD: green_light_trigger = True elif self.green_light_state != E2EStates.ARMED: - self.green_light_trigger_count = 0 + self.green_light_trigger_timer = 0 # Lead Departure Alert lead_depart_trigger = False @@ -87,15 +87,15 @@ class E2EAlertsHelper: self.last_lead_distance = lead_dRel if self.last_lead_distance != -1 and (lead_dRel - self.last_lead_distance > 1.0): - self.lead_depart_trigger_count += 1 + self.lead_depart_trigger_timer += 1 else: - self.lead_depart_trigger_count = 0 + self.lead_depart_trigger_timer = 0 - if self.lead_depart_trigger_count > TRIGGER_TIMER_THRESHOLD: + if self.lead_depart_trigger_timer * DT_MDL > TRIGGER_TIMER_THRESHOLD: lead_depart_trigger = True elif self.lead_depart_state != E2EStates.ARMED: self.last_lead_distance = -1 - self.lead_depart_trigger_count = 0 + self.lead_depart_trigger_timer = 0 return green_light_trigger, lead_depart_trigger From df35f48f3bb76861b9e013e7cc7e0cdddd9f2826 Mon Sep 17 00:00:00 2001 From: Jason Wen Date: Wed, 15 Oct 2025 22:56:08 -0400 Subject: [PATCH 7/9] magic --- sunnypilot/selfdrive/controls/lib/e2e_alerts_helper.py | 3 ++- 1 file changed, 2 insertions(+), 1 deletion(-) diff --git a/sunnypilot/selfdrive/controls/lib/e2e_alerts_helper.py b/sunnypilot/selfdrive/controls/lib/e2e_alerts_helper.py index e528815083..3c7cb014ab 100644 --- a/sunnypilot/selfdrive/controls/lib/e2e_alerts_helper.py +++ b/sunnypilot/selfdrive/controls/lib/e2e_alerts_helper.py @@ -13,6 +13,7 @@ from openpilot.sunnypilot import PARAMS_UPDATE_PERIOD from openpilot.sunnypilot.selfdrive.selfdrived.events import EventsSP GREEN_LIGHT_X_THRESHOLD = 30 +LEAD_DEPART_DIST_THRESHOLD = 1.0 TRIGGER_TIMER_THRESHOLD = 0.5 @@ -86,7 +87,7 @@ class E2EAlertsHelper: if self.last_lead_distance == -1 or lead_dRel < self.last_lead_distance: self.last_lead_distance = lead_dRel - if self.last_lead_distance != -1 and (lead_dRel - self.last_lead_distance > 1.0): + if self.last_lead_distance != -1 and (lead_dRel - self.last_lead_distance > LEAD_DEPART_DIST_THRESHOLD): self.lead_depart_trigger_timer += 1 else: self.lead_depart_trigger_timer = 0 From 074694d6600b7d7e27b399c0226610520e5f5336 Mon Sep 17 00:00:00 2001 From: Jason Wen Date: Wed, 15 Oct 2025 23:48:13 -0400 Subject: [PATCH 8/9] lead depart: only arm if we have a confirmed close lead for over a second after allowing alert --- .../controls/lib/e2e_alerts_helper.py | 24 ++++++++++++++++++- 1 file changed, 23 insertions(+), 1 deletion(-) diff --git a/sunnypilot/selfdrive/controls/lib/e2e_alerts_helper.py b/sunnypilot/selfdrive/controls/lib/e2e_alerts_helper.py index 3c7cb014ab..f3f38171c3 100644 --- a/sunnypilot/selfdrive/controls/lib/e2e_alerts_helper.py +++ b/sunnypilot/selfdrive/controls/lib/e2e_alerts_helper.py @@ -43,8 +43,13 @@ class E2EAlertsHelper: self.last_moving_frame = -1 self.allowed = False + self.last_allowed = False self.has_lead = False + self.lead_depart_arm_timer = 0 + self.lead_depart_confirmed_lead = False + self.lead_depart_armed = False + def _read_params(self) -> None: if self.frame % int(PARAMS_UPDATE_PERIOD / DT_MDL) == 0: self.green_light_alert_enabled = self._params.get_bool("GreenLightAlert") @@ -82,6 +87,21 @@ class E2EAlertsHelper: self.green_light_trigger_timer = 0 # Lead Departure Alert + close_lead_valid = self.has_lead and lead_dRel < 8.0 + if self.allowed and not self.last_allowed and close_lead_valid: + self.lead_depart_confirmed_lead = True + elif not self.allowed: + self.lead_depart_confirmed_lead = False + + if self.allowed and self.lead_depart_confirmed_lead and close_lead_valid: + self.lead_depart_arm_timer += 1 + + if self.lead_depart_arm_timer * DT_MDL >= 1.0: + self.lead_depart_armed = True + else: + self.lead_depart_arm_timer = 0 + self.lead_depart_armed = False + lead_depart_trigger = False if self.lead_depart_state == E2EStates.ARMED: if self.last_lead_distance == -1 or lead_dRel < self.last_lead_distance: @@ -98,6 +118,8 @@ class E2EAlertsHelper: self.last_lead_distance = -1 self.lead_depart_trigger_timer = 0 + self.last_allowed = self.allowed + return green_light_trigger, lead_depart_trigger @staticmethod @@ -141,7 +163,7 @@ class E2EAlertsHelper: self.lead_depart_state, self.lead_depart_alert = self.update_state_machine( self.lead_depart_state, self.lead_depart_alert_enabled, - self.allowed and self.has_lead, + self.allowed and self.lead_depart_armed, lead_depart_trigger ) From 6030bf4da3a60341e37a0df549a087b680be34fc Mon Sep 17 00:00:00 2001 From: Jason Wen Date: Wed, 15 Oct 2025 23:52:03 -0400 Subject: [PATCH 9/9] less --- sunnypilot/selfdrive/controls/lib/e2e_alerts_helper.py | 3 --- 1 file changed, 3 deletions(-) diff --git a/sunnypilot/selfdrive/controls/lib/e2e_alerts_helper.py b/sunnypilot/selfdrive/controls/lib/e2e_alerts_helper.py index f3f38171c3..1133c65ef2 100644 --- a/sunnypilot/selfdrive/controls/lib/e2e_alerts_helper.py +++ b/sunnypilot/selfdrive/controls/lib/e2e_alerts_helper.py @@ -133,9 +133,6 @@ class E2EAlertsHelper: if triggered: state = E2EStates.CONSUMED - elif not allowed: - state = E2EStates.INACTIVE - elif state == E2EStates.CONSUMED: pass