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 ff7be8a8be..003135cfaa 100644 --- a/sunnypilot/selfdrive/controls/lib/speed_limit/speed_limit_assist.py +++ b/sunnypilot/selfdrive/controls/lib/speed_limit/speed_limit_assist.py @@ -129,8 +129,16 @@ def get_v_target_from_control(self) -> float: if self._has_speed_limit: if self.pcm_op_long and self.is_enabled: return self._speed_limit_final_last + # ICBM vehicles need target in preActive state to adjust cluster speed + # This breaks the circular dependency: SLA preActive -> ICBM adjusts cluster -> SLA becomes active + # Non-PCM vehicles get target when active if not self.pcm_op_long and self.is_active: return self._speed_limit_final_last + # ICBM vehicles also get target in preActive state to break circular dependency + if (not self.pcm_op_long and self.is_enabled and + self.state == SpeedLimitAssistState.preActive and + self.CP_SP.intelligentCruiseButtonManagementAvailable): + return self._speed_limit_final_last # Fallback return V_CRUISE_UNSET 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 aa6650adda..fc82bd6df9 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 @@ -276,3 +276,54 @@ def test_maintain_states_with_no_changes(self): assert self.sla.state in [SpeedLimitAssistState.preActive, SpeedLimitAssistState.active] elif initial_state in ACTIVE_STATES: assert self.sla.state in ACTIVE_STATES + def test_icbm_preactive_target_provision(self): + """Test that ICBM vehicles get target in preActive state to break circular dependency""" + # Set up ICBM vehicle (non-PCM) + self.sla.pcm_op_long = False + self.sla.CP_SP.intelligentCruiseButtonManagementAvailable = True + + # Set up preActive state with speed limit + self.sla.state = SpeedLimitAssistState.preActive + self.sla.is_enabled = True + self.sla.is_active = False # Not active yet - this is the key condition + self.sla._has_speed_limit = True + self.sla._speed_limit_final_last = SPEED_LIMITS['city'] # 35 mph + + # Test that ICBM gets target in preActive state + v_target = self.sla.get_v_target_from_control() + assert v_target == SPEED_LIMITS['city'], f"Expected {SPEED_LIMITS['city']}, got {v_target}" + assert v_target != V_CRUISE_UNSET, "ICBM should get target in preActive state" + + def test_icbm_preactive_requires_icbm_availability(self): + """Test that non-ICBM vehicles don't get target in preActive state""" + # Set up non-ICBM vehicle (non-PCM) + self.sla.pcm_op_long = False + self.sla.CP_SP.intelligentCruiseButtonManagementAvailable = False + + # Set up preActive state with speed limit + self.sla.state = SpeedLimitAssistState.preActive + self.sla.is_enabled = True + self.sla.is_active = False + self.sla._has_speed_limit = True + self.sla._speed_limit_final_last = SPEED_LIMITS['city'] + + # Non-ICBM vehicles should not get target in preActive state + v_target = self.sla.get_v_target_from_control() + assert v_target == V_CRUISE_UNSET, "Non-ICBM vehicles should not get target in preActive state" + + def test_icbm_preactive_requires_enabled_state(self): + """Test that disabled ICBM vehicles don't get target in preActive state""" + # Set up ICBM vehicle (non-PCM) but disabled + self.sla.pcm_op_long = False + self.sla.CP_SP.intelligentCruiseButtonManagementAvailable = True + + # Set up preActive state with speed limit but disabled + self.sla.state = SpeedLimitAssistState.preActive + self.sla.is_enabled = False + self.sla.is_active = False + self.sla._has_speed_limit = True + self.sla._speed_limit_final_last = SPEED_LIMITS['city'] + + # Disabled ICBM vehicles should not get target + v_target = self.sla.get_v_target_from_control() + assert v_target == V_CRUISE_UNSET, "Disabled ICBM should not get target" \ No newline at end of file