mawei/sunnypilot
1
0
Fork 0
mirror of https://github.com/sunnypilot/sunnypilot.git synced 2026-07-10 11:52:13 +08:00
Code Issues Packages Projects Releases Wiki Activity

tests

Browse Source
This commit is contained in:
Jason Wen
2025-09-24 00:01:53 -04:00
parent 84725d8923
commit fd75d62f5a
4 changed files with 28 additions and 29 deletions
Download Patch File Download Diff File Expand all files Collapse all files
sunnypilot/selfdrive/controls/lib/speed_limit/__init__.py
+2 -2
View File
@@ -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
}
sunnypilot/selfdrive/controls/lib/speed_limit/speed_limit_assist.py
+3 -2
View File
@@ -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
sunnypilot/selfdrive/controls/lib/speed_limit/tests/test_speed_limit_assist.py
+21 -21
View File
@@ -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
sunnypilot/selfdrive/selfdrived/events.py
+2 -4
View File
@@ -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",