From 38ead65707034558bfd439c16d80798d881cd528 Mon Sep 17 00:00:00 2001 From: fl0yd Date: Mon, 15 Dec 2025 20:36:51 -0600 Subject: [PATCH 1/4] Fix ICBM/SLA circular dependency for Ford vehicles - Allow SLA to provide vTarget to ICBM when in preActive state - Breaks circular dependency: SLA preActive -> ICBM adjusts cluster -> SLA becomes active - Only affects non-PCM vehicles (ICBM-enabled brands like Ford, Mazda, etc.) - Resolves issue where SLA would timeout in preActive state waiting for cluster adjustment Fixes: Speed Limit Assist not working with Ford ICBM implementation --- .../selfdrive/controls/lib/speed_limit/speed_limit_assist.py | 5 ++++- 1 file changed, 4 insertions(+), 1 deletion(-) 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..49ab3660a2 100644 --- a/sunnypilot/selfdrive/controls/lib/speed_limit/speed_limit_assist.py +++ b/sunnypilot/selfdrive/controls/lib/speed_limit/speed_limit_assist.py @@ -129,7 +129,10 @@ 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 - if not self.pcm_op_long and self.is_active: + # ICBM vehicles need target in preActive state to adjust cluster speed + # This breaks the circular dependency: SLA preActive -> ICBM adjusts cluster -> SLA becomes active + if not self.pcm_op_long and (self.is_active or + (self.is_enabled and self.state == SpeedLimitAssistState.preActive)): return self._speed_limit_final_last # Fallback From ab9b4cdba1205274af8d82523440492ea05d8228 Mon Sep 17 00:00:00 2001 From: fl0yd Date: Mon, 15 Dec 2025 20:42:10 -0600 Subject: [PATCH 2/4] Add test for ICBM/SLA circular dependency fix - Test verifies ICBM vehicles get vTarget in preActive state - Ensures PCM vehicle behavior remains unchanged - Validates edge cases (no speed limit, disabled state) - Confirms fix resolves Ford ICBM + Speed Limit Assist integration --- .../tests/test_speed_limit_assist.py | 35 ++++++ test_sla_icbm_fix.py | 112 ++++++++++++++++++ 2 files changed, 147 insertions(+) create mode 100644 test_sla_icbm_fix.py 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..4c37595085 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,38 @@ 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 + + # 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() + + # Should return the speed limit, not V_CRUISE_UNSET + 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" + + # Verify PCM vehicles still work as before (enabled but not active in preActive) + self.sla.pcm_op_long = True + v_target_pcm = self.sla.get_v_target_from_control() + assert v_target_pcm == SPEED_LIMITS['city'], "PCM vehicles should still get target when enabled" + + # Verify ICBM vehicles don't get target when not enabled + self.sla.pcm_op_long = False + self.sla.is_enabled = False + v_target_disabled = self.sla.get_v_target_from_control() + assert v_target_disabled == V_CRUISE_UNSET, "Disabled ICBM should not get target" + + # Verify ICBM vehicles don't get target without speed limit + self.sla.is_enabled = True + self.sla._has_speed_limit = False + v_target_no_limit = self.sla.get_v_target_from_control() + assert v_target_no_limit == V_CRUISE_UNSET, "ICBM without speed limit should not get target" \ No newline at end of file diff --git a/test_sla_icbm_fix.py b/test_sla_icbm_fix.py new file mode 100644 index 0000000000..8173877dea --- /dev/null +++ b/test_sla_icbm_fix.py @@ -0,0 +1,112 @@ +#!/usr/bin/env python3 +""" +Quick test to verify the ICBM/SLA circular dependency fix +""" + +import sys + +# Add the sunnypilot path to import the module +sys.path.append('sunnypilot') +sys.path.append('openpilot') + +from cereal import custom, car +from openpilot.sunnypilot.selfdrive.controls.lib.speed_limit.speed_limit_assist import SpeedLimitAssist + +def test_icbm_sla_fix(): + """Test that SLA provides target to ICBM vehicles in preActive state""" + + # Create mock CarParams for ICBM vehicle (non-PCM) + CP = car.CarParams.new_message() + CP.openpilotLongitudinalControl = False # ICBM vehicle + CP.pcmCruise = False + + CP_SP = custom.CarParamsSP.new_message() + + # Create SLA instance + sla = SpeedLimitAssist(CP, CP_SP) + + # Set up test scenario: ICBM vehicle with speed limit in preActive state + sla.long_enabled = True + sla.enabled = True + sla.is_enabled = True + sla.is_active = False # Not active yet + sla.state = custom.LongitudinalPlanSP.SpeedLimit.AssistState.preActive + sla._has_speed_limit = True + sla._speed_limit_final_last = 35.0 # 35 m/s speed limit + + # Test the fix + v_target = sla.get_v_target_from_control() + + print("Test Results:") + print(f" PCM Op Long: {sla.pcm_op_long}") + print(f" Is Enabled: {sla.is_enabled}") + print(f" Is Active: {sla.is_active}") + print(f" State: {sla.state}") + print(f" Has Speed Limit: {sla._has_speed_limit}") + print(f" Speed Limit Final Last: {sla._speed_limit_final_last}") + print(f" V Target Returned: {v_target}") + + # Verify the fix works + if v_target == 35.0: + print("āœ… SUCCESS: SLA provides target to ICBM in preActive state") + return True + else: + print(f"āŒ FAILED: Expected 35.0, got {v_target}") + return False + +def test_pcm_vehicle_unchanged(): + """Test that PCM vehicles (non-ICBM) behavior is unchanged""" + + # Create mock CarParams for PCM vehicle + CP = car.CarParams.new_message() + CP.openpilotLongitudinalControl = True # PCM vehicle + CP.pcmCruise = True + + CP_SP = custom.CarParamsSP.new_message() + + # Create SLA instance + sla = SpeedLimitAssist(CP, CP_SP) + + # Set up test scenario: PCM vehicle with speed limit in preActive state + sla.long_enabled = True + sla.enabled = True + sla.is_enabled = True + sla.is_active = False # Not active yet + sla.state = custom.LongitudinalPlanSP.SpeedLimit.AssistState.preActive + sla._has_speed_limit = True + sla._speed_limit_final_last = 35.0 + + # Test that PCM behavior is unchanged + v_target = sla.get_v_target_from_control() + + print("\nPCM Vehicle Test:") + print(f" PCM Op Long: {sla.pcm_op_long}") + print(f" Is Enabled: {sla.is_enabled}") + print(f" Is Active: {sla.is_active}") + print(f" State: {sla.state}") + print(f" V Target Returned: {v_target}") + + # PCM vehicles should still get target when enabled (even in preActive) + if v_target == 35.0: + print("āœ… SUCCESS: PCM vehicle behavior unchanged") + return True + else: + print("āŒ FAILED: PCM vehicle should get target when enabled") + return False + +if __name__ == "__main__": + print("Testing ICBM/SLA Circular Dependency Fix\n") + + try: + test1_pass = test_icbm_sla_fix() + test2_pass = test_pcm_vehicle_unchanged() + + if test1_pass and test2_pass: + print("\nšŸŽ‰ All tests passed! The fix should work.") + else: + print("\nāŒ Some tests failed. Check the implementation.") + + except Exception as e: + print(f"āŒ Test failed with error: {e}") + print("This might be due to missing dependencies or import issues.") + print("Try running the integration test instead.") \ No newline at end of file From b437e04c3167da249c4341868d50dbef0c7a2c3a Mon Sep 17 00:00:00 2001 From: fl0yd Date: Mon, 15 Dec 2025 20:42:29 -0600 Subject: [PATCH 3/4] Clean up temporary test files --- test_sla_icbm_fix.py | 112 ------------------------------------------- 1 file changed, 112 deletions(-) delete mode 100644 test_sla_icbm_fix.py diff --git a/test_sla_icbm_fix.py b/test_sla_icbm_fix.py deleted file mode 100644 index 8173877dea..0000000000 --- a/test_sla_icbm_fix.py +++ /dev/null @@ -1,112 +0,0 @@ -#!/usr/bin/env python3 -""" -Quick test to verify the ICBM/SLA circular dependency fix -""" - -import sys - -# Add the sunnypilot path to import the module -sys.path.append('sunnypilot') -sys.path.append('openpilot') - -from cereal import custom, car -from openpilot.sunnypilot.selfdrive.controls.lib.speed_limit.speed_limit_assist import SpeedLimitAssist - -def test_icbm_sla_fix(): - """Test that SLA provides target to ICBM vehicles in preActive state""" - - # Create mock CarParams for ICBM vehicle (non-PCM) - CP = car.CarParams.new_message() - CP.openpilotLongitudinalControl = False # ICBM vehicle - CP.pcmCruise = False - - CP_SP = custom.CarParamsSP.new_message() - - # Create SLA instance - sla = SpeedLimitAssist(CP, CP_SP) - - # Set up test scenario: ICBM vehicle with speed limit in preActive state - sla.long_enabled = True - sla.enabled = True - sla.is_enabled = True - sla.is_active = False # Not active yet - sla.state = custom.LongitudinalPlanSP.SpeedLimit.AssistState.preActive - sla._has_speed_limit = True - sla._speed_limit_final_last = 35.0 # 35 m/s speed limit - - # Test the fix - v_target = sla.get_v_target_from_control() - - print("Test Results:") - print(f" PCM Op Long: {sla.pcm_op_long}") - print(f" Is Enabled: {sla.is_enabled}") - print(f" Is Active: {sla.is_active}") - print(f" State: {sla.state}") - print(f" Has Speed Limit: {sla._has_speed_limit}") - print(f" Speed Limit Final Last: {sla._speed_limit_final_last}") - print(f" V Target Returned: {v_target}") - - # Verify the fix works - if v_target == 35.0: - print("āœ… SUCCESS: SLA provides target to ICBM in preActive state") - return True - else: - print(f"āŒ FAILED: Expected 35.0, got {v_target}") - return False - -def test_pcm_vehicle_unchanged(): - """Test that PCM vehicles (non-ICBM) behavior is unchanged""" - - # Create mock CarParams for PCM vehicle - CP = car.CarParams.new_message() - CP.openpilotLongitudinalControl = True # PCM vehicle - CP.pcmCruise = True - - CP_SP = custom.CarParamsSP.new_message() - - # Create SLA instance - sla = SpeedLimitAssist(CP, CP_SP) - - # Set up test scenario: PCM vehicle with speed limit in preActive state - sla.long_enabled = True - sla.enabled = True - sla.is_enabled = True - sla.is_active = False # Not active yet - sla.state = custom.LongitudinalPlanSP.SpeedLimit.AssistState.preActive - sla._has_speed_limit = True - sla._speed_limit_final_last = 35.0 - - # Test that PCM behavior is unchanged - v_target = sla.get_v_target_from_control() - - print("\nPCM Vehicle Test:") - print(f" PCM Op Long: {sla.pcm_op_long}") - print(f" Is Enabled: {sla.is_enabled}") - print(f" Is Active: {sla.is_active}") - print(f" State: {sla.state}") - print(f" V Target Returned: {v_target}") - - # PCM vehicles should still get target when enabled (even in preActive) - if v_target == 35.0: - print("āœ… SUCCESS: PCM vehicle behavior unchanged") - return True - else: - print("āŒ FAILED: PCM vehicle should get target when enabled") - return False - -if __name__ == "__main__": - print("Testing ICBM/SLA Circular Dependency Fix\n") - - try: - test1_pass = test_icbm_sla_fix() - test2_pass = test_pcm_vehicle_unchanged() - - if test1_pass and test2_pass: - print("\nšŸŽ‰ All tests passed! The fix should work.") - else: - print("\nāŒ Some tests failed. Check the implementation.") - - except Exception as e: - print(f"āŒ Test failed with error: {e}") - print("This might be due to missing dependencies or import issues.") - print("Try running the integration test instead.") \ No newline at end of file From 232419c098d368de15e05417f4dddb78cbee93d9 Mon Sep 17 00:00:00 2001 From: fl0yd Date: Tue, 16 Dec 2025 14:16:36 -0600 Subject: [PATCH 4/4] Fix ICBM/SLA circular dependency for Ford vehicles - Allow ICBM vehicles to get speed targets in preActive state - Breaks circular dependency: SLA preActive -> ICBM adjusts cluster -> SLA active - Add comprehensive test coverage for ICBM-specific behavior - Maintains backward compatibility for non-ICBM vehicles This specifically addresses the Ford F-150 ICBM issue where Speed Limit Assist cannot activate due to a circular dependency. ICBM needs the speed target to adjust cluster speed, but SLA won't provide targets until active. This fix allows ICBM vehicles to get targets in preActive state, breaking the cycle. Fixes: Ford F-150 ICBM unable to work with Speed Limit Assist Addresses: https://community.sunnypilot.ai/t/ford-icbm-wip-discussion/1392 --- .../lib/speed_limit/speed_limit_assist.py | 9 +++- .../tests/test_speed_limit_assist.py | 44 +++++++++++++------ 2 files changed, 37 insertions(+), 16 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 49ab3660a2..003135cfaa 100644 --- a/sunnypilot/selfdrive/controls/lib/speed_limit/speed_limit_assist.py +++ b/sunnypilot/selfdrive/controls/lib/speed_limit/speed_limit_assist.py @@ -131,8 +131,13 @@ def get_v_target_from_control(self) -> float: 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 - if not self.pcm_op_long and (self.is_active or - (self.is_enabled and self.state == SpeedLimitAssistState.preActive)): + # 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 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 4c37595085..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 @@ -280,6 +280,7 @@ 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 @@ -290,24 +291,39 @@ def test_icbm_preactive_target_provision(self): # Test that ICBM gets target in preActive state v_target = self.sla.get_v_target_from_control() - - # Should return the speed limit, not V_CRUISE_UNSET 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" - # Verify PCM vehicles still work as before (enabled but not active in preActive) - self.sla.pcm_op_long = True - v_target_pcm = self.sla.get_v_target_from_control() - assert v_target_pcm == SPEED_LIMITS['city'], "PCM vehicles should still get target when enabled" + 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" - # Verify ICBM vehicles don't get target when not enabled + 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 - v_target_disabled = self.sla.get_v_target_from_control() - assert v_target_disabled == V_CRUISE_UNSET, "Disabled ICBM should not get target" + self.sla.is_active = False + self.sla._has_speed_limit = True + self.sla._speed_limit_final_last = SPEED_LIMITS['city'] - # Verify ICBM vehicles don't get target without speed limit - self.sla.is_enabled = True - self.sla._has_speed_limit = False - v_target_no_limit = self.sla.get_v_target_from_control() - assert v_target_no_limit == V_CRUISE_UNSET, "ICBM without speed limit should not get target" \ No newline at end of file + # 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