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,9 @@
"""
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.common.constants import CV
MIN_V = 20 * CV.KPH_TO_MS # Do not operate under 20 km/h
@@ -0,0 +1,261 @@
import json
import math
import platform
from openpilot.cereal import custom
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.navd.helpers import coordinate_from_param, Coordinate
from openpilot.sunnypilot.selfdrive.controls.lib.smart_cruise_control import MIN_V
MapState = VisionState = custom.LongitudinalPlanSP.SmartCruiseControl.MapState
ACTIVE_STATES = (MapState.turning, )
ENABLED_STATES = (MapState.enabled, MapState.overriding, *ACTIVE_STATES)
R = 6373000.0 # approximate radius of earth in meters
TO_RADIANS = math.pi / 180
TO_DEGREES = 180 / math.pi
TARGET_JERK = -0.6 # m/s^3 There's some jounce limits that are not consistent so we're fudging this some
TARGET_ACCEL = -1.2 # m/s^2 should match up with the long planner limit
TARGET_OFFSET = 1.0 # seconds - This controls how soon before the curve you reach the target velocity. It also helps
# reach the target velocity when inaccuracies in the distance modeling logic would cause overshoot.
# The value is multiplied against the target velocity to determine the additional distance. This is
# done to keep the distance calculations consistent but results in the offset actually being less
# time than specified depending on how much of a speed differential there is between v_ego and the
# target velocity.
def velocities_from_param(param: str, params: Params):
if params is None:
params = Params()
json_str = params.get(param)
if json_str is None:
return None
velocities = json.loads(json_str)
return velocities
def calculate_accel(t, target_jerk, a_ego):
return a_ego + target_jerk * t
def calculate_velocity(t, target_jerk, a_ego, v_ego):
return v_ego + a_ego * t + target_jerk/2 * (t ** 2)
def calculate_distance(t, target_jerk, a_ego, v_ego):
return t * v_ego + a_ego/2 * (t ** 2) + target_jerk/6 * (t ** 3)
# points should be in radians
# output is meters
def distance_to_point(ax, ay, bx, by):
a = math.sin((bx-ax)/2)*math.sin((bx-ax)/2) + math.cos(ax) * math.cos(bx)*math.sin((by-ay)/2)*math.sin((by-ay)/2)
c = 2 * math.atan2(math.sqrt(a), math.sqrt(1-a))
return R * c # in meters
class SmartCruiseControlMap:
v_target: float = 0
a_target: float = 0.
v_ego: float = 0.
a_ego: float = 0.
output_v_target: float = V_CRUISE_UNSET
output_a_target: float = 0.
def __init__(self):
self.params = Params()
self.mem_params = Params("/dev/shm/params") if platform.system() != "Darwin" else self.params
self.enabled = self.params.get_bool("SmartCruiseControlMap")
self.long_enabled = False
self.long_override = False
self.is_enabled = False
self.is_active = False
self.state = MapState.disabled
self.v_cruise = 0
self.target_lat = 0.0
self.target_lon = 0.0
self.frame = -1
self.last_position = coordinate_from_param("LastGPSPosition", self.mem_params) or Coordinate(0.0, 0.0)
self.target_velocities = velocities_from_param("MapTargetVelocities", self.mem_params) or []
def get_v_target_from_control(self) -> float:
if self.is_active:
return max(self.v_target, MIN_V)
return V_CRUISE_UNSET
def get_a_target_from_control(self) -> float:
return self.a_ego
def update_params(self):
if self.frame % int(PARAMS_UPDATE_PERIOD / DT_MDL) == 0:
self.enabled = self.params.get_bool("SmartCruiseControlMap")
def update_calculations(self) -> None:
self.last_position = coordinate_from_param("LastGPSPosition", self.mem_params) or Coordinate(0.0, 0.0)
lat = self.last_position.latitude
lon = self.last_position.longitude
self.target_velocities = velocities_from_param("MapTargetVelocities", self.mem_params) or []
if self.last_position is None or self.target_velocities is None:
return
min_dist = 1000
min_idx = 0
distances = []
# find our location in the path
for i in range(len(self.target_velocities)):
target_velocity = self.target_velocities[i]
tlat = target_velocity["latitude"]
tlon = target_velocity["longitude"]
d = distance_to_point(lat * TO_RADIANS, lon * TO_RADIANS, tlat * TO_RADIANS, tlon * TO_RADIANS)
distances.append(d)
if d < min_dist:
min_dist = d
min_idx = i
# only look at values from our current position forward
forward_points = self.target_velocities[min_idx:]
forward_distances = distances[min_idx:]
# find velocities that we are within the distance we need to adjust for
valid_velocities = []
for i in range(len(forward_points)):
target_velocity = forward_points[i]
tlat = target_velocity["latitude"]
tlon = target_velocity["longitude"]
tv = target_velocity["velocity"]
if tv > self.v_ego:
continue
d = forward_distances[i]
a_diff = (self.a_ego - TARGET_ACCEL)
accel_t = abs(a_diff / TARGET_JERK)
min_accel_v = calculate_velocity(accel_t, TARGET_JERK, self.a_ego, self.v_ego)
max_d = 0
if tv > min_accel_v:
# calculate time needed based on target jerk
a = 0.5 * TARGET_JERK
b = self.a_ego
c = self.v_ego - tv
t_a = -1 * ((b**2 - 4 * a * c) ** 0.5 + b) / 2 * a
t_b = ((b**2 - 4 * a * c) ** 0.5 - b) / 2 * a
if not isinstance(t_a, complex) and t_a > 0:
t = t_a
else:
t = t_b
if isinstance(t, complex):
continue
max_d = max_d + calculate_distance(t, TARGET_JERK, self.a_ego, self.v_ego)
else:
t = accel_t
max_d = calculate_distance(t, TARGET_JERK, self.a_ego, self.v_ego)
# calculate additional time needed based on target accel
t = abs((min_accel_v - tv) / TARGET_ACCEL)
max_d += calculate_distance(t, 0, TARGET_ACCEL, min_accel_v)
if d < max_d + tv * TARGET_OFFSET:
valid_velocities.append((float(tv), tlat, tlon))
# Find the smallest velocity we need to adjust for
min_v = 100.0
target_lat = 0.0
target_lon = 0.0
for tv, lat, lon in valid_velocities:
if tv < min_v:
min_v = tv
target_lat = lat
target_lon = lon
if self.v_target < min_v and not (self.target_lat == 0 and self.target_lon == 0):
for i in range(len(forward_points)):
target_velocity = forward_points[i]
tlat = target_velocity["latitude"]
tlon = target_velocity["longitude"]
tv = target_velocity["velocity"]
if tv > self.v_ego:
continue
if tlat == self.target_lat and tlon == self.target_lon and tv == self.v_target:
return
# not found so let's reset
self.v_target = 0.0
self.target_lat = 0.0
self.target_lon = 0.0
self.v_target = min_v
self.target_lat = target_lat
self.target_lon = target_lon
def _update_state_machine(self) -> tuple[bool, bool]:
# ENABLED, TURNING
if self.state != MapState.disabled:
if not self.long_enabled or not self.enabled:
self.state = MapState.disabled
elif self.long_override:
self.state = MapState.overriding
else:
# ENABLED
if self.state == MapState.enabled:
if self.v_cruise > self.v_target != 0:
self.state = MapState.turning
# TURNING
elif self.state == MapState.turning:
if self.v_cruise <= self.v_target or self.v_target == 0:
self.state = MapState.enabled
# OVERRIDING
elif self.state == MapState.overriding:
if not self.long_override:
if self.v_cruise > self.v_target != 0:
self.state = MapState.turning
else:
self.state = MapState.enabled
# DISABLED
elif self.state == MapState.disabled:
if self.long_enabled and self.enabled:
if self.long_override:
self.state = MapState.overriding
else:
self.state = MapState.enabled
enabled = self.state in ENABLED_STATES
active = self.state in ACTIVE_STATES
return enabled, active
def update(self, long_enabled: bool, long_override: bool, v_ego, a_ego, v_cruise) -> None:
self.long_enabled = long_enabled
self.long_override = long_override
self.v_ego = v_ego
self.a_ego = a_ego
self.v_cruise = v_cruise
self.update_params()
self.update_calculations()
self.is_enabled, self.is_active = self._update_state_machine()
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,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.
"""
import openpilot.cereal.messaging as messaging
from openpilot.sunnypilot.selfdrive.controls.lib.smart_cruise_control.vision_controller import SmartCruiseControlVision
from openpilot.sunnypilot.selfdrive.controls.lib.smart_cruise_control.map_controller import SmartCruiseControlMap
class SmartCruiseControl:
def __init__(self):
self.vision = SmartCruiseControlVision()
self.map = SmartCruiseControlMap()
def update(self, sm: messaging.SubMaster, long_enabled: bool, long_override: bool, v_ego: float, a_ego: float, v_cruise: float) -> None:
self.map.update(long_enabled, long_override, v_ego, a_ego, v_cruise)
self.vision.update(sm, long_enabled, long_override, v_ego, a_ego, v_cruise)
@@ -0,0 +1,58 @@
"""
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 platform
from openpilot.cereal import custom
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.selfdrive.controls.lib.smart_cruise_control.map_controller import SmartCruiseControlMap
MapState = VisionState = custom.LongitudinalPlanSP.SmartCruiseControl.MapState
class TestSmartCruiseControlMap:
def setup_method(self):
self.params = Params()
self.mem_params = Params("/dev/shm/params") if platform.system() != "Darwin" else self.params
self.reset_params()
self.scc_m = SmartCruiseControlMap()
def reset_params(self):
self.params.put_bool("SmartCruiseControlMap", True, block=True)
# TODO-SP: mock data from gpsLocation
self.params.put("LastGPSPosition", "{}", block=True)
self.params.put("MapTargetVelocities", "{}", block=True)
def test_initial_state(self):
assert self.scc_m.state == VisionState.disabled
assert not self.scc_m.is_active
assert self.scc_m.output_v_target == V_CRUISE_UNSET
assert self.scc_m.output_a_target == 0.
def test_system_disabled(self):
self.params.put_bool("SmartCruiseControlMap", False, block=True)
self.scc_m.enabled = self.params.get_bool("SmartCruiseControlMap")
for _ in range(int(10. / DT_MDL)):
self.scc_m.update(True, False, 0., 0., 0.)
assert self.scc_m.state == VisionState.disabled
assert not self.scc_m.is_active
def test_disabled(self):
for _ in range(int(10. / DT_MDL)):
self.scc_m.update(False, False, 0., 0., 0.)
assert self.scc_m.state == VisionState.disabled
def test_transition_disabled_to_enabled(self):
for _ in range(int(10. / DT_MDL)):
self.scc_m.update(True, False, 0., 0., 0.)
assert self.scc_m.state == VisionState.enabled
# TODO-SP: mock data from modelV2 to test other states
@@ -0,0 +1,214 @@
"""
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 numpy as np
import pytest
import openpilot.cereal.messaging as messaging
from openpilot.cereal import custom, log
from openpilot.common.params import Params
from openpilot.common.realtime import DT_MDL
from openpilot.selfdrive.car.cruise import V_CRUISE_UNSET
from openpilot.selfdrive.modeld.constants import ModelConstants
from openpilot.sunnypilot.selfdrive.controls.lib.smart_cruise_control import MIN_V
from openpilot.sunnypilot.selfdrive.controls.lib.smart_cruise_control.vision_controller import SmartCruiseControlVision, _ENTERING_PRED_LAT_ACC_TH
VisionState = custom.LongitudinalPlanSP.SmartCruiseControl.VisionState
def _th_above_f32(th: float) -> float:
"""
Return the next representable float32 *above* `th`.
This avoids flaky comparisons around thresholds due to float32 rounding.
"""
th32 = np.float32(th)
above32 = np.nextafter(th32, np.float32(np.inf), dtype=np.float32)
return float(above32)
def _build_single_spike_filtered(n: int, base: float = 1.0) -> np.ndarray:
"""
Create an array where max() is >= threshold but p97 is < threshold.
This demonstrates the behavior difference vs np.amax().
Note: We intentionally construct using float32-representable values to match
the data path through cereal/capnp.
"""
th = float(_ENTERING_PRED_LAT_ACC_TH)
th32 = float(np.float32(th))
# numpy percentile default is linear interpolation: idx=(n-1)*p/100
idx = (n - 1) * 0.97
w = float(idx - np.floor(idx))
base32 = float(np.float32(base))
# Choose spike so that p97 = base + w*(spike-base) < th
# -> spike < base + (th-base)/w. Use a margin (0.9) and ensure spike >= th.
if w == 0.0:
spike = th32 + 1.0
else:
spike = base32 + (th32 - base32) / w * 0.9
spike = max(spike, th32 + 0.01)
arr = np.full(n, base32, dtype=np.float32)
arr[-1] = np.float32(spike)
return arr
def generate_modelV2():
model = messaging.new_message('modelV2')
position = log.XYZTData.new_message()
speed = 30
position.x = [float(x) for x in (speed + 0.5) * np.array(ModelConstants.T_IDXS)]
model.modelV2.position = position
orientation = log.XYZTData.new_message()
curvature = 0.05
orientation.x = [float(curvature) for _ in ModelConstants.T_IDXS]
orientation.y = [0.0 for _ in ModelConstants.T_IDXS]
model.modelV2.orientation = orientation
orientationRate = log.XYZTData.new_message()
orientationRate.z = [float(z) for z in ModelConstants.T_IDXS]
model.modelV2.orientationRate = orientationRate
velocity = log.XYZTData.new_message()
velocity.x = [float(x) for x in (speed + 0.5) * np.ones_like(ModelConstants.T_IDXS)]
velocity.x[0] = float(speed) # always start at current speed
model.modelV2.velocity = velocity
acceleration = log.XYZTData.new_message()
acceleration.x = [float(x) for x in np.zeros_like(ModelConstants.T_IDXS)]
acceleration.y = [float(y) for y in np.zeros_like(ModelConstants.T_IDXS)]
model.modelV2.acceleration = acceleration
return model
def generate_carState():
car_state = messaging.new_message('carState')
speed = 30
v_cruise = 50
car_state.carState.vEgo = float(speed)
car_state.carState.standstill = False
car_state.carState.vCruise = float(v_cruise * 3.6)
return car_state
def generate_controlsState():
controls_state = messaging.new_message('controlsState')
controls_state.controlsState.curvature = 0.05
return controls_state
class TestSmartCruiseControlVision:
def setup_method(self):
self.params = Params()
self.reset_params()
self.scc_v = SmartCruiseControlVision()
mdl = generate_modelV2()
cs = generate_carState()
controls_state = generate_controlsState()
self.sm = {'modelV2': mdl.modelV2, 'carState': cs.carState, 'controlsState': controls_state.controlsState}
def reset_params(self):
self.params.put_bool("SmartCruiseControlVision", True, block=True)
def test_initial_state(self):
assert self.scc_v.state == VisionState.disabled
assert not self.scc_v.is_active
assert self.scc_v.output_v_target == V_CRUISE_UNSET
assert self.scc_v.output_a_target == 0.
def test_system_disabled(self):
self.params.put_bool("SmartCruiseControlVision", False, block=True)
self.scc_v.enabled = self.params.get_bool("SmartCruiseControlVision")
for _ in range(int(10. / DT_MDL)):
self.scc_v.update(self.sm, True, False, 0., 0., 0.)
assert self.scc_v.state == VisionState.disabled
assert not self.scc_v.is_active
def test_disabled(self):
for _ in range(int(10. / DT_MDL)):
self.scc_v.update(self.sm, False, False, 0., 0., 0.)
assert self.scc_v.state == VisionState.disabled
def test_transition_disabled_to_enabled(self):
for _ in range(int(10. / DT_MDL)):
self.scc_v.update(self.sm, True, False, 0., 0., 0.)
assert self.scc_v.state == VisionState.enabled
@pytest.mark.parametrize(
"case, should_enter",
[
("p97_just_above_threshold", True),
("single_spike_filtered", False),
("persistent_high_values", True),
],
ids=[
"p97>threshold_enters",
"single_spike_max_large_but_p97_below_threshold",
"high_values_persist_trigger_entering",
],
)
def test_max_pred_lat_acc_uses_p97_and_threshold(self, case, should_enter):
n = len(ModelConstants.T_IDXS)
th = float(_ENTERING_PRED_LAT_ACC_TH)
if case == "p97_just_above_threshold":
# Use the next representable float32 above threshold to avoid float32 rounding flakiness.
val = _th_above_f32(th)
pred_lat_accels = np.full(n, np.float32(val), dtype=np.float32)
elif case == "single_spike_filtered":
pred_lat_accels = _build_single_spike_filtered(n, base=1.0)
elif case == "persistent_high_values":
# Make enough "high" samples so p97 is driven by the persistent trend, not a single outlier.
high_count = max(2, int(np.ceil(n * 0.03)) + 1)
pred_lat_accels = np.full(n, np.float32(1.0), dtype=np.float32)
pred_lat_accels[-high_count:] = np.float32(2.0)
pred_lat_accels[-1] = np.float32(8.0) # keep one big outlier too
else:
raise AssertionError(f"Unknown case: {case}")
# Override model predictions so:
# predicted_lat_accels = abs(orientationRate.z) * velocity.x == pred_lat_accels
mdl = generate_modelV2()
mdl.modelV2.velocity.x = [1.0 for _ in range(n)]
mdl.modelV2.orientationRate.z = [float(x) for x in pred_lat_accels]
self.sm["modelV2"] = mdl.modelV2
v_ego = float(MIN_V + 5.0)
# 1st update: disabled -> enabled
self.scc_v.update(self.sm, True, False, v_ego, 0.0, 0.0)
# 2nd update: evaluate entering condition from enabled state
self.scc_v.update(self.sm, True, False, v_ego, 0.0, 0.0)
# Controller does percentile on numpy float64 arrays (values already quantized by capnp),
# so compute expected in float64 to match behavior and avoid interpolation/rounding deltas.
expected_p97 = float(np.percentile(pred_lat_accels.astype(np.float64), 97))
# allow tiny numeric differences due to float conversions/interpolation
assert np.isclose(self.scc_v.max_pred_lat_acc, expected_p97, rtol=1e-6, atol=1e-5)
if should_enter:
# We assert entering primarily by state (this is the actual intended behavior).
assert self.scc_v.state == VisionState.entering
# Optional sanity: should be >= threshold with some margin (since we used nextafter above threshold).
assert self.scc_v.max_pred_lat_acc > th
else:
# Difference vs np.amax(): max can be above threshold, but p97 stays below it.
assert float(np.max(pred_lat_accels)) >= th
assert self.scc_v.max_pred_lat_acc < th
assert self.scc_v.state == VisionState.enabled
# TODO-SP: mock modelV2 data to test other states
@@ -0,0 +1,203 @@
"""
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 numpy as np
import openpilot.cereal.messaging as messaging
from openpilot.cereal import custom
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.controls.lib.smart_cruise_control import MIN_V
VisionState = custom.LongitudinalPlanSP.SmartCruiseControl.VisionState
ACTIVE_STATES = (VisionState.entering, VisionState.turning, VisionState.leaving)
ENABLED_STATES = (VisionState.enabled, VisionState.overriding, *ACTIVE_STATES)
_ENTERING_PRED_LAT_ACC_TH = 1.3 # Predicted Lat Acc threshold to trigger entering turn state.
_ABORT_ENTERING_PRED_LAT_ACC_TH = 1.1 # Predicted Lat Acc threshold to abort entering state if speed drops.
_TURNING_LAT_ACC_TH = 1.6 # Lat Acc threshold to trigger turning state.
_LEAVING_LAT_ACC_TH = 1.3 # Lat Acc threshold to trigger leaving turn state.
_FINISH_LAT_ACC_TH = 1.1 # Lat Acc threshold to trigger the end of the turn cycle.
_A_LAT_REG_MAX = 2. # Maximum lateral acceleration
_NO_OVERSHOOT_TIME_HORIZON = 4. # s. Time to use for velocity desired based on a_target when not overshooting.
# Lookup table for the minimum smooth deceleration during the ENTERING state
# depending on the actual maximum absolute lateral acceleration predicted on the turn ahead.
_ENTERING_SMOOTH_DECEL_V = [-0.2, -1.] # min decel value allowed on ENTERING state
_ENTERING_SMOOTH_DECEL_BP = [1.3, 3.] # absolute value of lat acc ahead
# Lookup table for the acceleration for the TURNING state
# depending on the current lateral acceleration of the vehicle.
_TURNING_ACC_V = [0.5, 0., -0.4] # acc value
_TURNING_ACC_BP = [1.5, 2.3, 3.] # absolute value of current lat acc
_LEAVING_ACC = 0.5 # Conformable acceleration to regain speed while leaving a turn.
class SmartCruiseControlVision:
v_target: float = 0
a_target: float = 0.
v_ego: float = 0.
a_ego: float = 0.
output_v_target: float = V_CRUISE_UNSET
output_a_target: float = 0.
def __init__(self):
self.params = Params()
self.frame = -1
self.long_enabled = False
self.long_override = False
self.is_enabled = False
self.is_active = False
self.enabled = self.params.get_bool("SmartCruiseControlVision")
self.v_cruise_setpoint = 0.
self.state = VisionState.disabled
self.current_lat_acc = 0.
self.max_pred_lat_acc = 0.
def get_a_target_from_control(self) -> float:
return self.a_target
def get_v_target_from_control(self) -> float:
if self.is_active:
return max(self.v_target, MIN_V) + self.a_target * _NO_OVERSHOOT_TIME_HORIZON
return V_CRUISE_UNSET
def _update_params(self) -> None:
if self.frame % int(PARAMS_UPDATE_PERIOD / DT_MDL) == 0:
self.enabled = self.params.get_bool("SmartCruiseControlVision")
def _update_calculations(self, sm: messaging.SubMaster) -> None:
if not self.long_enabled:
return
else:
rate_plan = np.array(np.abs(sm['modelV2'].orientationRate.z))
vel_plan = np.array(sm['modelV2'].velocity.x)
self.current_lat_acc = self.v_ego ** 2 * abs(sm['controlsState'].curvature)
# get the maximum lat accel from the model
predicted_lat_accels = rate_plan * vel_plan
self.max_pred_lat_acc = np.percentile(predicted_lat_accels, 97)
# get the maximum curve based on the current velocity
v_ego = max(self.v_ego, 0.1) # ensure a value greater than 0 for calculations
max_curve = self.max_pred_lat_acc / (v_ego**2)
# Get the target velocity for the maximum curve
self.v_target = (_A_LAT_REG_MAX / max_curve) ** 0.5
def _update_state_machine(self) -> tuple[bool, bool]:
# ENABLED, ENTERING, TURNING, LEAVING, OVERRIDING
if self.state != VisionState.disabled:
# longitudinal and feature disable always have priority in a non-disabled state
if not self.long_enabled or not self.enabled:
self.state = VisionState.disabled
elif self.long_override:
self.state = VisionState.overriding
else:
# ENABLED
if self.state == VisionState.enabled:
# Do not enter a turn control cycle if the speed is low.
if self.v_ego <= MIN_V:
pass
# If significant lateral acceleration is predicted ahead, then move to Entering turn state.
elif self.max_pred_lat_acc >= _ENTERING_PRED_LAT_ACC_TH:
self.state = VisionState.entering
# OVERRIDING
elif self.state == VisionState.overriding:
if not self.long_override:
self.state = VisionState.enabled
# ENTERING
elif self.state == VisionState.entering:
# Transition to Turning if current lateral acceleration is over the threshold.
if self.current_lat_acc >= _TURNING_LAT_ACC_TH:
self.state = VisionState.turning
# Abort if the predicted lateral acceleration drops
elif self.max_pred_lat_acc < _ABORT_ENTERING_PRED_LAT_ACC_TH:
self.state = VisionState.enabled
# TURNING
elif self.state == VisionState.turning:
# Transition to Leaving if current lateral acceleration drops below a threshold.
if self.current_lat_acc <= _LEAVING_LAT_ACC_TH:
self.state = VisionState.leaving
# LEAVING
elif self.state == VisionState.leaving:
# Transition back to Turning if current lateral acceleration goes back over the threshold.
if self.current_lat_acc >= _TURNING_LAT_ACC_TH:
self.state = VisionState.turning
# Finish if current lateral acceleration goes below a threshold.
elif self.current_lat_acc < _FINISH_LAT_ACC_TH:
self.state = VisionState.enabled
# DISABLED
elif self.state == VisionState.disabled:
if self.long_enabled and self.enabled:
if self.long_override:
self.state = VisionState.overriding
else:
self.state = VisionState.enabled
enabled = self.state in ENABLED_STATES
active = self.state in ACTIVE_STATES
return enabled, active
def _update_solution(self) -> float:
# DISABLED, ENABLED, OVERRIDING
if self.state not in ACTIVE_STATES:
# when not overshooting, calculate v_turn as the speed at the prediction horizon when following
# the smooth deceleration.
a_target = self.a_ego
# ENTERING
elif self.state == VisionState.entering:
# when not overshooting, target a smooth deceleration in preparation for a sharp turn to come.
a_target = np.interp(self.max_pred_lat_acc, _ENTERING_SMOOTH_DECEL_BP, _ENTERING_SMOOTH_DECEL_V)
# TURNING
elif self.state == VisionState.turning:
# When turning, we provide a target acceleration that is comfortable for the lateral acceleration felt.
a_target = np.interp(self.current_lat_acc, _TURNING_ACC_BP, _TURNING_ACC_V)
# LEAVING
elif self.state == VisionState.leaving:
# When leaving, we provide a comfortable acceleration to regain speed.
a_target = _LEAVING_ACC
else:
raise NotImplementedError(f"SCC-V state not supported: {self.state}")
return a_target
def update(self, sm: messaging.SubMaster, long_enabled: bool, long_override: bool, v_ego: float, a_ego: float,
v_cruise_setpoint: float) -> None:
self.long_enabled = long_enabled
self.long_override = long_override
self.v_ego = v_ego
self.a_ego = a_ego
self.v_cruise_setpoint = v_cruise_setpoint
self._update_params()
self._update_calculations(sm)
self.is_enabled, self.is_active = self._update_state_machine()
self.a_target = self._update_solution()
self.output_v_target = self.get_v_target_from_control()
self.output_a_target = self.get_a_target_from_control()
self.frame += 1