This commit is contained in:
Jason Wen
2025-09-02 01:07:27 -04:00
parent db9e410bee
commit 6acb23ba30
2 changed files with 15 additions and 14 deletions
@@ -28,9 +28,10 @@ class SpeedLimitController:
_speed_limit: float
_distance: float
_source: Source
_v_ego: float
_a_ego: float
_v_offset: float
v_ego: float
a_ego: float
v_offset: float
last_valid_speed_limit_final: float
def __init__(self, CP):
self.params = Params()
@@ -50,7 +51,7 @@ class SpeedLimitController:
self.initial_max_set = False
self._speed_limit = 0.
self.speed_limit_prev = 0.
self.last_valid_speed_limit_offsetted = 0.
self.last_valid_speed_limit_final = 0.
self._distance = 0.
self._source = Source.none
self.state = SpeedLimitControlState.disabled
@@ -113,23 +114,23 @@ class SpeedLimitController:
if self.is_active:
# If we have a current valid speed limit, use it
if self._speed_limit > 0:
self.last_valid_speed_limit_offsetted = self.speed_limit_final
self.last_valid_speed_limit_final = self.speed_limit_final
return self.speed_limit_final
# If no current speed limit but we have a last valid one, use that
if self.last_valid_speed_limit_offsetted > 0:
return self.last_valid_speed_limit_offsetted
if self.last_valid_speed_limit_final > 0:
return self.last_valid_speed_limit_final
# Fallback
return V_CRUISE_UNSET
@property
def v_cruise_setpoint_changed(self) -> bool:
return self.v_cruise_setpoint != self.v_cruise_setpoint_prev
return bool(self.v_cruise_setpoint != self.v_cruise_setpoint_prev)
@property
def speed_limit_changed(self) -> bool:
return self._speed_limit != self.speed_limit_prev
return bool(self._speed_limit != self.speed_limit_prev)
def get_offset(self, offset_type: OffsetType, offset_value: int) -> float:
if offset_type == OffsetType.off:
@@ -153,13 +154,13 @@ class SpeedLimitController:
return Engage.auto
def initial_max_set_confirmed(self) -> bool:
return abs(self.v_cruise_setpoint - REQUIRED_INITIAL_MAX_SET_SPEED) <= CRUISE_SPEED_TOLERANCE
return bool(abs(self.v_cruise_setpoint - REQUIRED_INITIAL_MAX_SET_SPEED) <= CRUISE_SPEED_TOLERANCE)
def detect_manual_cruise_change(self) -> bool:
# If cruise speed changed and it's not what SLC would set
if self.v_cruise_setpoint_changed:
expected_cruise = self.speed_limit_final
return abs(self.v_cruise_setpoint - expected_cruise) > CRUISE_SPEED_TOLERANCE
return bool(abs(self.v_cruise_setpoint - expected_cruise) > CRUISE_SPEED_TOLERANCE)
return False
@@ -51,7 +51,7 @@ class TestSpeedLimitController:
self.slc.initial_max_set = False
self.slc._speed_limit = 0.
self.slc.speed_limit_prev = 0.
self.slc.last_valid_speed_limit_offsetted = 0.
self.slc.last_valid_speed_limit_final = 0.
self.slc._distance = 0.
self.slc._source = Source.none
self.events_sp.clear()
@@ -166,7 +166,7 @@ class TestSpeedLimitController:
def test_invalid_speed_limits_handling(self):
self.slc.state = SpeedLimitControlState.active
self.slc.v_cruise_setpoint_prev = REQUIRED_INITIAL_MAX_SET_SPEED
self.slc.last_valid_speed_limit_offsetted = SPEED_LIMITS['city']
self.slc.last_valid_speed_limit_final = SPEED_LIMITS['city']
invalid_limits = [-10, 0, 200 * CV.MPH_TO_MS]
@@ -179,7 +179,7 @@ class TestSpeedLimitController:
self.slc.state = SpeedLimitControlState.active
self.slc.v_cruise_setpoint_prev = REQUIRED_INITIAL_MAX_SET_SPEED
old_speed_limit = SPEED_LIMITS['city']
self.slc.last_valid_speed_limit_offsetted = old_speed_limit
self.slc.last_valid_speed_limit_final = old_speed_limit
v_cruise_slc = self.slc.update(True, SPEED_LIMITS['city'], 0, REQUIRED_INITIAL_MAX_SET_SPEED, 0, 0, Source.car_state, self.events_sp)
assert self.slc.state in ACTIVE_STATES