Compare commits

..

3 Commits

Author SHA1 Message Date
firestarsdog 8d246f3c0d yo 2026-08-26 09:46:20 -04:00
firestarsdog b2885f9f89 Jolene 2026-08-26 05:12:30 -04:00
firestarsdog 01599193df Aethergauge Pretty 2026-08-26 02:59:13 -04:00
29 changed files with 1286 additions and 1055 deletions
-5
View File
@@ -221,11 +221,6 @@ struct StarPilotPlan @0xf98d843bfd7004a3 {
trackingLead @36 :Bool;
stopSignConfirmed @37 :Bool;
pulseGlideCoasting @38 :Bool; # developer-only P&G phase for on-road status UI
# Curve Speed Controller diagnostics, for tuning and rollout validation
cscOverridden @39 :Bool; # driver cancelled this curve with RES+
cscLearnedLatAccel @40 :Float32; # learned comfort at the current curvature, before margin
cscBindingDistance @41 :Float32; # distance to the horizon point setting the target, m
approachStopLength @42 :Float32; # pre-commit distance to a detected stop, m; 0 when off
}
struct StarPilotRadarState @0xb86e6369214c01c8 {
Binary file not shown.

Before

Width:  |  Height:  |  Size: 13 KiB

After

Width:  |  Height:  |  Size: 11 KiB

+11 -75
View File
@@ -169,27 +169,12 @@ CURVATURE_HOLD_OPPOSITE_RELEASE = 0.01 # 1/m
CURVATURE_HOLD_CONFIRM_MIN = 0.003 # 1/m (~7 deg) of wound curvature before capture
CURVATURE_HOLD_CONFIRM_SWEPT = 0.6 # rad of heading swept this blinker cycle; past this the push is exit-shaping, not initiation
# Suppress low-speed action spikes while the model's spatial path remains straight.
TWITCH_GUARD_MAX_SPEED = 4.0
TWITCH_GUARD_FADE_SPEED = 3.0
TWITCH_GUARD_DURATION = 1.5
TWITCH_GUARD_PLAN_RATIO = 4.0
TWITCH_GUARD_FLOOR = 0.002
TWITCH_GUARD_STRAIGHT_LO = 0.005
TWITCH_GUARD_STRAIGHT_HI = 0.014
TWITCH_GUARD_MIN_REACH = 12.0
def _plan_circle_curvature(xs, ys, lookahead: float) -> float:
# Fit curvature through the plan point at the requested lookahead.
# curvature of the circle through the origin, tangent to the car's heading, passing
# through the plan point ~lookahead meters ahead: kappa = 2y / (x^2 + y^2)
px, py = 0.0, 0.0
for x, y in zip(xs, ys, strict=False):
try:
x, y = float(x), float(y)
except (TypeError, ValueError, OverflowError):
return 0.0
if not (math.isfinite(x) and math.isfinite(y)):
return 0.0
for x, y in zip(xs, ys):
px, py = x, y
if math.hypot(x, y) >= lookahead:
break
@@ -200,7 +185,11 @@ def _plan_circle_curvature(xs, ys, lookahead: float) -> float:
def _plan_dual_probe(model_v2, d_near: float, d_far: float) -> float:
# Use the smaller magnitude of near and far probes to avoid early turn bias.
# Min-magnitude of a near and a far circle fit. The far probe alone assumes the turn
# starts immediately, which over-winds wide turns whose arc begins several meters out
# (wide multi-lane lefts): the near probe reads ~straight there and only grows as the
# car approaches the arc, so the readout self-scales to the turn geometry. Sign
# disagreement means no coherent turn ahead: contribute nothing.
xs, ys = model_v2.position.x, model_v2.position.y
near = _plan_circle_curvature(xs, ys, d_near)
far = _plan_circle_curvature(xs, ys, d_far)
@@ -233,52 +222,8 @@ def get_plan_turn_onset_dist(model_v2) -> float:
def get_plan_reach(model_v2) -> float:
try:
xs = model_v2.position.x
return float(xs[-1]) if len(xs) else 0.0
except (AttributeError, IndexError, TypeError, ValueError, OverflowError):
return 0.0
def _plan_positions_are_finite(model_v2) -> bool:
try:
xs, ys = model_v2.position.x, model_v2.position.y
return len(xs) == len(ys) and all(
math.isfinite(float(x)) and math.isfinite(float(y)) for x, y in zip(xs, ys, strict=True)
)
except (AttributeError, TypeError, ValueError, OverflowError):
return False
def limit_curvature_to_plan(model_v2, curvature: float, v_ego: float) -> float:
if not (math.isfinite(curvature) and math.isfinite(v_ego)):
return curvature
if v_ego >= TWITCH_GUARD_MAX_SPEED or curvature == 0.0:
return curvature
if not _plan_positions_are_finite(model_v2):
return curvature
reach = get_plan_reach(model_v2)
if not math.isfinite(reach) or reach < TWITCH_GUARD_MIN_REACH:
return curvature
plan = abs(_plan_circle_curvature(model_v2.position.x, model_v2.position.y,
CURVATURE_HOLD_PLAN_LOOKAHEAD_FAR))
if not math.isfinite(plan):
return curvature
straightness = (plan - TWITCH_GUARD_STRAIGHT_LO) / (TWITCH_GUARD_STRAIGHT_HI - TWITCH_GUARD_STRAIGHT_LO)
limit = max(TWITCH_GUARD_PLAN_RATIO * plan * min(max(straightness, 0.0), 1.0), TWITCH_GUARD_FLOOR)
if abs(curvature) <= limit:
return curvature
fade = (TWITCH_GUARD_MAX_SPEED - v_ego) / (TWITCH_GUARD_MAX_SPEED - TWITCH_GUARD_FADE_SPEED)
fade = min(max(fade, 0.0), 1.0)
return curvature + (math.copysign(limit, curvature) - curvature) * fade
def update_twitch_guard(remaining: float, v_ego: float, standstill: bool) -> float:
if not (math.isfinite(remaining) and math.isfinite(v_ego)):
return 0.0
if standstill or abs(v_ego) <= 0.3:
return TWITCH_GUARD_DURATION
return max(remaining - DT_CTRL, 0.0)
xs = model_v2.position.x
return xs[-1] if len(xs) else 0.0
def get_control_lateral_smooth_seconds(brand: str, v_ego: float, vehicle_smooth_seconds: float) -> float:
@@ -401,7 +346,6 @@ class Controls:
self.turn_hold_handoff_t = 0.0
self.turn_hold_done = False
self.turn_blinker_swept = 0.0
self.twitch_guard_remaining = 0.0
self.kona_non_scc_lateral_active = False
self.pose_calibrator = PoseCalibrator()
@@ -457,7 +401,6 @@ class Controls:
def state_control(self):
CS = self.sm['carState']
self.twitch_guard_remaining = update_twitch_guard(self.twitch_guard_remaining, CS.vEgo, CS.standstill)
# Update VehicleModel
lp = self.sm['liveParameters']
@@ -517,11 +460,7 @@ class Controls:
# EcuDisableFailed is set when car started in READY mode (ECU disable was rejected)
# Disable longitudinal so stock ACC works instead
self.update_ecu_disable_failed()
CC.longActive = (
CC.enabled and not any(e.overrideLongitudinal for e in self.sm['onroadEvents']) and
not self.sm['starpilotCarState'].pauseLongitudinal and self.CP.openpilotLongitudinalControl and
not self.ecu_disable_failed
)
CC.longActive = CC.enabled and not any(e.overrideLongitudinal for e in self.sm['onroadEvents']) and not self.sm['starpilotCarState'].pauseLongitudinal and self.CP.openpilotLongitudinalControl and not self.ecu_disable_failed
actuators = CC.actuators
actuators.longControlState = self.LoC.long_control_state
@@ -560,9 +499,6 @@ class Controls:
# here is positive for RIGHT turns (pauseturn log: left turn at +148 deg steering
# angle logs desiredCurvature -0.07), so the blinker maps right=+1, left=-1.
blinker_dir = float(CS.rightBlinker) - float(CS.leftBlinker)
if (CC.latActive and self.twitch_guard_remaining > 0.0 and
blinker_dir == 0.0 and self.turn_hold_curvature == 0.0):
new_desired_curvature = limit_curvature_to_plan(model_v2, new_desired_curvature, CS.vEgo)
# heading swept in the blinker's direction over the whole blinker cycle (any speed):
# discriminates a turn not yet made from one being exited (see the re-arm below)
if blinker_dir == 0.0:
+4 -47
View File
@@ -85,8 +85,7 @@ class LaneCenteringController:
def _covers(x, distance: float) -> bool:
return bool(x[0] <= distance <= x[-1])
@staticmethod
def _raw_correction(model_v2, v_ego: float, offset: float, e2e_authority: float) -> tuple[bool, float]:
def _raw_correction(self, model_v2, v_ego: float, offset: float, e2e_authority: float) -> tuple[bool, float]:
try:
lane_lines = model_v2.laneLines
probs = np.asarray(model_v2.laneLineProbs, dtype=float)
@@ -106,13 +105,11 @@ class LaneCenteringController:
right_y = np.asarray(lane_lines[2].y, dtype=float)
pos_x = np.asarray(model_v2.position.x, dtype=float)
pos_y = np.asarray(model_v2.position.y, dtype=float)
if not (LaneCenteringController._valid_path(left_x, left_y) and
LaneCenteringController._valid_path(right_x, right_y) and
LaneCenteringController._valid_path(pos_x, pos_y)):
if not (self._valid_path(left_x, left_y) and self._valid_path(right_x, right_y) and self._valid_path(pos_x, pos_y)):
return False, 0.0
lookahead = float(np.clip(v_ego, 8.0, 35.0))
if not all(LaneCenteringController._covers(x, lookahead) for x in (left_x, right_x, pos_x)):
if not all(self._covers(x, lookahead) for x in (left_x, right_x, pos_x)):
return False, 0.0
left = float(np.interp(lookahead, left_x, left_y))
@@ -133,7 +130,7 @@ class LaneCenteringController:
try:
pos_y_std = np.asarray(model_v2.position.yStd, dtype=float)
if LaneCenteringController._valid_path(pos_x, pos_y_std):
if self._valid_path(pos_x, pos_y_std):
path_std = float(np.interp(lookahead, pos_x, pos_y_std))
if 0.0 <= path_std <= _E2E_MAX_PATH_STD:
break_in = np.clip(
@@ -148,43 +145,3 @@ class LaneCenteringController:
return True, float(2.0 * error / lookahead ** 2)
except (AttributeError, IndexError, TypeError, ValueError):
return False, 0.0
def get_raw_lane_centering_correction(model_v2, v_ego: float, offset: float,
e2e_authority: float) -> tuple[bool, float]:
"""Return the instantaneous lane-centering correction without controller filtering."""
return LaneCenteringController._raw_correction(model_v2, v_ego, offset, e2e_authority)
def get_lane_centering_visual_direction(model_v2, v_ego: float, offset: float, e2e_authority: float,
enabled: bool, lat_active: bool, pause_on_signal: bool = False,
turn_signal_active: bool = False,
applied_correction: float | None = None) -> int:
"""Return 1 for a right correction, -1 for left, and 0 when no correction is active."""
if not enabled or not lat_active or (pause_on_signal and turn_signal_active):
return 0
try:
v_ego = float(v_ego)
offset = float(offset)
e2e_authority = float(e2e_authority)
if not np.isfinite([v_ego, offset, e2e_authority]).all() or v_ego < _MIN_V_EGO:
return 0
if model_v2.meta.laneChangeState != log.LaneChangeState.off:
return 0
except (AttributeError, TypeError, ValueError):
return 0
valid, correction = get_raw_lane_centering_correction(
model_v2,
v_ego,
float(np.clip(offset, -_MAX_OFFSET, _MAX_OFFSET)),
float(np.clip(e2e_authority, 0.0, 1.0)),
)
if not valid or not np.isfinite(correction):
return 0
if correction == 0.0:
if applied_correction is None or not np.isfinite(applied_correction) or applied_correction == 0.0:
return 0
correction = applied_correction
return 1 if correction > 0.0 else -1
+1 -16
View File
@@ -9,7 +9,6 @@ from openpilot.common.filter_simple import FirstOrderFilter
from openpilot.common.realtime import DT_MDL
from openpilot.selfdrive.modeld.constants import ModelConstants
from openpilot.starpilot.common.model_versions import is_tinygrad_model_version
from openpilot.starpilot.controls.lib.starpilot_vcruise import FT_TO_M, OFFSET_FT_MAX, OFFSET_FT_MIN
from openpilot.selfdrive.controls.lib.longcontrol import LongCtrlState
from openpilot.selfdrive.controls.lib.longitudinal_mpc_lib.long_mpc import LongitudinalMpc
from openpilot.selfdrive.controls.lib.longitudinal_mpc_lib.long_mpc import desired_follow_distance
@@ -382,11 +381,6 @@ def get_vehicle_min_accel(CP, v_ego):
# Restored planner constants retained by CEM, stop, and departure paths.
A_CRUISE_MIN = -1.0
# The stop distance runs ~9 m long through the mid-approach, which leaves the obstacle slack
# so it stays silent and deceleration sags. Multiplicative so the trim scales with what is
# left. Note the car parks where the obstacle sits, so this is also a placement bias — 0.85
# stopped ~4.6 m short, 0.93 ~1.6 m.
FORCE_STOP_OBSTACLE_TRIM = 0.93
STANDSTILL_LEAD_CREEP_RELEASE_MIN_LEAD_SPEED = 0.25
STANDSTILL_LEAD_CREEP_RELEASE_MIN_LEAD_ACCEL = 0.08
STANDSTILL_LEAD_CREEP_RELEASE_MIN_GAP_MARGIN = 0.1
@@ -2223,17 +2217,8 @@ class LongitudinalPlanner:
force_stop_x = None
force_stop_handoff_m = get_force_stop_handoff_distance(self.CP.carFingerprint)
if sm['starpilotPlan'].forcingStop and sm['starpilotPlan'].forcingStopLength > force_stop_handoff_m:
stop_length = float(sm['starpilotPlan'].forcingStopLength)
else:
# pre-commit the envelope is only a speed ceiling, which the solver tracks with a lag;
# getattr so a stale cereal build degrades to the old behaviour instead of raising
stop_length = float(getattr(sm['starpilotPlan'], 'approachStopLength', 0.0))
if stop_length > force_stop_handoff_m:
# ForceStopDistanceOffset shifts the perceived line for the v_cruise ceiling, so it has
# to shift the obstacle too or the slider barely moves anything now that stop_x leads.
offset_ft = max(OFFSET_FT_MIN, min(OFFSET_FT_MAX, int(getattr(starpilot_toggles, 'force_stop_distance_offset', 0) or 0)))
force_stop_x = (
stop_length * FORCE_STOP_OBSTACLE_TRIM + offset_ft * FT_TO_M + STOP_DISTANCE +
float(sm['starpilotPlan'].forcingStopLength) + STOP_DISTANCE +
get_force_stop_distance_bias(self.CP.carFingerprint)
)
+3 -15
View File
@@ -53,8 +53,6 @@ ADJACENT_STOP_REST_FRAMES = 15
ADJACENT_STOP_MIN_Y = 1.8 # m — inside this is our own lane
ADJACENT_STOP_MAX_Y = 7.5 # m — beyond this is roadside, not an adjacent lane
ADJACENT_STOP_MAX_D = 110.0 # m
ADJACENT_STOP_QUEUE_GAP_M = 5.0 # m — anything stopped beyond the furthest qualifier means
# the bar is past it too, so the hint would stop us short
class KalmanParams:
@@ -181,10 +179,6 @@ class Track:
if self.leadTrackID == self.identifier:
return False
return self.in_adjacent_lane(model_data)
def in_adjacent_lane(self, model_data: capnp._DynamicStructReader):
"""Lane geometry only, no deceleration history — also used to spot a queue ahead."""
if not (ADJACENT_STOP_MIN_Y < abs(self.yRel) < ADJACENT_STOP_MAX_Y):
return False
@@ -404,10 +398,9 @@ def get_adjacent_lead(tracks: dict[int, Track], standstill: bool, model_data: ca
def get_adjacent_stopped(tracks: dict[int, Track], model_data: capnp._DynamicStructReader) -> dict[str, Any]:
"""Stop-line hint: a vehicle that decelerated to a stop in a neighbouring lane.
Takes the FARTHEST qualifying vehicle, then drops the hint entirely if a queue reaches
past it. Cars already stopped when we acquire them never show the moving -> stopped
transition, so the qualifying set is biased toward the back of a line; without this the
hint marks a mid-queue bumper and stops us short of the bar.
Takes the FARTHEST qualifying vehicle: in a queue the front car sits at the bar and the
rest are closer to us, so the nearest one underestimates the distance. The consumer only
shortens with this, so underestimating is the harmful direction.
"""
if len(model_data.laneLines) < 4:
return {'status': False}
@@ -417,11 +410,6 @@ def get_adjacent_stopped(tracks: dict[int, Track], model_data: capnp._DynamicStr
return {'status': False}
furthest = max(candidates, key=lambda c: c.dRel)
for c in tracks.values():
if (c.dRel > furthest.dRel + ADJACENT_STOP_QUEUE_GAP_M and
abs(c.vLead) < ADJACENT_STOP_REST_V and
c.in_adjacent_lane(model_data)):
return {'status': False}
return {
'status': True,
'dRel': float(furthest.dRel),
@@ -3,7 +3,7 @@ from types import SimpleNamespace
import numpy as np
import pytest
from openpilot.selfdrive.controls.lib.lane_centering import LaneCenteringController, get_lane_centering_visual_direction
from openpilot.selfdrive.controls.lib.lane_centering import LaneCenteringController
_V_EGO = 20.0
@@ -174,20 +174,3 @@ def test_correction_is_smoothed_and_capped():
_, steady = _converge(model, authority=0.0)
assert 0.0 < first < steady
assert np.isclose(steady, 0.004 * 0.30, atol=1e-6)
def test_visual_direction_matches_curvature_sign():
# Positive curvature is right in this tree's convention.
assert get_lane_centering_visual_direction(_model(left=-1.5, right=2.1), _V_EGO, 0.0, 0.0, True, True) == 1
assert get_lane_centering_visual_direction(_model(left=-2.1, right=1.5), _V_EGO, 0.0, 0.0, True, True) == -1
def test_visual_direction_requires_both_primary_lane_lines():
model = _model(left=-1.5, right=2.1)
model.laneLineProbs[2] = 0.2
assert get_lane_centering_visual_direction(model, _V_EGO, 0.0, 0.0, True, True) == 0
def test_visual_direction_uses_filtered_correction_in_deadband():
model = _model()
assert get_lane_centering_visual_direction(model, _V_EGO, 0.0, 0.0, True, True, applied_correction=0.001) == 1
@@ -526,7 +526,6 @@ def make_sm(v_ego: float, desired_accel: float, min_accel: float, *, experimenta
forcingStop=False,
redLight=False,
forcingStopLength=2,
approachStopLength=0.0,
),
}
@@ -2991,39 +2990,6 @@ def test_modeld_action_uses_current_action_head_scaling_for_v15(monkeypatch):
assert not action.shouldStop
def test_modeld_action_uses_current_action_head_scaling_for_v16(monkeypatch):
monkeypatch.setenv("DEBUG", "0")
fake_commonmodel = types.ModuleType("openpilot.selfdrive.modeld.models.commonmodel_pyx")
fake_commonmodel.DrivingModelFrame = object
fake_commonmodel.CLContext = object
monkeypatch.setitem(sys.modules, fake_commonmodel.__name__, fake_commonmodel)
from openpilot.selfdrive.modeld import modeld
prev_action = log.ModelDataV2.Action.new_message()
prev_action.desiredCurvature = 0.05
prev_action.desiredAcceleration = -0.2
toggles = SimpleNamespace(vEgoStopping=0.42)
action = modeld.get_action_from_model(
{"action": np.array([[12.0, -0.8]], dtype=np.float32)},
prev_action,
lat_action_t=0.2,
long_action_t=0.73,
v_ego=5.0,
mlsim=True,
is_v9=False,
is_v14=False,
is_v15=False,
starpilot_toggles=toggles,
is_v16=True,
)
assert action.desiredCurvature == pytest.approx(modeld.smooth_value(0.48, prev_action.desiredCurvature, modeld.LAT_SMOOTH_SECONDS))
assert action.desiredAcceleration < -0.2
assert not action.shouldStop
def test_publish_force_stop_handoff_sets_should_stop_when_vcruise_zero():
class FakePM:
def __init__(self):
@@ -7,9 +7,7 @@ from openpilot.common.realtime import DT_MDL
from openpilot.starpilot.common.starpilot_variables import PLANNER_TIME
from openpilot.starpilot.controls.lib.curve_speed_controller import CSC_MAX_DECEL_RATE, CurveSpeedController
from openpilot.starpilot.controls.lib.starpilot_vcruise import (
FORCE_STOP_CAP_SLACK_M,
FORCE_STOP_TURN_VETO_STOP_SEEN_HOLD_TIME,
STANDSTILL_FORCE_STOP_LIGHT_HOLD_TIME,
StarPilotVCruise,
get_active_slc_control_target,
get_lead_veto_distance,
@@ -59,8 +57,6 @@ def make_vcruise(*, red_light=False, raw_model_stopped=False, forcing_stop=False
vcruise.forcing_stop = forcing_stop
vcruise.force_stop_timer = 1.0 if forcing_stop else 0.0
vcruise.tracked_model_length = 0.0 if forcing_stop else planner.model_length
# what the not-committed branch would have left behind on the frame before commit
vcruise.force_stop_distance_cap = planner.model_length
return planner, vcruise
@@ -525,15 +521,14 @@ def test_force_stop_stays_committed_while_moving_even_if_scene_opens():
def test_force_stop_reanchors_when_model_reopens_path_without_stop_action():
planner, vcruise = make_vcruise(red_light=False, raw_model_stopped=False, forcing_stop=True)
planner.model_length = 90.0
vcruise.tracked_model_length = 60.0
vcruise.force_stop_distance_cap = 90.0
planner.model_length = 40.0
vcruise.tracked_model_length = 10.0
sm = make_sm(standstill=False)
sm["modelV2"] = SimpleNamespace(action=SimpleNamespace(shouldStop=False))
result = update_vcruise(vcruise, sm, make_toggles(), now=0.0, v_ego=1.5)
assert vcruise.tracked_model_length == pytest.approx(90.0)
assert vcruise.tracked_model_length == pytest.approx(40.0)
assert result > 5.0
@@ -565,49 +560,6 @@ def test_santa_fe_force_stop_holds_through_low_speed_detector_dropout():
assert result == pytest.approx(0.0)
def test_force_stop_does_not_reanchor_inside_reanchor_floor():
planner, vcruise = make_vcruise(red_light=False, raw_model_stopped=False, forcing_stop=True)
planner.model_length = 90.0
vcruise.tracked_model_length = 25.0
sm = make_sm(standstill=False)
sm["modelV2"] = SimpleNamespace(action=SimpleNamespace(shouldStop=False))
update_vcruise(vcruise, sm, make_toggles(), now=0.0, v_ego=1.5)
assert vcruise.tracked_model_length < 25.0
def test_force_stop_reanchor_bounded_by_distance_driven():
# The line can't recede: a ballooning horizon may not push the stop past where it was at
# commit minus the distance driven since.
planner, vcruise = make_vcruise(red_light=False, raw_model_stopped=False, forcing_stop=True)
planner.model_length = 200.0
vcruise.tracked_model_length = 60.0
vcruise.force_stop_distance_cap = 70.0
sm = make_sm(standstill=False)
sm["modelV2"] = SimpleNamespace(action=SimpleNamespace(shouldStop=False))
update_vcruise(vcruise, sm, make_toggles(), now=0.0, v_ego=10.0)
assert vcruise.tracked_model_length <= 70.0 + FORCE_STOP_CAP_SLACK_M
assert vcruise.tracked_model_length < 100.0 # nowhere near the 200 m the horizon claimed
def test_force_stop_cap_slack_tapers_near_the_line():
# Slack protects against an under-read at commit; held near the line it would just aim the
# solver that far past the stop bar.
planner, vcruise = make_vcruise(red_light=False, raw_model_stopped=False, forcing_stop=True)
planner.model_length = 200.0
vcruise.tracked_model_length = 60.0
vcruise.force_stop_distance_cap = 12.0
sm = make_sm(standstill=False)
sm["modelV2"] = SimpleNamespace(action=SimpleNamespace(shouldStop=False))
update_vcruise(vcruise, sm, make_toggles(), now=0.0, v_ego=5.0)
assert vcruise.tracked_model_length < 12.0 + FORCE_STOP_CAP_SLACK_M / 2.0
def test_force_stop_does_not_reanchor_committed_model_stop():
planner, vcruise = make_vcruise(red_light=False, raw_model_stopped=False, forcing_stop=True)
planner.model_length = 40.0
@@ -889,16 +841,16 @@ def test_standstill_light_hold_expires_and_does_not_rearm_from_stopped_model():
assert update_vcruise(vcruise, sm, toggles, now=0.0) == pytest.approx(0.0)
assert vcruise.standstill_force_stop_reason == "light"
assert update_vcruise(vcruise, sm, toggles, now=STANDSTILL_FORCE_STOP_LIGHT_HOLD_TIME - 0.1) == pytest.approx(0.0)
assert update_vcruise(vcruise, sm, toggles, now=4.9) == pytest.approx(0.0)
assert vcruise.forcing_stop
assert update_vcruise(vcruise, sm, toggles, now=STANDSTILL_FORCE_STOP_LIGHT_HOLD_TIME + 0.1) == pytest.approx(20.0)
assert update_vcruise(vcruise, sm, toggles, now=5.1) == pytest.approx(20.0)
assert not vcruise.forcing_stop
assert not vcruise.standstill_force_stop_hold
# The red-light model remains stopped, but Force Stop must stay released so
# Experimental Mode can own the red-to-green departure.
assert update_vcruise(vcruise, sm, toggles, now=STANDSTILL_FORCE_STOP_LIGHT_HOLD_TIME + 0.2) == pytest.approx(20.0)
assert update_vcruise(vcruise, sm, toggles, now=5.2) == pytest.approx(20.0)
assert not vcruise.forcing_stop
+1 -104
View File
@@ -1,40 +1,13 @@
import math
import types
from cereal import car
import pytest
from openpilot.common.realtime import DT_CTRL
from openpilot.selfdrive.controls.controlsd import (
TWITCH_GUARD_DURATION,
TWITCH_GUARD_FLOOR,
TWITCH_GUARD_MAX_SPEED,
get_control_lateral_smooth_seconds,
limit_curvature_to_plan,
turn_lead_allowed,
update_twitch_guard,
)
from openpilot.selfdrive.controls.controlsd import get_control_lateral_smooth_seconds, turn_lead_allowed
LateralControlMode = car.CarControl.Actuators.LateralControlMode
def _plan(xs, ys):
return types.SimpleNamespace(position=types.SimpleNamespace(x=xs, y=ys))
def _arc_plan(radius, n=200):
return _plan([radius * math.sin(i / n) for i in range(n)],
[radius * (1.0 - math.cos(i / n)) for i in range(n)])
STRAIGHT_PLAN = _plan([i * 0.5 for i in range(200)], [0.0] * 200)
STANDSTILL_STUB_PLAN = _plan([0.0, 0.3], [0.0, 0.0])
TURN_PLAN = _arc_plan(30.0)
GENTLE_BEND_PLAN = _arc_plan(143.0)
def test_turn_lead_is_suppressed_only_during_applied_angle_control():
assert not turn_lead_allowed("rivian", LateralControlMode.angle)
assert turn_lead_allowed("rivian", LateralControlMode.torque)
@@ -64,79 +37,3 @@ def test_subaru_control_smoothing_uses_vehicle_schedule(v_ego, expected):
])
def test_rivian_control_smoothing_remains_speed_scheduled(v_ego, expected):
assert get_control_lateral_smooth_seconds("rivian", v_ego, 0.4) == pytest.approx(expected)
@pytest.mark.parametrize("curvature", [0.0155, -0.0155])
def test_twitch_against_a_straight_plan_is_clamped_to_the_floor(curvature):
guarded = limit_curvature_to_plan(STRAIGHT_PLAN, curvature, 1.2)
assert abs(guarded) == pytest.approx(TWITCH_GUARD_FLOOR)
assert math.copysign(1.0, guarded) == math.copysign(1.0, curvature)
def test_command_already_below_the_floor_is_untouched():
assert limit_curvature_to_plan(STRAIGHT_PLAN, 0.0015, 1.2) == pytest.approx(0.0015)
@pytest.mark.parametrize("v_ego", [TWITCH_GUARD_MAX_SPEED, 6.0, 30.0])
def test_guard_is_inactive_above_its_speed_band(v_ego):
assert limit_curvature_to_plan(STRAIGHT_PLAN, 0.0155, v_ego) == pytest.approx(0.0155)
def test_guard_fades_out_across_the_speed_band():
full = limit_curvature_to_plan(STRAIGHT_PLAN, 0.0155, 1.2)
half = limit_curvature_to_plan(STRAIGHT_PLAN, 0.0155, 3.5)
assert full < half < 0.0155
@pytest.mark.parametrize("ratio", [0.8, 1.0, 2.0, 3.0])
def test_real_turns_tracking_their_own_plan_are_untouched(ratio):
action = (1.0 / 30.0) * ratio
assert limit_curvature_to_plan(TURN_PLAN, action, 1.2) == pytest.approx(action)
def test_a_barely_bending_plan_does_not_license_a_large_command():
guarded = limit_curvature_to_plan(GENTLE_BEND_PLAN, 0.0155, 1.2)
assert TWITCH_GUARD_FLOOR < guarded < 0.008
@pytest.mark.parametrize("plan", [STANDSTILL_STUB_PLAN, _plan([], [])])
def test_guard_stands_down_when_the_plan_is_too_short_to_judge(plan):
assert limit_curvature_to_plan(plan, 0.0155, 0.4) == pytest.approx(0.0155)
def test_zero_command_stays_zero():
assert limit_curvature_to_plan(STRAIGHT_PLAN, 0.0, 1.2) == 0.0
@pytest.mark.parametrize("bad_plan", [
_plan([0.0, math.nan, 20.0], [0.0, 0.0, 0.0]),
_plan([0.0, math.inf, 20.0], [0.0, 0.0, 0.0]),
_plan([0.0, 20.0], [0.0]),
])
def test_invalid_plan_data_leaves_curvature_untouched(bad_plan):
assert limit_curvature_to_plan(bad_plan, 0.0155, 1.2) == pytest.approx(0.0155)
@pytest.mark.parametrize("value", [math.nan, math.inf, -math.inf])
def test_nonfinite_guard_inputs_disarm(value):
assert update_twitch_guard(value, 1.0, False) == 0.0
assert update_twitch_guard(TWITCH_GUARD_DURATION, value, False) == 0.0
def test_twitch_guard_arms_at_standstill_or_creep_speed():
assert update_twitch_guard(0.0, 0.0, True) == TWITCH_GUARD_DURATION
assert update_twitch_guard(0.0, 0.3, False) == TWITCH_GUARD_DURATION
def test_twitch_guard_decays_after_pullaway_and_expires():
remaining = update_twitch_guard(0.0, 0.0, True)
remaining = update_twitch_guard(remaining, 1.0, False)
assert remaining == pytest.approx(TWITCH_GUARD_DURATION - DT_CTRL)
for _ in range(int(TWITCH_GUARD_DURATION / DT_CTRL) + 1):
remaining = update_twitch_guard(remaining, 1.0, False)
assert remaining == 0.0
def test_twitch_guard_does_not_arm_while_moving():
assert update_twitch_guard(0.0, 1.0, False) == 0.0
+3 -7
View File
@@ -204,14 +204,13 @@ def _packed_policy_shapes(input_shapes, include_prev_feature=False):
shapes[key] = tuple(shape)
if include_prev_feature:
features_shape = input_shapes["features_buffer"]
shapes["prev_feat"] = (features_shape[0], math.prod(features_shape[2:]))
shapes["prev_feat"] = (features_shape[0], features_shape[2])
return shapes, [math.prod(shape) for shape in shapes.values()]
def make_split_input_queues(vision_input_shapes, policy_input_shapes, frame_skip, device):
queues, npy = make_warp_input_queues(vision_input_shapes, frame_skip, device)
features_shape = policy_input_shapes["features_buffer"]
feature_dim = math.prod(features_shape[2:])
desire_key = _detect_desire_key(policy_input_shapes)
desire_shape = policy_input_shapes[desire_key]
packed_shapes, packed_sizes = _packed_policy_shapes(policy_input_shapes)
@@ -225,7 +224,7 @@ def make_split_input_queues(vision_input_shapes, policy_input_shapes, frame_skip
})
queues.update({
"feat_q": Tensor(
np.zeros((frame_skip * (features_shape[1] - 1) + 1, features_shape[0], feature_dim), dtype=np.float32),
np.zeros((frame_skip * (features_shape[1] - 1) + 1, features_shape[0], features_shape[2]), dtype=np.float32),
device=device,
).contiguous().realize(),
"desire_q": Tensor(
@@ -240,7 +239,6 @@ def make_split_input_queues(vision_input_shapes, policy_input_shapes, frame_skip
def make_supercombo_input_queues(input_shapes, frame_skip, device):
queues, npy = make_warp_input_queues(input_shapes, frame_skip, device)
features_shape = input_shapes["features_buffer"]
feature_dim = math.prod(features_shape[2:])
desire_key = _detect_desire_key(input_shapes)
desire_shape = input_shapes[desire_key]
packed_shapes, packed_sizes = _packed_policy_shapes(input_shapes, include_prev_feature=True)
@@ -254,7 +252,7 @@ def make_supercombo_input_queues(input_shapes, frame_skip, device):
})
queues.update({
"feat_q": Tensor(
np.zeros((frame_skip * features_shape[1], features_shape[0], feature_dim), dtype=np.float32),
np.zeros((frame_skip * features_shape[1], features_shape[0], features_shape[2]), dtype=np.float32),
device=device,
).contiguous().realize(),
"desire_q": Tensor(
@@ -337,7 +335,6 @@ def make_run_split_policy(vision_runner, policy_runners, metadata, policy_order,
vision_output = next(iter(vision_runner({road_key: img, wide_key: big_img}).values())).cast("float32")
new_feature = vision_output[:, vision_features_slice].reshape(1, -1).unsqueeze(0)
features_buffer = shift_and_sample(feat_q, new_feature, sample_skip_fn)
features_buffer = features_buffer.reshape(policy_metadata["input_shapes"]["features_buffer"])
policy_inputs = {
"features_buffer": features_buffer,
@@ -391,7 +388,6 @@ def make_run_supercombo(model_runner, metadata, frame_skip, image_history_pipeli
features_buffer = shift_and_sample(
feat_q, previous_feature.reshape(1, 1, -1), sample_skip_fn,
)
features_buffer = features_buffer.reshape(input_shapes["features_buffer"])
model_inputs = {
road_key: img,
wide_key: big_img,
+12 -27
View File
@@ -1,6 +1,4 @@
#!/usr/bin/env python3
from collections.abc import Callable
import ctypes
from functools import cached_property
import os
import struct
@@ -161,10 +159,8 @@ class ChestnutState:
if self.big and "AMD" in Device._opened_devices and self.sends % 100 == 1:
try:
smu = Device["AMD"].iface.dev_impl.smu
metrics_t = smu.smu_mod.SmuMetricsExternal_t
smu._send_msg(smu.smu_mod.PPSMC_MSG_TransferTableSmu2Dram, smu.smu_mod.TABLE_SMU_METRICS, timeout=100)
metrics_buf = bytearray(smu.adev.vram.view(smu.driver_table_paddr, ctypes.sizeof(metrics_t))[:])
metrics = metrics_t.from_buffer(metrics_buf).SmuMetrics
metrics = smu.read_table(smu.smu_mod.SmuMetricsExternal_t, smu.smu_mod.TABLE_SMU_METRICS).SmuMetrics
self.metrics = {
"tempC": metrics.AvgTemperature[smu.smu_mod.TEMP_HOTSPOT],
"memoryTempC": metrics.AvgTemperature[smu.smu_mod.TEMP_MEM],
@@ -281,11 +277,10 @@ def _close_tinygrad_disk_cache_connection() -> None:
def get_action_from_model(model_output: dict[str, np.ndarray], prev_action: log.ModelDataV2.Action,
lat_action_t: float, long_action_t: float, v_ego: float, mlsim: bool,
is_v9: bool, is_v14: bool, is_v15: bool, starpilot_toggles,
lat_smooth_seconds=LAT_SMOOTH_SECONDS, long_smooth_seconds=LONG_SMOOTH_SECONDS,
is_v16: bool = False) -> log.ModelDataV2.Action:
if is_v14 or is_v15 or is_v16:
lat_smooth_seconds=LAT_SMOOTH_SECONDS, long_smooth_seconds=LONG_SMOOTH_SECONDS) -> log.ModelDataV2.Action:
if is_v14 or is_v15:
desired_curv_unscaled, desired_accel = model_output['action'][0]
if is_v15 or is_v16:
if is_v15:
desired_curvature = float(desired_curv_unscaled) / max(1.0, v_ego) ** 2
else:
desired_curvature = float(desired_curv_unscaled) / 100.0
@@ -469,7 +464,6 @@ class ModelState:
self.is_v9 = self.policy_generation == "v9"
self.is_v14 = self.policy_generation == "v14"
self.is_v15 = self.policy_generation == "v15"
self.is_v16 = self.policy_generation == "v16"
self.mlsim = is_tinygrad_model_version(self.policy_generation)
if write_model_version:
params.put("ModelVersion", self.policy_generation)
@@ -573,8 +567,7 @@ class ModelState:
self._reset_state()
def run(self, bufs: dict[str, VisionBuf], transforms: dict[str, np.ndarray],
inputs: dict[str, np.ndarray], prepare_only: bool,
after_enqueue: Callable[[], None] | None = None) -> dict[str, np.ndarray] | None:
inputs: dict[str, np.ndarray], prepare_only: bool) -> dict[str, np.ndarray] | None:
frames: dict[str, Tensor] = {}
for key, buf in bufs.items():
ptr = np.frombuffer(buf.data, dtype=np.uint8).ctypes.data
@@ -620,12 +613,11 @@ class ModelState:
img=img,
big_img=big_img,
)
if after_enqueue is not None:
after_enqueue()
outputs = [output.numpy().flatten() for output in output_tensors]
if self.uses_external_gpu and any(not np.isfinite(output).all() for output in outputs):
raise RuntimeError("external GPU model output not finite")
cloudlog.error("external GPU model output not finite, dropping frame")
return None
if self.model_type == "supercombo":
model_output = outputs[0]
@@ -927,17 +919,7 @@ def main(demo=False):
mt1 = time.perf_counter()
try:
send_chestnut = (
chestnut_state is not None and
run_count % round(ModelConstants.MODEL_FREQ / SERVICE_LIST["chestnutState"].frequency) == 0
)
model_output = model.run(
bufs,
transforms,
inputs,
prepare_only,
chestnut_state.send if send_chestnut else None,
)
model_output = model.run(bufs, transforms, inputs, prepare_only)
except Exception:
if not external_gpu_active or small_model is None:
raise
@@ -971,7 +953,7 @@ def main(demo=False):
lat_action_t,
long_action_t,
v_ego, model.mlsim, model.is_v9, model.is_v14, model.is_v15, starpilot_toggles,
lat_smooth_seconds, long_smooth_seconds, is_v16=model.is_v16,
lat_smooth_seconds, long_smooth_seconds,
)
prev_action = action
fill_model_msg(drivingdata_send, modelv2_send, model_output, action,
@@ -1000,6 +982,9 @@ def main(demo=False):
if sm.updated['starpilotPlan']:
starpilot_toggles = get_starpilot_toggles(sm)
if chestnut_state is not None and run_count % round(ModelConstants.MODEL_FREQ / SERVICE_LIST["chestnutState"].frequency) == 0:
chestnut_state.send()
if __name__ == "__main__":
try:
import argparse
+12 -10
View File
@@ -3,7 +3,6 @@ from types import MethodType
from types import SimpleNamespace
import numpy as np
import pytest
from openpilot.selfdrive.modeld import modeld
from openpilot.selfdrive.modeld.helpers import dump_oob, load_oob, tinygrad_dev_config
@@ -38,18 +37,17 @@ def test_external_gpu_uses_a_longer_load_watchdog():
assert modeld.BIG_MODEL_RUN_WAIT_TIMEOUT_MS == 3000
def test_external_gpu_signal_wait_yields_between_usb_polls(monkeypatch):
def test_external_gpu_signal_wait_matches_upstream_busy_poll():
from tinygrad.runtime import ops_amd
sleeps = []
monkeypatch.setattr(ops_amd.time, "sleep", sleeps.append)
signal = ops_amd.AMDSignal.__new__(ops_amd.AMDSignal)
signal.should_return = False
signal.owner = SimpleNamespace(is_usb=lambda: True, iface=SimpleNamespace(sleep=lambda _: None))
signal.owner = SimpleNamespace(is_usb=lambda: True, iface=SimpleNamespace(sleep=sleeps.append))
signal._sleep(0)
assert sleeps == [ops_amd.AMD_USB_POLL_US / 1e6]
assert sleeps == []
def test_native_amd_signal_keeps_existing_short_wait_behavior():
@@ -203,7 +201,7 @@ def test_external_gpu_load_finishes_before_native_model_can_start(monkeypatch):
]
def test_external_gpu_nonfinite_outputs_trigger_fallback(monkeypatch):
def test_external_gpu_nonfinite_outputs_are_dropped_without_escalating(monkeypatch):
class FakeTensor:
@staticmethod
def from_blob(*_args, **_kwargs):
@@ -237,7 +235,13 @@ def test_external_gpu_nonfinite_outputs_trigger_fallback(monkeypatch):
state.image_history_pipeline = modeld.IMAGE_HISTORY_IN_POLICY
state.warp_enqueue = lambda **_kwargs: object()
state.run_policy = lambda **_kwargs: (FakeOutput(),)
state._reset_state = MethodType(
lambda self: (_ for _ in ()).throw(AssertionError("upstream does not reset or escalate transient non-finite output")),
state,
)
monkeypatch.setattr(modeld, "Tensor", FakeTensor)
monkeypatch.setattr(modeld.cloudlog, "error", lambda *_args, **_kwargs: None)
buffers = {
"img": SimpleNamespace(data=bytearray(4)),
"big_img": SimpleNamespace(data=bytearray(4)),
@@ -248,10 +252,8 @@ def test_external_gpu_nonfinite_outputs_trigger_fallback(monkeypatch):
}
inputs = {"desire_pulse": np.zeros(8, dtype=np.float32)}
callbacks = []
with pytest.raises(RuntimeError, match="external GPU model output not finite"):
state.run(buffers, transforms, inputs, False, lambda: callbacks.append("sent"))
assert callbacks == ["sent"]
for _ in range(10):
assert state.run(buffers, transforms, inputs, False) is None
def test_out_of_band_artifact_round_trip():
+1 -137
View File
@@ -1,11 +1,6 @@
import json
import os
import threading
import time
import urllib.error
import urllib.request
from collections.abc import Callable
from dataclasses import dataclass
from enum import IntEnum
import pyray as rl
@@ -23,40 +18,6 @@ from openpilot.system.ui.widgets.label import UnifiedLabel
from openpilot.system.ui.widgets.scroller import NavScroller
UPDATER_TIMEOUT = 10.0
FAST_UPDATE_HOLD_SECONDS = 0.8
FAST_UPDATE_POLL_SECONDS = 0.5
FAST_UPDATE_REQUEST_TIMEOUT = 5.0
@dataclass
class FastUpdateDisplayState:
stage: str = "idle"
message: str = ""
error: str = ""
def _galaxy_api_url(path: str) -> str:
port = os.getenv("SP_GALAXY_PORT", "8082")
return f"http://127.0.0.1:{port}/api/update/fast{path}"
def _galaxy_fast_update_request(path: str = "", method: str = "GET") -> dict:
request = urllib.request.Request(_galaxy_api_url(path), method=method)
try:
with urllib.request.urlopen(request, timeout=FAST_UPDATE_REQUEST_TIMEOUT) as response:
payload = json.loads(response.read().decode("utf-8"))
except urllib.error.HTTPError as error:
try:
payload = json.loads(error.read().decode("utf-8"))
except (json.JSONDecodeError, UnicodeDecodeError):
payload = {}
raise RuntimeError(payload.get("error") or str(error)) from error
except (OSError, urllib.error.URLError, json.JSONDecodeError, UnicodeDecodeError) as error:
raise RuntimeError(f"Galaxy fast update unavailable: {error}") from error
if not isinstance(payload, dict):
raise RuntimeError("Galaxy returned an invalid fast update response")
return payload
def _split_description(desc: str) -> tuple[str, str, str, str] | None:
@@ -140,10 +101,6 @@ class CheckUpdateButton(BigButton):
self._waiting_for_updater_t: float | None = None
self._hide_value_t: float | None = None
self._state: UpdaterState = UpdaterState.IDLE
self._press_start_t: float | None = None
self._press_action = ""
self._fast_update_state = FastUpdateDisplayState()
self._fast_update_state_lock = threading.Lock()
ui_state.add_offroad_transition_callback(self.offroad_transition)
@@ -151,24 +108,9 @@ class CheckUpdateButton(BigButton):
if ui_state.is_offroad():
self.set_enabled(True)
def _handle_mouse_press(self, mouse_pos: MousePos):
super()._handle_mouse_press(mouse_pos)
self._press_start_t = rl.get_time()
self._press_action = self.get_value()
def _handle_mouse_release(self, mouse_pos: MousePos):
held_for = 0.0 if self._press_start_t is None else rl.get_time() - self._press_start_t
self._press_start_t = None
super()._handle_mouse_release(mouse_pos)
if held_for >= FAST_UPDATE_HOLD_SECONDS:
self._show_fast_update_confirmation()
return
if self._get_fast_update_state().stage == "error":
self._set_fast_update_state(stage="idle")
return
if not system_time_valid():
dlg = BigDialog("", tr("Please connect to Wi-Fi to update."))
gui_app.push_widget(dlg)
@@ -179,75 +121,13 @@ class CheckUpdateButton(BigButton):
self.set_icon(self._txt_update_icon)
def run():
if self._press_action == "download update":
if self.get_value() == "download update":
_request_update_download()
else:
_request_update_check()
threading.Thread(target=run, daemon=True).start()
def _show_fast_update_confirmation(self):
if not system_time_valid():
gui_app.push_widget(BigDialog("", tr("Please connect to Wi-Fi to update.")))
return
if ui_state.started:
return
gui_app.push_widget(BigConfirmationDialog(
"slide to\nfast update",
self._txt_update_icon,
self._start_fast_update,
red=True,
))
def _set_fast_update_state(self, *, stage: str, message: str = "", error: str = ""):
with self._fast_update_state_lock:
self._fast_update_state = FastUpdateDisplayState(stage, message, error)
def _get_fast_update_state(self) -> FastUpdateDisplayState:
with self._fast_update_state_lock:
state = self._fast_update_state
return FastUpdateDisplayState(state.stage, state.message, state.error)
def _start_fast_update(self):
if ui_state.started:
return
state = self._get_fast_update_state()
if state.stage not in ("idle", "error"):
return
self._set_fast_update_state(stage="starting", message="starting fast update...")
threading.Thread(target=self._run_fast_update, daemon=True).start()
def _run_fast_update(self):
try:
_galaxy_fast_update_request(method="POST")
while True:
status = _galaxy_fast_update_request("/status")
stage = str(status.get("stage") or "updating")
error = str(status.get("lastError") or "").strip()
message = str(
status.get("progressDetail") or
status.get("progressLabel") or
status.get("message") or
"fast update in progress..."
).strip()
if error or stage == "error":
self._set_fast_update_state(stage="error", message="fast update failed", error=error or message)
return
self._set_fast_update_state(stage=stage, message=message)
if not bool(status.get("running")):
return
time.sleep(FAST_UPDATE_POLL_SECONDS)
except Exception as error:
current = self._get_fast_update_state()
if current.stage != "rebooting":
self._set_fast_update_state(stage="error", message="fast update failed", error=str(error))
def set_value(self, value: str):
super().set_value(value)
self.set_text("" if value else "check for update")
@@ -256,25 +136,9 @@ class CheckUpdateButton(BigButton):
super()._update_state()
if ui_state.started:
self._press_start_t = None
self.set_enabled(False)
return
fast_update_state = self._get_fast_update_state()
if fast_update_state.stage != "idle":
self.set_rotate_icon(fast_update_state.stage not in ("error", "rebooting"))
if fast_update_state.stage == "error":
self.set_enabled(True)
self.set_value(fast_update_state.error or fast_update_state.message)
elif fast_update_state.stage == "rebooting":
self.set_enabled(False)
self.set_value("update complete\nrebooting...")
else:
self.set_enabled(False)
self.set_value(fast_update_state.message or "fast update in progress...")
self.set_text("fast update")
return
updater_state = ui_state.params.get("UpdaterState") or ""
if self._state == UpdaterState.WAITING_FOR_UPDATER:
@@ -1,98 +0,0 @@
from types import SimpleNamespace
from openpilot.selfdrive.ui.mici.layouts.settings import software
from openpilot.selfdrive.ui.mici.widgets.button import BigButton
def _make_button() -> software.CheckUpdateButton:
button = object.__new__(software.CheckUpdateButton)
button._press_start_t = None
button._press_action = ""
button._fast_update_state = software.FastUpdateDisplayState()
button._fast_update_state_lock = software.threading.Lock()
return button
def test_fast_update_uses_local_galaxy_api(monkeypatch):
monkeypatch.delenv("SP_GALAXY_PORT", raising=False)
assert software._galaxy_api_url("") == "http://127.0.0.1:8082/api/update/fast"
assert software._galaxy_api_url("/status") == "http://127.0.0.1:8082/api/update/fast/status"
def test_long_press_opens_fast_update_confirmation(monkeypatch):
button = _make_button()
button._press_start_t = 1.0
confirmation_calls = []
monkeypatch.setattr(software.rl, "get_time", lambda: 1.0 + software.FAST_UPDATE_HOLD_SECONDS)
monkeypatch.setattr(BigButton, "_handle_mouse_release", lambda *_args: None)
monkeypatch.setattr(button, "_show_fast_update_confirmation", lambda: confirmation_calls.append(True))
button._handle_mouse_release(SimpleNamespace())
assert confirmation_calls == [True]
def test_short_press_keeps_normal_updater_path(monkeypatch):
button = _make_button()
button._press_start_t = 1.0
button._press_action = "download update"
downloads = []
confirmations = []
monkeypatch.setattr(software.rl, "get_time", lambda: 1.0 + software.FAST_UPDATE_HOLD_SECONDS - 0.1)
monkeypatch.setattr(BigButton, "_handle_mouse_release", lambda *_args: None)
monkeypatch.setattr(software, "system_time_valid", lambda: True)
monkeypatch.setattr(software, "_request_update_download", lambda: downloads.append(True))
monkeypatch.setattr(button, "_show_fast_update_confirmation", lambda: confirmations.append(True))
class ImmediateThread:
def __init__(self, target, daemon):
self.target = target
def start(self):
self.target()
monkeypatch.setattr(software.threading, "Thread", ImmediateThread)
button.set_enabled = lambda *_args: None
button.set_icon = lambda *_args: None
button._state = software.UpdaterState.IDLE
button._txt_update_icon = None
button._handle_mouse_release(SimpleNamespace())
assert downloads == [True]
assert confirmations == []
def test_fast_update_worker_uses_galaxy_endpoint(monkeypatch):
button = _make_button()
requests = []
responses = [
{"message": "started"},
{
"running": True,
"stage": "updating",
"progressDetail": "Fetching latest shallow commit...",
"lastError": "",
},
{
"running": False,
"stage": "rebooting",
"progressDetail": "Update complete. Please wait for device to reboot.",
"lastError": "",
},
]
def request(path="", method="GET"):
requests.append((path, method))
return responses.pop(0)
monkeypatch.setattr(software, "_galaxy_fast_update_request", request)
monkeypatch.setattr(software.time, "sleep", lambda *_args: None)
button._run_fast_update()
assert requests == [("", "POST"), ("/status", "GET"), ("/status", "GET")]
assert button._get_fast_update_state().stage == "rebooting"
+1 -1
View File
@@ -158,7 +158,7 @@ class HudRenderer(Widget):
self._txt_exclamation_point: rl.Texture = gui_app.texture('icons_mici/exclamation_point.png', 44, 44)
self._txt_egpu_loading: rl.Texture = gui_app.texture('icons_mici/egpu_loading.png', 60, 44)
self._txt_egpu_green: rl.Texture = gui_app.texture('icons_mici/egpu_green.png', 60, 44)
self._txt_egpu_orange: rl.Texture = gui_app.texture('icons_mici/egpu_orange.png', 75, 44)
self._txt_egpu_orange: rl.Texture = gui_app.texture('icons_mici/egpu_orange.png', 60, 44)
self._txt_egpu_crossed: rl.Texture = gui_app.texture('icons_mici/egpu_crossed.png', 60, 52)
self._egpu_icon: rl.Texture | None = None
+9 -34
View File
@@ -6,7 +6,6 @@ from cereal import messaging, car
from dataclasses import dataclass, field
from openpilot.common.constants import CV
from openpilot.common.filter_simple import FirstOrderFilter
from openpilot.selfdrive.controls.lib.lane_centering import get_lane_centering_visual_direction
from openpilot.selfdrive.locationd.calibrationd import HEIGHT_INIT
from openpilot.selfdrive.ui.lib.starpilot_theme import get_param_color, get_theme_color, get_visual_color, is_stock_color_scheme, with_alpha
from openpilot.selfdrive.ui.onroad.starpilot.rainbow_path import RainbowPath
@@ -362,31 +361,7 @@ class ModelRenderer(Widget):
return LeadVehicle(glow=glow, chevron=chevron, fill_alpha=int(fill_alpha))
def _lane_centering_direction(self) -> int:
toggles = ui_state.starpilot_toggles
sm = ui_state.sm
if not sm.valid.get("modelV2", False) or not sm.valid.get("carState", False):
return 0
car_state = sm["carState"]
applied_correction = None
if sm.valid.get("controlsState", False):
try:
applied_correction = sm["controlsState"].desiredCurvature - sm["modelV2"].action.desiredCurvature
except (AttributeError, TypeError, ValueError):
pass
return get_lane_centering_visual_direction(
sm["modelV2"], car_state.vEgo,
toggles.get("lane_center_offset", 0.0),
toggles.get("lane_centering_e2e_authority", 1.0),
bool(toggles.get("lane_centering", False)),
ui_state.status == UIStatus.ENGAGED or ui_state.always_on_lateral_active,
bool(toggles.get("lane_centering_pause_on_signal", True)),
bool(car_state.leftBlinker or car_state.rightBlinker),
applied_correction,
)
def _lane_line_palette(self) -> tuple[bool, rl.Color, rl.Color, int]:
def _lane_line_palette(self) -> tuple[bool, rl.Color, rl.Color, bool]:
stock_scheme = is_stock_color_scheme(self._params)
line_status = UIStatus.ENGAGED if ui_state.status == UIStatus.DISENGAGED and ui_state.always_on_lateral_active else ui_state.status
@@ -399,15 +374,15 @@ class ModelRenderer(Widget):
if lane_color is None:
lane_color = STOCK_LANE_LINES_COLOR if stock_scheme else get_theme_color("LaneLines", STOCK_LANE_LINES_COLOR)
lane_centering_direction = self._lane_centering_direction()
return stock_scheme, edge_color, lane_color, lane_centering_direction
lane_centering_active = bool(ui_state.starpilot_toggles.get("lane_centering", False)) and (
ui_state.status == UIStatus.ENGAGED or ui_state.always_on_lateral_active
)
return stock_scheme, edge_color, lane_color, lane_centering_active
def _get_ll_color(self, prob: float, adjacent: bool, left: bool, stock_scheme: bool,
edge_color: rl.Color, lane_color: rl.Color, lane_centering_direction: int = 0):
edge_color: rl.Color, lane_color: rl.Color, lane_centering_active: bool = False):
alpha = np.clip(prob, 0.0, 0.7)
lane_centering_line = adjacent and ((lane_centering_direction > 0 and not left) or
(lane_centering_direction < 0 and left))
if lane_centering_line:
if lane_centering_active:
color = rl.Color(OCEAN_BLUE_LANE_LINES_COLOR.r, OCEAN_BLUE_LANE_LINES_COLOR.g,
OCEAN_BLUE_LANE_LINES_COLOR.b, int(alpha * OCEAN_BLUE_LANE_LINES_COLOR.a))
elif adjacent:
@@ -433,13 +408,13 @@ class ModelRenderer(Widget):
def _draw_lane_lines(self):
"""Draw lane lines and road edges"""
"""Two closest lines should be green (lane line or road edges)"""
stock_scheme, edge_color, lane_color, lane_centering_direction = self._lane_line_palette()
stock_scheme, edge_color, lane_color, lane_centering_active = self._lane_line_palette()
for i, lane_line in enumerate(self._lane_lines):
if lane_line.projected_points.size == 0:
continue
color = self._get_ll_color(float(self._lane_line_probs[i]), i in (1, 2), i in (0, 1),
stock_scheme, edge_color, lane_color, lane_centering_direction)
stock_scheme, edge_color, lane_color, lane_centering_active)
draw_polygon(self._rect, lane_line.projected_points, color)
for i, road_edge in enumerate(self._road_edges):
+7 -31
View File
@@ -5,7 +5,6 @@ from cereal import messaging, car
from dataclasses import dataclass, field
from openpilot.common.filter_simple import FirstOrderFilter
from openpilot.common.constants import CV
from openpilot.selfdrive.controls.lib.lane_centering import get_lane_centering_visual_direction
from openpilot.selfdrive.locationd.calibrationd import HEIGHT_INIT
from openpilot.selfdrive.ui.lib.starpilot_theme import get_param_color, get_theme_color, get_visual_color, is_stock_color_scheme, with_alpha
from openpilot.selfdrive.ui.onroad.radar_tracks import project_radar_points
@@ -370,35 +369,15 @@ class ModelRenderer(Widget):
return LeadVehicle(glow=glow, chevron=chevron, fill_alpha=int(fill_alpha))
def _lane_centering_direction(self) -> int:
toggles = ui_state.starpilot_toggles
sm = ui_state.sm
if not sm.valid.get("modelV2", False) or not sm.valid.get("carState", False):
return 0
car_state = sm["carState"]
applied_correction = None
if sm.valid.get("controlsState", False):
try:
applied_correction = sm["controlsState"].desiredCurvature - sm["modelV2"].action.desiredCurvature
except (AttributeError, TypeError, ValueError):
pass
return get_lane_centering_visual_direction(
sm["modelV2"], car_state.vEgo,
toggles.get("lane_center_offset", 0.0),
toggles.get("lane_centering_e2e_authority", 1.0),
bool(toggles.get("lane_centering", False)),
ui_state.status == UIStatus.ENGAGED or ui_state.always_on_lateral_active,
bool(toggles.get("lane_centering_pause_on_signal", True)),
bool(car_state.leftBlinker or car_state.rightBlinker),
applied_correction,
)
def _draw_lane_lines(self):
"""Draw lane lines and road edges"""
lane_centering_direction = self._lane_centering_direction()
lane_centering_active = bool(ui_state.starpilot_toggles.get("lane_centering", False)) and (
ui_state.status == UIStatus.ENGAGED or ui_state.always_on_lateral_active
)
lane_lines_override = get_param_color(self._params, "LaneLinesColor", STOCK_LANE_LINES_COLOR.a)
if lane_lines_override is not None:
if lane_centering_active:
lane_lines_color = OCEAN_BLUE_LANE_LINES_COLOR
elif lane_lines_override is not None:
lane_lines_color = lane_lines_override
elif is_stock_color_scheme(self._params):
lane_lines_color = STOCK_LANE_LINES_COLOR
@@ -410,10 +389,7 @@ class ModelRenderer(Widget):
continue
alpha = np.clip(self._lane_line_probs[i], 0.0, 0.7)
lane_centering_line = (lane_centering_direction > 0 and i == 2) or \
(lane_centering_direction < 0 and i == 1)
line_color = OCEAN_BLUE_LANE_LINES_COLOR if lane_centering_line else lane_lines_color
color = with_alpha(line_color, int(alpha * line_color.a))
color = with_alpha(lane_lines_color, int(alpha * lane_lines_color.a))
draw_polygon(self._rect, lane_line.projected_points, color)
for i, road_edge in enumerate(self._road_edges):
+290 -63
View File
@@ -11,6 +11,11 @@ from openpilot.system.ui.lib.application import gui_app
from openpilot.system.ui.lib.text_measure import measure_text_cached
from openpilot.selfdrive.ui.onroad.starpilot.starpilot_border import _csc_state, _intensity, _glow_color
from openpilot.selfdrive.ui.lib.starpilot_status import get_border_color
from openpilot.selfdrive.ui.onroad.starpilot.widget_style import (
CONTROL_BORDER,
CONTROL_BORDER_WIDTH,
draw_control_card,
)
# --- Scale factor (single knob for all pixel-space dimensions) ---
@@ -35,6 +40,31 @@ ROAD_THICKNESS = 4.0 * SCALE
ROAD_HALF_SIZE = 40.0 * SCALE
ROAD_EDGE_INSET = 2.0 * SCALE
FILL_ALPHA = 90
ROAD_CONTOUR_WIDTH = 2.0 * SCALE
# The gauge overlays an unconstrained camera image. It shares the visible
# control-card frame used by Set Speed and MAP, then owns two contained
# viewports: animated road graphics above, glanceable status metrics below.
# Nothing may draw outside the card.
AETHER_CONTENT_INSET_X = max(5.0 * SCALE, CONTROL_BORDER_WIDTH / 2.0 + 3.0 * SCALE)
AETHER_ROAD_TOP_INSET = max(5.0 * SCALE, CONTROL_BORDER_WIDTH / 2.0 + 3.0 * SCALE)
AETHER_ROAD_VIEWPORT_HEIGHT = 75.0 * SCALE
AETHER_ROAD_TO_CRADLE_GAP = 2.0 * SCALE
AETHER_CRADLE_TOP = 83.0 * SCALE
AETHER_CRADLE_BOTTOM_INSET = max(2.0 * SCALE, CONTROL_BORDER_WIDTH / 2.0 + 2.0 * SCALE)
AETHER_LABEL_SIZE = int(16.0 * SCALE)
AETHER_LABEL_SLOT_HEIGHT = AETHER_LABEL_SIZE
AETHER_VALUE_GAP = 2.0 * SCALE
AETHER_VALUE_MAX_SIZE = int(40.0 * SCALE)
AETHER_VALUE_MIN_SIZE = int(24.0 * SCALE)
AETHER_LEAD_MAX_SIZE = int(32.0 * SCALE)
AETHER_LEAD_MIN_SIZE = int(18.0 * SCALE)
AETHER_REDUCTION_SIZE = int(16.0 * SCALE)
AETHER_REDUCTION_GAP = 4.0 * SCALE
AETHER_ACCENT_GAP = 2.0 * SCALE
AETHER_UNIT_SIZE = int(18.0 * SCALE)
AETHER_UNIT_GAP = 2.0 * SCALE
STOP_SNAP_THRESHOLD = 0.5
STOP_LERP_RATE = 0.25
@@ -51,7 +81,10 @@ LEAD_STOPPED_SPEED_THRESHOLD = 1.0
COLOR_FORCE_STOP = rl.Color(255, 30, 60, 255)
COLOR_LEAD_STOPPED = rl.Color(255, 60, 60, 255)
COLOR_LEAD_SLOWER = rl.Color(255, 191, 0, 255)
COLOR_SHADOW = rl.Color(0, 0, 0, 100)
COLOR_CONTOUR = rl.Color(2, 6, 9, 235)
COLOR_AETHER_CARD = rl.Color(9, 14, 18, 226)
COLOR_PRIMARY_TEXT = rl.Color(245, 250, 252, 255)
COLOR_SECONDARY_TEXT = rl.Color(225, 235, 240, 235)
COLOR_STOP_SIGN_OUTLINE = rl.Color(255, 255, 255, 255)
COLOR_STOP_LINE_GLOW = rl.Color(255, 30, 60, 255)
COLOR_STOP_LINE_CORE = rl.Color(255, 200, 200, 255)
@@ -150,12 +183,119 @@ def _get_perspective_offset(t: float, data: 'AetherGaugeData | None') -> float:
max_offset_top = math.tanh(path_y_far * PERSPECTIVE_GAIN) * PERSPECTIVE_MAX_OFFSET
return max_offset_top * (t ** PERSPECTIVE_EXPONENT)
@dataclass(frozen=True)
class AetherGaugeLayout:
card: rl.Rectangle
road: rl.Rectangle
cradle: rl.Rectangle
def _snapped_rect(x: float, y: float, width: float, height: float) -> rl.Rectangle:
return rl.Rectangle(
int(round(x)), int(round(y)), max(1, int(round(width))), max(1, int(round(height))),
)
def _aether_layout(rect: rl.Rectangle) -> AetherGaugeLayout:
"""Return the one geometry contract used by every AetherGauge primitive."""
# Match the Set Speed and MAP frame exactly; internal viewports provide the
# clearance needed by the denser AetherGauge road and metric visuals.
card = _snapped_rect(rect.x, rect.y, rect.width, rect.height)
cradle_top = int(round(card.y + AETHER_CRADLE_TOP))
cradle_bottom = int(round(card.y + card.height - AETHER_CRADLE_BOTTOM_INSET))
road_y = int(round(card.y + AETHER_ROAD_TOP_INSET))
road_height = min(
int(round(AETHER_ROAD_VIEWPORT_HEIGHT)),
max(1, cradle_top - road_y - int(round(AETHER_ROAD_TO_CRADLE_GAP))),
)
content_x = int(round(card.x + AETHER_CONTENT_INSET_X))
content_width = max(1, int(round(card.width - 2.0 * AETHER_CONTENT_INSET_X)))
return AetherGaugeLayout(
card=card,
road=_snapped_rect(content_x, road_y, content_width, road_height),
cradle=_snapped_rect(content_x, cradle_top, content_width, max(1, cradle_bottom - cradle_top)),
)
def _draw_aether_card(layout: AetherGaugeLayout, alpha: float = 1.0) -> None:
"""Draw the shared control frame with AetherGauge's deeper contrast fill."""
draw_control_card(
layout.card,
fill=_fade(COLOR_AETHER_CARD, alpha),
border=_fade(CONTROL_BORDER, alpha),
)
def _text_outline_px(size: int) -> int:
return max(1, int(round(size / (28.0 * SCALE))))
def _draw_text_with_shadow(font: rl.Font, text: str, pos: rl.Vector2, size: int, color: rl.Color, alpha: float = 1.0):
for dx, dy in ((-1, -1), (1, -1), (-1, 1), (1, 1)):
rl.draw_text_ex(font, text, rl.Vector2(pos.x + dx, pos.y + dy), size, 0, _fade(rl.BLACK, alpha))
outline_px = _text_outline_px(size)
for dx, dy in (
(-outline_px, 0), (outline_px, 0), (0, -outline_px), (0, outline_px),
(-outline_px, -outline_px), (outline_px, -outline_px),
(-outline_px, outline_px), (outline_px, outline_px),
):
rl.draw_text_ex(font, text, rl.Vector2(pos.x + dx, pos.y + dy), size, 0, _fade(COLOR_CONTOUR, alpha))
rl.draw_text_ex(font, text, pos, size, 0, _fade(color, alpha))
def _fit_text_size(font: rl.Font, text: str, max_size: int, min_size: int,
max_width: float, max_height: float) -> tuple[int, rl.Vector2]:
"""Measure down to a guaranteed-safe text size for the supplied content box."""
for size in range(max_size, min_size - 1, -1):
text_size = measure_text_cached(font, text, size)
outline = _text_outline_px(size)
if text_size.x + 2 * outline <= max_width and text_size.y + 2 * outline <= max_height:
return size, text_size
# Keep pathological future values contained even if they exceed the intended
# type scale. Normal gauge data never reaches this fallback.
for size in range(min_size - 1, 0, -1):
text_size = measure_text_cached(font, text, size)
outline = _text_outline_px(size)
if text_size.x + 2 * outline <= max_width and text_size.y + 2 * outline <= max_height:
return size, text_size
return 1, measure_text_cached(font, text, 1)
def _fit_numeric_line(font_bold: rl.Font, font_medium: rl.Font, value: str, reduction: str,
max_width: float, max_height: float) -> 'tuple[int, rl.Vector2, rl.Vector2 | None, bool]':
"""Fit the primary value and optional reduction inline, or reflow the reduction."""
reduction_size = measure_text_cached(font_medium, reduction, AETHER_REDUCTION_SIZE) if reduction else None
for value_font_size in range(AETHER_VALUE_MAX_SIZE, 0, -1):
value_size = measure_text_cached(font_bold, value, value_font_size)
value_outline = _text_outline_px(value_font_size)
if value_size.y + 2 * value_outline > max_height:
continue
if reduction_size is None:
if value_size.x + 2 * value_outline <= max_width:
return value_font_size, value_size, None, True
continue
reduction_outline = _text_outline_px(AETHER_REDUCTION_SIZE)
inline_width = value_size.x + AETHER_REDUCTION_GAP + reduction_size.x + 2 * max(value_outline, reduction_outline)
if inline_width <= max_width:
return value_font_size, value_size, reduction_size, True
if value_font_size == AETHER_VALUE_MIN_SIZE:
break
# The normal source values fit inline. This fallback keeps a future long
# value contained by using the reserved label row for the reduction.
value_font_size, value_size = _fit_text_size(
font_bold, value, AETHER_VALUE_MIN_SIZE, 1, max_width, max_height,
)
return value_font_size, value_size, reduction_size, False
# --- Data model ---
class IndicatorType(Enum):
@@ -178,6 +318,14 @@ class AetherGaugeData:
is_numeric: bool = False
def _state_label(data: AetherGaugeData) -> str:
return {
IndicatorType.FORCE_STOP: "STOP",
IndicatorType.STOP_LIGHT: "RED LIGHT",
IndicatorType.LEAD: "LEAD",
}.get(data.indicator_type, "")
# --- Source functions (replaces class-based sources) ---
def _build_curve_gauge_data(curvature: float, target_speed: float, v_cruise: float) -> AetherGaugeData:
@@ -460,16 +608,13 @@ class AetherGauge:
if not data:
return
if cx is None or bottom is None:
if cx is None:
base_cx = rect.x + rect.width / 2
cy_speed = rect.y + 180 * SCALE
speed_text = str(round(current_speed))
speed_text_size = measure_text_cached(font_bold, speed_text, int(176 * SCALE))
icx = base_cx - speed_text_size.x / 2 - 70.0 * SCALE
icy = cy_speed - 39.5 * SCALE
else:
icx = cx
icy = bottom - ROAD_HALF_SIZE
if data.indicator_type != self._last_indicator_type:
self._dist_filter.x = data.indicator_value
@@ -497,60 +642,71 @@ class AetherGauge:
data.unit = unit
if data.indicator_type in (IndicatorType.ROAD_CURVE, IndicatorType.FORCE_STOP, IndicatorType.LEAD, IndicatorType.STOP_LIGHT):
self._render_unified_road(rect, icx, icy, data, font_bold, font_medium, alpha)
self._render_unified_road(rect, icx, data, font_bold, font_medium, alpha)
def _render_unified_road(self, rect, icx, icy, data, font_bold, font_medium, alpha=1.0):
bottom = icy + ROAD_HALF_SIZE
def _render_unified_road(self, rect, icx, data, font_bold, font_medium, alpha=1.0):
layout = _aether_layout(rect)
_draw_aether_card(layout, alpha)
bottom = layout.road.y + layout.road.height
road_icy = bottom - ROAD_HALF_SIZE
if data.indicator_type in (IndicatorType.FORCE_STOP, IndicatorType.STOP_LIGHT):
distance = 15.0
else:
distance = data.indicator_value
points_left = []
points_right = []
self._current_road_h = self._road_h_filter.update(_get_road_height(data))
road_h = self._current_road_h
# Custom widgets render after AugmentedRoadView releases its content
# scissor. Keep every animated primitive inside the road viewport here.
rl.begin_scissor_mode(
int(layout.road.x), int(layout.road.y), int(layout.road.width), int(layout.road.height),
)
try:
points_left = []
points_right = []
self._current_road_h = min(self._road_h_filter.update(_get_road_height(data)), layout.road.height)
road_h = self._current_road_h
for i in range(ROAD_SEGMENTS + 1):
t = i / ROAD_SEGMENTS
offset = _get_perspective_offset(t, data)
cx_t = icx + offset
y_t = bottom - t * road_h
w_t = ROAD_W_BOTTOM - t * (ROAD_W_BOTTOM - ROAD_W_TOP)
for i in range(ROAD_SEGMENTS + 1):
t = i / ROAD_SEGMENTS
offset = _get_perspective_offset(t, data)
cx_t = icx + offset
y_t = bottom - t * road_h
w_t = ROAD_W_BOTTOM - t * (ROAD_W_BOTTOM - ROAD_W_TOP)
points_left.append(rl.Vector2(cx_t - w_t, y_t))
points_right.append(rl.Vector2(cx_t + w_t, y_t))
points_left.append(rl.Vector2(cx_t - w_t, y_t))
points_right.append(rl.Vector2(cx_t + w_t, y_t))
fill_color = _fade(_with_alpha(data.color, FILL_ALPHA), alpha)
for i in range(ROAD_SEGMENTS):
t = i / ROAD_SEGMENTS
stroke = ROAD_THICKNESS * (1.0 - 0.6 * t)
rl.draw_triangle(points_left[i], points_right[i], points_left[i+1], fill_color)
rl.draw_triangle(points_right[i], points_right[i+1], points_left[i+1], fill_color)
rl.draw_line_ex(points_left[i], points_left[i+1], stroke, _fade(COLOR_SHADOW, alpha))
rl.draw_line_ex(points_right[i], points_right[i+1], stroke, _fade(COLOR_SHADOW, alpha))
rl.draw_line_ex(points_left[i], points_left[i+1], stroke, _fade(data.color, alpha))
rl.draw_line_ex(points_right[i], points_right[i+1], stroke, _fade(data.color, alpha))
fill_color = _fade(_with_alpha(data.color, FILL_ALPHA), alpha)
for i in range(ROAD_SEGMENTS):
t = i / ROAD_SEGMENTS
stroke = ROAD_THICKNESS * (1.0 - 0.6 * t)
rl.draw_triangle(points_left[i], points_right[i], points_left[i+1], fill_color)
rl.draw_triangle(points_right[i], points_right[i+1], points_left[i+1], fill_color)
rl.draw_line_ex(points_left[i], points_left[i+1], stroke + ROAD_CONTOUR_WIDTH, _fade(COLOR_CONTOUR, alpha))
rl.draw_line_ex(points_right[i], points_right[i+1], stroke + ROAD_CONTOUR_WIDTH, _fade(COLOR_CONTOUR, alpha))
rl.draw_line_ex(points_left[i], points_left[i+1], stroke, _fade(data.color, alpha))
rl.draw_line_ex(points_right[i], points_right[i+1], stroke, _fade(data.color, alpha))
it = data.indicator_type
it = data.indicator_type
if it in (IndicatorType.STOP_LIGHT, IndicatorType.FORCE_STOP):
self._draw_stop_line(icx, bottom, distance, data, alpha)
if it in (IndicatorType.STOP_LIGHT, IndicatorType.FORCE_STOP):
self._draw_stop_line(icx, bottom, distance, data, alpha)
if it == IndicatorType.LEAD:
self._draw_lead_car(icx, bottom, data, alpha)
if it == IndicatorType.LEAD:
self._draw_lead_car(icx, bottom, data, alpha)
if it in (IndicatorType.ROAD_CURVE, IndicatorType.FORCE_STOP, IndicatorType.STOP_LIGHT):
self._draw_standard_chevrons(icx, bottom, distance, data, alpha)
if it in (IndicatorType.ROAD_CURVE, IndicatorType.FORCE_STOP, IndicatorType.STOP_LIGHT):
self._draw_standard_chevrons(icx, bottom, distance, data, alpha)
if it == IndicatorType.STOP_LIGHT:
self._draw_traffic_light(icx, icy, distance, data, alpha)
if it == IndicatorType.STOP_LIGHT:
self._draw_traffic_light(icx, road_icy, distance, data, alpha)
if it == IndicatorType.FORCE_STOP:
self._draw_approaching_stop_sign(icx, icy, bottom, distance, data, font_bold, alpha)
if it == IndicatorType.FORCE_STOP:
self._draw_approaching_stop_sign(icx, road_icy, bottom, distance, data, font_bold, alpha)
finally:
rl.end_scissor_mode()
self._draw_mini_cradle(icx, bottom, data, font_bold, font_medium, alpha)
self._draw_mini_cradle(layout.cradle, data, font_bold, font_medium, alpha)
def _draw_stop_line(self, icx, bottom, distance, data, alpha=1.0):
t, cx_line, cy_line = _road_xy(distance, icx, bottom, data, self._current_road_h)
@@ -562,6 +718,10 @@ class AetherGauge:
fade = 1.0 - t * 0.5
glow_a = int(150 * fade)
rl.draw_line_ex(
p_left, p_right, max(3.0 * SCALE, 7.0 * SCALE * fade) + ROAD_CONTOUR_WIDTH,
_fade(COLOR_CONTOUR, alpha),
)
rl.draw_line_ex(p_left, p_right, max(3.0 * SCALE, 7.0 * SCALE * fade), _fade(_with_alpha(COLOR_STOP_LINE_GLOW, glow_a), alpha))
rl.draw_line_ex(p_left, p_right, max(1.5 * SCALE, 3.5 * SCALE * fade), _fade(COLOR_STOP_LINE_CORE, alpha))
@@ -642,7 +802,10 @@ class AetherGauge:
chev_a = max(0, min(255, int(data.color.a * (1.0 - t / t_lead) * math.sin(t / t_lead * math.pi))))
c_color = _fade(_with_alpha(data.color, chev_a), alpha)
c_contour = _fade(_with_alpha(COLOR_CONTOUR, int(chev_a * 0.9)), alpha)
rl.draw_line_ex(rl.Vector2(lx, cy_t - chevron_w * 0.5), rl.Vector2(cx_t, cy_t), chevron_thick + ROAD_CONTOUR_WIDTH, c_contour)
rl.draw_line_ex(rl.Vector2(rx, cy_t - chevron_w * 0.5), rl.Vector2(cx_t, cy_t), chevron_thick + ROAD_CONTOUR_WIDTH, c_contour)
rl.draw_line_ex(rl.Vector2(lx, cy_t - chevron_w * 0.5), rl.Vector2(cx_t, cy_t), chevron_thick, c_color)
rl.draw_line_ex(rl.Vector2(rx, cy_t - chevron_w * 0.5), rl.Vector2(cx_t, cy_t), chevron_thick, c_color)
@@ -690,7 +853,10 @@ class AetherGauge:
chev_a = max(0, min(255, int(data.color.a * alpha_factor)))
chev_color = _fade(_with_alpha(data.color, chev_a), alpha)
chev_shadow = _fade(rl.Color(0, 0, 0, int(chev_a * 0.5)), alpha)
chev_contour = _fade(_with_alpha(COLOR_CONTOUR, int(chev_a * 0.9)), alpha)
rl.draw_line_ex(rl.Vector2(lx, ly), rl.Vector2(cx_t, cy_t), chevron_thick + ROAD_CONTOUR_WIDTH, chev_contour)
rl.draw_line_ex(rl.Vector2(rx, ry), rl.Vector2(cx_t, cy_t), chevron_thick + ROAD_CONTOUR_WIDTH, chev_contour)
rl.draw_line_ex(rl.Vector2(lx, ly + 1.5), rl.Vector2(cx_t, cy_t + 1.5), chevron_thick, chev_shadow)
rl.draw_line_ex(rl.Vector2(rx, ry + 1.5), rl.Vector2(cx_t, cy_t + 1.5), chevron_thick, chev_shadow)
rl.draw_line_ex(rl.Vector2(lx, ly), rl.Vector2(cx_t, cy_t), chevron_thick, chev_color)
@@ -761,18 +927,53 @@ class AetherGauge:
stop_txt_size = measure_text_cached(font_bold, "STOP", stop_font_size)
rl.draw_text_ex(font_bold, "STOP", rl.Vector2(cx_stop - stop_txt_size.x / 2, y_sign - stop_txt_size.y / 2), stop_font_size, 0, _fade(rl.WHITE, alpha))
def _draw_mini_cradle(self, cx, bottom, data, font_bold, font_medium, alpha=1.0):
def _draw_mini_cradle(self, cradle, data, font_bold, font_medium, alpha=1.0):
if not data.text:
return
if data.is_numeric:
val_size = measure_text_cached(font_bold, data.text, int(50 * SCALE))
val_pos = rl.Vector2(int(cx - val_size.x / 2), int(bottom + 6 * SCALE))
_draw_text_with_shadow(font_bold, data.text, val_pos, int(50 * SCALE), data.color, alpha)
label = _state_label(data)
metric_top = int(round(cradle.y + AETHER_LABEL_SLOT_HEIGHT + AETHER_VALUE_GAP))
cradle_right = cradle.x + cradle.width
accent_y = int(val_pos.y + val_size.y + 2 * SCALE)
accent_w = int(val_size.x + 16 * SCALE)
accent_x = int(cx - accent_w / 2)
if data.is_numeric:
unit_size = measure_text_cached(font_medium, data.unit, AETHER_UNIT_SIZE) if data.unit else None
footer_height = AETHER_ACCENT_GAP
if unit_size is not None:
footer_height += AETHER_UNIT_GAP + unit_size.y
max_metric_height = max(1.0, cradle.y + cradle.height - metric_top - footer_height)
max_metric_width = max(1.0, cradle.width - 2 * _text_outline_px(AETHER_VALUE_MAX_SIZE))
value_font_size, value_size, reduction_size, reduction_inline = _fit_numeric_line(
font_bold, font_medium, data.text, data.reduction_text, max_metric_width, max_metric_height,
)
value_outline = _text_outline_px(value_font_size)
reduction_outline = _text_outline_px(AETHER_REDUCTION_SIZE) if reduction_size is not None else 0
group_width = value_size.x
if reduction_size is not None and reduction_inline:
group_width += AETHER_REDUCTION_GAP + reduction_size.x
value_x = int(round(cradle.x + (cradle.width - group_width) / 2))
value_pos = rl.Vector2(value_x, metric_top)
label_max_width = cradle.width - 2 * _text_outline_px(AETHER_LABEL_SIZE)
if label and reduction_size is not None and not reduction_inline:
label_max_width -= reduction_size.x + AETHER_REDUCTION_GAP + reduction_outline
if label:
label_font_size, label_size = _fit_text_size(
font_medium, label, AETHER_LABEL_SIZE, 1, max(1.0, label_max_width), AETHER_LABEL_SLOT_HEIGHT,
)
label_x = cradle.x + value_outline if reduction_size is not None and not reduction_inline else cradle.x + (cradle.width - label_size.x) / 2
label_y = cradle.y + (AETHER_LABEL_SLOT_HEIGHT - label_size.y) / 2
_draw_text_with_shadow(
font_medium, label, rl.Vector2(int(round(label_x)), int(round(label_y))),
label_font_size, COLOR_PRIMARY_TEXT, alpha,
)
_draw_text_with_shadow(font_bold, data.text, value_pos, value_font_size, COLOR_PRIMARY_TEXT, alpha)
accent_y = int(round(value_pos.y + value_size.y + AETHER_ACCENT_GAP))
accent_w = min(max_metric_width, value_size.x + 16 * SCALE)
accent_x = int(round(value_pos.x + value_size.x / 2 - accent_w / 2))
accent_x = max(int(round(cradle.x + value_outline)), min(accent_x, int(round(cradle_right - value_outline - accent_w))))
rl.draw_line_ex(
rl.Vector2(accent_x, accent_y),
rl.Vector2(accent_x + accent_w, accent_y),
@@ -780,16 +981,42 @@ class AetherGauge:
_fade(_with_alpha(data.color, 160), alpha),
)
if data.reduction_text:
red_size = measure_text_cached(font_medium, data.reduction_text, int(22 * SCALE))
red_pos = rl.Vector2(int(cx + val_size.x / 2 + 6 * SCALE), int(val_pos.y + val_size.y / 2 - red_size.y / 2))
_draw_text_with_shadow(font_medium, data.reduction_text, red_pos, int(22 * SCALE), COLOR_REDUCTION, alpha)
if reduction_size is not None:
if reduction_inline:
reduction_x = int(round(value_pos.x + value_size.x + AETHER_REDUCTION_GAP))
reduction_y = int(round(value_pos.y + (value_size.y - reduction_size.y) / 2))
else:
reduction_x = int(round(cradle_right - reduction_size.x - reduction_outline))
reduction_y = int(round(cradle.y + (AETHER_LABEL_SLOT_HEIGHT - reduction_size.y) / 2))
_draw_text_with_shadow(
font_medium, data.reduction_text, rl.Vector2(reduction_x, reduction_y),
AETHER_REDUCTION_SIZE, COLOR_REDUCTION, alpha,
)
if data.unit:
unit_size = measure_text_cached(font_medium, data.unit, int(20 * SCALE))
unit_pos = rl.Vector2(int(cx - unit_size.x / 2), int(accent_y + 3 * SCALE))
_draw_text_with_shadow(font_medium, data.unit, unit_pos, int(20 * SCALE), rl.Color(255, 255, 255, 180), alpha)
if unit_size is not None:
unit_pos = rl.Vector2(
int(round(cradle.x + (cradle.width - unit_size.x) / 2)),
int(round(accent_y + AETHER_UNIT_GAP)),
)
_draw_text_with_shadow(font_medium, data.unit, unit_pos, AETHER_UNIT_SIZE, COLOR_SECONDARY_TEXT, alpha)
else:
val_size = measure_text_cached(font_bold, data.text, int(32 * SCALE))
val_pos = rl.Vector2(int(cx - val_size.x / 2), int(bottom + 10 * SCALE))
_draw_text_with_shadow(font_bold, data.text, val_pos, int(32 * SCALE), data.color, alpha)
if label:
label_font_size, label_size = _fit_text_size(
font_medium, label, AETHER_LABEL_SIZE, 1,
cradle.width - 2 * _text_outline_px(AETHER_LABEL_SIZE), AETHER_LABEL_SLOT_HEIGHT,
)
label_pos = rl.Vector2(
int(round(cradle.x + (cradle.width - label_size.x) / 2)),
int(round(cradle.y + (AETHER_LABEL_SLOT_HEIGHT - label_size.y) / 2)),
)
_draw_text_with_shadow(font_medium, label, label_pos, label_font_size, COLOR_PRIMARY_TEXT, alpha)
lead_font_size, lead_size = _fit_text_size(
font_bold, data.text, AETHER_LEAD_MAX_SIZE, AETHER_LEAD_MIN_SIZE,
cradle.width - 2 * _text_outline_px(AETHER_LEAD_MAX_SIZE),
cradle.y + cradle.height - metric_top,
)
lead_pos = rl.Vector2(
int(round(cradle.x + (cradle.width - lead_size.x) / 2)), metric_top,
)
_draw_text_with_shadow(font_bold, data.text, lead_pos, lead_font_size, COLOR_PRIMARY_TEXT, alpha)
+405 -143
View File
@@ -9,12 +9,12 @@ from openpilot.system.ui.lib.application import gui_app, FontWeight
from openpilot.system.ui.lib.multilang import tr
from openpilot.system.ui.lib.text_measure import measure_text_cached
from openpilot.selfdrive.ui.onroad.starpilot.widget_style import (
CONTROL_BG, CONTROL_BORDER, CONTROL_BORDER_WIDTH, CONTROL_ROUNDNESS, CONTROL_SEGMENTS, SLC_HEIGHT,
CONTROL_BG, CONTROL_BORDER, CONTROL_BORDER_WIDTH, CONTROL_ROUNDNESS, CONTROL_SEGMENTS,
draw_control_card, roundness_for,
)
from openpilot.selfdrive.ui.onroad.starpilot.source_bubble_layout import (
enabled_source_titles, fit_source_label, source_abbreviated_value_text,
source_content_metrics, source_value_text, visible_source_rows,
SourceBubbleModel, SourceBubbleTransition, enabled_source_titles,
source_value_text, visible_source_rows,
)
from openpilot.selfdrive.ui.lib.starpilot_state import starpilot_state
@@ -35,7 +35,7 @@ SOURCE_DEFS = [
("Dashboard", "Dash", "dashboard_sl", "Dashboard", "dashboard"),
("Map Data", "MAP", "map_sl", "Map Data", "map"),
("Vision", "VISION", "vision_sl", "Vision", "camera"),
("Mapbox", "MBOX", "mapbox_sl", "Mapbox", "map"),
("Mapbox", "MBOX", "mapbox_sl", "Mapbox", "navigation"),
("Upcoming", "NEXT", "next_sl", "Next", "next"),
]
@@ -179,7 +179,6 @@ def _get_slc_state():
'offset_str': offset_str,
'speed_conversion': speed_conversion,
'speed_unit': " km/h" if ui_state.is_metric else " mph",
'slc_abbreviated_sources': params.get_bool("SLCAbbreviatedSources"),
'slc_active_sources_only': params.get_bool("SLCActiveSourcesOnly"),
'slc_enabled_sources': enabled_source_titles(
primary_priority,
@@ -417,27 +416,33 @@ def _draw_sign(state: dict, rect: rl.Rectangle, *, pending: bool = False):
# Fixed outer footprint; the content scale adapts to the visible row count.
_SOURCE_PANEL_WIDTH = 248
_SOURCE_PANEL_GAP = 20
_SOURCE_PANEL_PAD_X = 9
_SOURCE_PANEL_PAD_Y = 2
_SOURCE_PANEL_PAD_X = 10
_SOURCE_PANEL_PAD_Y = 8
_SOURCE_PANEL_BG = rl.Color(0, 0, 0, 175)
_SOURCE_PANEL_BORDER = rl.Color(196, 205, 208, 80)
_SOURCE_DIVIDER = rl.Color(196, 205, 208, 100)
_SOURCE_ACTIVE_BAR = rl.Color(CONTROL_BORDER.r, CONTROL_BORDER.g, CONTROL_BORDER.b, 230)
_SOURCE_ICON_MUTED = rl.Color(160, 170, 175, 200)
_SOURCE_LABEL_MUTED = rl.Color(166, 166, 166, 255)
_SOURCE_ACTIVE_BAR_WIDTH = 6.0
_SOURCE_ACTIVE_BAR_HEIGHT = 36.0
_SOURCE_ACTIVE_BAR_X = 2.0
_SOURCE_ACTIVE_BAR_ROW_INSET = 3.0
_SOURCE_ICON_MUTED = rl.Color(184, 194, 198, 230)
_SOURCE_LABEL_MUTED = rl.Color(205, 211, 214, 240)
_SOURCE_MISSING = rl.Color(155, 166, 171, 220)
_SOURCE_ACTIVE_BAR_WIDTH = 5.0
_SOURCE_ACTIVE_BAR_X = 6.0
_SOURCE_ACTIVE_BAR_CORNER_INSET = 20.0
_SOURCE_ACTIVE_BAR_MIN_HEIGHT = 20.0
_SOURCE_MIN_LABEL_VALUE_GAP = 6.0
_SOURCE_COMPACT_LABELS = {
"Dashboard": "Dash",
"Map Data": "OSM",
"Vision": "Vision",
"Mapbox": "Mapbox",
"Next": "Next",
}
_SOURCE_READABLE_ICON_SIZE = 32.0
_SOURCE_READABLE_ICON_GAP = 8.0
_SOURCE_LABEL_FONT = 28
_SOURCE_MIN_LABEL_FONT = 18
_SOURCE_VALUE_FONT = 38
_SOURCE_DENSE_VALUE_FONT = 38
_SOURCE_DENSE_FOOTER_VALUE_FONT = 36
_SOURCE_DENSE_ICON_SIZE = 32
_SOURCE_DENSE_SMALL_ICON_SIZE = 28
_SOURCE_DENSE_GAP = 8.0
_SOURCE_DENSE_FOOTER_HEIGHT = 60.0
_SOURCE_MIN_VALUE_FONT = 30
_SOURCE_MIN_MESSAGE_FONT = 20
def _draw_source_icon(icon_key: str, x: float, y: float, size: float, color: rl.Color) -> None:
@@ -477,7 +482,7 @@ def _draw_source_icon(icon_key: str, x: float, y: float, size: float, color: rl.
rl.draw_rectangle_rounded(body, 0.20, 8, color)
lens = rl.Vector2(cx, y + size * 0.54)
lens_outer = size * 0.17
rl.draw_circle_v(lens, lens_outer, _SOURCE_PANEL_BG)
rl.draw_circle_v(lens, lens_outer, _color_with_alpha(_SOURCE_PANEL_BG, color.a))
rl.draw_ring(lens, size * 0.105, lens_outer, 0, 360, max(24, int(size * 0.25)), color)
rl.draw_rectangle_rounded(
rl.Rectangle(x + size * 0.30, y + size * 0.18, size * 0.23, size * 0.15),
@@ -503,7 +508,7 @@ def _draw_source_icon(icon_key: str, x: float, y: float, size: float, color: rl.
rl.Vector2(cx, y + size * 0.86),
color,
)
rl.draw_circle_v(pin_center, size * 0.09, _SOURCE_PANEL_BG)
rl.draw_circle_v(pin_center, size * 0.09, _color_with_alpha(_SOURCE_PANEL_BG, color.a))
else: # Dashboard / fallback
dashboard_scale = 1.22
pivot = rl.Vector2(cx, cy + size * 0.17)
@@ -536,32 +541,355 @@ def _draw_source_icon(icon_key: str, x: float, y: float, size: float, color: rl.
rl.draw_circle_v(pivot, max(2.0, size * 0.06 * dashboard_scale), color)
def _draw_sources_bubble_empty_state(panel_rect: rl.Rectangle) -> None:
"""Draw the 3-line centered empty state when no sources are available."""
font = _get_semi_bold()
font_size = 30
line_gap = 6.0
lines = (tr("NO"), tr("SOURCES"), tr("AVAILABLE"))
line_sizes = [measure_text_cached(font, line, font_size) for line in lines]
total_h = sum(sz.y for sz in line_sizes) + line_gap * (len(lines) - 1)
curr_y = round(panel_rect.y + (panel_rect.height - total_h) / 2)
for line, sz in zip(lines, line_sizes):
pos_x = round(panel_rect.x + (panel_rect.width - sz.x) / 2)
rl.draw_text_ex(font, line, rl.Vector2(pos_x, curr_y), font_size, 0, _WHITE)
curr_y += round(sz.y + line_gap)
def _color_with_alpha(color: rl.Color, alpha: int) -> rl.Color:
return rl.Color(color.r, color.g, color.b, round(color.a * alpha / 255))
def _draw_sources_bubble(state: dict, sign_rect: rl.Rectangle):
"""Draw the expanded source list attached to the SLC card."""
def _source_model(state: dict) -> SourceBubbleModel:
enabled_sources = state.get('slc_enabled_sources', ())
rows = visible_source_rows(
SOURCE_DEFS,
state,
state['speed_limit_source'],
enabled_sources,
state.get('slc_active_sources_only', False),
)
if rows:
return SourceBubbleModel(rows)
reason = "No sources" if not enabled_sources else "No data"
return SourceBubbleModel((), reason)
def _fit_font_to_height(font, text: str, requested_size: int, max_height: float, minimum_size: int) -> int:
size = requested_size
while size > minimum_size and measure_text_cached(font, text, size).y > max_height:
size -= 2
return size
def _fit_readable_label(font_semi, label_text: str, max_width: float, initial_size: int = _SOURCE_LABEL_FONT) -> int:
"""Scale readable label font down if constrained by long translations."""
size = initial_size
while size > _SOURCE_MIN_LABEL_FONT and measure_text_cached(font_semi, label_text, size).x > max_width:
size -= 2
return size
def _draw_sources_bubble_empty_state(panel_rect: rl.Rectangle, reason: str, alpha: int, font_semi) -> None:
message = tr("No sources") if reason == "No sources" else tr("No data")
font_size = _SOURCE_LABEL_FONT
max_width = panel_rect.width - 2 * _SOURCE_PANEL_PAD_X
max_height = panel_rect.height - 2 * _SOURCE_PANEL_PAD_Y
size = measure_text_cached(font_semi, message, font_size)
while font_size > _SOURCE_MIN_MESSAGE_FONT and (size.x > max_width or size.y > max_height):
font_size -= 2
size = measure_text_cached(font_semi, message, font_size)
rl.draw_text_ex(
font_semi,
message,
rl.Vector2(round(panel_rect.x + (panel_rect.width - size.x) / 2),
round(panel_rect.y + (panel_rect.height - size.y) / 2)),
font_size,
0,
_color_with_alpha(_SOURCE_LABEL_MUTED, alpha),
)
def _source_text_color(item, alpha: int) -> rl.Color:
if not item.has_reading:
return _color_with_alpha(_SOURCE_MISSING, alpha)
return _color_with_alpha(_WHITE if item.is_active else _SOURCE_LABEL_MUTED, alpha)
def _source_icon_color(item, alpha: int) -> rl.Color:
if not item.has_reading:
return _color_with_alpha(_SOURCE_MISSING, alpha)
return _color_with_alpha(_WHITE if item.is_active else _SOURCE_ICON_MUTED, alpha)
def _draw_readable_sources(
model: SourceBubbleModel,
panel_rect: rl.Rectangle,
alpha: int,
font_semi,
font_bold,
*,
draw_chrome: bool,
) -> None:
content_left = panel_rect.x + _SOURCE_PANEL_PAD_X
content_right = panel_rect.x + panel_rect.width - _SOURCE_PANEL_PAD_X
content_top = panel_rect.y + _SOURCE_PANEL_PAD_Y
content_height = panel_rect.height - 2 * _SOURCE_PANEL_PAD_Y
row_height = content_height / len(model.items)
value_font = _fit_font_to_height(
font_bold,
"000",
_SOURCE_VALUE_FONT,
max(1.0, row_height - 4),
_SOURCE_MIN_VALUE_FONT,
)
icon_size = _SOURCE_READABLE_ICON_SIZE
icon_left = content_left + _SOURCE_ACTIVE_BAR_WIDTH + _SOURCE_MIN_LABEL_VALUE_GAP
label_left = icon_left + icon_size + _SOURCE_READABLE_ICON_GAP
for index, item in enumerate(model.items):
row_y = content_top + index * row_height
if index and draw_chrome:
divider_y = round(row_y)
rl.draw_line_ex(
rl.Vector2(content_left, divider_y),
rl.Vector2(content_right, divider_y),
1,
_color_with_alpha(_SOURCE_DIVIDER, alpha),
)
if item.is_active and draw_chrome:
row_inset = min(12.0, row_height * 0.16)
bar_top = max(panel_rect.y + _SOURCE_ACTIVE_BAR_CORNER_INSET, row_y + row_inset)
bar_bottom = min(
panel_rect.y + panel_rect.height - _SOURCE_ACTIVE_BAR_CORNER_INSET,
row_y + row_height - row_inset,
)
active_bar_height = max(_SOURCE_ACTIVE_BAR_MIN_HEIGHT, bar_bottom - bar_top)
active_bar_rect = rl.Rectangle(
panel_rect.x + _SOURCE_ACTIVE_BAR_X,
round(bar_top),
_SOURCE_ACTIVE_BAR_WIDTH,
round(active_bar_height),
)
rl.draw_rectangle_rounded(active_bar_rect, 0.5, 4, _color_with_alpha(_SOURCE_ACTIVE_BAR, alpha))
# Icon
icon_y = round(row_y + (row_height - icon_size) / 2)
_draw_source_icon(item.icon_key, icon_left, icon_y, icon_size, _source_icon_color(item, alpha))
# Value
value_text = source_value_text(item.value)
value_size = measure_text_cached(font_bold, value_text, value_font)
value_y = round(row_y + (row_height - value_size.y) / 2)
value_pos = rl.Vector2(round(content_right - value_size.x), value_y)
# Label (fitted to available width between icon and speed value)
label_text = tr(item.label)
max_label_width = max(10.0, (content_right - value_size.x - _SOURCE_MIN_LABEL_VALUE_GAP) - label_left)
label_font = _fit_readable_label(font_semi, label_text, max_label_width, _SOURCE_LABEL_FONT)
label_size = measure_text_cached(font_semi, label_text, label_font)
label_y = round(row_y + (row_height - label_size.y) / 2)
rl.draw_text_ex(
font_semi,
label_text,
rl.Vector2(label_left, label_y),
label_font,
0,
_source_text_color(item, alpha),
)
rl.draw_text_ex(font_bold, value_text, value_pos, value_font, 0, _source_text_color(item, alpha))
def _fit_dense_group(font_bold, value_text: str, cell: rl.Rectangle, icon_size: float, value_font: int) -> tuple[float, int, rl.Vector2]:
"""Fit an icon/value group inside a dense cell without crossing its divider."""
horizontal_edge = 8.0
vertical_edge = 2.0
fitted_icon = icon_size
fitted_font = value_font
fit_text = "0" * max(3, len(value_text))
max_value_height = max(1.0, cell.height - 2 * vertical_edge)
while fitted_font >= 32:
value_size = measure_text_cached(font_bold, fit_text, fitted_font)
if (value_size.y <= max_value_height and
fitted_icon + _SOURCE_DENSE_GAP + value_size.x <= cell.width - horizontal_edge):
return fitted_icon, fitted_font, value_size
fitted_font -= 2
while fitted_icon >= 20:
value_size = measure_text_cached(font_bold, fit_text, fitted_font)
if (value_size.y <= max_value_height and
fitted_icon + _SOURCE_DENSE_GAP + value_size.x <= cell.width - horizontal_edge):
return fitted_icon, fitted_font, value_size
fitted_icon -= 2
while fitted_font >= 20:
value_size = measure_text_cached(font_bold, fit_text, fitted_font)
if (value_size.y <= max_value_height and
fitted_icon + _SOURCE_DENSE_GAP + value_size.x <= cell.width - horizontal_edge):
return fitted_icon, fitted_font, value_size
fitted_font -= 2
fitted_icon = min(fitted_icon, max(20.0, cell.height - 2 * vertical_edge))
value_size = measure_text_cached(font_bold, fit_text, fitted_font)
return fitted_icon, fitted_font, value_size
def _draw_dense_source_cell(
item,
cell: rl.Rectangle,
alpha: int,
font_bold,
*,
icon_size: float,
value_font: int,
draw_chrome: bool,
footer_label: str | None = None,
font_semi = None,
) -> None:
is_active = item.is_active
if draw_chrome:
if is_active:
rl.draw_rectangle_rounded(cell, 0.30, 8, _color_with_alpha(rl.Color(255, 255, 255, 28), alpha))
rl.draw_rectangle_rounded_lines_ex(cell, 0.30, 8, 1.0, _color_with_alpha(rl.Color(255, 255, 255, 90), alpha))
bar_h = max(16.0, cell.height - 20.0)
bar_r = rl.Rectangle(cell.x + 4.0, round(cell.y + (cell.height - bar_h) / 2), 4.0, round(bar_h))
rl.draw_rectangle_rounded(bar_r, 0.5, 4, _color_with_alpha(_SOURCE_ACTIVE_BAR, alpha))
else:
rl.draw_rectangle_rounded(cell, 0.30, 8, _color_with_alpha(rl.Color(255, 255, 255, 10), alpha))
value_text = source_value_text(item.value)
icon_size, value_font, fit_size = _fit_dense_group(font_bold, value_text, cell, icon_size, value_font)
value_size = measure_text_cached(font_bold, value_text, value_font)
if footer_label and font_semi:
label_text = tr(footer_label)
label_font = 24
label_size = measure_text_cached(font_semi, label_text, label_font)
total_group_w = icon_size + _SOURCE_DENSE_GAP + label_size.x + 16.0 + fit_size.x
group_left = cell.x + (cell.width - total_group_w) / 2
if is_active:
group_left += 2.0
icon_y = cell.y + (cell.height - icon_size) / 2
label_y = cell.y + (cell.height - label_size.y) / 2
text_y = cell.y + (cell.height - value_size.y) / 2
_draw_source_icon(item.icon_key, round(group_left), round(icon_y), icon_size, _source_icon_color(item, alpha))
rl.draw_text_ex(
font_semi,
label_text,
rl.Vector2(round(group_left + icon_size + _SOURCE_DENSE_GAP), round(label_y)),
label_font,
0,
_source_text_color(item, alpha),
)
rl.draw_text_ex(
font_bold,
value_text,
rl.Vector2(
round(group_left + icon_size + _SOURCE_DENSE_GAP + label_size.x + 16.0 + (fit_size.x - value_size.x) / 2),
round(text_y),
),
value_font,
0,
_source_text_color(item, alpha),
)
else:
total_group_w = icon_size + _SOURCE_DENSE_GAP + fit_size.x
group_left = cell.x + (cell.width - total_group_w) / 2
if is_active:
group_left += 2.0
icon_y = cell.y + (cell.height - icon_size) / 2
text_y = cell.y + (cell.height - value_size.y) / 2
_draw_source_icon(item.icon_key, round(group_left), round(icon_y), icon_size, _source_icon_color(item, alpha))
rl.draw_text_ex(
font_bold,
value_text,
rl.Vector2(
round(group_left + icon_size + _SOURCE_DENSE_GAP + (fit_size.x - value_size.x) / 2),
round(text_y),
),
value_font,
0,
_source_text_color(item, alpha),
)
def _draw_dense_sources(model: SourceBubbleModel, panel_rect: rl.Rectangle, alpha: int, font_semi, font_bold, *, draw_chrome: bool) -> None:
pad = 8.0
gap = 6.0
content_w = panel_rect.width - 2 * pad
content_h = panel_rect.height - 2 * pad
if len(model.items) == 5:
footer_h = min(44.0, content_h * 0.28)
grid_h = content_h - footer_h - gap
cell_w = (content_w - gap) / 2
cell_h = (grid_h - gap) / 2
for index, item in enumerate(model.items[:4]):
row, column = divmod(index, 2)
_draw_dense_source_cell(
item,
rl.Rectangle(
panel_rect.x + pad + column * (cell_w + gap),
panel_rect.y + pad + row * (cell_h + gap),
cell_w,
cell_h,
),
alpha,
font_bold,
icon_size=_SOURCE_DENSE_SMALL_ICON_SIZE,
value_font=_SOURCE_DENSE_FOOTER_VALUE_FONT,
draw_chrome=draw_chrome,
)
_draw_dense_source_cell(
model.items[4],
rl.Rectangle(panel_rect.x + pad, panel_rect.y + pad + grid_h + gap, content_w, footer_h),
alpha,
font_bold,
icon_size=_SOURCE_DENSE_SMALL_ICON_SIZE,
value_font=_SOURCE_DENSE_FOOTER_VALUE_FONT,
draw_chrome=draw_chrome,
footer_label=model.items[4].label,
font_semi=font_semi,
)
return
cell_w = (content_w - gap) / 2
cell_h = (content_h - gap) / 2
for index, item in enumerate(model.items):
row, column = divmod(index, 2)
_draw_dense_source_cell(
item,
rl.Rectangle(
panel_rect.x + pad + column * (cell_w + gap),
panel_rect.y + pad + row * (cell_h + gap),
cell_w,
cell_h,
),
alpha,
font_bold,
icon_size=_SOURCE_DENSE_ICON_SIZE,
value_font=_SOURCE_DENSE_VALUE_FONT,
draw_chrome=draw_chrome,
)
def _draw_source_bubble_content(
model: SourceBubbleModel,
panel_rect: rl.Rectangle,
alpha: int,
font_semi,
font_bold,
*,
draw_chrome: bool,
) -> None:
if alpha <= 0:
return
if model.mode == "empty":
_draw_sources_bubble_empty_state(panel_rect, model.empty_reason or "No data", alpha, font_semi)
elif model.mode == "dense":
_draw_dense_sources(model, panel_rect, alpha, font_semi, font_bold, draw_chrome=draw_chrome)
else:
_draw_readable_sources(model, panel_rect, alpha, font_semi, font_bold, draw_chrome=draw_chrome)
def _draw_sources_bubble(
state: dict,
sign_rect: rl.Rectangle,
transition: SourceBubbleTransition | None = None,
) -> None:
"""Draw the expanded source bubble inside its fixed attached footprint."""
font_semi = _get_semi_bold()
font_bold = _get_bold()
active_source = state['speed_limit_source']
enabled_sources = state.get('slc_enabled_sources', ())
active_only = state.get('slc_active_sources_only', False)
abbreviated = state.get('slc_abbreviated_sources', False)
panel_rect = rl.Rectangle(
sign_rect.x + sign_rect.width + _SOURCE_PANEL_GAP,
sign_rect.y,
@@ -573,113 +901,47 @@ def _draw_sources_bubble(state: dict, sign_rect: rl.Rectangle):
panel_rect, CONTROL_ROUNDNESS, CONTROL_SEGMENTS, 1, _SOURCE_PANEL_BORDER,
)
rows = [
(
panel_label,
_SOURCE_COMPACT_LABELS[panel_label],
icon_key,
value,
is_active,
)
for panel_label, icon_key, value, is_active in visible_source_rows(
SOURCE_DEFS, state, active_source, enabled_sources, active_only,
)
]
model = _source_model(state)
if transition is None:
outgoing, incoming, alpha = model, None, 1.0
else:
outgoing, incoming, alpha = transition.update(model, rl.get_time())
if not rows:
_draw_sources_bubble_empty_state(panel_rect)
return
row_h = (panel_rect.height - 2 * _SOURCE_PANEL_PAD_Y) / len(rows)
content_left = panel_rect.x + _SOURCE_PANEL_PAD_X
content_right = panel_rect.x + panel_rect.width - _SOURCE_PANEL_PAD_X
font_size, icon_size, icon_gap = source_content_metrics(len(rows))
label_left = (
content_left + _SOURCE_ACTIVE_BAR_WIDTH + _SOURCE_MIN_LABEL_VALUE_GAP
if abbreviated else content_left + icon_size + icon_gap
is_transitioning = incoming is not None
outgoing_chrome = not is_transitioning or alpha < 0.5
incoming_chrome = not is_transitioning or alpha >= 0.5
_draw_source_bubble_content(
outgoing,
panel_rect,
round(255 * (1.0 - alpha)),
font_semi,
font_bold,
draw_chrome=outgoing_chrome,
)
_draw_source_bubble_content(
incoming or outgoing,
panel_rect,
round(255 * alpha),
font_semi,
font_bold,
draw_chrome=incoming_chrome,
)
for index, (panel_label, compact_label, icon_key, value, is_active) in enumerate(rows):
row_y = panel_rect.y + _SOURCE_PANEL_PAD_Y + index * row_h
if index:
divider_y = round(row_y)
rl.draw_line_ex(
rl.Vector2(content_left, divider_y),
rl.Vector2(content_right, divider_y),
1,
_SOURCE_DIVIDER,
)
if is_active:
active_bar_height = min(
_SOURCE_ACTIVE_BAR_HEIGHT,
max(10.0, row_h - 2 * _SOURCE_ACTIVE_BAR_ROW_INSET),
)
active_bar_rect = rl.Rectangle(
panel_rect.x + _SOURCE_ACTIVE_BAR_X,
round(row_y + (row_h - active_bar_height) / 2),
_SOURCE_ACTIVE_BAR_WIDTH,
active_bar_height,
)
rl.draw_rectangle_rounded(active_bar_rect, 0.5, 4, _SOURCE_ACTIVE_BAR)
value_text = source_value_text(value)
text_color = _WHITE if is_active else _SOURCE_LABEL_MUTED
if abbreviated:
text_font = font_bold if is_active else font_semi
label_text = fit_source_label(
f"{tr(compact_label)}-{source_abbreviated_value_text(value)}",
"",
content_right - label_left,
lambda text: measure_text_cached(text_font, text, font_size).x,
)
label_size = measure_text_cached(text_font, label_text, font_size)
text_y = round(row_y + (row_h - label_size.y) / 2)
rl.draw_text_ex(
text_font,
label_text,
rl.Vector2(label_left, text_y),
font_size,
0,
text_color,
)
continue
compact_label = tr(compact_label)
full_label = tr(panel_label)
value_size = measure_text_cached(font_bold, value_text, font_size)
max_label_width = max(
0.0,
content_right - label_left - _SOURCE_MIN_LABEL_VALUE_GAP - value_size.x,
)
label_text = fit_source_label(
full_label,
compact_label,
max_label_width,
lambda text: measure_text_cached(font_semi, text, font_size).x,
)
label_size = measure_text_cached(font_semi, label_text, font_size)
text_height = max(label_size.y, value_size.y)
text_y = round(row_y + (row_h - text_height) / 2)
icon_y = round(row_y + (row_h - icon_size) / 2)
icon_color = _WHITE if is_active else _SOURCE_ICON_MUTED
_draw_source_icon(icon_key, content_left, icon_y, icon_size, icon_color)
label_pos = rl.Vector2(label_left, text_y)
value_pos = rl.Vector2(round(content_right - value_size.x), text_y)
rl.draw_text_ex(font_semi, label_text, label_pos, font_size, 0, text_color)
rl.draw_text_ex(font_bold, value_text, value_pos, font_size, 0, text_color)
# ── Public API ────────────────────────────────────────────────────────
def render_speed_limit_at(state: dict, rect: rl.Rectangle, expanded: bool = False) -> Optional[rl.Rectangle]:
def render_speed_limit_at(
state: dict,
rect: rl.Rectangle,
expanded: bool = False,
source_bubble_transition: SourceBubbleTransition | None = None,
) -> Optional[rl.Rectangle]:
"""Render the SLC sign and optional source bubble at a layout rect."""
flashing_pending = state['speed_limit_changed'] and state['unconfirmed_valid']
if flashing_pending:
if source_bubble_transition is not None:
source_bubble_transition.reset()
_draw_sign(state, rect, pending=True)
return None
@@ -689,6 +951,6 @@ def render_speed_limit_at(state: dict, rect: rl.Rectangle, expanded: bool = Fals
visual_rect = rl.Rectangle(rect.x, rect.y, EU_SIGN_SIZE, EU_SIGN_SIZE) if use_vienna else rect
if expanded:
_draw_sources_bubble(state, visual_rect)
_draw_sources_bubble(state, visual_rect, source_bubble_transition)
return visual_rect
@@ -1,12 +1,50 @@
"""Pure layout decisions for the on-road speed-limit source bubble."""
"""Pure source selection and transition decisions for the SLC bubble."""
import math
from collections.abc import Callable, Iterable, Mapping
from collections.abc import Iterable, Mapping
from dataclasses import dataclass, replace
SOURCE_DISPLAY_ORDER = ("Dashboard", "Map Data", "Vision", "Mapbox", "Upcoming")
SOURCE_PRIORITY_NAMES = frozenset(("Dashboard", "Map Data", "Vision"))
SOURCE_BUBBLE_SETTLE_SECONDS = 0.12
SOURCE_BUBBLE_TRANSITION_SECONDS = 0.20
@dataclass(frozen=True)
class SourceBubbleItem:
"""One source row/cell in canonical display order."""
source_id: str
label: str
icon_key: str
value: float
has_reading: bool
is_active: bool
@dataclass(frozen=True)
class SourceBubbleModel:
"""The source roster to render inside the fixed bubble footprint."""
items: tuple[SourceBubbleItem, ...]
empty_reason: str | None = None
@property
def mode(self) -> str:
if not self.items:
return "empty"
return "dense" if len(self.items) >= 4 else "readable"
@property
def structural_key(self) -> tuple[str, tuple[tuple[str, str, str], ...], str | None]:
return (
self.mode,
tuple((item.source_id, item.label, item.icon_key) for item in self.items),
self.empty_reason,
)
def enabled_source_titles(
primary_priority: str,
@@ -16,13 +54,7 @@ def enabled_source_titles(
mapbox_enabled: bool,
dashboard_available: bool = True,
) -> tuple[str, ...]:
"""Return source rows that are eligible under the current SLC settings.
``Highest`` and ``Lowest`` are aggregate priority modes. They consider the
two non-vision controller inputs directly; Vision is only a controller input
when it is explicitly selected in one of the priority slots. Mapbox is a
separate fallback toggle, and Next is derived from the selected map source.
"""
"""Return source rows eligible under the current SLC settings."""
if primary_priority in ("Highest", "Lowest"):
enabled = {"Dashboard", "Map Data"}
else:
@@ -45,13 +77,8 @@ def enabled_source_titles(
return tuple(source for source in SOURCE_DISPLAY_ORDER if source in enabled)
def source_content_metrics(row_count: int) -> tuple[int, int, int]:
"""Return logical text size, icon size, and icon gap for the visible rows."""
if row_count <= 3:
return 30, 34, 7
if row_count == 4:
return 30, 32, 7
return 28, 30, 6
def _has_reading(value: float) -> bool:
return math.isfinite(value) and value > 0 and round(value) > 0
def visible_source_rows(
@@ -59,54 +86,138 @@ def visible_source_rows(
values: Mapping[str, float],
active_source: str,
enabled_sources: Iterable[str],
active_only: bool = True,
) -> list[tuple[str, str, float, bool]]:
"""Return enabled source rows that possess a valid positive speed reading."""
active_only: bool = False,
) -> tuple[SourceBubbleItem, ...]:
"""Return enabled sources, optionally filtering those without readings.
Enabled sources remain visible with an em dash by default. This preserves
source discoverability without reserving space for sources disabled in the
user's settings.
"""
enabled = set(enabled_sources)
rows = []
for title, _abbrev, value_key, panel_label, icon_key in source_defs:
for title, _active_label, value_key, panel_label, icon_key in source_defs:
if title not in enabled:
continue
value = values[value_key]
has_reading = math.isfinite(value) and value > 0 and round(value) > 0
if not has_reading:
has_reading = _has_reading(value)
if active_only and not has_reading:
continue
rows.append((
panel_label,
icon_key,
value,
active_source == title,
rows.append(SourceBubbleItem(
source_id=title,
label=panel_label,
icon_key=icon_key,
value=value,
has_reading=has_reading,
is_active=active_source == title,
))
return rows
def fit_source_label(
full_label: str,
compact_label: str,
max_width: float,
measure_width: Callable[[str], float],
) -> str:
"""Choose the longest useful label that leaves room for the value column."""
for label in (full_label, compact_label):
if measure_width(label) <= max_width:
return label
ellipsis = ""
candidate = compact_label or full_label
while candidate and measure_width(candidate + ellipsis) > max_width:
candidate = candidate[:-1]
return f"{candidate}{ellipsis}" if candidate else ellipsis
return tuple(rows)
def source_value_text(value: float) -> str:
"""Format a source speed, keeping missing and non-finite values explicit."""
if not math.isfinite(value) or value <= 0:
if not _has_reading(value):
return ""
rounded = int(round(value))
return "" if rounded <= 0 else str(rounded)
return str(int(round(value)))
def source_abbreviated_value_text(value: float) -> str:
"""Format a compact source value using the established missing-value marker."""
value_text = source_value_text(value)
return "X" if value_text == "" else value_text
def _ease_out_cubic(progress: float) -> float:
progress = min(1.0, max(0.0, progress))
return 1.0 - (1.0 - progress) ** 3
def _refresh_model_values(base: SourceBubbleModel, live: SourceBubbleModel) -> SourceBubbleModel:
"""Keep structural content stable while refreshing values during a blend."""
live_by_id = {item.source_id: item for item in live.items}
refreshed = tuple(
live_by_id.get(item.source_id, replace(item, value=0.0, has_reading=False))
for item in base.items
)
return SourceBubbleModel(refreshed, base.empty_reason)
@dataclass
class SourceBubbleTransition:
"""Coalesce roster changes and crossfade stable bubble presentations."""
settle_seconds: float = SOURCE_BUBBLE_SETTLE_SECONDS
duration_seconds: float = SOURCE_BUBBLE_TRANSITION_SECONDS
_current: SourceBubbleModel | None = None
_pending: SourceBubbleModel | None = None
_pending_since: float | None = None
_from: SourceBubbleModel | None = None
_to: SourceBubbleModel | None = None
_started: float | None = None
def reset(self) -> None:
self._current = None
self._pending = None
self._pending_since = None
self._from = None
self._to = None
self._started = None
def _begin(self, target: SourceBubbleModel, now: float) -> None:
if self._current is None:
self._current = target
return
self._from = self._current
self._to = target
self._started = now
def _queue(self, model: SourceBubbleModel, now: float) -> None:
if self._pending is None or self._pending.structural_key != model.structural_key:
self._pending_since = now
self._pending = model
def update(
self,
model: SourceBubbleModel,
now: float,
) -> tuple[SourceBubbleModel, SourceBubbleModel | None, float]:
"""Return outgoing model, incoming model, and eased incoming alpha."""
if self._current is None:
self._current = model
return model, None, 1.0
if self._from is not None and self._to is not None and self._started is not None:
if model.structural_key == self._to.structural_key:
self._to = model
elif model.structural_key == self._from.structural_key:
self._current = model
self._pending = None
self._pending_since = None
self._from = None
self._to = None
self._started = None
return model, None, 1.0
else:
self._from = _refresh_model_values(self._from, model)
self._queue(model, now)
progress = (now - self._started) / max(self.duration_seconds, 1e-6)
if progress < 1.0:
return self._from, self._to, _ease_out_cubic(progress)
self._current = self._to
self._from = None
self._to = None
self._started = None
if model.structural_key == self._current.structural_key:
self._current = model
self._pending = None
self._pending_since = None
return self._current, None, 1.0
self._current = _refresh_model_values(self._current, model)
self._queue(model, now)
if self._pending_since is not None and now - self._pending_since >= self.settle_seconds:
target = self._pending
self._pending = None
self._pending_since = None
self._begin(target, now)
if self._from is not None and self._to is not None:
return self._from, self._to, 0.0
return self._current, None, 1.0
@@ -2,7 +2,7 @@ import pyray as rl
from openpilot.common.filter_simple import FirstOrderFilter
from openpilot.system.ui.lib.application import gui_app, FontWeight
from openpilot.selfdrive.ui.onroad.starpilot.widgets.base import LayoutWidget
from openpilot.selfdrive.ui.onroad.starpilot.aethergauge import AetherGauge, _fade
from openpilot.selfdrive.ui.onroad.starpilot.aethergauge import AetherGauge
from openpilot.selfdrive.ui.onroad.starpilot.widget_style import CONTROL_WIDTH
class AetherGaugeWidget(LayoutWidget):
@@ -29,12 +29,10 @@ class AetherGaugeWidget(LayoutWidget):
return
cx = rect.x + rect.width / 2
# Set the road bottom to rect.y + 145, leaving 115px for the text cradle underneath
bottom = rect.y + 145.0
# AetherGauge derives its bounded road and metric viewports from rect.
self._aethergauge.render(
rect, self._font_bold, self._font_medium,
current_speed=self.hud_renderer.speed,
cx=cx,
bottom=bottom,
alpha=alpha,
)
@@ -6,6 +6,7 @@ from openpilot.selfdrive.ui.onroad.starpilot.widgets.base import LayoutWidget
from openpilot.selfdrive.ui.onroad.starpilot.slc_speed_limit import (
_get_slc_state, render_speed_limit_at, EU_SIGN_SIZE,
)
from openpilot.selfdrive.ui.onroad.starpilot.source_bubble_layout import SourceBubbleTransition
from openpilot.selfdrive.ui.onroad.starpilot.widget_style import CONTROL_WIDTH, SLC_HEIGHT
@@ -16,6 +17,7 @@ class SpeedLimitWidget(LayoutWidget):
super().__init__("speed_limit", priority=2)
self._slc_state: dict | None = None
self._sign_rect: Optional[rl.Rectangle] = None
self._source_bubble_transition = SourceBubbleTransition()
@property
def _hit_rect(self) -> rl.Rectangle:
@@ -33,6 +35,7 @@ class SpeedLimitWidget(LayoutWidget):
self._slc_state = _get_slc_state()
if self._slc_state is None:
self._sign_rect = None
self._source_bubble_transition.reset()
return False
return True
@@ -51,7 +54,14 @@ class SpeedLimitWidget(LayoutWidget):
return
params = ui_state.ui_params
expanded = params.get_bool("SpeedLimitSources")
self._sign_rect = render_speed_limit_at(self._slc_state, rect, expanded)
if not expanded:
self._source_bubble_transition.reset()
self._sign_rect = render_speed_limit_at(
self._slc_state,
rect,
expanded,
self._source_bubble_transition,
)
def _handle_mouse_press(self, mouse_pos) -> None:
state = self._slc_state
+139 -1
View File
@@ -31,16 +31,20 @@ mock_ui_state = types.SimpleNamespace(
},
)
from openpilot.selfdrive.ui.onroad.starpilot import aethergauge
from openpilot.selfdrive.ui.onroad.starpilot import widget_style
from openpilot.selfdrive.ui.onroad.starpilot.aethergauge import (
AetherGauge,
AetherGaugeData,
IndicatorType,
_aether_layout,
_cem_curvature_data,
_draw_aether_card,
_is_cem_curvature,
_is_curve_speed,
_is_lead,
_is_stop_light,
_lead_data,
_state_label,
)
from openpilot.starpilot.common.experimental_state import CEStatus
@@ -295,7 +299,7 @@ def test_monotonic_ratchet_clamp_prevents_upward_bounce(monkeypatch):
rendered_data = []
gauge = AetherGauge()
monkeypatch.setattr(gauge, "_render_unified_road", lambda rect, cx, cy, data, fb, fm, alpha: rendered_data.append(data))
monkeypatch.setattr(gauge, "_render_unified_road", lambda rect, cx, data, fb, fm, alpha: rendered_data.append(data))
# Initial frame at 30m
_set_plan(redLight=True, forcingStopLength=30.0)
@@ -307,3 +311,137 @@ def test_monotonic_ratchet_clamp_prevents_upward_bounce(monkeypatch):
gauge.render(None, None, None, current_speed=10.0, cx=100.0, bottom=200.0)
# Ratchet clamp must prevent the display number from increasing above 30m!
assert int(rendered_data[-1].text) <= 30
def test_aether_card_reuses_the_shared_control_frame(monkeypatch):
triangles = []
fills = []
keylines = []
monkeypatch.setattr(aethergauge.rl, "draw_triangle", lambda *args: triangles.append(args))
monkeypatch.setattr(aethergauge.rl, "draw_rectangle_rounded", lambda *args: fills.append(args))
monkeypatch.setattr(aethergauge.rl, "draw_rectangle_rounded_lines_ex", lambda *args: keylines.append(args))
rect = aethergauge.rl.Rectangle(10, 20, 176, 260)
layout = _aether_layout(rect)
_draw_aether_card(layout, 0.5)
assert not triangles
assert len(fills) == 1
assert len(keylines) == 1
card, roundness, segments, fill = fills[0]
keyline_card, keyline_roundness, keyline_segments, border_width, border = keylines[0]
assert card.x == rect.x
assert card.y == rect.y
assert card.width == rect.width
assert card.height == rect.height
assert keyline_card.x == card.x
assert keyline_card.y == card.y
assert keyline_card.width == card.width
assert keyline_card.height == card.height
assert roundness == keyline_roundness == widget_style.CONTROL_ROUNDNESS
assert segments == keyline_segments == widget_style.CONTROL_SEGMENTS
assert border_width == widget_style.CONTROL_BORDER_WIDTH
assert fill.a == int(aethergauge.COLOR_AETHER_CARD.a * 0.5)
assert (border.r, border.g, border.b, border.a) == (
widget_style.CONTROL_BORDER.r,
widget_style.CONTROL_BORDER.g,
widget_style.CONTROL_BORDER.b,
int(widget_style.CONTROL_BORDER.a * 0.5),
)
def test_aether_layout_keeps_road_and_cradle_inside_one_card():
layout = _aether_layout(aethergauge.rl.Rectangle(10, 20, 176, 260))
card_right = layout.card.x + layout.card.width
card_bottom = layout.card.y + layout.card.height
inner_border = widget_style.CONTROL_BORDER_WIDTH / 2.0
for viewport in (layout.road, layout.cradle):
assert layout.card.x <= viewport.x
assert viewport.x + viewport.width <= card_right
assert layout.card.y <= viewport.y
assert viewport.y + viewport.height <= card_bottom
assert layout.road.x >= layout.card.x + inner_border
assert layout.road.y >= layout.card.y + inner_border
assert layout.cradle.y + layout.cradle.height <= card_bottom - inner_border
assert layout.road.y + layout.road.height < layout.cradle.y
def _fake_text_size(_, text, size):
return aethergauge.rl.Vector2(max(1, len(text) * size * 0.5), size)
def _assert_text_calls_fit_card(text_calls, card):
card_right = card.x + card.width
card_bottom = card.y + card.height
for text, pos, size in text_calls:
measured = _fake_text_size(None, text, size)
assert card.x <= pos.x
assert pos.x + measured.x <= card_right
assert card.y <= pos.y
assert pos.y + measured.y <= card_bottom
@pytest.mark.parametrize("data", [
AetherGaugeData(
text="120", unit="km/h", color=aethergauge.COLOR_CEM_SPEED,
indicator_type=IndicatorType.ROAD_CURVE, reduction_text="-20", is_numeric=True,
),
AetherGaugeData(
text="999", unit="ft", color=aethergauge.COLOR_FORCE_STOP,
indicator_type=IndicatorType.STOP_LIGHT, is_numeric=True,
),
AetherGaugeData(
text="STOPPED", color=aethergauge.COLOR_LEAD_STOPPED,
indicator_type=IndicatorType.LEAD,
),
])
def test_aether_cradle_fits_text_within_card(monkeypatch, data):
text_calls = []
monkeypatch.setattr(aethergauge, "measure_text_cached", _fake_text_size)
monkeypatch.setattr(
aethergauge.rl, "draw_text_ex",
lambda _, text, pos, size, __, ___: text_calls.append((text, pos, size)),
)
monkeypatch.setattr(aethergauge.rl, "draw_line_ex", lambda *args: None)
layout = _aether_layout(aethergauge.rl.Rectangle(10, 20, 176, 260))
AetherGauge()._draw_mini_cradle(layout.cradle, data, object(), object())
_assert_text_calls_fit_card(text_calls, layout.card)
def test_aether_road_render_scissors_the_animated_viewport(monkeypatch):
scissor_calls = []
events = []
monkeypatch.setattr(aethergauge, "measure_text_cached", _fake_text_size)
monkeypatch.setattr(aethergauge.rl, "begin_scissor_mode", lambda *args: scissor_calls.append(args))
monkeypatch.setattr(aethergauge.rl, "end_scissor_mode", lambda: events.append("end_scissor"))
monkeypatch.setattr(aethergauge.rl, "draw_rectangle_rounded", lambda *args: None)
monkeypatch.setattr(aethergauge.rl, "draw_rectangle_rounded_lines_ex", lambda *args: None)
monkeypatch.setattr(aethergauge.rl, "draw_triangle", lambda *args: None)
monkeypatch.setattr(aethergauge.rl, "draw_line_ex", lambda *args: None)
monkeypatch.setattr(aethergauge.rl, "draw_text_ex", lambda *args: events.append("text"))
monkeypatch.setattr(aethergauge.rl, "get_frame_time", lambda: 0.0)
rect = aethergauge.rl.Rectangle(10, 20, 176, 260)
layout = _aether_layout(rect)
data = AetherGaugeData(
text="23", unit="mph", color=aethergauge.COLOR_CEM_SPEED,
indicator_type=IndicatorType.ROAD_CURVE, indicator_value=0.005, is_numeric=True,
)
AetherGauge()._render_unified_road(rect, 98.0, data, object(), object())
assert scissor_calls == [(
int(layout.road.x), int(layout.road.y), int(layout.road.width), int(layout.road.height),
)]
assert events.index("end_scissor") < events.index("text")
def test_state_labels_make_urgent_and_lead_states_explicit():
assert _state_label(AetherGaugeData(text="", indicator_type=IndicatorType.FORCE_STOP)) == "STOP"
assert _state_label(AetherGaugeData(text="", indicator_type=IndicatorType.STOP_LIGHT)) == "RED LIGHT"
assert _state_label(AetherGaugeData(text="", indicator_type=IndicatorType.LEAD)) == "LEAD"
assert _state_label(AetherGaugeData(text="", indicator_type=IndicatorType.ROAD_CURVE)) == ""
+194 -36
View File
@@ -1,36 +1,117 @@
from openpilot.selfdrive.ui.onroad.starpilot.source_bubble_layout import (
SourceBubbleItem,
SourceBubbleModel,
SourceBubbleTransition,
enabled_source_titles,
fit_source_label,
source_abbreviated_value_text,
source_content_metrics,
source_value_text,
visible_source_rows,
)
def test_source_content_metrics_scale_with_visible_row_count():
assert source_content_metrics(3) == (30, 34, 7)
assert source_content_metrics(4) == (30, 32, 7)
assert source_content_metrics(5) == (28, 30, 6)
def test_fit_source_label_preserves_a_safe_value_column_gap():
def width(text: str) -> int:
return len(text) * 10
assert fit_source_label("Dashboard", "Dash", 80, width) == "Dash"
assert fit_source_label("Vision", "Vision", 70, width) == "Vision"
assert fit_source_label("Dashboard", "Dash", 20, width) == "D…"
def test_source_value_text_keeps_missing_values_as_a_dash():
assert source_value_text(0) == ""
assert source_value_text(0.1) == ""
assert source_value_text(55) == "55"
assert source_value_text(float("nan")) == ""
assert source_value_text(float("inf")) == ""
assert source_abbreviated_value_text(0) == "X"
assert source_abbreviated_value_text(55) == "55"
def test_source_bubble_ignores_legacy_abbreviation_state(monkeypatch):
from openpilot.selfdrive.ui.onroad.starpilot import slc_speed_limit as slc
text_calls = []
icon_calls = []
monkeypatch.setattr(slc, "_get_bold", lambda: object())
monkeypatch.setattr(slc, "_get_semi_bold", lambda: object())
monkeypatch.setattr(slc, "tr", lambda text: text)
monkeypatch.setattr(
slc,
"measure_text_cached",
lambda _font, text, size: slc.rl.Vector2(len(text) * size * 0.5, size),
)
monkeypatch.setattr(slc.rl, "draw_rectangle_rounded", lambda *args: None)
monkeypatch.setattr(slc.rl, "draw_rectangle_rounded_lines_ex", lambda *args: None)
monkeypatch.setattr(slc.rl, "draw_line_ex", lambda *args: None)
monkeypatch.setattr(slc.rl, "draw_text_ex", lambda _font, text, *_args: text_calls.append(text))
monkeypatch.setattr(slc, "_draw_source_icon", lambda icon_key, *_args: icon_calls.append(icon_key))
state = {
"speed_limit_source": "Dashboard",
"slc_enabled_sources": ("Dashboard", "Map Data"),
"slc_active_sources_only": False,
"dashboard_sl": 45.0,
"map_sl": 30.0,
}
def render(abbreviated: bool):
text_calls.clear()
icon_calls.clear()
slc._draw_sources_bubble(
{**state, "slc_abbreviated_sources": abbreviated},
slc.rl.Rectangle(0, 0, 176, 196),
)
return list(icon_calls), list(text_calls)
abbreviated_rows = render(True)
full_rows = render(False)
assert abbreviated_rows == full_rows
assert abbreviated_rows[0] == ["dashboard", "map"]
assert abbreviated_rows[1] == ["Dashboard", "45", "Map Data", "30"]
def test_dense_source_bubble_uses_distinct_icons_and_no_source_names(monkeypatch):
from openpilot.selfdrive.ui.onroad.starpilot import slc_speed_limit as slc
text_calls = []
icon_calls = []
monkeypatch.setattr(slc, "_get_bold", lambda: object())
monkeypatch.setattr(slc, "_get_semi_bold", lambda: object())
monkeypatch.setattr(slc, "tr", lambda text: text)
monkeypatch.setattr(
slc,
"measure_text_cached",
lambda _font, text, size: slc.rl.Vector2(len(text) * size * 0.5, size),
)
monkeypatch.setattr(slc.rl, "draw_rectangle_rounded", lambda *args: None)
monkeypatch.setattr(slc.rl, "draw_rectangle_rounded_lines_ex", lambda *args: None)
monkeypatch.setattr(slc.rl, "draw_line_ex", lambda *args: None)
monkeypatch.setattr(slc.rl, "draw_text_ex", lambda _font, text, *_args: text_calls.append(text))
monkeypatch.setattr(slc, "_draw_source_icon", lambda icon_key, *_args: icon_calls.append(icon_key))
state = {
"speed_limit_source": "Mapbox",
"slc_enabled_sources": ("Dashboard", "Map Data", "Vision", "Mapbox", "Upcoming"),
"slc_active_sources_only": False,
"dashboard_sl": 45.0,
"map_sl": 30.0,
"vision_sl": 50.0,
"mapbox_sl": 35.0,
"next_sl": 20.0,
}
slc._draw_sources_bubble(state, slc.rl.Rectangle(0, 0, 176, 196))
assert text_calls == ["45", "30", "50", "35", "Next", "20"]
assert icon_calls == ["dashboard", "map", "camera", "navigation", "next"]
def test_dense_source_group_fits_three_digit_values_inside_cell(monkeypatch):
from openpilot.selfdrive.ui.onroad.starpilot import slc_speed_limit as slc
monkeypatch.setattr(
slc,
"measure_text_cached",
lambda _font, text, size: slc.rl.Vector2(len(text) * size * 0.5, size),
)
icon_size, value_font, value_size = slc._fit_dense_group(
object(), "120", slc.rl.Rectangle(0, 0, 114, 49), 36, 48,
)
assert icon_size + slc._SOURCE_DENSE_GAP + value_size.x <= 106
assert max(icon_size, value_size.y) <= 47
assert value_font < 48
def test_enabled_source_titles_follow_priority_and_fallback_settings():
@@ -59,29 +140,106 @@ def test_visible_source_rows_honor_active_only_and_source_order():
]
values = {"dashboard": 45.0, "map": 0.0, "vision": 50.0, "mapbox": 30.0, "next": 20.0}
# Map Data has value 0.0, so it is omitted; Dashboard (45.0) is active
# Map Data has no reading but remains visible when active-only is disabled.
assert visible_source_rows(
source_defs, values, "Dashboard", ("Dashboard", "Map Data"),
) == [
("Dashboard", "dashboard", 45.0, True),
]
# When Map Data is the active target but has 0.0 reading, Dashboard is inactive (available standby)
) == (
SourceBubbleItem("Dashboard", "Dashboard", "dashboard", 45.0, True, True),
SourceBubbleItem("Map Data", "Map Data", "map", 0.0, False, False),
)
# Active-only hides the unavailable Map Data row and preserves canonical order.
assert visible_source_rows(
source_defs, values, "Map Data", ("Dashboard", "Map Data"),
) == [
("Dashboard", "dashboard", 45.0, False),
]
source_defs, values, "Map Data", ("Dashboard", "Map Data"), active_only=True,
) == (
SourceBubbleItem("Dashboard", "Dashboard", "dashboard", 45.0, True, False),
)
# Multiple available sources with readings appear in canonical order
assert visible_source_rows(
source_defs, values, "Vision", ("Dashboard", "Map Data", "Vision"),
) == [
("Dashboard", "dashboard", 45.0, False),
("Vision", "camera", 50.0, True),
]
# When no sources have a valid speed reading (> 0), returns empty list (triggers empty state)
) == (
SourceBubbleItem("Dashboard", "Dashboard", "dashboard", 45.0, True, False),
SourceBubbleItem("Map Data", "Map Data", "map", 0.0, False, False),
SourceBubbleItem("Vision", "Vision", "camera", 50.0, True, True),
)
# Active-only removes all sources without a valid speed reading.
assert visible_source_rows(
source_defs, {key: 0.0 for key in values}, "Map Data", ("Map Data",),
) == []
source_defs, dict.fromkeys(values, 0.0), "Map Data", ("Map Data",), active_only=True,
) == ()
def test_source_bubble_models_select_readable_and_dense_modes():
item = SourceBubbleItem("Vision", "Vision", "camera", 50.0, True, True)
assert SourceBubbleModel((item,)).mode == "readable"
assert SourceBubbleModel(tuple(item for _ in range(3))).mode == "readable"
assert SourceBubbleModel(tuple(item for _ in range(4))).mode == "dense"
assert SourceBubbleModel(tuple(item for _ in range(5))).mode == "dense"
assert SourceBubbleModel((), "NO SOURCE DATA").mode == "empty"
def test_source_bubble_transition_settles_and_crossfades_structural_changes():
first = SourceBubbleModel((SourceBubbleItem("Dashboard", "Dashboard", "dashboard", 45.0, True, True),))
second = SourceBubbleModel(tuple(
SourceBubbleItem(source, source, icon, value, True, source == "Vision")
for source, icon, value in (
("Dashboard", "dashboard", 45.0),
("Map Data", "map", 30.0),
("Vision", "camera", 50.0),
("Mapbox", "navigation", 35.0),
)
))
transition = SourceBubbleTransition()
assert transition.update(first, 0.0) == (first, None, 1.0)
outgoing, incoming, alpha = transition.update(second, 0.05)
assert outgoing.structural_key == first.structural_key
assert outgoing.items[0].is_active is False
assert incoming is None
assert alpha == 1.0
outgoing, incoming, alpha = transition.update(second, 0.18)
assert outgoing.structural_key == first.structural_key
assert incoming is second
assert alpha == 0.0
_, incoming, alpha = transition.update(second, 0.28)
assert incoming is second
assert 0.0 < alpha < 1.0
assert transition.update(second, 0.5) == (second, None, 1.0)
def test_source_bubble_transition_coalesces_latest_pending_layout():
first = SourceBubbleModel((SourceBubbleItem("Dashboard", "Dashboard", "dashboard", 45.0, True, True),))
second = SourceBubbleModel(tuple(
SourceBubbleItem(source, source, icon, 45.0, True, False)
for source, icon in (("Dashboard", "dashboard"), ("Map Data", "map"))
))
third = SourceBubbleModel(tuple(
SourceBubbleItem(source, source, icon, 45.0, True, False)
for source, icon in (("Dashboard", "dashboard"), ("Map Data", "map"), ("Vision", "camera"))
))
transition = SourceBubbleTransition()
transition.update(first, 0.0)
transition.update(second, 0.05)
transition.update(third, 0.10)
outgoing, incoming, alpha = transition.update(third, 0.23)
assert outgoing.structural_key == first.structural_key
assert incoming is third
assert alpha == 0.0
def test_source_bubble_transition_reverses_to_the_latest_roster():
first = SourceBubbleModel((SourceBubbleItem("Dashboard", "Dashboard", "dashboard", 45.0, True, True),))
second = SourceBubbleModel(tuple(
SourceBubbleItem(source, source, icon, 45.0, True, False)
for source, icon in (("Dashboard", "dashboard"), ("Map Data", "map"))
))
transition = SourceBubbleTransition()
transition.update(first, 0.0)
transition.update(second, 0.05)
transition.update(second, 0.18)
assert transition.update(first, 0.20) == (first, None, 1.0)
assert transition.update(first, 0.40) == (first, None, 1.0)
def test_source_label_color_override_and_engagement_states():
+2
View File
@@ -3,6 +3,8 @@ from __future__ import annotations
import math
from openpilot.selfdrive.controls.lib.longitudinal_planner import get_max_accel
ACCELERATION_PROFILES = {
"STANDARD": 0,
"ECO": 1,
+2 -28
View File
@@ -19,9 +19,7 @@ CSC_MIN_SPEED = CITY_SPEED_LIMIT * CV.MPH_TO_MS
CSC_CURVE_RELEASE_HOLD_TIME = 0.75
OVERRIDE_FORCE_STOP_TIMER = 10
STANDSTILL_FORCE_STOP_CLEAR_TIME = 0.75
# Open-loop — green is undetectable at standstill, so this only needs to cover the
# handoff to CEM+model ownership. Extra seconds are pure departure lag.
STANDSTILL_FORCE_STOP_LIGHT_HOLD_TIME = 2.0
STANDSTILL_FORCE_STOP_LIGHT_HOLD_TIME = 5.0
FORCE_STOP_LIGHT_CLEAR_TIME = 0.5
SLC_LEAD_DROP_RELAXATION_MIN_SPEED = 20.0 * CV.MPH_TO_MS
SLC_LEAD_DROP_RELAXATION_MIN_DISTANCE = 30.0
@@ -59,8 +57,6 @@ LEAD_VETO_M_OVERRIDES = {
}
FORCE_STOP_APPROACH_DECEL = 0.65 # m/s^2 — speed ceiling before commit. LOWER = more early
# braking; don't go under FORCE_STOP_MODEL_APPROACH_DECEL
# approachStopLength is published RAW: model_length converges from above, so rate-limiting
# it inward freezes it far out and the constraint never binds. Tried, measured, don't re-add.
ADAS_MAX_MS = 17.88 # 40 mph — cross-street ADAS guard
DASH_SEED_M = 27.0 # ~88 ft — typical ADAS detection distance, used to snap
# tracked length closer when dashboard confirms a sign
@@ -78,14 +74,6 @@ FORCE_STOP_TURN_VETO_STEERING_ANGLE = 25.0
FORCE_STOP_CURVE_VETO_MAX_ROAD_CURVATURE = 0.003
FORCE_STOP_TURN_VETO_STOP_SEEN_HOLD_TIME = 4.0
FORCE_STOP_DISTANCE_REANCHOR_MIN_GAP = 3.0 # m — ignore small model-horizon noise
FORCE_STOP_REANCHOR_MIN_M = 40.0 # m — inside this only ratchet down; shouldStop doesn't
# assert until ~10 m, so horizon jitter would release the stop
FORCE_STOP_CAP_SLACK_M = 15.0 # m — the line can't move away, so tracked can never exceed
# what it was at commit minus distance driven. Slack covers an
# under-read at commit; without it that would stop us short.
FORCE_STOP_CAP_TAPER_M = 60.0 # m — slack fades to 0 as the cap closes. The solver aims at
# tracked, so slack held near the line is braking for a stop bar
# that far past the real one.
# Knob bounds (mirror of UI slider; defense in depth)
OFFSET_FT_MIN = -20
@@ -189,11 +177,9 @@ class StarPilotVCruise:
self.force_stop_from_light = False
self.force_stop_light_clear_since = None
self.controls_enabled_previously = False
self.approach_stop_length = 0.0 # published as starpilotPlan.approachStopLength
# Kinematic distance estimator. Same attribute also published as
# starpilotPlan.forcingStopLength, so the existing reader keeps working.
self.tracked_model_length = 0.0
self.force_stop_distance_cap = 0.0 # odometry ceiling, re-seeded until commit
self.stop_sign_confirmed = False
self.stop_seen_on_approach_at = None
@@ -617,9 +603,6 @@ class StarPilotVCruise:
offset_ft = max(OFFSET_FT_MIN, min(OFFSET_FT_MAX, offset_ft_raw))
offset_m = offset_ft * FT_TO_M
# cleared on every path; only the far-approach envelope below republishes it
self.approach_stop_length = 0.0
if force_standstill_enabled and not self.override_force_standstill:
self.forcing_stop = True
self.tracked_model_length = 0.0
@@ -647,7 +630,7 @@ class StarPilotVCruise:
model_wants_stop = False
if (
not dash_active and
self.tracked_model_length > max(force_stop_handoff_m, FORCE_STOP_REANCHOR_MIN_M) and
self.tracked_model_length > force_stop_handoff_m and
not model_wants_stop and
model_length > self.tracked_model_length + FORCE_STOP_DISTANCE_REANCHOR_MIN_GAP and
(
@@ -659,12 +642,6 @@ class StarPilotVCruise:
self.tracked_model_length = model_length
else:
self.tracked_model_length = min(self.tracked_model_length, model_length)
# Odometry ceiling: the line can't recede, so a re-anchor may never exceed what we
# had at commit minus what we've driven. Bounds a ballooning horizon (seen +95 m)
# that the REANCHOR_MIN floor can't catch, since that floor trusts the estimate.
self.force_stop_distance_cap = max(self.force_stop_distance_cap - (v_ego * DT_MDL), 0.0)
cap_slack = FORCE_STOP_CAP_SLACK_M * min(self.force_stop_distance_cap / FORCE_STOP_CAP_TAPER_M, 1.0)
self.tracked_model_length = min(self.tracked_model_length, self.force_stop_distance_cap + cap_slack)
if dash_active:
if model_length < DASH_MODEL_AGREE_M:
self.tracked_model_length = min(self.tracked_model_length, DASH_SEED_M)
@@ -698,7 +675,6 @@ class StarPilotVCruise:
self.stop_sign_confirmed = False
self.tracked_model_length = self.starpilot_planner.model_length
self.force_stop_distance_cap = self.tracked_model_length
targets = [v_cruise]
if self.csc_target >= CSC_MIN_SPEED:
@@ -744,8 +720,6 @@ class StarPilotVCruise:
adjacent_stop_d = self._get_adjacent_stop_distance(sm)
if adjacent_stop_d is not None:
approach_d = min(approach_d, adjacent_stop_d)
# pre-offset, so it hands off to forcingStopLength at commit without a step
self.approach_stop_length = max(approach_d, 0.0)
approach_d += offset_m + force_stop_distance_bias_m
if approach_d > force_stop_handoff_m:
targets.append(math.sqrt(2.0 * FORCE_STOP_APPROACH_DECEL * (approach_d - force_stop_handoff_m)))
+2 -8
View File
@@ -31,10 +31,7 @@ from openpilot.starpilot.controls.lib.starpilot_vcruise import StarPilotVCruise
from openpilot.starpilot.controls.lib.weather_checker import WeatherChecker
RADARLESS_TRACK_HOLD_TIME = 0.45
FORCE_STOP_JERK_SCALE = 0.20 # accel-change cost multiplier for the whole stop approach,
# envelope included (125 -> 25). Lower = reaches the braking
# target sooner; it does not make the target deeper. Response
# is super-linear here, so raise it if onset feels like a step.
FORCE_STOP_JERK_SCALE = 0.32 # accel-change cost multiplier while forcing_stop (125 -> ~40)
FORCE_STOP_JERK_SCALE_OVERRIDES = {
# The Elantra's current force-stop ramp is smooth, but it waits too long
# before building decel and then arrives at the initial brake too abruptly.
@@ -311,9 +308,7 @@ class StarPilotPlanner:
except (KeyError, IndexError, TypeError, AttributeError):
car_params = None
# Also while the far-approach envelope is running: at onset the ramp reaches only
# ~-0.5 m/s^2 after a second, so the first seconds of a detected red are mostly lost.
if self.starpilot_vcruise.forcing_stop or self.starpilot_vcruise.approach_stop_length > 0.0:
if self.starpilot_vcruise.forcing_stop:
jerk_scale = get_force_stop_jerk_scale(car_params)
elif self.tracking_lead:
# Elantra vision leads can hand off from cruise to lead0 while closing
@@ -351,7 +346,6 @@ class StarPilotPlanner:
starpilotPlan.forcingStop = self.starpilot_vcruise.forcing_stop
starpilotPlan.forcingStopLength = self.starpilot_vcruise.tracked_model_length
starpilotPlan.approachStopLength = float(self.starpilot_vcruise.approach_stop_length)
starpilotPlan.stopSignConfirmed = self.starpilot_vcruise.stop_sign_confirmed
starpilotPlan.starpilotEvents = self.starpilot_events.events.to_msg()
+3 -5
View File
@@ -1,6 +1,6 @@
from __future__ import annotations
from typing import cast
import os, ctypes, struct, hashlib, functools, importlib, mmap, errno, array, contextlib, sys, weakref, itertools, collections, atexit, time
import os, ctypes, struct, hashlib, functools, importlib, mmap, errno, array, contextlib, sys, weakref, itertools, collections, atexit
assert sys.platform != 'win32'
from dataclasses import dataclass
from tinygrad.runtime.support.hcq import HCQCompiled, HCQAllocator, HCQBuffer, HWQueue, CLikeArgsState, HCQSignal, HCQProgram, FileIOInterface
@@ -25,7 +25,6 @@ SQTT = ContextVar("SQTT", abs(VIZ.value)>=2)
SQTT_ITRACE_SE_MASK, SQTT_LIMIT_SE, SQTT_SIMD_SEL, SQTT_TOKEN_EXCLUDE = \
ContextVar("SQTT_ITRACE_SE_MASK", 0b11), ContextVar("SQTT_LIMIT_SE", 0), ContextVar("SQTT_SIMD_SEL", 0), ContextVar("SQTT_TOKEN_EXCLUDE", 0)
PMC = ContextVar("PMC", abs(VIZ.value)>=2)
AMD_USB_POLL_US = getenv("AMD_USB_POLL_US", 500) # microseconds to sleep between USB signal polls. 0 disables
EVENT_INDEX_PARTIAL_FLUSH = 4 # based on a comment in nvd.h
WAIT_REG_MEM_FUNCTION_EQ = 3 # ==
WAIT_REG_MEM_FUNCTION_NEQ = 4 # !=
@@ -46,9 +45,8 @@ class AMDSignal(HCQSignal):
def __init__(self, *args, **kwargs): super().__init__(*args, **{**kwargs, 'timestamp_divider': 100})
def _sleep(self, time_spent_since_last_sleep_ms:int):
# USB signals live in VRAM across the link, so yield between polls. Native AMD only blocks after 200 ms.
if self.owner is not None and self.owner.is_usb() and AMD_USB_POLL_US: time.sleep(AMD_USB_POLL_US / 1e6)
elif time_spent_since_last_sleep_ms > 200 and self.owner is not None: self.owner.iface.sleep(200)
# Reasonable to sleep for long workloads (which take more than 200ms) and only timeline signals.
if time_spent_since_last_sleep_ms > 200 and self.owner is not None: self.owner.iface.sleep(200)
class AMDComputeQueue(HWQueue):
def __init__(self, dev:AMDDevice):