From 7da44126700a468dcbdb5190f67fe290073c49e3 Mon Sep 17 00:00:00 2001 From: Jason Wen Date: Tue, 23 Sep 2025 17:41:54 -0400 Subject: [PATCH] use consolidate method to evaluate set speed status --- .../controls/lib/speed_limit/__init__.py | 8 ++++--- .../lib/speed_limit/speed_limit_assist.py | 24 ++++++++++++++----- 2 files changed, 23 insertions(+), 9 deletions(-) diff --git a/sunnypilot/selfdrive/controls/lib/speed_limit/__init__.py b/sunnypilot/selfdrive/controls/lib/speed_limit/__init__.py index 696d9c6cc6..c1429b0774 100644 --- a/sunnypilot/selfdrive/controls/lib/speed_limit/__init__.py +++ b/sunnypilot/selfdrive/controls/lib/speed_limit/__init__.py @@ -7,6 +7,8 @@ See the LICENSE.md file in the root directory for more details. LIMIT_ADAPT_ACC = -1. # m/s^2 Ideal acceleration for the adapting (braking) phase when approaching speed limits. LIMIT_MAX_MAP_DATA_AGE = 10. # s Maximum time to hold to map data, then consider it invalid inside limits controllers. -# Speed Limit Assist Auto mode constants -REQUIRED_INITIAL_MAX_SET_SPEED = 35.7632 # m/s 80 MPH # TODO-SP: customizable with params -CRUISE_SPEED_TOLERANCE = 0.44704 # m/s ±1 MPH tolerance # TODO-SP: metric vs imperial +# Speed Limit Assist constants +PCM_LONG_REQUIRED_MAX_SET_SPEED = { + True: 130, # km/h + False: 80, # mph +} diff --git a/sunnypilot/selfdrive/controls/lib/speed_limit/speed_limit_assist.py b/sunnypilot/selfdrive/controls/lib/speed_limit/speed_limit_assist.py index 5faa86a0f0..0dd3fb89ca 100644 --- a/sunnypilot/selfdrive/controls/lib/speed_limit/speed_limit_assist.py +++ b/sunnypilot/selfdrive/controls/lib/speed_limit/speed_limit_assist.py @@ -116,8 +116,15 @@ class SpeedLimitAssist: self.is_metric = self.params.get_bool("IsMetric") self.enabled = self.params.get("SpeedLimitMode", return_default=True) == Mode.assist - def initial_max_set_confirmed(self) -> bool: - return bool(abs(self.v_cruise_cluster - REQUIRED_INITIAL_MAX_SET_SPEED) <= CRUISE_SPEED_TOLERANCE) + @property + def target_set_speed_confirmed(self) -> bool: + speed_conv = CV.MS_TO_KPH if self.is_metric else CV.MS_TO_MPH + speed_limit_final_conv = round(self.speed_limit_final * speed_conv) + v_cruise_cluster_conv = round(self.v_cruise_cluster * speed_conv) + + target_set_speed_conv = PCM_LONG_REQUIRED_MAX_SET_SPEED[self.is_metric] if self.pcm_op_long else speed_limit_final_conv + + return v_cruise_cluster_conv == target_set_speed_conv def update_calculations(self, v_cruise_cluster: float) -> None: self.v_cruise_cluster = v_cruise_cluster if not np.isnan(v_cruise_cluster) else 0.0 @@ -146,7 +153,7 @@ class SpeedLimitAssist: else: self.state = SpeedLimitAssistState.pending - def update_state_machine(self): + def update_state_machine_pcm_op_long(self): self._state_prev = self.state self.long_engaged_timer = max(0, self.long_engaged_timer - 1) @@ -182,7 +189,7 @@ class SpeedLimitAssist: # PRE_ACTIVE elif self.state == SpeedLimitAssistState.preActive: - if self.initial_max_set_confirmed(): + if self.target_set_speed_confirmed: self._update_pcm_long_confirmed_state() elif self.pre_active_timer <= PRE_ACTIVE_GUARD_PERIOD: # Timeout - session ended @@ -200,7 +207,7 @@ class SpeedLimitAssist: self.long_engaged_timer = int(DISABLED_GUARD_PERIOD / DT_MDL) elif self.long_engaged_timer <= 0: - if self.initial_max_set_confirmed(): + if self.target_set_speed_confirmed: self._update_pcm_long_confirmed_state() else: self.state = SpeedLimitAssistState.preActive @@ -241,7 +248,12 @@ class SpeedLimitAssist: self.update_params() self.update_calculations(v_cruise_cluster) - self.is_enabled, self.is_active = self.update_state_machine() + + if self.pcm_op_long: + self.is_enabled, self.is_active = self.update_state_machine_pcm_op_long() + else: + pass + self.update_events(events_sp) # Update change tracking variables