sunnypilot v2026.002.000
version: sunnypilot v2026.002.000 (staging) date: 2026-07-21T07:44:02 master commit: 2c334ede443d7391d27575af8c854a095ba702a8
This commit is contained in:
commit
de77c89cf6
3995 files changed
+1260688
No files matched your search
@@ -0,0 +1,19 @@
|
||||
"""
|
||||
Copyright (c) 2021-, Haibin Wen, sunnypilot, and a number of other contributors.
|
||||
|
||||
This file is part of sunnypilot and is licensed under the MIT License.
|
||||
See the LICENSE.md file in the root directory for more details.
|
||||
"""
|
||||
LIMIT_ADAPT_ACC = -1. # m/s^2 Ideal acceleration for the adapting (braking) phase when approaching speed limits.
|
||||
LIMIT_MAX_MAP_DATA_AGE = 10. # s Maximum time to hold to map data, then consider it invalid inside limits controllers.
|
||||
|
||||
# Speed Limit Assist constants
|
||||
PCM_LONG_REQUIRED_MAX_SET_SPEED = {
|
||||
True: (33.3333, 36.1111), # km/h, (120, 130)
|
||||
False: (31.2928, 35.7632), # mph, (70, 80)
|
||||
}
|
||||
|
||||
CONFIRM_SPEED_THRESHOLD = {
|
||||
True: 80, # km/h
|
||||
False: 50, # mph
|
||||
}
|
||||
@@ -0,0 +1,29 @@
|
||||
"""
|
||||
Copyright (c) 2021-, Haibin Wen, sunnypilot, and a number of other contributors.
|
||||
|
||||
This file is part of sunnypilot and is licensed under the MIT License.
|
||||
See the LICENSE.md file in the root directory for more details.
|
||||
"""
|
||||
|
||||
from openpilot.sunnypilot import IntEnumBase
|
||||
|
||||
|
||||
class Policy(IntEnumBase):
|
||||
car_state_only = 0
|
||||
map_data_only = 1
|
||||
car_state_priority = 2
|
||||
map_data_priority = 3
|
||||
combined = 4
|
||||
|
||||
|
||||
class OffsetType(IntEnumBase):
|
||||
off = 0
|
||||
fixed = 1
|
||||
percentage = 2
|
||||
|
||||
|
||||
class Mode(IntEnumBase):
|
||||
off = 0
|
||||
information = 1
|
||||
warning = 2
|
||||
assist = 3
|
||||
@@ -0,0 +1,45 @@
|
||||
"""
|
||||
Copyright (c) 2021-, Haibin Wen, sunnypilot, and a number of other contributors.
|
||||
|
||||
This file is part of sunnypilot and is licensed under the MIT License.
|
||||
See the LICENSE.md file in the root directory for more details.
|
||||
"""
|
||||
|
||||
from openpilot.cereal import custom
|
||||
from opendbc.car.structs import car
|
||||
from openpilot.common.constants import CV
|
||||
from openpilot.common.params import Params
|
||||
from openpilot.sunnypilot.selfdrive.controls.lib.speed_limit.common import Mode as SpeedLimitMode
|
||||
|
||||
|
||||
def compare_cluster_target(v_cruise_cluster: float, target_set_speed: float, is_metric: bool) -> tuple[bool, bool]:
|
||||
speed_conv = CV.MS_TO_KPH if is_metric else CV.MS_TO_MPH
|
||||
v_cruise_cluster_conv = round(v_cruise_cluster * speed_conv)
|
||||
target_set_speed_conv = round(target_set_speed * speed_conv)
|
||||
|
||||
req_plus = v_cruise_cluster_conv < target_set_speed_conv
|
||||
req_minus = v_cruise_cluster_conv > target_set_speed_conv
|
||||
|
||||
return req_plus, req_minus
|
||||
|
||||
|
||||
def set_speed_limit_assist_availability(CP: car.CarParams, CP_SP: custom.CarParamsSP, params: Params = None) -> bool:
|
||||
if params is None:
|
||||
params = Params()
|
||||
|
||||
is_release = params.get_bool("IsReleaseSpBranch")
|
||||
disallow_in_release = CP.brand == "tesla" and is_release
|
||||
always_disallow = CP.brand == "rivian"
|
||||
allowed = True
|
||||
|
||||
if disallow_in_release or always_disallow:
|
||||
allowed = False
|
||||
|
||||
if not CP.openpilotLongitudinalControl and CP_SP.pcmCruiseSpeed:
|
||||
allowed = False
|
||||
|
||||
if not allowed:
|
||||
if params.get("SpeedLimitMode", return_default=True) == SpeedLimitMode.assist:
|
||||
params.put("SpeedLimitMode", int(SpeedLimitMode.warning), block=True)
|
||||
|
||||
return allowed
|
||||
@@ -0,0 +1,415 @@
|
||||
"""
|
||||
Copyright (c) 2021-, Haibin Wen, sunnypilot, and a number of other contributors.
|
||||
|
||||
This file is part of sunnypilot and is licensed under the MIT License.
|
||||
See the LICENSE.md file in the root directory for more details.
|
||||
"""
|
||||
import time
|
||||
|
||||
from openpilot.cereal import custom
|
||||
from opendbc.car.structs import car
|
||||
from openpilot.common.params import Params
|
||||
from openpilot.common.constants import CV
|
||||
from openpilot.common.realtime import DT_MDL
|
||||
from openpilot.selfdrive.controls.lib.drive_helpers import CONTROL_N
|
||||
from openpilot.selfdrive.modeld.constants import ModelConstants
|
||||
from openpilot.sunnypilot import PARAMS_UPDATE_PERIOD
|
||||
from openpilot.sunnypilot.selfdrive.selfdrived.events import EventsSP
|
||||
from openpilot.sunnypilot.selfdrive.controls.lib.speed_limit import PCM_LONG_REQUIRED_MAX_SET_SPEED, CONFIRM_SPEED_THRESHOLD
|
||||
from openpilot.sunnypilot.selfdrive.controls.lib.speed_limit.common import Mode
|
||||
from openpilot.sunnypilot.selfdrive.controls.lib.speed_limit.helpers import compare_cluster_target, set_speed_limit_assist_availability
|
||||
|
||||
ButtonType = car.CarState.ButtonEvent.Type
|
||||
EventNameSP = custom.OnroadEventSP.EventName
|
||||
SpeedLimitAssistState = custom.LongitudinalPlanSP.SpeedLimit.AssistState
|
||||
SpeedLimitSource = custom.LongitudinalPlanSP.SpeedLimit.Source
|
||||
|
||||
ACTIVE_STATES = (SpeedLimitAssistState.active, SpeedLimitAssistState.adapting)
|
||||
ENABLED_STATES = (SpeedLimitAssistState.preActive, SpeedLimitAssistState.pending, *ACTIVE_STATES)
|
||||
|
||||
DISABLED_GUARD_PERIOD = 0.5 # secs.
|
||||
# secs. Time to wait after activation before considering temp deactivation signal.
|
||||
PRE_ACTIVE_GUARD_PERIOD = {
|
||||
True: 15,
|
||||
False: 5,
|
||||
}
|
||||
SPEED_LIMIT_CHANGED_HOLD_PERIOD = 1 # secs. Time to wait after speed limit change before switching to preActive.
|
||||
|
||||
LIMIT_MIN_ACC = -1.5 # m/s^2 Maximum deceleration allowed for limit controllers to provide.
|
||||
LIMIT_MAX_ACC = 1.0 # m/s^2 Maximum acceleration allowed for limit controllers to provide while active.
|
||||
LIMIT_MIN_SPEED = 8.33 # m/s, Minimum speed limit to provide as solution on limit controllers.
|
||||
LIMIT_SPEED_OFFSET_TH = -1. # m/s Maximum offset between speed limit and current speed for adapting state.
|
||||
V_CRUISE_UNSET = 255.
|
||||
|
||||
CRUISE_BUTTONS_PLUS = (ButtonType.accelCruise, ButtonType.resumeCruise)
|
||||
CRUISE_BUTTONS_MINUS = (ButtonType.decelCruise, ButtonType.setCruise)
|
||||
CRUISE_BUTTON_CONFIRM_HOLD = 0.5 # secs.
|
||||
|
||||
|
||||
class SpeedLimitAssist:
|
||||
_speed_limit_final_last: float
|
||||
_distance: float
|
||||
v_ego: float
|
||||
a_ego: float
|
||||
v_offset: float
|
||||
|
||||
def __init__(self, CP: car.CarParams, CP_SP: custom.CarParamsSP):
|
||||
self.params = Params()
|
||||
self.CP = CP
|
||||
self.CP_SP = CP_SP
|
||||
self.frame = -1
|
||||
self.long_engaged_timer = 0
|
||||
self.pre_active_timer = 0
|
||||
self.is_metric = self.params.get_bool("IsMetric")
|
||||
set_speed_limit_assist_availability(self.CP, self.CP_SP, self.params)
|
||||
self.enabled = self.params.get("SpeedLimitMode", return_default=True) == Mode.assist
|
||||
self.long_enabled = False
|
||||
self.long_enabled_prev = False
|
||||
self.is_enabled = False
|
||||
self.is_active = False
|
||||
self.output_v_target = V_CRUISE_UNSET
|
||||
self.output_a_target = 0.
|
||||
self.v_ego = 0.
|
||||
self.a_ego = 0.
|
||||
self.v_offset = 0.
|
||||
self.target_set_speed_conv = 0
|
||||
self.prev_target_set_speed_conv = 0
|
||||
self.v_cruise_cluster = 0.
|
||||
self.v_cruise_cluster_prev = 0.
|
||||
self.v_cruise_cluster_conv = 0
|
||||
self.prev_v_cruise_cluster_conv = 0
|
||||
self._has_speed_limit = False
|
||||
self._speed_limit = 0.
|
||||
self._speed_limit_final_last = 0.
|
||||
self.speed_limit_prev = 0.
|
||||
self.speed_limit_final_last_conv = 0
|
||||
self.prev_speed_limit_final_last_conv = 0
|
||||
self._distance = 0.
|
||||
self.state = SpeedLimitAssistState.disabled
|
||||
self._state_prev = SpeedLimitAssistState.disabled
|
||||
self.pcm_op_long = CP.openpilotLongitudinalControl and CP.pcmCruise
|
||||
|
||||
self._plus_hold = 0.
|
||||
self._minus_hold = 0.
|
||||
self._last_carstate_ts = 0.
|
||||
|
||||
# TODO-SP: SLA's own output_a_target for planner
|
||||
# Solution functions mapped to respective states
|
||||
self.acceleration_solutions = {
|
||||
SpeedLimitAssistState.disabled: self.get_current_acceleration_as_target,
|
||||
SpeedLimitAssistState.inactive: self.get_current_acceleration_as_target,
|
||||
SpeedLimitAssistState.preActive: self.get_current_acceleration_as_target,
|
||||
SpeedLimitAssistState.pending: self.get_current_acceleration_as_target,
|
||||
SpeedLimitAssistState.adapting: self.get_adapting_state_target_acceleration,
|
||||
SpeedLimitAssistState.active: self.get_active_state_target_acceleration,
|
||||
}
|
||||
|
||||
@property
|
||||
def speed_limit_changed(self) -> bool:
|
||||
return self._has_speed_limit and bool(self._speed_limit != self.speed_limit_prev)
|
||||
|
||||
@property
|
||||
def v_cruise_cluster_changed(self) -> bool:
|
||||
return bool(self.v_cruise_cluster_conv != self.prev_v_cruise_cluster_conv)
|
||||
|
||||
@property
|
||||
def target_set_speed_confirmed(self) -> bool:
|
||||
return bool(self.v_cruise_cluster_conv == self.target_set_speed_conv)
|
||||
|
||||
@property
|
||||
def v_cruise_cluster_below_confirm_speed_threshold(self) -> bool:
|
||||
return bool(self.v_cruise_cluster_conv < CONFIRM_SPEED_THRESHOLD[self.is_metric])
|
||||
|
||||
def update_active_event(self, events_sp: EventsSP) -> None:
|
||||
if self.v_cruise_cluster_below_confirm_speed_threshold:
|
||||
events_sp.add(EventNameSP.speedLimitChanged)
|
||||
else:
|
||||
events_sp.add(EventNameSP.speedLimitActive)
|
||||
|
||||
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:
|
||||
return self._speed_limit_final_last
|
||||
|
||||
# Fallback
|
||||
return V_CRUISE_UNSET
|
||||
|
||||
# TODO-SP: SLA's own output_a_target for planner
|
||||
def get_a_target_from_control(self) -> float:
|
||||
return self.a_ego
|
||||
|
||||
def update_params(self) -> None:
|
||||
if self.frame % int(PARAMS_UPDATE_PERIOD / DT_MDL) == 0:
|
||||
self.is_metric = self.params.get_bool("IsMetric")
|
||||
set_speed_limit_assist_availability(self.CP, self.CP_SP, self.params)
|
||||
self.enabled = self.params.get("SpeedLimitMode", return_default=True) == Mode.assist
|
||||
|
||||
def update_car_state(self, CS: car.CarState) -> None:
|
||||
now = time.monotonic()
|
||||
self._last_carstate_ts = now
|
||||
|
||||
for b in CS.buttonEvents:
|
||||
if not b.pressed:
|
||||
if b.type in CRUISE_BUTTONS_PLUS:
|
||||
self._plus_hold = max(self._plus_hold, now + CRUISE_BUTTON_CONFIRM_HOLD)
|
||||
elif b.type in CRUISE_BUTTONS_MINUS:
|
||||
self._minus_hold = max(self._minus_hold, now + CRUISE_BUTTON_CONFIRM_HOLD)
|
||||
|
||||
def _get_button_release(self, req_plus: bool, req_minus: bool) -> bool:
|
||||
now = time.monotonic()
|
||||
if req_plus and now <= self._plus_hold:
|
||||
self._plus_hold = 0.
|
||||
return True
|
||||
elif req_minus and now <= self._minus_hold:
|
||||
self._minus_hold = 0.
|
||||
return True
|
||||
|
||||
# expired
|
||||
if now > self._plus_hold:
|
||||
self._plus_hold = 0.
|
||||
if now > self._minus_hold:
|
||||
self._minus_hold = 0.
|
||||
return False
|
||||
|
||||
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
|
||||
|
||||
# Update current velocity offset (error)
|
||||
self.v_offset = self._speed_limit_final_last - self.v_ego
|
||||
|
||||
self.speed_limit_final_last_conv = round(self._speed_limit_final_last * speed_conv)
|
||||
self.v_cruise_cluster_conv = round(self.v_cruise_cluster * speed_conv)
|
||||
|
||||
cst_low, cst_high = PCM_LONG_REQUIRED_MAX_SET_SPEED[self.is_metric]
|
||||
pcm_long_required_max = cst_low if self._has_speed_limit and self.speed_limit_final_last_conv < CONFIRM_SPEED_THRESHOLD[self.is_metric] else \
|
||||
cst_high
|
||||
pcm_long_required_max_set_speed_conv = round(pcm_long_required_max * speed_conv)
|
||||
|
||||
self.target_set_speed_conv = pcm_long_required_max_set_speed_conv if self.pcm_op_long else self.speed_limit_final_last_conv
|
||||
|
||||
@property
|
||||
def apply_confirm_speed_threshold(self) -> bool:
|
||||
# below CST: always require user confirmation
|
||||
if self.v_cruise_cluster_below_confirm_speed_threshold:
|
||||
return True
|
||||
|
||||
# at/above CST:
|
||||
# - new speed limit >= CST: auto change
|
||||
# - new speed limit < CST: user confirmation required
|
||||
return bool(self.speed_limit_final_last_conv < CONFIRM_SPEED_THRESHOLD[self.is_metric])
|
||||
|
||||
def get_current_acceleration_as_target(self) -> float:
|
||||
return self.a_ego
|
||||
|
||||
def get_adapting_state_target_acceleration(self) -> float:
|
||||
if self._distance > 0:
|
||||
return (self._speed_limit_final_last ** 2 - self.v_ego ** 2) / (2. * self._distance)
|
||||
|
||||
return self.v_offset / float(ModelConstants.T_IDXS[CONTROL_N])
|
||||
|
||||
def get_active_state_target_acceleration(self) -> float:
|
||||
return self.v_offset / float(ModelConstants.T_IDXS[CONTROL_N])
|
||||
|
||||
def _update_confirmed_state(self):
|
||||
if self._has_speed_limit:
|
||||
if self.v_offset < LIMIT_SPEED_OFFSET_TH:
|
||||
self.state = SpeedLimitAssistState.adapting
|
||||
else:
|
||||
self.state = SpeedLimitAssistState.active
|
||||
else:
|
||||
self.state = SpeedLimitAssistState.pending
|
||||
|
||||
def _update_non_pcm_long_confirmed_state(self) -> bool:
|
||||
if self.target_set_speed_confirmed:
|
||||
return True
|
||||
|
||||
if self.state != SpeedLimitAssistState.preActive:
|
||||
return False
|
||||
|
||||
req_plus, req_minus = compare_cluster_target(self.v_cruise_cluster, self._speed_limit_final_last, self.is_metric)
|
||||
|
||||
return self._get_button_release(req_plus, req_minus)
|
||||
|
||||
def update_state_machine_pcm_op_long(self):
|
||||
self.long_engaged_timer = max(0, self.long_engaged_timer - 1)
|
||||
self.pre_active_timer = max(0, self.pre_active_timer - 1)
|
||||
|
||||
# ACTIVE, ADAPTING, PENDING, PRE_ACTIVE, INACTIVE
|
||||
if self.state != SpeedLimitAssistState.disabled:
|
||||
if not self.long_enabled or not self.enabled:
|
||||
self.state = SpeedLimitAssistState.disabled
|
||||
|
||||
else:
|
||||
# ACTIVE
|
||||
if self.state == SpeedLimitAssistState.active:
|
||||
if self.v_cruise_cluster_changed:
|
||||
self.state = SpeedLimitAssistState.inactive
|
||||
elif self.speed_limit_changed and self.apply_confirm_speed_threshold:
|
||||
self.state = SpeedLimitAssistState.preActive
|
||||
self.pre_active_timer = int(PRE_ACTIVE_GUARD_PERIOD[self.pcm_op_long] / DT_MDL)
|
||||
elif self._has_speed_limit and self.v_offset < LIMIT_SPEED_OFFSET_TH:
|
||||
self.state = SpeedLimitAssistState.adapting
|
||||
|
||||
# ADAPTING
|
||||
elif self.state == SpeedLimitAssistState.adapting:
|
||||
if self.v_cruise_cluster_changed:
|
||||
self.state = SpeedLimitAssistState.inactive
|
||||
elif self.speed_limit_changed and self.apply_confirm_speed_threshold:
|
||||
self.state = SpeedLimitAssistState.preActive
|
||||
self.pre_active_timer = int(PRE_ACTIVE_GUARD_PERIOD[self.pcm_op_long] / DT_MDL)
|
||||
elif self.v_offset >= LIMIT_SPEED_OFFSET_TH:
|
||||
self.state = SpeedLimitAssistState.active
|
||||
|
||||
# PENDING
|
||||
elif self.state == SpeedLimitAssistState.pending:
|
||||
if self.target_set_speed_confirmed:
|
||||
self._update_confirmed_state()
|
||||
elif self.speed_limit_changed:
|
||||
self.state = SpeedLimitAssistState.preActive
|
||||
self.pre_active_timer = int(PRE_ACTIVE_GUARD_PERIOD[self.pcm_op_long] / DT_MDL)
|
||||
|
||||
# PRE_ACTIVE
|
||||
elif self.state == SpeedLimitAssistState.preActive:
|
||||
if self.target_set_speed_confirmed:
|
||||
self._update_confirmed_state()
|
||||
elif self.pre_active_timer <= 0:
|
||||
# Timeout - session ended
|
||||
self.state = SpeedLimitAssistState.inactive
|
||||
|
||||
# INACTIVE
|
||||
elif self.state == SpeedLimitAssistState.inactive:
|
||||
pass
|
||||
|
||||
# DISABLED
|
||||
elif self.state == SpeedLimitAssistState.disabled:
|
||||
if self.long_enabled and self.enabled:
|
||||
# start or reset preActive timer if initially enabled or manual set speed change detected
|
||||
if not self.long_enabled_prev or self.v_cruise_cluster_changed:
|
||||
self.long_engaged_timer = int(DISABLED_GUARD_PERIOD / DT_MDL)
|
||||
|
||||
elif self.long_engaged_timer <= 0:
|
||||
if self.target_set_speed_confirmed:
|
||||
self._update_confirmed_state()
|
||||
elif self._has_speed_limit:
|
||||
self.state = SpeedLimitAssistState.preActive
|
||||
self.pre_active_timer = int(PRE_ACTIVE_GUARD_PERIOD[self.pcm_op_long] / DT_MDL)
|
||||
else:
|
||||
self.state = SpeedLimitAssistState.pending
|
||||
|
||||
enabled = self.state in ENABLED_STATES
|
||||
active = self.state in ACTIVE_STATES
|
||||
|
||||
return enabled, active
|
||||
|
||||
def update_state_machine_non_pcm_long(self):
|
||||
self.long_engaged_timer = max(0, self.long_engaged_timer - 1)
|
||||
self.pre_active_timer = max(0, self.pre_active_timer - 1)
|
||||
|
||||
# ACTIVE, ADAPTING, PENDING, PRE_ACTIVE, INACTIVE
|
||||
if self.state != SpeedLimitAssistState.disabled:
|
||||
if not self.long_enabled or not self.enabled:
|
||||
self.state = SpeedLimitAssistState.disabled
|
||||
|
||||
else:
|
||||
# ACTIVE
|
||||
if self.state == SpeedLimitAssistState.active:
|
||||
if self.v_cruise_cluster_changed:
|
||||
self.state = SpeedLimitAssistState.inactive
|
||||
|
||||
elif self.speed_limit_changed and self.apply_confirm_speed_threshold:
|
||||
self.state = SpeedLimitAssistState.preActive
|
||||
self.pre_active_timer = int(PRE_ACTIVE_GUARD_PERIOD[self.pcm_op_long] / DT_MDL)
|
||||
|
||||
# PRE_ACTIVE
|
||||
elif self.state == SpeedLimitAssistState.preActive:
|
||||
if self._update_non_pcm_long_confirmed_state():
|
||||
self.state = SpeedLimitAssistState.active
|
||||
elif self.pre_active_timer <= 0:
|
||||
# Timeout - session ended
|
||||
self.state = SpeedLimitAssistState.inactive
|
||||
|
||||
# INACTIVE
|
||||
elif self.state == SpeedLimitAssistState.inactive:
|
||||
if self.speed_limit_changed:
|
||||
self.state = SpeedLimitAssistState.preActive
|
||||
self.pre_active_timer = int(PRE_ACTIVE_GUARD_PERIOD[self.pcm_op_long] / DT_MDL)
|
||||
elif self._update_non_pcm_long_confirmed_state():
|
||||
self.state = SpeedLimitAssistState.active
|
||||
|
||||
# DISABLED
|
||||
elif self.state == SpeedLimitAssistState.disabled:
|
||||
if self.long_enabled and self.enabled:
|
||||
# start or reset preActive timer if initially enabled or manual set speed change detected
|
||||
if not self.long_enabled_prev or self.v_cruise_cluster_changed:
|
||||
self.long_engaged_timer = int(DISABLED_GUARD_PERIOD / DT_MDL)
|
||||
|
||||
elif self.long_engaged_timer <= 0:
|
||||
if self._update_non_pcm_long_confirmed_state():
|
||||
self.state = SpeedLimitAssistState.active
|
||||
elif self._has_speed_limit:
|
||||
self.state = SpeedLimitAssistState.preActive
|
||||
self.pre_active_timer = int(PRE_ACTIVE_GUARD_PERIOD[self.pcm_op_long] / DT_MDL)
|
||||
else:
|
||||
self.state = SpeedLimitAssistState.inactive
|
||||
|
||||
enabled = self.state in ENABLED_STATES
|
||||
active = self.state in ACTIVE_STATES
|
||||
|
||||
return enabled, active
|
||||
|
||||
def update_events(self, events_sp: EventsSP) -> None:
|
||||
if self.state == SpeedLimitAssistState.preActive:
|
||||
events_sp.add(EventNameSP.speedLimitPreActive)
|
||||
|
||||
if self.state == SpeedLimitAssistState.pending and self._state_prev != SpeedLimitAssistState.pending:
|
||||
events_sp.add(EventNameSP.speedLimitPending)
|
||||
|
||||
if self.is_active:
|
||||
if self._state_prev not in ACTIVE_STATES:
|
||||
self.update_active_event(events_sp)
|
||||
|
||||
# only notify if we acquire a valid speed limit
|
||||
# do not check has_speed_limit here
|
||||
elif self._speed_limit != self.speed_limit_prev:
|
||||
if self.speed_limit_prev <= 0:
|
||||
self.update_active_event(events_sp)
|
||||
elif self.speed_limit_prev > 0 and self._speed_limit > 0:
|
||||
self.update_active_event(events_sp)
|
||||
|
||||
def update(self, long_enabled: bool, long_override: bool, v_ego: float, a_ego: float, v_cruise_cluster: float, speed_limit: float,
|
||||
speed_limit_final_last: float, has_speed_limit: bool, distance: float, events_sp: EventsSP) -> None:
|
||||
self.long_enabled = long_enabled
|
||||
self.v_ego = v_ego
|
||||
self.a_ego = a_ego
|
||||
|
||||
self._has_speed_limit = has_speed_limit
|
||||
self._speed_limit = speed_limit
|
||||
self._speed_limit_final_last = speed_limit_final_last
|
||||
self._distance = distance
|
||||
|
||||
self.update_params()
|
||||
self.update_calculations(v_cruise_cluster)
|
||||
|
||||
self._state_prev = self.state
|
||||
if self.pcm_op_long:
|
||||
self.is_enabled, self.is_active = self.update_state_machine_pcm_op_long()
|
||||
else:
|
||||
self.is_enabled, self.is_active = self.update_state_machine_non_pcm_long()
|
||||
|
||||
self.update_events(events_sp)
|
||||
|
||||
# Update change tracking variables
|
||||
self.speed_limit_prev = self._speed_limit
|
||||
self.v_cruise_cluster_prev = self.v_cruise_cluster
|
||||
self.long_enabled_prev = self.long_enabled
|
||||
self.prev_target_set_speed_conv = self.target_set_speed_conv
|
||||
self.prev_v_cruise_cluster_conv = self.v_cruise_cluster_conv
|
||||
self.prev_speed_limit_final_last_conv = self.speed_limit_final_last_conv
|
||||
|
||||
self.output_v_target = self.get_v_target_from_control()
|
||||
self.output_a_target = self.get_a_target_from_control()
|
||||
|
||||
self.frame += 1
|
||||
@@ -0,0 +1,190 @@
|
||||
"""
|
||||
Copyright (c) 2021-, Haibin Wen, sunnypilot, and a number of other contributors.
|
||||
|
||||
This file is part of sunnypilot and is licensed under the MIT License.
|
||||
See the LICENSE.md file in the root directory for more details.
|
||||
"""
|
||||
import time
|
||||
|
||||
import openpilot.cereal.messaging as messaging
|
||||
from openpilot.cereal import custom
|
||||
from openpilot.common.constants import CV
|
||||
from openpilot.common.gps import get_gps_location_service
|
||||
from openpilot.common.params import Params
|
||||
from openpilot.common.realtime import DT_MDL
|
||||
from openpilot.sunnypilot import PARAMS_UPDATE_PERIOD, get_sanitize_int_param
|
||||
from openpilot.sunnypilot.selfdrive.controls.lib.speed_limit import LIMIT_MAX_MAP_DATA_AGE, LIMIT_ADAPT_ACC
|
||||
from openpilot.sunnypilot.selfdrive.controls.lib.speed_limit.common import Policy, OffsetType
|
||||
|
||||
SpeedLimitSource = custom.LongitudinalPlanSP.SpeedLimit.Source
|
||||
|
||||
ALL_SOURCES = tuple(SpeedLimitSource.schema.enumerants.values())
|
||||
|
||||
|
||||
class SpeedLimitResolver:
|
||||
limit_solutions: dict[custom.LongitudinalPlanSP.SpeedLimit.Source, float]
|
||||
distance_solutions: dict[custom.LongitudinalPlanSP.SpeedLimit.Source, float]
|
||||
v_ego: float
|
||||
speed_limit: float
|
||||
speed_limit_last: float
|
||||
speed_limit_final: float
|
||||
speed_limit_final_last: float
|
||||
distance: float
|
||||
source: custom.LongitudinalPlanSP.SpeedLimit.Source
|
||||
speed_limit_offset: float
|
||||
|
||||
def __init__(self):
|
||||
self.params = Params()
|
||||
self.frame = -1
|
||||
|
||||
self._gps_location_service = get_gps_location_service(self.params)
|
||||
self.limit_solutions = {} # Store for speed limit solutions from different sources
|
||||
self.distance_solutions = {} # Store for distance to current speed limit start for different sources
|
||||
|
||||
self.policy = self.params.get("SpeedLimitPolicy", return_default=True)
|
||||
self.policy = get_sanitize_int_param(
|
||||
"SpeedLimitPolicy",
|
||||
Policy.min().value,
|
||||
Policy.max().value,
|
||||
self.params
|
||||
)
|
||||
self._policy_to_sources_map = {
|
||||
Policy.car_state_only: [SpeedLimitSource.car],
|
||||
Policy.map_data_only: [SpeedLimitSource.map],
|
||||
Policy.car_state_priority: [SpeedLimitSource.car, SpeedLimitSource.map],
|
||||
Policy.map_data_priority: [SpeedLimitSource.map, SpeedLimitSource.car],
|
||||
Policy.combined: [SpeedLimitSource.car, SpeedLimitSource.map],
|
||||
}
|
||||
self.source = SpeedLimitSource.none
|
||||
for source in ALL_SOURCES:
|
||||
self._reset_limit_sources(source)
|
||||
|
||||
self.is_metric = self.params.get_bool("IsMetric")
|
||||
self.offset_type = get_sanitize_int_param(
|
||||
"SpeedLimitOffsetType",
|
||||
OffsetType.min().value,
|
||||
OffsetType.max().value,
|
||||
self.params
|
||||
)
|
||||
self.offset_value = self.params.get("SpeedLimitValueOffset", return_default=True)
|
||||
|
||||
self.speed_limit = 0.
|
||||
self.speed_limit_last = 0.
|
||||
self.speed_limit_final = 0.
|
||||
self.speed_limit_final_last = 0.
|
||||
self.speed_limit_offset = 0.
|
||||
|
||||
def update_speed_limit_states(self) -> None:
|
||||
self.speed_limit_final = self.speed_limit + self.speed_limit_offset
|
||||
|
||||
if self.speed_limit > 0.:
|
||||
self.speed_limit_last = self.speed_limit
|
||||
self.speed_limit_final_last = self.speed_limit_final
|
||||
|
||||
@property
|
||||
def speed_limit_valid(self) -> bool:
|
||||
return self.speed_limit > 0.
|
||||
|
||||
@property
|
||||
def speed_limit_last_valid(self) -> bool:
|
||||
return self.speed_limit_last > 0.
|
||||
|
||||
def update_params(self):
|
||||
if self.frame % int(PARAMS_UPDATE_PERIOD / DT_MDL) == 0:
|
||||
self.policy = self.params.get("SpeedLimitPolicy", return_default=True)
|
||||
self.is_metric = self.params.get_bool("IsMetric")
|
||||
self.offset_type = self.params.get("SpeedLimitOffsetType", return_default=True)
|
||||
self.offset_value = self.params.get("SpeedLimitValueOffset", return_default=True)
|
||||
|
||||
def _get_speed_limit_offset(self) -> float:
|
||||
if self.offset_type == OffsetType.off:
|
||||
return 0
|
||||
elif self.offset_type == OffsetType.fixed:
|
||||
return float(self.offset_value * (CV.KPH_TO_MS if self.is_metric else CV.MPH_TO_MS))
|
||||
elif self.offset_type == OffsetType.percentage:
|
||||
return float(self.offset_value * 0.01 * self.speed_limit)
|
||||
else:
|
||||
raise NotImplementedError("Offset not supported")
|
||||
|
||||
def _reset_limit_sources(self, source: custom.LongitudinalPlanSP.SpeedLimit.Source) -> None:
|
||||
self.limit_solutions[source] = 0.
|
||||
self.distance_solutions[source] = 0.
|
||||
|
||||
def _get_from_car_state(self, sm: messaging.SubMaster) -> None:
|
||||
self._reset_limit_sources(SpeedLimitSource.car)
|
||||
self.limit_solutions[SpeedLimitSource.car] = sm['carStateSP'].speedLimit
|
||||
self.distance_solutions[SpeedLimitSource.car] = 0.
|
||||
|
||||
def _get_from_map_data(self, sm: messaging.SubMaster) -> None:
|
||||
self._reset_limit_sources(SpeedLimitSource.map)
|
||||
self._process_map_data(sm)
|
||||
|
||||
def _process_map_data(self, sm: messaging.SubMaster) -> None:
|
||||
gps_data = sm[self._gps_location_service]
|
||||
map_data = sm['liveMapDataSP']
|
||||
|
||||
gps_fix_age = time.monotonic() - gps_data.unixTimestampMillis * 1e-3
|
||||
if gps_fix_age > LIMIT_MAX_MAP_DATA_AGE:
|
||||
return
|
||||
|
||||
speed_limit = map_data.speedLimit if map_data.speedLimitValid else 0.
|
||||
next_speed_limit = map_data.speedLimitAhead if map_data.speedLimitAheadValid else 0.
|
||||
|
||||
self._calculate_map_data_limits(sm, speed_limit, next_speed_limit)
|
||||
|
||||
def _calculate_map_data_limits(self, sm: messaging.SubMaster, speed_limit: float, next_speed_limit: float) -> None:
|
||||
gps_data = sm[self._gps_location_service]
|
||||
map_data = sm['liveMapDataSP']
|
||||
|
||||
distance_since_fix = self.v_ego * (time.monotonic() - gps_data.unixTimestampMillis * 1e-3)
|
||||
distance_to_speed_limit_ahead = max(0., map_data.speedLimitAheadDistance - distance_since_fix)
|
||||
|
||||
self.limit_solutions[SpeedLimitSource.map] = speed_limit
|
||||
self.distance_solutions[SpeedLimitSource.map] = 0.
|
||||
|
||||
# FIXME-SP: this is not working as expected
|
||||
if 0. < next_speed_limit < self.v_ego:
|
||||
adapt_time = (next_speed_limit - self.v_ego) / LIMIT_ADAPT_ACC
|
||||
adapt_distance = self.v_ego * adapt_time + 0.5 * LIMIT_ADAPT_ACC * adapt_time ** 2
|
||||
|
||||
if distance_to_speed_limit_ahead <= adapt_distance:
|
||||
self.limit_solutions[SpeedLimitSource.map] = next_speed_limit
|
||||
self.distance_solutions[SpeedLimitSource.map] = distance_to_speed_limit_ahead
|
||||
|
||||
def _get_source_solution_according_to_policy(self) -> custom.LongitudinalPlanSP.SpeedLimit.Source:
|
||||
sources_for_policy = self._policy_to_sources_map[self.policy]
|
||||
|
||||
if self.policy != Policy.combined:
|
||||
# They are ordered in the order of preference, so we pick the first that's non-zero
|
||||
for source in sources_for_policy:
|
||||
if self.limit_solutions[source] > 0.:
|
||||
return source
|
||||
return SpeedLimitSource.none
|
||||
|
||||
sources_with_limits = [(s, limit) for s, limit in [(s, self.limit_solutions[s]) for s in sources_for_policy] if limit > 0.]
|
||||
if sources_with_limits:
|
||||
return min(sources_with_limits, key=lambda x: x[1])[0]
|
||||
|
||||
return SpeedLimitSource.none
|
||||
|
||||
def _resolve_limit_sources(self, sm: messaging.SubMaster) -> tuple[float, float, custom.LongitudinalPlanSP.SpeedLimit.Source]:
|
||||
"""Get limit solutions from each data source"""
|
||||
self._get_from_car_state(sm)
|
||||
self._get_from_map_data(sm)
|
||||
|
||||
source = self._get_source_solution_according_to_policy()
|
||||
speed_limit = self.limit_solutions[source] if source else 0.
|
||||
distance = self.distance_solutions[source] if source else 0.
|
||||
|
||||
return speed_limit, distance, source
|
||||
|
||||
def update(self, v_ego: float, sm: messaging.SubMaster) -> None:
|
||||
self.v_ego = v_ego
|
||||
self.update_params()
|
||||
|
||||
self.speed_limit, self.distance, self.source = self._resolve_limit_sources(sm)
|
||||
self.speed_limit_offset = self._get_speed_limit_offset()
|
||||
|
||||
self.update_speed_limit_states()
|
||||
|
||||
self.frame += 1
|
||||
Whitespace-only changes.
+278
@@ -0,0 +1,278 @@
|
||||
"""
|
||||
Copyright (c) 2021-, Haibin Wen, sunnypilot, and a number of other contributors.
|
||||
|
||||
This file is part of sunnypilot and is licensed under the MIT License.
|
||||
See the LICENSE.md file in the root directory for more details.
|
||||
"""
|
||||
|
||||
import pytest
|
||||
|
||||
from openpilot.cereal import custom
|
||||
from opendbc.car.car_helpers import interfaces
|
||||
from opendbc.car.rivian.values import CAR as RIVIAN
|
||||
from opendbc.car.tesla.values import CAR as TESLA
|
||||
from opendbc.car.toyota.values import CAR as TOYOTA
|
||||
from openpilot.common.constants import CV
|
||||
from openpilot.common.params import Params
|
||||
from openpilot.common.realtime import DT_MDL
|
||||
from openpilot.selfdrive.car.cruise import V_CRUISE_UNSET
|
||||
from openpilot.sunnypilot import PARAMS_UPDATE_PERIOD
|
||||
from openpilot.sunnypilot.selfdrive.car import interfaces as sunnypilot_interfaces
|
||||
from openpilot.sunnypilot.selfdrive.controls.lib.speed_limit import PCM_LONG_REQUIRED_MAX_SET_SPEED
|
||||
from openpilot.sunnypilot.selfdrive.controls.lib.speed_limit.common import Mode
|
||||
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
|
||||
|
||||
SpeedLimitAssistState = custom.LongitudinalPlanSP.SpeedLimit.AssistState
|
||||
|
||||
ALL_STATES = tuple(SpeedLimitAssistState.schema.enumerants.values())
|
||||
|
||||
SPEED_LIMITS = {
|
||||
'residential': 25 * CV.MPH_TO_MS, # 25 mph
|
||||
'city': 35 * CV.MPH_TO_MS, # 35 mph
|
||||
'highway': 65 * CV.MPH_TO_MS, # 65 mph
|
||||
'freeway': 80 * CV.MPH_TO_MS, # 80 mph
|
||||
}
|
||||
|
||||
DEFAULT_CAR = TOYOTA.TOYOTA_RAV4_TSS2
|
||||
|
||||
|
||||
@pytest.fixture
|
||||
def car_name(request):
|
||||
return getattr(request, "param", DEFAULT_CAR)
|
||||
|
||||
|
||||
@pytest.fixture(autouse=True)
|
||||
def set_car_name_on_instance(request, car_name):
|
||||
instance = getattr(request, "instance", None)
|
||||
if instance:
|
||||
instance.car_name = car_name
|
||||
|
||||
|
||||
class TestSpeedLimitAssist:
|
||||
|
||||
def setup_method(self, method):
|
||||
self.params = Params()
|
||||
self.reset_custom_params()
|
||||
self.events_sp = EventsSP()
|
||||
CI = self._setup_platform(self.car_name)
|
||||
self.sla = SpeedLimitAssist(CI.CP, CI.CP_SP)
|
||||
self.sla.pre_active_timer = int(PRE_ACTIVE_GUARD_PERIOD[self.sla.pcm_op_long] / DT_MDL)
|
||||
self.pcm_long_max_set_speed = PCM_LONG_REQUIRED_MAX_SET_SPEED[self.sla.is_metric][1] # use 80 MPH for now
|
||||
self.speed_conv = CV.MS_TO_KPH if self.sla.is_metric else CV.MS_TO_MPH
|
||||
|
||||
def teardown_method(self, method):
|
||||
self.reset_state()
|
||||
|
||||
def _setup_platform(self, car_name):
|
||||
CarInterface = interfaces[car_name]
|
||||
CP = CarInterface.get_non_essential_params(car_name)
|
||||
CP_SP = CarInterface.get_non_essential_params_sp(CP, car_name)
|
||||
CI = CarInterface(CP, CP_SP)
|
||||
CI.CP.openpilotLongitudinalControl = True # always assume it's openpilot longitudinal
|
||||
sunnypilot_interfaces.setup_interfaces(CI, self.params)
|
||||
return CI
|
||||
|
||||
def reset_custom_params(self):
|
||||
self.params.put("IsReleaseSpBranch", True, block=True)
|
||||
self.params.put("SpeedLimitMode", int(Mode.assist), block=True)
|
||||
self.params.put_bool("IsMetric", False, block=True)
|
||||
self.params.put("SpeedLimitOffsetType", 0, block=True)
|
||||
self.params.put("SpeedLimitValueOffset", 0, block=True)
|
||||
|
||||
def reset_state(self):
|
||||
self.sla.state = SpeedLimitAssistState.disabled
|
||||
self.sla.frame = -1
|
||||
self.sla.last_op_engaged_frame = 0
|
||||
self.sla.op_engaged = False
|
||||
self.sla.op_engaged_prev = False
|
||||
self.sla._speed_limit = 0.
|
||||
self.sla.speed_limit_prev = 0.
|
||||
self.sla.last_valid_speed_limit_offsetted = 0.
|
||||
self.sla._distance = 0.
|
||||
self.events_sp.clear()
|
||||
|
||||
def initialize_active_state(self, initialize_v_cruise):
|
||||
self.sla.state = SpeedLimitAssistState.active
|
||||
self.sla.v_cruise_cluster = initialize_v_cruise
|
||||
self.sla.v_cruise_cluster_prev = initialize_v_cruise
|
||||
self.sla.prev_v_cruise_cluster_conv = round(initialize_v_cruise * self.speed_conv)
|
||||
|
||||
def test_initial_state(self):
|
||||
assert self.sla.state == SpeedLimitAssistState.disabled
|
||||
assert not self.sla.is_enabled
|
||||
assert not self.sla.is_active
|
||||
assert V_CRUISE_UNSET == self.sla.get_v_target_from_control()
|
||||
|
||||
@pytest.mark.parametrize("car_name", [RIVIAN.RIVIAN_R1, TESLA.TESLA_MODEL_Y], indirect=True)
|
||||
def test_disallowed_brands(self, car_name):
|
||||
"""
|
||||
Speed Limit Assist is disabled for the following brands and conditions:
|
||||
- All Tesla and is a release branch;
|
||||
- All Rivian
|
||||
"""
|
||||
assert not self.sla.enabled
|
||||
|
||||
# stay disallowed even when the param may have changed from somewhere else
|
||||
self.params.put("SpeedLimitMode", int(Mode.assist), block=True)
|
||||
for _ in range(int(PARAMS_UPDATE_PERIOD / DT_MDL)):
|
||||
self.sla.update(True, False, SPEED_LIMITS['city'], 0, SPEED_LIMITS['highway'], SPEED_LIMITS['city'],
|
||||
SPEED_LIMITS['city'], True, 0, self.events_sp)
|
||||
assert not self.sla.enabled
|
||||
|
||||
def test_disabled(self):
|
||||
self.params.put("SpeedLimitMode", int(Mode.off), block=True)
|
||||
for _ in range(int(10. / DT_MDL)):
|
||||
self.sla.update(True, False, SPEED_LIMITS['city'], 0, SPEED_LIMITS['highway'], SPEED_LIMITS['city'], SPEED_LIMITS['city'], True, 0, self.events_sp)
|
||||
assert self.sla.state == SpeedLimitAssistState.disabled
|
||||
|
||||
def test_transition_disabled_to_preactive(self):
|
||||
for _ in range(int(3. / DT_MDL)):
|
||||
self.sla.update(True, False, SPEED_LIMITS['city'], 0, SPEED_LIMITS['highway'], SPEED_LIMITS['city'], SPEED_LIMITS['city'], True, 0, self.events_sp)
|
||||
assert self.sla.state == SpeedLimitAssistState.preActive
|
||||
assert self.sla.is_enabled and not self.sla.is_active
|
||||
|
||||
def test_transition_disabled_to_pending_no_speed_limit_not_max_initial_set_speed(self):
|
||||
for _ in range(int(3. / DT_MDL)):
|
||||
self.sla.update(True, False, SPEED_LIMITS['highway'], 0, SPEED_LIMITS['city'], 0, 0, False, 0, self.events_sp)
|
||||
assert self.sla.state == SpeedLimitAssistState.pending
|
||||
assert self.sla.is_enabled and not self.sla.is_active
|
||||
|
||||
def test_preactive_to_active_with_max_speed_confirmation(self):
|
||||
self.sla.state = SpeedLimitAssistState.preActive
|
||||
self.sla.update(True, False, SPEED_LIMITS['city'], 0, self.pcm_long_max_set_speed, SPEED_LIMITS['highway'],
|
||||
SPEED_LIMITS['highway'], True, 0, self.events_sp)
|
||||
assert self.sla.state == SpeedLimitAssistState.active
|
||||
assert self.sla.is_enabled and self.sla.is_active
|
||||
assert self.sla.output_v_target == SPEED_LIMITS['highway']
|
||||
|
||||
def test_preactive_timeout_to_inactive(self):
|
||||
self.sla.state = SpeedLimitAssistState.preActive
|
||||
self.sla.update(True, False, SPEED_LIMITS['city'], 0, SPEED_LIMITS['highway'], SPEED_LIMITS['city'], SPEED_LIMITS['city'], True, 0, self.events_sp)
|
||||
|
||||
for _ in range(int(PRE_ACTIVE_GUARD_PERIOD[self.sla.pcm_op_long] / DT_MDL)):
|
||||
self.sla.update(True, False, SPEED_LIMITS['city'], 0, SPEED_LIMITS['highway'], SPEED_LIMITS['city'], SPEED_LIMITS['city'], True, 0, self.events_sp)
|
||||
assert self.sla.state == SpeedLimitAssistState.inactive
|
||||
|
||||
def test_preactive_to_pending_no_speed_limit(self):
|
||||
self.sla.state = SpeedLimitAssistState.preActive
|
||||
self.sla.update(True, False, SPEED_LIMITS['highway'], 0, self.pcm_long_max_set_speed, 0, 0, False, 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.v_cruise_cluster_prev = self.pcm_long_max_set_speed
|
||||
self.sla.prev_v_cruise_cluster_conv = round(self.pcm_long_max_set_speed * self.speed_conv)
|
||||
|
||||
self.sla.update(True, False, SPEED_LIMITS['highway'], 0, self.pcm_long_max_set_speed,
|
||||
SPEED_LIMITS['highway'], SPEED_LIMITS['highway'], True, 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.v_cruise_cluster_prev = self.pcm_long_max_set_speed
|
||||
self.sla.prev_v_cruise_cluster_conv = round(self.pcm_long_max_set_speed * self.speed_conv)
|
||||
|
||||
self.sla.update(True, False, SPEED_LIMITS['highway'] + 5, 0, self.pcm_long_max_set_speed,
|
||||
SPEED_LIMITS['highway'], SPEED_LIMITS['highway'], True, 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(self.pcm_long_max_set_speed)
|
||||
|
||||
self.sla.update(True, False, SPEED_LIMITS['highway'] + 2, 0, self.pcm_long_max_set_speed, SPEED_LIMITS['highway'],
|
||||
SPEED_LIMITS['highway'], True, 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 = self.pcm_long_max_set_speed
|
||||
self.sla.prev_v_cruise_cluster_conv = round(self.pcm_long_max_set_speed * self.speed_conv)
|
||||
|
||||
self.sla.update(True, False, SPEED_LIMITS['city'], 0, self.pcm_long_max_set_speed, SPEED_LIMITS['highway'],
|
||||
SPEED_LIMITS['highway'], True, 0, self.events_sp)
|
||||
assert self.sla.state == SpeedLimitAssistState.active
|
||||
|
||||
def test_manual_cruise_change_detection(self):
|
||||
self.sla.state = SpeedLimitAssistState.active
|
||||
expected_cruise = SPEED_LIMITS['highway']
|
||||
self.sla.v_cruise_cluster_prev = expected_cruise
|
||||
|
||||
different_cruise = SPEED_LIMITS['highway'] + 5
|
||||
self.sla.update(True, False, SPEED_LIMITS['city'], 0, different_cruise, SPEED_LIMITS['city'], SPEED_LIMITS['city'], True, 0, self.events_sp)
|
||||
assert self.sla.state == SpeedLimitAssistState.inactive
|
||||
|
||||
# TODO-SP: test lower CST cases
|
||||
def test_rapid_speed_limit_changes(self):
|
||||
self.initialize_active_state(self.pcm_long_max_set_speed)
|
||||
speed_limits = [SPEED_LIMITS['highway'], SPEED_LIMITS['freeway']]
|
||||
|
||||
for _, speed_limit in enumerate(speed_limits):
|
||||
self.sla.update(True, False, speed_limit, 0, self.pcm_long_max_set_speed, speed_limit, speed_limit, True, 0, self.events_sp)
|
||||
assert self.sla.state in ACTIVE_STATES
|
||||
|
||||
def test_invalid_speed_limits_handling(self):
|
||||
self.initialize_active_state(self.pcm_long_max_set_speed)
|
||||
|
||||
invalid_limits = [-10, 0, 200 * CV.MPH_TO_MS]
|
||||
|
||||
for invalid_limit in invalid_limits:
|
||||
self.sla.update(True, False, SPEED_LIMITS['city'], 0, self.pcm_long_max_set_speed, invalid_limit, SPEED_LIMITS['city'], True, 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(self.pcm_long_max_set_speed)
|
||||
old_speed_limit = SPEED_LIMITS['city']
|
||||
|
||||
self.sla.update(True, False, SPEED_LIMITS['city'], 0, self.pcm_long_max_set_speed, 0, old_speed_limit, True, 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 = self.pcm_long_max_set_speed
|
||||
self.sla.prev_v_cruise_cluster_conv = round(self.pcm_long_max_set_speed * self.speed_conv)
|
||||
|
||||
distance = 100.0
|
||||
current_speed = SPEED_LIMITS['freeway']
|
||||
target_speed = SPEED_LIMITS['highway']
|
||||
|
||||
self.sla.update(True, False, current_speed, 0, self.pcm_long_max_set_speed, target_speed, target_speed, True, 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(self.pcm_long_max_set_speed)
|
||||
|
||||
self.sla.update(False, False, SPEED_LIMITS['city'], 0, self.pcm_long_max_set_speed, SPEED_LIMITS['city'],
|
||||
SPEED_LIMITS['city'], True, 0, self.events_sp)
|
||||
assert self.sla.state == SpeedLimitAssistState.disabled
|
||||
assert self.sla.output_v_target == V_CRUISE_UNSET
|
||||
|
||||
def test_maintain_states_with_no_changes(self):
|
||||
"""Test that states are maintained when no significant changes occur"""
|
||||
test_states = [
|
||||
SpeedLimitAssistState.preActive,
|
||||
SpeedLimitAssistState.pending,
|
||||
SpeedLimitAssistState.active,
|
||||
SpeedLimitAssistState.adapting
|
||||
]
|
||||
|
||||
for state in test_states:
|
||||
self.sla.state = state
|
||||
self.sla.op_engaged = True
|
||||
|
||||
initial_state = state
|
||||
|
||||
self.sla.update(True, False, SPEED_LIMITS['city'], 0, self.pcm_long_max_set_speed, SPEED_LIMITS['city'], SPEED_LIMITS['city'], True, 0, self.events_sp)
|
||||
|
||||
assert self.sla.state in ALL_STATES # Sanity check
|
||||
|
||||
if initial_state == SpeedLimitAssistState.preActive:
|
||||
assert self.sla.state in [SpeedLimitAssistState.preActive, SpeedLimitAssistState.active]
|
||||
elif initial_state in ACTIVE_STATES:
|
||||
assert self.sla.state in ACTIVE_STATES
|
||||
+144
@@ -0,0 +1,144 @@
|
||||
"""
|
||||
Copyright (c) 2021-, Haibin Wen, sunnypilot, and a number of other contributors.
|
||||
|
||||
This file is part of sunnypilot and is licensed under the MIT License.
|
||||
See the LICENSE.md file in the root directory for more details.
|
||||
"""
|
||||
import random
|
||||
import time
|
||||
|
||||
import pytest
|
||||
from pytest_mock import MockerFixture
|
||||
|
||||
from openpilot.cereal import custom
|
||||
from openpilot.sunnypilot.selfdrive.controls.lib.speed_limit import LIMIT_MAX_MAP_DATA_AGE
|
||||
|
||||
from openpilot.sunnypilot.selfdrive.controls.lib.speed_limit.speed_limit_resolver import SpeedLimitResolver, ALL_SOURCES
|
||||
from openpilot.sunnypilot.selfdrive.controls.lib.speed_limit.common import Policy
|
||||
|
||||
SpeedLimitSource = custom.LongitudinalPlanSP.SpeedLimit.Source
|
||||
|
||||
|
||||
def create_mock(properties, mocker: MockerFixture):
|
||||
mock = mocker.MagicMock()
|
||||
for _property, value in properties.items():
|
||||
setattr(mock, _property, value)
|
||||
return mock
|
||||
|
||||
|
||||
def setup_sm_mock(mocker: MockerFixture):
|
||||
cruise_speed_limit = random.uniform(0, 120)
|
||||
live_map_data_limit = random.uniform(0, 120)
|
||||
|
||||
car_state = create_mock({
|
||||
'gasPressed': False,
|
||||
'brakePressed': False,
|
||||
'standstill': False,
|
||||
}, mocker)
|
||||
car_state_sp = create_mock({
|
||||
'speedLimit': cruise_speed_limit,
|
||||
}, mocker)
|
||||
live_map_data = create_mock({
|
||||
'speedLimit': live_map_data_limit,
|
||||
'speedLimitValid': True,
|
||||
'speedLimitAhead': 0.,
|
||||
'speedLimitAheadValid': 0.,
|
||||
'speedLimitAheadDistance': 0.,
|
||||
}, mocker)
|
||||
gps_data = create_mock({
|
||||
'unixTimestampMillis': time.monotonic() * 1e3,
|
||||
}, mocker)
|
||||
sm_mock = mocker.MagicMock()
|
||||
sm_mock.__getitem__.side_effect = lambda key: {
|
||||
'carState': car_state,
|
||||
'liveMapDataSP': live_map_data,
|
||||
'carStateSP': car_state_sp,
|
||||
'gpsLocation': gps_data,
|
||||
}[key]
|
||||
return sm_mock
|
||||
|
||||
|
||||
parametrized_policies = pytest.mark.parametrize(
|
||||
"policy, sm_key, function_key", [
|
||||
(Policy.car_state_only, 'carStateSP', SpeedLimitSource.car),
|
||||
(Policy.car_state_priority, 'carStateSP', SpeedLimitSource.car),
|
||||
(Policy.map_data_only, 'liveMapDataSP', SpeedLimitSource.map),
|
||||
(Policy.map_data_priority, 'liveMapDataSP', SpeedLimitSource.map),
|
||||
],
|
||||
ids=lambda val: val.name if hasattr(val, 'name') else str(val)
|
||||
)
|
||||
|
||||
|
||||
@pytest.mark.parametrize("resolver_class", [SpeedLimitResolver])
|
||||
class TestSpeedLimitResolverValidation:
|
||||
|
||||
@pytest.mark.parametrize("policy", list(Policy), ids=lambda policy: policy.name)
|
||||
def test_initial_state(self, resolver_class, policy):
|
||||
resolver = resolver_class()
|
||||
resolver.policy = policy
|
||||
for source in ALL_SOURCES:
|
||||
if source in resolver.limit_solutions:
|
||||
assert resolver.limit_solutions[source] == 0.
|
||||
assert resolver.distance_solutions[source] == 0.
|
||||
|
||||
@parametrized_policies
|
||||
def test_resolver(self, resolver_class, policy, sm_key, function_key, mocker: MockerFixture):
|
||||
resolver = resolver_class()
|
||||
resolver.policy = policy
|
||||
sm_mock = setup_sm_mock(mocker)
|
||||
source_speed_limit = sm_mock[sm_key].speedLimit
|
||||
|
||||
# Assert the resolver
|
||||
resolver.update(source_speed_limit, sm_mock)
|
||||
assert resolver.speed_limit == source_speed_limit
|
||||
assert resolver.source == ALL_SOURCES[function_key]
|
||||
|
||||
def test_resolver_combined(self, resolver_class, mocker: MockerFixture):
|
||||
resolver = resolver_class()
|
||||
resolver.policy = Policy.combined
|
||||
sm_mock = setup_sm_mock(mocker)
|
||||
socket_to_source = {'carStateSP': SpeedLimitSource.car, 'liveMapDataSP': SpeedLimitSource.map}
|
||||
minimum_key, minimum_speed_limit = min(
|
||||
((key, sm_mock[key].speedLimit) for key in
|
||||
socket_to_source.keys()), key=lambda x: x[1])
|
||||
|
||||
# Assert the resolver
|
||||
resolver.update(minimum_speed_limit, sm_mock)
|
||||
assert resolver.speed_limit == minimum_speed_limit
|
||||
assert resolver.source == socket_to_source[minimum_key]
|
||||
|
||||
@parametrized_policies
|
||||
def test_parser(self, resolver_class, policy, sm_key, function_key, mocker: MockerFixture):
|
||||
resolver = resolver_class()
|
||||
resolver.policy = policy
|
||||
sm_mock = setup_sm_mock(mocker)
|
||||
source_speed_limit = sm_mock[sm_key].speedLimit
|
||||
|
||||
# Assert the parsing
|
||||
resolver.update(source_speed_limit, sm_mock)
|
||||
assert resolver.limit_solutions[ALL_SOURCES[function_key]] == source_speed_limit
|
||||
assert resolver.distance_solutions[ALL_SOURCES[function_key]] == 0.
|
||||
|
||||
@pytest.mark.parametrize("policy", list(Policy), ids=lambda policy: policy.name)
|
||||
def test_resolve_interaction_in_update(self, resolver_class, policy, mocker: MockerFixture):
|
||||
v_ego = 50
|
||||
resolver = resolver_class()
|
||||
resolver.policy = policy
|
||||
|
||||
sm_mock = setup_sm_mock(mocker)
|
||||
resolver.update(v_ego, sm_mock)
|
||||
|
||||
# After resolution
|
||||
assert resolver.speed_limit is not None
|
||||
assert resolver.distance is not None
|
||||
assert resolver.source is not None
|
||||
|
||||
@pytest.mark.parametrize("policy", list(Policy), ids=lambda policy: policy.name)
|
||||
def test_old_map_data_ignored(self, resolver_class, policy, mocker: MockerFixture):
|
||||
resolver = resolver_class()
|
||||
resolver.policy = policy
|
||||
sm_mock = mocker.MagicMock()
|
||||
sm_mock['gpsLocation'].unixTimestampMillis = (time.monotonic() - 2 * LIMIT_MAX_MAP_DATA_AGE) * 1e3
|
||||
resolver._get_from_map_data(sm_mock)
|
||||
assert resolver.limit_solutions[SpeedLimitSource.map] == 0.
|
||||
assert resolver.distance_solutions[SpeedLimitSource.map] == 0.
|
||||
Reference in new issue
Block a user