diff --git a/sunnypilot/selfdrive/controls/lib/speed_limit/__init__.py b/sunnypilot/selfdrive/controls/lib/speed_limit/__init__.py index c1429b0774..d7f68eb587 100644 --- a/sunnypilot/selfdrive/controls/lib/speed_limit/__init__.py +++ b/sunnypilot/selfdrive/controls/lib/speed_limit/__init__.py @@ -9,6 +9,6 @@ LIMIT_MAX_MAP_DATA_AGE = 10. # s Maximum time to hold to map data, then conside # Speed Limit Assist constants PCM_LONG_REQUIRED_MAX_SET_SPEED = { - True: 130, # km/h - False: 80, # mph + True: 36.1111, # 130 km/h + False: 35.7632, # 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 004aa2a789..9f47773fda 100644 --- a/sunnypilot/selfdrive/controls/lib/speed_limit/speed_limit_assist.py +++ b/sunnypilot/selfdrive/controls/lib/speed_limit/speed_limit_assist.py @@ -129,14 +129,15 @@ class SpeedLimitAssist: def update_calculations(self, v_cruise_cluster: float) -> None: speed_conv = CV.MS_TO_KPH if self.is_metric else CV.MS_TO_MPH - self.v_cruise_cluster = v_cruise_cluster if not np.isnan(v_cruise_cluster) else 0.0 + self.v_cruise_cluster = v_cruise_cluster # Update current velocity offset (error) self.v_offset = self.speed_limit_final - self.v_ego self.speed_limit_final_conv = round(self.speed_limit_final * speed_conv) self.v_cruise_cluster_conv = round(self.v_cruise_cluster * speed_conv) - self.target_set_speed_conv = PCM_LONG_REQUIRED_MAX_SET_SPEED[self.is_metric] if self.pcm_op_long else self.speed_limit_final_conv + pcm_long_required_max_set_speed_conv = round(PCM_LONG_REQUIRED_MAX_SET_SPEED[self.is_metric] * speed_conv) + self.target_set_speed_conv = pcm_long_required_max_set_speed_conv if self.pcm_op_long else self.speed_limit_final_conv def get_current_acceleration_as_target(self) -> float: return self.a_ego 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 ff468f02c4..770f8e6eaa 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 @@ -14,7 +14,7 @@ from openpilot.common.realtime import DT_MDL from openpilot.selfdrive.car.cruise import V_CRUISE_UNSET from openpilot.sunnypilot.selfdrive.car import interfaces as sunnypilot_interfaces from openpilot.sunnypilot.selfdrive.controls.lib.speed_limit.common import Mode -from openpilot.sunnypilot.selfdrive.controls.lib.speed_limit import REQUIRED_INITIAL_MAX_SET_SPEED +from openpilot.sunnypilot.selfdrive.controls.lib.speed_limit import PCM_LONG_REQUIRED_MAX_SET_SPEED from openpilot.sunnypilot.selfdrive.controls.lib.speed_limit.speed_limit_assist import SpeedLimitAssist, \ PRE_ACTIVE_GUARD_PERIOD, ACTIVE_STATES from openpilot.sunnypilot.selfdrive.selfdrived.events import EventsSP @@ -37,7 +37,7 @@ class TestSpeedLimitAssist: self.params = Params() self.reset_custom_params() self.events_sp = EventsSP() - CI = self._setup_platform(TOYOTA.TOYOTA_RAV4_TSS2_2022) + CI = self._setup_platform(TOYOTA.TOYOTA_RAV4_TSS2) self.sla = SpeedLimitAssist(CI.CP) self.sla.pre_active_timer = int(PRE_ACTIVE_GUARD_PERIOD / DT_MDL) @@ -95,7 +95,7 @@ class TestSpeedLimitAssist: def test_preactive_to_active_with_max_speed_confirmation(self): self.sla.state = SpeedLimitAssistState.preActive - self.sla.update(True, False, SPEED_LIMITS['city'], 0, REQUIRED_INITIAL_MAX_SET_SPEED, SPEED_LIMITS['city'], 0, 0, self.events_sp) + self.sla.update(True, False, SPEED_LIMITS['city'], 0, PCM_LONG_REQUIRED_MAX_SET_SPEED[self.sla.is_metric], SPEED_LIMITS['city'], 0, 0, self.events_sp) assert self.sla.state == SpeedLimitAssistState.active assert self.sla.is_enabled and self.sla.is_active, f"enabled: {self.sla.is_enabled}, active: {self.sla.is_active}" assert self.sla.output_v_target == SPEED_LIMITS['city'] @@ -110,32 +110,32 @@ class TestSpeedLimitAssist: def test_preactive_to_pending_no_speed_limit(self): self.sla.state = SpeedLimitAssistState.preActive - self.sla.update(True, False, SPEED_LIMITS['city'], 0, REQUIRED_INITIAL_MAX_SET_SPEED, 0, 0, 0, self.events_sp) + self.sla.update(True, False, SPEED_LIMITS['city'], 0, PCM_LONG_REQUIRED_MAX_SET_SPEED[self.sla.is_metric], 0, 0, 0, self.events_sp) assert self.sla.state == SpeedLimitAssistState.pending assert self.sla.is_enabled and not self.sla.is_active def test_pending_to_active_when_speed_limit_available(self): self.sla.state = SpeedLimitAssistState.pending - self.sla.update(True, False, SPEED_LIMITS['city'], 0, REQUIRED_INITIAL_MAX_SET_SPEED, SPEED_LIMITS['city'], 0, 0, self.events_sp) + self.sla.update(True, False, SPEED_LIMITS['city'], 0, PCM_LONG_REQUIRED_MAX_SET_SPEED[self.sla.is_metric], SPEED_LIMITS['city'], 0, 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, REQUIRED_INITIAL_MAX_SET_SPEED, SPEED_LIMITS['city'], 0, 0, self.events_sp) + self.sla.update(True, False, SPEED_LIMITS['city'] + 5, 0, PCM_LONG_REQUIRED_MAX_SET_SPEED[self.sla.is_metric], SPEED_LIMITS['city'], 0, 0, self.events_sp) assert self.sla.state == SpeedLimitAssistState.adapting assert self.sla.is_enabled and self.sla.is_active def test_active_to_adapting_transition(self): - self.initialize_active_state(REQUIRED_INITIAL_MAX_SET_SPEED) + self.initialize_active_state(PCM_LONG_REQUIRED_MAX_SET_SPEED[self.sla.is_metric]) - self.sla.update(True, False, SPEED_LIMITS['city'] + 2, 0, REQUIRED_INITIAL_MAX_SET_SPEED, SPEED_LIMITS['city'], 0, 0, self.events_sp) + self.sla.update(True, False, SPEED_LIMITS['city'] + 2, 0, PCM_LONG_REQUIRED_MAX_SET_SPEED[self.sla.is_metric], SPEED_LIMITS['city'], 0, 0, self.events_sp) assert self.sla.state == SpeedLimitAssistState.adapting def test_adapting_to_active_transition(self): self.sla.state = SpeedLimitAssistState.adapting - self.sla.v_cruise_cluster_prev = REQUIRED_INITIAL_MAX_SET_SPEED + self.sla.v_cruise_cluster_prev = PCM_LONG_REQUIRED_MAX_SET_SPEED[self.sla.is_metric] - self.sla.update(True, False, SPEED_LIMITS['city'], 0, REQUIRED_INITIAL_MAX_SET_SPEED, SPEED_LIMITS['city'], 0, 0, self.events_sp) + self.sla.update(True, False, SPEED_LIMITS['city'], 0, PCM_LONG_REQUIRED_MAX_SET_SPEED[self.sla.is_metric], SPEED_LIMITS['city'], 0, 0, self.events_sp) assert self.sla.state == SpeedLimitAssistState.active def test_manual_cruise_change_detection(self): @@ -148,49 +148,49 @@ class TestSpeedLimitAssist: assert self.sla.state == SpeedLimitAssistState.inactive def test_rapid_speed_limit_changes(self): - self.initialize_active_state(REQUIRED_INITIAL_MAX_SET_SPEED) + self.initialize_active_state(PCM_LONG_REQUIRED_MAX_SET_SPEED[self.sla.is_metric]) speed_limits = [SPEED_LIMITS['city'], SPEED_LIMITS['highway'], SPEED_LIMITS['residential']] for _, speed_limit in enumerate(speed_limits): - self.sla.update(True, False, speed_limit, 0, REQUIRED_INITIAL_MAX_SET_SPEED, speed_limit, 0, 0, self.events_sp) + self.sla.update(True, False, speed_limit, 0, PCM_LONG_REQUIRED_MAX_SET_SPEED[self.sla.is_metric], speed_limit, 0, 0, self.events_sp) assert self.sla.state in ACTIVE_STATES def test_invalid_speed_limits_handling(self): - self.initialize_active_state(REQUIRED_INITIAL_MAX_SET_SPEED) + self.initialize_active_state(PCM_LONG_REQUIRED_MAX_SET_SPEED[self.sla.is_metric]) self.sla.last_valid_speed_limit_final = SPEED_LIMITS['city'] invalid_limits = [-10, 0, 200 * CV.MPH_TO_MS] for invalid_limit in invalid_limits: - self.sla.update(True, False, SPEED_LIMITS['city'], 0, REQUIRED_INITIAL_MAX_SET_SPEED, invalid_limit, 0, 0, self.events_sp) + self.sla.update(True, False, SPEED_LIMITS['city'], 0, PCM_LONG_REQUIRED_MAX_SET_SPEED[self.sla.is_metric], invalid_limit, 0, 0, self.events_sp) assert isinstance(self.sla.output_v_target, (int, float)) assert self.sla.output_v_target == V_CRUISE_UNSET or self.sla.output_v_target > 0 def test_stale_data_handling(self): - self.initialize_active_state(REQUIRED_INITIAL_MAX_SET_SPEED) + self.initialize_active_state(PCM_LONG_REQUIRED_MAX_SET_SPEED[self.sla.is_metric]) old_speed_limit = SPEED_LIMITS['city'] self.sla.last_valid_speed_limit_final = old_speed_limit - self.sla.update(True, False, SPEED_LIMITS['city'], 0, REQUIRED_INITIAL_MAX_SET_SPEED, 0, 0, 0, self.events_sp) + self.sla.update(True, False, SPEED_LIMITS['city'], 0, PCM_LONG_REQUIRED_MAX_SET_SPEED[self.sla.is_metric], 0, 0, 0, self.events_sp) assert self.sla.state in ACTIVE_STATES assert self.sla.output_v_target == old_speed_limit def test_distance_based_adapting(self): self.sla.state = SpeedLimitAssistState.adapting - self.sla.v_cruise_cluster_prev = REQUIRED_INITIAL_MAX_SET_SPEED + self.sla.v_cruise_cluster_prev = PCM_LONG_REQUIRED_MAX_SET_SPEED[self.sla.is_metric] distance = 100.0 current_speed = SPEED_LIMITS['highway'] target_speed = SPEED_LIMITS['city'] - self.sla.update(True, False, current_speed, 0, REQUIRED_INITIAL_MAX_SET_SPEED, target_speed, 0, distance, self.events_sp) + self.sla.update(True, False, current_speed, 0, PCM_LONG_REQUIRED_MAX_SET_SPEED[self.sla.is_metric], target_speed, 0, distance, self.events_sp) assert self.sla.state == SpeedLimitAssistState.adapting assert self.sla.output_v_target == target_speed # TODO-SP: assert expected accel, need to enable self.acceleration_solutions def test_long_disengaged_to_disabled(self): - self.initialize_active_state(REQUIRED_INITIAL_MAX_SET_SPEED) + self.initialize_active_state(PCM_LONG_REQUIRED_MAX_SET_SPEED[self.sla.is_metric]) - self.sla.update(False, False, SPEED_LIMITS['city'], 0, REQUIRED_INITIAL_MAX_SET_SPEED, SPEED_LIMITS['city'], + self.sla.update(False, False, SPEED_LIMITS['city'], 0, PCM_LONG_REQUIRED_MAX_SET_SPEED[self.sla.is_metric], SPEED_LIMITS['city'], 0, 0, self.events_sp) assert self.sla.state == SpeedLimitAssistState.disabled assert self.sla.output_v_target == V_CRUISE_UNSET @@ -210,7 +210,7 @@ class TestSpeedLimitAssist: initial_state = state - self.sla.update(True, False, SPEED_LIMITS['city'], 0, REQUIRED_INITIAL_MAX_SET_SPEED,SPEED_LIMITS['city'], 0, 0, self.events_sp) + self.sla.update(True, False, SPEED_LIMITS['city'], 0, PCM_LONG_REQUIRED_MAX_SET_SPEED[self.sla.is_metric],SPEED_LIMITS['city'], 0, 0, self.events_sp) assert self.sla.state in ALL_STATES # Sanity check diff --git a/sunnypilot/selfdrive/selfdrived/events.py b/sunnypilot/selfdrive/selfdrived/events.py index f9e6c1e92c..978580669c 100644 --- a/sunnypilot/selfdrive/selfdrived/events.py +++ b/sunnypilot/selfdrive/selfdrived/events.py @@ -3,7 +3,7 @@ from cereal import log, car, custom from openpilot.common.constants import CV from openpilot.sunnypilot.selfdrive.selfdrived.events_base import EventsBase, Priority, ET, Alert, \ NoEntryAlert, ImmediateDisableAlert, EngagementAlert, NormalPermanentAlert, AlertCallbackType, wrong_car_mode_alert -from openpilot.sunnypilot.selfdrive.controls.lib.speed_limit import REQUIRED_INITIAL_MAX_SET_SPEED, CRUISE_SPEED_TOLERANCE +from openpilot.sunnypilot.selfdrive.controls.lib.speed_limit import PCM_LONG_REQUIRED_MAX_SET_SPEED AlertSize = log.SelfdriveState.AlertSize @@ -29,9 +29,7 @@ def speed_limit_adjust_alert(CP: car.CarParams, CS: car.CarState, sm: messaging. def speed_limit_pre_active_alert(CP: car.CarParams, CS: car.CarState, sm: messaging.SubMaster, metric: bool, soft_disable_time: int, personality) -> Alert: - max_initial_set_speed = round(REQUIRED_INITIAL_MAX_SET_SPEED * (CV.MS_TO_KPH if metric else CV.MS_TO_MPH)) - if metric: - max_initial_set_speed += round(CRUISE_SPEED_TOLERANCE * CV.MS_TO_KPH) + max_initial_set_speed = PCM_LONG_REQUIRED_MAX_SET_SPEED[metric] speed_unit = "km/h" if metric else "mph" return Alert( "Speed Limit Assist: Activation Required",