sunnypilot v2026.002.000

version: sunnypilot v2026.002.000 (staging)
date: 2026-07-21T07:44:02
master commit: 2c334ede443d7391d27575af8c854a095ba702a8
This commit is contained in:
github-actions[bot] committed 2026-07-21 07:44:02 +00:00
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.
@@ -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
@@ -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.