From 1e7fc15a040dca7f3a3e7143edb6333086d224ff Mon Sep 17 00:00:00 2001 From: Jason Wen Date: Fri, 3 Oct 2025 18:24:24 -0400 Subject: [PATCH] Speed Limit Assist: Update state machine for PCM long cars (#1311) * Speed Limit Assist: Update state machine for PCM long cars * new test to catch this wrong transition * tldr --- .../lib/speed_limit/speed_limit_assist.py | 14 ++++++++------ .../tests/test_speed_limit_assist.py | 18 ++++++++++++++++-- 2 files changed, 24 insertions(+), 8 deletions(-) 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 5d798949a9..302d8cd14d 100644 --- a/sunnypilot/selfdrive/controls/lib/speed_limit/speed_limit_assist.py +++ b/sunnypilot/selfdrive/controls/lib/speed_limit/speed_limit_assist.py @@ -247,11 +247,11 @@ class SpeedLimitAssist: # PENDING elif self.state == SpeedLimitAssistState.pending: - if self._has_speed_limit: - if self.v_offset < LIMIT_SPEED_OFFSET_TH: - self.state = SpeedLimitAssistState.adapting - else: - self.state = SpeedLimitAssistState.active + if self.target_set_speed_confirmed: + self._update_confirmed_state() + elif self.speed_limit_changed: + self.state = SpeedLimitAssistState.preActive + self.pre_active_timer = int(PRE_ACTIVE_GUARD_PERIOD / DT_MDL) # PRE_ACTIVE elif self.state == SpeedLimitAssistState.preActive: @@ -275,9 +275,11 @@ class SpeedLimitAssist: elif self.long_engaged_timer <= 0: if self.target_set_speed_confirmed: self._update_confirmed_state() - else: + elif self._has_speed_limit: self.state = SpeedLimitAssistState.preActive self.pre_active_timer = int(PRE_ACTIVE_GUARD_PERIOD / DT_MDL) + else: + self.state = SpeedLimitAssistState.pending enabled = self.state in ENABLED_STATES active = self.state in ACTIVE_STATES diff --git a/sunnypilot/selfdrive/controls/lib/speed_limit/tests/test_speed_limit_assist.py b/sunnypilot/selfdrive/controls/lib/speed_limit/tests/test_speed_limit_assist.py index 3b2716265d..d2c7a4716d 100644 --- a/sunnypilot/selfdrive/controls/lib/speed_limit/tests/test_speed_limit_assist.py +++ b/sunnypilot/selfdrive/controls/lib/speed_limit/tests/test_speed_limit_assist.py @@ -96,6 +96,12 @@ class TestSpeedLimitAssist: assert self.sla.state == SpeedLimitAssistState.preActive assert self.sla.is_enabled and not self.sla.is_active + def test_transition_disabled_to_pending_no_speed_limit_not_max_initial_set_speed(self): + for _ in range(int(3. / DT_MDL)): + self.sla.update(True, False, SPEED_LIMITS['highway'], 0, SPEED_LIMITS['city'], 0, 0, False, 0, self.events_sp) + assert self.sla.state == SpeedLimitAssistState.pending + assert self.sla.is_enabled and not self.sla.is_active + def test_preactive_to_active_with_max_speed_confirmation(self): self.sla.state = SpeedLimitAssistState.preActive self.sla.update(True, False, SPEED_LIMITS['city'], 0, self.pcm_long_max_set_speed, SPEED_LIMITS['highway'], @@ -120,12 +126,20 @@ class TestSpeedLimitAssist: def test_pending_to_active_when_speed_limit_available(self): self.sla.state = SpeedLimitAssistState.pending - self.sla.update(True, False, SPEED_LIMITS['city'], 0, self.pcm_long_max_set_speed, SPEED_LIMITS['city'], SPEED_LIMITS['city'], True, 0, self.events_sp) + self.sla.v_cruise_cluster_prev = self.pcm_long_max_set_speed + self.sla.prev_v_cruise_cluster_conv = round(self.pcm_long_max_set_speed * self.speed_conv) + + self.sla.update(True, False, SPEED_LIMITS['highway'], 0, self.pcm_long_max_set_speed, + SPEED_LIMITS['highway'], SPEED_LIMITS['highway'], True, 0, self.events_sp) assert self.sla.state == SpeedLimitAssistState.active def test_pending_to_adapting_when_below_speed_limit(self): self.sla.state = SpeedLimitAssistState.pending - self.sla.update(True, False, SPEED_LIMITS['city'] + 5, 0, self.pcm_long_max_set_speed, SPEED_LIMITS['city'], SPEED_LIMITS['city'], True, 0, self.events_sp) + self.sla.v_cruise_cluster_prev = self.pcm_long_max_set_speed + self.sla.prev_v_cruise_cluster_conv = round(self.pcm_long_max_set_speed * self.speed_conv) + + self.sla.update(True, False, SPEED_LIMITS['highway'] + 5, 0, self.pcm_long_max_set_speed, + SPEED_LIMITS['highway'], SPEED_LIMITS['highway'], True, 0, self.events_sp) assert self.sla.state == SpeedLimitAssistState.adapting assert self.sla.is_enabled and self.sla.is_active