Compare commits

...

5 Commits

Author SHA1 Message Date
Prabhaav Pillai 371f063362 Add traffic border color to C4 2026-08-20 17:50:08 -04:00
Prabhaav Pillai 49f061eee4 Fix image rendering position in C4 and C3 preview functions 2026-08-20 17:23:01 -04:00
firestar5683 d8090be19d fotos glo glo 2026-08-20 15:31:44 -05:00
Prabhaav Pillai d78902d73b Add C4 side camera support with curved rectangle rendering
Cherry-picked corrected Feature Request 510203 implementation from 8768e08341f2d4044776f5ba846e606dc9cd3f1d.

Co-authored-by: Prabhaav Pillai <prabhaav@msu.edu>
2026-08-20 14:07:11 -05:00
firestarsdog a0cf285394 Dough 2026-08-20 03:53:52 -04:00
22 changed files with 755 additions and 207 deletions
@@ -22,14 +22,14 @@ _LEGACY_2025_REENGAGE_MAX_STEER_RATE = 2.0
_LEGACY_2025_REENGAGE_MAX_ANGLE_DELTA = 1.0
_LEGACY_2025_RECLAIM_FRAMES = 36
_LEGACY_2025_RECLAIM_EXPONENT = 2.5
_ASCENT_OVERRIDE_HOLD_FRAMES = 10
_ASCENT_REENGAGE_SETTLE_FRAMES = 8
_ASCENT_REENGAGE_MAX_STEER_RATE = 2.0
_ASCENT_REENGAGE_MAX_ANGLE_DELTA = 1.0
_ASCENT_RECLAIM_FRAMES = 36
_ASCENT_RECLAIM_EXPONENT = 2.5
_ASCENT_MADS_MIN_SPEED = 0.44704
_ASCENT_MADS_MAX_STEER_ANGLE = 120.0
_ANGLE_OVERRIDE_HOLD_FRAMES = 10
_ANGLE_REENGAGE_SETTLE_FRAMES = 8
_ANGLE_REENGAGE_MAX_STEER_RATE = 2.0
_ANGLE_REENGAGE_MAX_ANGLE_DELTA = 1.0
_ANGLE_RECLAIM_FRAMES = 36
_ANGLE_RECLAIM_EXPONENT = 2.5
_ANGLE_MADS_MIN_SPEED = 0.44704
_ANGLE_MADS_MAX_STEER_ANGLE = 120.0
def get_safety_CP():
@@ -50,13 +50,13 @@ class CarController(CarControllerBase):
self.legacy_2025_reengage_reference_angle = 0.0
self.legacy_2025_reclaim_frames = 0
self.legacy_2025_reclaim_start_angle = 0.0
self.ascent_lkas_active = False
self.ascent_handoff_active = False
self.ascent_override_hold_frames = 0
self.ascent_reengage_settle_frames = 0
self.ascent_reengage_reference_angle = 0.0
self.ascent_reclaim_frames = 0
self.ascent_reclaim_start_angle = 0.0
self.angle_lkas_active = False
self.angle_handoff_active = False
self.angle_override_hold_frames = 0
self.angle_reengage_settle_frames = 0
self.angle_reengage_reference_angle = 0.0
self.angle_reclaim_frames = 0
self.angle_reclaim_start_angle = 0.0
self.cruise_button_prev = 0
self.steer_rate_counter = 0
@@ -137,67 +137,67 @@ class CarController(CarControllerBase):
self.legacy_2025_reclaim_frames -= 1
return target_angle
def _reset_ascent_handoff(self):
self.ascent_handoff_active = False
self.ascent_override_hold_frames = 0
self.ascent_reengage_settle_frames = 0
self.ascent_reengage_reference_angle = 0.0
self.ascent_reclaim_frames = 0
self.ascent_reclaim_start_angle = 0.0
def _reset_angle_handoff(self):
self.angle_handoff_active = False
self.angle_override_hold_frames = 0
self.angle_reengage_settle_frames = 0
self.angle_reengage_reference_angle = 0.0
self.angle_reclaim_frames = 0
self.angle_reclaim_start_angle = 0.0
def _ascent_manual_handoff(self, CS, lat_active):
def _angle_manual_handoff(self, CS, lat_active):
if not lat_active:
self._reset_ascent_handoff()
self._reset_angle_handoff()
return False
if CS.out.steeringPressed:
self.ascent_handoff_active = True
self.ascent_override_hold_frames = _ASCENT_OVERRIDE_HOLD_FRAMES
self.ascent_reengage_settle_frames = 0
self.ascent_reengage_reference_angle = CS.out.steeringAngleDeg
self.ascent_reclaim_frames = 0
self.angle_handoff_active = True
self.angle_override_hold_frames = _ANGLE_OVERRIDE_HOLD_FRAMES
self.angle_reengage_settle_frames = 0
self.angle_reengage_reference_angle = CS.out.steeringAngleDeg
self.angle_reclaim_frames = 0
return True
if not self.ascent_handoff_active and not self.ascent_lkas_active and \
abs(CS.out.steeringRateDeg) > _ASCENT_REENGAGE_MAX_STEER_RATE:
self.ascent_handoff_active = True
self.ascent_reengage_reference_angle = CS.out.steeringAngleDeg
if not self.angle_handoff_active and not self.angle_lkas_active and \
abs(CS.out.steeringRateDeg) > _ANGLE_REENGAGE_MAX_STEER_RATE:
self.angle_handoff_active = True
self.angle_reengage_reference_angle = CS.out.steeringAngleDeg
if not self.ascent_handoff_active:
if not self.angle_handoff_active:
return False
if self.ascent_override_hold_frames > 0:
self.ascent_override_hold_frames -= 1
if self.ascent_override_hold_frames == 0:
self.ascent_reengage_reference_angle = CS.out.steeringAngleDeg
if self.angle_override_hold_frames > 0:
self.angle_override_hold_frames -= 1
if self.angle_override_hold_frames == 0:
self.angle_reengage_reference_angle = CS.out.steeringAngleDeg
return True
wheel_stable = abs(CS.out.steeringRateDeg) <= _ASCENT_REENGAGE_MAX_STEER_RATE and \
abs(CS.out.steeringAngleDeg - self.ascent_reengage_reference_angle) <= _ASCENT_REENGAGE_MAX_ANGLE_DELTA
wheel_stable = abs(CS.out.steeringRateDeg) <= _ANGLE_REENGAGE_MAX_STEER_RATE and \
abs(CS.out.steeringAngleDeg - self.angle_reengage_reference_angle) <= _ANGLE_REENGAGE_MAX_ANGLE_DELTA
if wheel_stable:
self.ascent_reengage_settle_frames += 1
self.angle_reengage_settle_frames += 1
else:
self.ascent_reengage_settle_frames = 0
self.ascent_reengage_reference_angle = CS.out.steeringAngleDeg
self.angle_reengage_settle_frames = 0
self.angle_reengage_reference_angle = CS.out.steeringAngleDeg
if self.ascent_reengage_settle_frames < _ASCENT_REENGAGE_SETTLE_FRAMES:
if self.angle_reengage_settle_frames < _ANGLE_REENGAGE_SETTLE_FRAMES:
return True
self.ascent_handoff_active = False
self.ascent_reengage_settle_frames = 0
self.ascent_reclaim_frames = _ASCENT_RECLAIM_FRAMES
self.ascent_reclaim_start_angle = CS.out.steeringAngleDeg
self.angle_handoff_active = False
self.angle_reengage_settle_frames = 0
self.angle_reclaim_frames = _ANGLE_RECLAIM_FRAMES
self.angle_reclaim_start_angle = CS.out.steeringAngleDeg
return True
def _ascent_reclaim_target(self, target_angle):
if self.ascent_reclaim_frames <= 0:
def _angle_reclaim_target(self, target_angle):
if self.angle_reclaim_frames <= 0:
return target_angle
progress = (_ASCENT_RECLAIM_FRAMES - self.ascent_reclaim_frames + 1) / _ASCENT_RECLAIM_FRAMES
eased_progress = progress ** _ASCENT_RECLAIM_EXPONENT
target_angle = self.ascent_reclaim_start_angle + eased_progress * \
(target_angle - self.ascent_reclaim_start_angle)
self.ascent_reclaim_frames -= 1
progress = (_ANGLE_RECLAIM_FRAMES - self.angle_reclaim_frames + 1) / _ANGLE_RECLAIM_FRAMES
eased_progress = progress ** _ANGLE_RECLAIM_EXPONENT
target_angle = self.angle_reclaim_start_angle + eased_progress * \
(target_angle - self.angle_reclaim_start_angle)
self.angle_reclaim_frames -= 1
return target_angle
def lateral_angle(self, CC, CS):
@@ -224,30 +224,41 @@ class CarController(CarControllerBase):
self.legacy_2025_lkas_active = lkas_active
return subarucan.create_steering_control_angle(self.packer, apply_steer, lkas_active, self.angle_bus)
if self.CP.carFingerprint == CAR.SUBARU_ASCENT_2023:
if self.CP.carFingerprint in (CAR.SUBARU_ASCENT_2023, CAR.SUBARU_OUTBACK_2023):
mads_only = CC.latActive and not CC.enabled
mads_only_ok = CS.out.vEgoRaw > _ASCENT_MADS_MIN_SPEED and \
abs(CS.out.steeringAngleDeg) < _ASCENT_MADS_MAX_STEER_ANGLE
mads_only_ok = CS.out.vEgoRaw > _ANGLE_MADS_MIN_SPEED and \
abs(CS.out.steeringAngleDeg) < _ANGLE_MADS_MAX_STEER_ANGLE
lkas_available = CC.latActive and (not mads_only or mads_only_ok) and \
CS.out.gearShifter == structs.CarState.GearShifter.drive and not CS.out.standstill
manual_handoff = self._ascent_manual_handoff(CS, lkas_available)
manual_handoff = self._angle_manual_handoff(CS, lkas_available)
lkas_active = lkas_available and not manual_handoff
if lkas_active and not self.ascent_lkas_active:
if lkas_active and not self.angle_lkas_active:
self.apply_steer_last = CS.out.steeringAngleDeg
steer_target = self._ascent_reclaim_target(CC.actuators.steeringAngleDeg) if lkas_active else CC.actuators.steeringAngleDeg
apply_steer = apply_std_steer_angle_limits(
steer_target,
self.apply_steer_last,
CS.out.vEgoRaw,
CS.out.steeringAngleDeg,
lkas_active,
self.p.FIXED_ANGLE_LIMITS,
)
steer_target = self._angle_reclaim_target(CC.actuators.steeringAngleDeg) if lkas_active else CC.actuators.steeringAngleDeg
if self.CP.carFingerprint == CAR.SUBARU_ASCENT_2023:
apply_steer = apply_std_steer_angle_limits(
steer_target,
self.apply_steer_last,
CS.out.vEgoRaw,
CS.out.steeringAngleDeg,
lkas_active,
self.p.FIXED_ANGLE_LIMITS,
)
else:
apply_steer = apply_steer_angle_limits_vm(
steer_target,
self.apply_steer_last,
CS.out.vEgoRaw,
CS.out.steeringAngleDeg,
lkas_active,
self.p,
self.VM,
)
self.apply_steer_last = apply_steer
self.ascent_lkas_active = lkas_active
self.angle_lkas_active = lkas_active
return subarucan.create_steering_control_angle(self.packer, apply_steer, lkas_active, self.angle_bus)
abs_torque = abs(CS.out.steeringTorque)
@@ -51,6 +51,8 @@ class CarInterface(CarInterfaceBase):
if ret.flags & SubaruFlags.LKAS_ANGLE:
ret.steerControlType = structs.CarParams.SteerControlType.angle
if candidate == CAR.SUBARU_OUTBACK_2023:
ret.lateralSmoothSeconds = 0.4
elif candidate in (CAR.SUBARU_ASCENT, CAR.SUBARU_ASCENT_2023):
ret.steerActuatorDelay = 0.3 # end-to-end angle controller
@@ -202,6 +202,7 @@ def test_outback_2023_uses_d_platform_bus_layout():
assert parsers[Bus.main].bus == CanBus.main
assert controller.angle_bus == CanBus.main
assert controller.status_bus == CanBus.main
assert CP.lateralSmoothSeconds == pytest.approx(0.4)
def test_legacy_2025_uses_gen2_angle_bus_layout():
@@ -458,8 +459,9 @@ def test_ascent_angle_controller_uses_fixed_angle_rate_limits():
assert CS.out.steeringAngleDeg < parser.vl["ES_LKAS_ANGLE"]["LKAS_Output"] < -25.0
def test_ascent_angle_controller_yields_until_manual_steering_settles():
CP = CarInterface.get_non_essential_params(CAR.SUBARU_ASCENT_2023)
@pytest.mark.parametrize("platform", (CAR.SUBARU_ASCENT_2023, CAR.SUBARU_OUTBACK_2023))
def test_angle_controller_yields_until_manual_steering_settles(platform):
CP = CarInterface.get_non_essential_params(platform)
controller = CarController({}, CP)
CC = SimpleNamespace(enabled=True, latActive=True, actuators=SimpleNamespace(steeringAngleDeg=-10.0))
CS = SimpleNamespace(out=SimpleNamespace(
+3 -3
View File
@@ -221,9 +221,9 @@ def get_plan_reach(model_v2) -> float:
def get_control_lateral_smooth_seconds(brand: str, v_ego: float, vehicle_smooth_seconds: float) -> float:
if brand != "rivian":
return LAT_SMOOTH_SECONDS
return get_car_lateral_smooth_seconds(brand, v_ego, vehicle_smooth_seconds)
if brand == "rivian" or (brand == "subaru" and vehicle_smooth_seconds > 0.0):
return get_car_lateral_smooth_seconds(brand, v_ego, vehicle_smooth_seconds)
return LAT_SMOOTH_SECONDS
def turn_lead_allowed(brand: str, lateral_control_mode: car.CarControl.Actuators.LateralControlMode) -> bool:
@@ -21,6 +21,15 @@ def test_non_rivian_control_smoothing_matches_starpilot(v_ego):
assert get_control_lateral_smooth_seconds("toyota", v_ego, 0.0) == 0.1
@pytest.mark.parametrize(("v_ego", "expected"), [
(0.0, 0.4),
(5.0, 0.2),
(30.0, 0.0),
])
def test_subaru_control_smoothing_uses_vehicle_schedule(v_ego, expected):
assert get_control_lateral_smooth_seconds("subaru", v_ego, 0.4) == pytest.approx(expected)
@pytest.mark.parametrize(("v_ego", "expected"), [
(0.0, 0.4),
(5.0, 0.2),
+6 -6
View File
@@ -68,8 +68,8 @@ def _model_smooth_seconds(params, key, default):
return round(min(max(value, SMOOTH_SECONDS_STEP), 2.0) / SMOOTH_SECONDS_STEP) * SMOOTH_SECONDS_STEP
def _should_publish_model_output(model_output, vipc_dropped_frames: int) -> bool:
return model_output is not None and vipc_dropped_frames == 0
def _should_publish_model_output(model_output, vipc_dropped_frames: int, external_gpu_active: bool = False) -> bool:
return model_output is not None and (external_gpu_active or vipc_dropped_frames == 0)
MIN_LAT_CONTROL_SPEED = 0.3
@@ -129,7 +129,7 @@ def get_lateral_smooth_seconds(v_ego: float, maximum: float = 0.0) -> float:
def get_car_lateral_smooth_seconds(brand: str, v_ego: float, maximum: float) -> float:
if brand == "rivian":
if brand in ("rivian", "subaru"):
return get_lateral_smooth_seconds(v_ego, maximum)
return maximum
@@ -844,7 +844,7 @@ def main(demo=False):
is_rhd = sm["driverMonitoringState"].isRHD
frame_id = sm["roadCameraState"].frameId
v_ego = max(sm["carState"].vEgo, 0.)
lat_smooth_default = CP.lateralSmoothSeconds if CP.brand == "rivian" else LAT_SMOOTH_SECONDS
lat_smooth_default = CP.lateralSmoothSeconds if (CP.brand == "rivian" or CP.lateralSmoothSeconds > 0.0) else LAT_SMOOTH_SECONDS
lat_smooth_maximum = _model_smooth_seconds(params, "LatSmoothSeconds", lat_smooth_default)
lat_smooth_seconds = get_car_lateral_smooth_seconds(CP.brand, v_ego, lat_smooth_maximum)
lat_delay = sm["liveDelay"].lateralDelay + lat_smooth_seconds
@@ -939,10 +939,10 @@ def main(demo=False):
mt2 = time.perf_counter()
model_execution_time = mt2 - mt1
if model_output is not None and vipc_dropped_frames > 0:
if model_output is not None and vipc_dropped_frames > 0 and not external_gpu_active:
cloudlog.error(f"suppressing model output after dropping {vipc_dropped_frames} frames")
if _should_publish_model_output(model_output, vipc_dropped_frames):
if _should_publish_model_output(model_output, vipc_dropped_frames, external_gpu_active):
modelv2_send = messaging.new_message('modelV2')
starpilot_modelv2_send = messaging.new_message('starpilotModelV2')
drivingdata_send = messaging.new_message('drivingModelData')
@@ -24,6 +24,17 @@ def test_non_rivian_cars_keep_configured_starpilot_smoothing(v_ego):
assert get_car_lateral_smooth_seconds("toyota", v_ego, 0.4) == 0.4
@pytest.mark.parametrize(("v_ego", "expected"), [
(0.0, 0.4),
(2.0, 0.4),
(5.0, 0.2),
(8.0, 0.0),
(30.0, 0.0),
])
def test_subaru_uses_low_speed_configured_smoothing(v_ego, expected):
assert get_car_lateral_smooth_seconds("subaru", v_ego, 0.4) == pytest.approx(expected)
@pytest.mark.parametrize(("v_ego", "maximum", "expected"), [
(0.0, 0.4, 0.4),
(5.0, 0.4, 0.2),
+10 -7
View File
@@ -11,14 +11,17 @@ class FakeParams:
self.values[key] = value
@pytest.mark.parametrize(("model_output", "dropped_frames", "expected"), [
(object(), 0, True),
(object(), 1, False),
(object(), 2, False),
(None, 0, False),
@pytest.mark.parametrize(("model_output", "dropped_frames", "external_gpu_active", "expected"), [
(object(), 0, False, True),
(object(), 1, False, False),
(object(), 2, False, False),
(object(), 1, True, True),
(object(), 2, True, True),
(None, 0, False, False),
(None, 1, True, False),
])
def test_model_output_is_suppressed_after_vipc_drop(model_output, dropped_frames, expected):
assert modeld._should_publish_model_output(model_output, dropped_frames) is expected
def test_model_output_is_suppressed_after_vipc_drop(model_output, dropped_frames, external_gpu_active, expected):
assert modeld._should_publish_model_output(model_output, dropped_frames, external_gpu_active) is expected
def test_incompatible_downloaded_model_falls_back_to_builtin(monkeypatch):
@@ -37,6 +37,36 @@ 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_cpu():
from tinygrad.runtime import ops_amd
sleeps = []
signal = ops_amd.AMDSignal.__new__(ops_amd.AMDSignal)
signal.should_return = False
signal.owner = SimpleNamespace(is_usb=lambda: True, iface=SimpleNamespace(sleep=sleeps.append))
signal._sleep(0)
assert sleeps == [1]
def test_native_amd_signal_keeps_existing_short_wait_behavior():
from tinygrad.runtime import ops_amd
sleeps = []
signal = ops_amd.AMDSignal.__new__(ops_amd.AMDSignal)
signal.should_return = False
signal.owner = SimpleNamespace(is_usb=lambda: False, iface=SimpleNamespace(sleep=sleeps.append))
signal._sleep(199)
assert sleeps == []
signal._sleep(201)
assert sleeps == [200]
def test_external_gpu_power_must_be_stable_after_vehicle_start():
panda_type = modeld.log.PandaState.PandaType.tres
@@ -20,6 +20,8 @@ from openpilot.selfdrive.ui.mici.onroad.starpilot_status import (
get_border_color,
)
from openpilot.selfdrive.ui.mici.onroad.cameraview import CameraView
from openpilot.selfdrive.ui.onroad.starpilot.pip_sidecam import PipSideCamera
from openpilot.selfdrive.ui.onroad.starpilot.starpilot_border import get_traffic_border_colors
from openpilot.selfdrive.ui.lib.starpilot_visuals import get_border_width
from openpilot.starpilot.common.favorite_slots import is_favorite_action_key, load_favorite_slots, toggle_favorite_slot
from openpilot.system.ui.lib.application import FontWeight, gui_app, MousePos, MouseEvent
@@ -583,6 +585,10 @@ class AugmentedRoadView(CameraView):
# debug
self._pm = messaging.PubMaster(['uiDebug'])
# C4 sidecam: fills road preview as a curved rectangle. Only shown
# on the road camera screen, gated via widget visibility
self._pip_sidecam = self._child(PipSideCamera(shape="curved"))
self._pip_sidecam.set_visible(lambda: self.stream_type == ROAD_CAM)
@staticmethod
def _controls_ready() -> bool:
@@ -787,6 +793,17 @@ class AugmentedRoadView(CameraView):
rl.draw_rectangle(int(self.rect.x), int(self.rect.y), int(self.rect.width), int(self.rect.height), rl.Color(0, 0, 0, 175))
self._offroad_label.render(self._content_rect)
# C4 sidecam renders last (on top) and only when showing the road camera
# Inset by the border so the pill never covers the green/orange status border.
border = self._get_border_width()
preview_rect = rl.Rectangle(
self._content_rect.x + border,
self._content_rect.y + border,
max(1, self._content_rect.width - 2 * border),
max(1, self._content_rect.height - 2 * border),
)
self._pip_sidecam.render(preview_rect)
# publish uiDebug
msg = messaging.new_message('uiDebug')
msg.uiDebug.drawTimeMillis = (time.monotonic() - start_draw) * 1000
@@ -809,6 +826,17 @@ class AugmentedRoadView(CameraView):
int(self._content_rect.height),
)
rl.draw_rectangle_rounded_lines_ex(border_rect, 0.12, 16, border_size, get_border_color(ui_state))
if (colors := get_traffic_border_colors()) is not None:
for x, w, color in (
(border_rect.x, border_rect.width / 2, colors[0]),
(border_rect.x + border_rect.width / 2, border_rect.width - border_rect.width / 2, colors[1]),
):
if color.a > 0:
rl.begin_scissor_mode(int(x), int(border_rect.y), int(w), int(border_rect.height))
rl.draw_rectangle_rounded_lines_ex(border_rect, 0.12, 16, border_size, color)
rl.end_scissor_mode()
rl.end_scissor_mode()
def _get_border_width(self) -> int:
+160 -28
View File
@@ -10,6 +10,7 @@ import pyray as rl
from msgq.visionipc import VisionIpcClient, VisionStreamType
from openpilot.selfdrive.ui.ui_state import ui_state
from openpilot.starpilot.common.vision_bsm import get_fresh_vasm_state
from openpilot.system.ui.widgets import Widget
PIP_SHADER_VERSION = """
#version 300 es
@@ -93,6 +94,70 @@ void main() {
}
"""
# curved-rectangle variant for the mici display.
PIP_CURVED_FRAGMENT_SHADER = PIP_SHADER_VERSION + """
in vec2 fragTexCoord;
uniform sampler2D texture0;
uniform sampler2D texture1;
uniform vec2 uCropMin;
uniform vec2 uCropSize;
uniform int uFlipX;
uniform vec2 uRectSize;
out vec4 fragColor;
const float CORNER_RADIUS_FRACTION = 0.22;
const float CURVE_AMOUNT = 0.07;
const float EDGE_DARKEN = 0.14;
const float RIM_BLEND = 0.06;
void main() {
vec2 p = fragTexCoord * 2.0 - 1.0;
float halfW = uRectSize.x * 0.5;
float halfH = uRectSize.y * 0.5;
float radius = CORNER_RADIUS_FRACTION * min(uRectSize.x, uRectSize.y);
// Rounded-rectangle SDF in pixel space; mask before sampling.
vec2 q = abs(vec2(p.x * halfW, p.y * halfH)) - (vec2(halfW, halfH) - radius);
float dist = length(max(q, 0.0)) + min(max(q.x, q.y), 0.0) - radius;
float aa = max(fwidth(dist), 0.00001);
float alpha = 1.0 - smoothstep(-aa, aa, dist);
if (dist > aa) {
discard;
}
// Gentle convex curvature along both axes to mimic the curved OLED panel.
float curve = CURVE_AMOUNT * (1.0 - p.x * p.x) * (1.0 - p.y * p.y);
vec2 sampleCoord = clamp(fragTexCoord + curve * vec2(0.0, 0.5), 0.001, 0.999);
// The saved mask is a square crop, but the curved panel is wider than tall.
// Sample an aspect-matched horizontal band of that square (centered) instead
// of stretching it, so the image is never distorted. The circle's diameter
// (the square crop) becomes the panel's length; the height follows the aspect.
float aspect = uRectSize.x / max(uRectSize.y, 0.0001);
vec2 cropCoord = sampleCoord;
if (aspect >= 1.0) {
cropCoord.y = 0.5 + (sampleCoord.y - 0.5) / aspect;
} else {
cropCoord.x = 0.5 + (sampleCoord.x - 0.5) * aspect;
}
if (uFlipX == 1) {
cropCoord.x = 1.0 - cropCoord.x;
}
vec2 uv = uCropMin + cropCoord * uCropSize;
float y = texture(texture0, uv).r;
vec2 c = texture(texture1, uv).ra - 0.5;
vec3 rgb = vec3(y + 1.402 * c.y, y - 0.344 * c.x - 0.714 * c.y, y + 1.772 * c.x);
// Let the rim blend into the camera image and the UI underneath it.
float edgeShade = smoothstep(-radius, 0.0, dist);
rgb *= mix(1.0, 1.0 - EDGE_DARKEN, edgeShade);
float rim = smoothstep(radius * 0.55, radius, radius - dist);
rgb = mix(rgb, vec3(0.48, 0.70, 1.0), rim * RIM_BLEND);
fragColor = vec4(rgb, alpha);
}
"""
UNIFORM_VEC2 = rl.ShaderUniformDataType.SHADER_UNIFORM_VEC2
UNIFORM_INT = rl.ShaderUniformDataType.SHADER_UNIFORM_INT
@@ -110,10 +175,18 @@ BUBBLE_RADIUS_MIN = 180
BUBBLE_RADIUS_MAX = 420
BUBBLE_MARGIN = 24
class PipSideCamera(Widget):
"""Overlays the adjacent side window from the dcamera.
Drawn as a circular bubble on the big screen (shape="bubble") or as a
curved rectangle filling the whole road preview on the C4 (shape="curved").
"""
def __init__(self, shape: str = "bubble"):
super().__init__()
if shape not in ("bubble", "curved"):
raise ValueError(f"Unknown PipSideCamera shape: {shape!r}")
self._shape = shape
class PipSideCamera:
"""Renders a circular pip bubble of the adjacent side window from the dcamera."""
def __init__(self):
self._params = ui_state.params
self._params_memory = ui_state.params_memory
@@ -132,6 +205,8 @@ class PipSideCamera:
self._show_on_bsm = False
self._mask = {}
self._last_param_refresh = 0.0
self._side_activation_time: dict[str, float] = {}
self._active_sides: set[str] = set()
self.shader = rl.load_shader_from_memory(PIP_VERTEX_SHADER, PIP_FRAGMENT_SHADER)
self._texture1_loc = rl.get_shader_location(self.shader, "texture1")
@@ -140,6 +215,13 @@ class PipSideCamera:
self._flip_x_loc = rl.get_shader_location(self.shader, "uFlipX")
self._flip_x_value = rl.ffi.new("int[1]", [1])
self.curved_shader = rl.load_shader_from_memory(PIP_VERTEX_SHADER, PIP_CURVED_FRAGMENT_SHADER)
self._curved_texture1_loc = rl.get_shader_location(self.curved_shader, "texture1")
self._curved_crop_min_loc = rl.get_shader_location(self.curved_shader, "uCropMin")
self._curved_crop_size_loc = rl.get_shader_location(self.curved_shader, "uCropSize")
self._curved_flip_x_loc = rl.get_shader_location(self.curved_shader, "uFlipX")
self._curved_rect_size_loc = rl.get_shader_location(self.curved_shader, "uRectSize")
self_ref = weakref.ref(self)
def offroad_transition_callback():
@@ -157,16 +239,19 @@ class PipSideCamera:
self.client = VisionIpcClient("camerad", self._stream_type, conflate=True)
def close(self):
if self._closed:
if getattr(self, "_closed", False):
return
self._closed = True
if getattr(self, "_offroad_transition_callback", None) is not None:
ui_state.remove_offroad_transition_callback(self._offroad_transition_callback)
self._offroad_transition_callback = None
self._clear_textures()
if self.shader and self.shader.id:
rl.unload_shader(self.shader)
self.shader.id = 0
if (shader := getattr(self, "shader", None)) is not None and shader.id:
rl.unload_shader(shader)
shader.id = 0
if (curved := getattr(self, "curved_shader", None)) is not None and curved.id:
rl.unload_shader(curved)
curved.id = 0
self.frame = None
self.client = None
@@ -247,16 +332,10 @@ class PipSideCamera:
cy = content_rect.y + content_rect.height - margin - radius
return rl.Rectangle(cx - radius, cy - radius, radius * 2, radius * 2)
def render(self, content_rect: rl.Rectangle):
if not ui_state.started:
return
sides = self.active_sides()
if not sides:
return
def _acquire_frame(self) -> bool:
"""Ensure connection and refresh the Y/UV textures from the latest driver frame."""
if not self._ensure_connection():
return
return False
buffer = self.client.recv(timeout_ms=0)
if buffer:
@@ -264,10 +343,10 @@ class PipSideCamera:
self._last_frame_id = int(getattr(buffer, "frame_id", -1))
self._texture_needs_update = True
if self.frame is None:
return
return False
if not self.texture_y or not self.texture_uv:
return
return False
if self._texture_needs_update:
y_data = self.frame.data[: self.frame.uv_offset]
@@ -275,13 +354,47 @@ class PipSideCamera:
rl.update_texture(self.texture_y, rl.ffi.cast("void *", rl.ffi.from_buffer(y_data)))
rl.update_texture(self.texture_uv, rl.ffi.cast("void *", rl.ffi.from_buffer(uv_data)))
self._texture_needs_update = False
return True
for side in sides:
crop = self._crop_rect(side)
if crop is None:
continue
bubble = self._bubble_rect(content_rect, side)
self._draw_bubble(bubble, crop)
def _render(self, content_rect: rl.Rectangle):
"""Fetch the current driver frame, then draw it for the configured shape."""
if not ui_state.started:
return None
sides = self.active_sides()
if not sides or not self._acquire_frame():
return None
if self._shape == "curved":
# C4: one crop fills the whole road preview as a curved rectangle.
side = self._pick_side(sides)
if side is not None:
crop = self._crop_rect(side)
if crop is not None:
self._draw_curved(content_rect, crop)
else:
# Raybig: one circular bubble per active side.
for side in sides:
crop = self._crop_rect(side)
if crop is None:
continue
bubble = self._bubble_rect(content_rect, side)
self._draw_bubble(bubble, crop)
return None
def _pick_side(self, sides: list[str]) -> str | None:
"""Return the active side whose blinker/BSM most recently turned on.
Only a rising edge (inactive -> active) refreshes the timestamp
"""
now = time.monotonic()
active = set(sides)
for side in active - self._active_sides:
self._side_activation_time[side] = now
self._active_sides = active
if not sides:
return None
return max(sides, key=lambda side: self._side_activation_time.get(side, 0.0))
def _draw_bubble(self, bubble: rl.Rectangle, crop: rl.Rectangle):
tex_w = float(self.texture_y.width)
@@ -300,6 +413,25 @@ class PipSideCamera:
rl.draw_texture_pro(self.texture_y, src_rect, dst_rect, rl.Vector2(0, 0), 0.0, rl.WHITE)
rl.end_shader_mode()
def _draw_curved(self, content_rect: rl.Rectangle, crop: rl.Rectangle):
tex_w = float(self.texture_y.width)
tex_h = float(self.texture_y.height)
crop_min = rl.Vector2(crop.x / tex_w, crop.y / tex_h)
crop_size = rl.Vector2(crop.width / tex_w, crop.height / tex_h)
rect_size = rl.Vector2(content_rect.width, content_rect.height)
src_rect = rl.Rectangle(0, 0, tex_w, tex_h)
dst_rect = rl.Rectangle(content_rect.x, content_rect.y, content_rect.width, content_rect.height)
rl.begin_shader_mode(self.curved_shader)
rl.set_shader_value(self.curved_shader, self._curved_crop_min_loc, crop_min, UNIFORM_VEC2)
rl.set_shader_value(self.curved_shader, self._curved_crop_size_loc, crop_size, UNIFORM_VEC2)
rl.set_shader_value(self.curved_shader, self._curved_flip_x_loc, self._flip_x_value, UNIFORM_INT)
rl.set_shader_value(self.curved_shader, self._curved_rect_size_loc, rect_size, UNIFORM_VEC2)
rl.set_shader_value_texture(self.curved_shader, self._curved_texture1_loc, self.texture_uv)
rl.draw_texture_pro(self.texture_y, src_rect, dst_rect, rl.Vector2(0, 0), 0.0, rl.WHITE)
rl.end_shader_mode()
def _ensure_connection(self) -> bool:
if not self.client.is_connected():
self.frame = None
@@ -324,9 +456,9 @@ class PipSideCamera:
int(self.client.height // 2), 1, rl.PixelFormat.PIXELFORMAT_UNCOMPRESSED_GRAY_ALPHA))
def _clear_textures(self):
if self.texture_y and self.texture_y.id:
rl.unload_texture(self.texture_y)
if (texture_y := getattr(self, "texture_y", None)) is not None and texture_y.id:
rl.unload_texture(texture_y)
self.texture_y = None
if self.texture_uv and self.texture_uv.id:
rl.unload_texture(self.texture_uv)
if (texture_uv := getattr(self, "texture_uv", None)) is not None and texture_uv.id:
rl.unload_texture(texture_uv)
self.texture_uv = None
@@ -4,6 +4,7 @@ from typing import Optional
import pyray as rl
from openpilot.common.constants import CV
from openpilot.selfdrive.ui.ui_state import ui_state, UIStatus
from openpilot.selfdrive.ui.onroad.hud_renderer import COLORS
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
@@ -144,7 +145,7 @@ def _get_slc_state():
)
slc_overridden_speed = plan.slcOverriddenSpeed
# Keep the source limit visible; override state only dims the sign.
# Keep the source limit visible when overridden.
speed_limit = plan.slcSpeedLimit
# Resolved limit in m/s (pre-conversion, pre-offset) — feeds the vision pulse
@@ -224,14 +225,14 @@ def _active_source_label(state: dict) -> str:
return _ACTIVE_SOURCE_LABELS.get(source, source.upper())
def _source_label_color(alpha: int) -> rl.Color:
def _source_label_color(alpha: int, is_overridden: bool = False) -> rl.Color:
"""Match Set Speed's MAX label color."""
if ui_state.status == UIStatus.ENGAGED:
base = rl.Color(128, 216, 166, 255)
elif ui_state.status in (UIStatus.DISENGAGED, UIStatus.OVERRIDE):
base = rl.Color(145, 155, 149, 255)
if is_overridden or ui_state.status in (UIStatus.DISENGAGED, UIStatus.OVERRIDE):
base = COLORS.DISENGAGED
elif ui_state.status == UIStatus.ENGAGED:
base = COLORS.ENGAGED
else:
base = rl.Color(166, 166, 166, 255)
base = COLORS.GREY
return _speed_limit_pulse_color(base, alpha)
@@ -265,7 +266,8 @@ def _draw_offset_chip(rect: rl.Rectangle, offset_str: str, color: rl.Color) -> N
def _draw_us_sign(x: float, y: float, sign_width: float, sign_height: float,
speed_text: str, offset_str: str,
source_label: str, alpha: int, show_offset: bool, *, pending: bool = False):
source_label: str, alpha: int, show_offset: bool, *,
pending: bool = False, is_overridden: bool = False):
"""Draw the NA control card at (x, y).
The card keeps the SLC's label/value hierarchy while sharing the exact
@@ -305,7 +307,7 @@ def _draw_us_sign(x: float, y: float, sign_width: float, sign_height: float,
elif show_offset:
# Offset ON: source at the top, speed below it, and the offset in a chip.
source_size = measure_text_cached(font_semi, source_label, FONT_SOURCE)
source_color = _source_label_color(alpha)
source_color = _source_label_color(alpha, is_overridden=is_overridden)
rl.draw_text_ex(font_semi, source_label, rl.Vector2(cx - source_size.x / 2, y + 8), FONT_SOURCE, 0, source_color)
speed_size = measure_text_cached(font_bold, speed_text, FONT_SPEED)
@@ -314,7 +316,7 @@ def _draw_us_sign(x: float, y: float, sign_width: float, sign_height: float,
else:
# Offset OFF: match Set Speed typography.
source_size = measure_text_cached(font_semi, source_label, FONT_SOURCE)
source_color = _source_label_color(alpha)
source_color = _source_label_color(alpha, is_overridden=is_overridden)
rl.draw_text_ex(font_semi, source_label, rl.Vector2(cx - source_size.x / 2, y + 27), FONT_SOURCE, 0, source_color)
speed_size = measure_text_cached(font_bold, speed_text, FONT_SPEED)
@@ -327,23 +329,19 @@ def _draw_eu_sign(x: float, y: float, speed_text: str, offset_str: str,
source_label: str, text_alpha: int, show_offset: bool, *, pending: bool = False):
"""Draw EU-style (Vienna) speed limit sign at (x, y).
White disk with a pulsable red ring and pulsable black text. The disk
fill, ring, and text all carry the sign-wide ``text_alpha`` (e.g. 72 when
driver-overridden, 255 otherwise), so the road shows through when dimmed
without losing legibility. The pre-existing pending-text blink
(black <-> red) composes with the vision pulse: outside the pulse window
the blink is unchanged, inside it both colors are eased toward
White disk with a pulsable red ring and pulsable black text. The pre-existing
pending-text blink (black <-> red) composes with the vision pulse: outside the
pulse window the blink is unchanged, inside it both colors are eased toward
VISION_SPEED_LIMIT_PULSE_COLOR.
"""
center_x = x + EU_SIGN_SIZE / 2
center_y = y + EU_SIGN_SIZE / 2
radius = EU_SIGN_SIZE / 2
# White disk fill; alpha-dims with the sign so an overridden limit fades
# against the road.
# White disk fill.
rl.draw_circle(int(center_x), int(center_y), radius, rl.Color(255, 255, 255, text_alpha))
# Red ring; eased toward VISION_SPEED_LIMIT_PULSE_COLOR when a Vision-sourced
# limit just changed, and alpha-dims with the sign.
# limit just changed.
ring_color = _speed_limit_pulse_color(rl.Color(201, 34, 49, 255), text_alpha)
rl.draw_ring(rl.Vector2(center_x, center_y), radius - RED_RING_WIDTH, radius,
0, 360, 64, ring_color)
@@ -398,14 +396,11 @@ def _draw_sign(state: dict, rect: rl.Rectangle, *, pending: bool = False):
# Pending shows the unconfirmed value, full opacity
speed_text = ("\u2013" if state['unconfirmed_speed_limit'] <= 1
else str(int(round(state['unconfirmed_speed_limit']))))
text_alpha = 255
else:
speed_text = state['speed_limit_str']
# Override dim: when the driver has manually overridden the speed limit,
# fade the sign to alpha=72 to indicate it's no longer the auto-detected
# value.
text_alpha = 72 if state['slc_overridden_speed'] != 0 else 255
text_alpha = 255
is_overridden = not pending and state['slc_overridden_speed'] != 0
source_label = _active_source_label(state)
if state['use_vienna']:
@@ -413,7 +408,8 @@ def _draw_sign(state: dict, rect: rl.Rectangle, *, pending: bool = False):
state['show_offset'], pending=pending)
else:
_draw_us_sign(rect.x, rect.y, rect.width, rect.height, speed_text, state['offset_str'],
source_label, text_alpha, state['show_offset'], pending=pending)
source_label, text_alpha, state['show_offset'], pending=pending,
is_overridden=is_overridden)
# ── Sources Bubble (expandable overlay) ────────────────────────────────
@@ -170,55 +170,65 @@ def _render_csc_glow(border_rect: rl.Rectangle, border_width: float = UI_BORDER_
_smoothed_steer = 0.0
def get_traffic_border_colors() -> tuple[rl.Color, rl.Color] | None:
sm = ui_state.sm
car_state = sm["carState"] if sm.valid.get("carState", False) else None
if car_state is None:
return None
params = ui_state.ui_params
show_signal = params.get_bool("SignalMetrics")
show_blindspot = params.get_bool("BlindSpotMetrics")
if not (show_signal or show_blindspot):
return None
left_blindspot = car_state.leftBlindspot
right_blindspot = car_state.rightBlindspot
if ui_state.starpilot_toggles.get("v_asm_enabled", False):
vasm_left, vasm_right = get_fresh_vasm_state(ui_state.params_memory)
left_blindspot = left_blindspot or vasm_left
right_blindspot = right_blindspot or vasm_right
left_blinker = car_state.leftBlinker
right_blinker = car_state.rightBlinker
if not ((show_signal and (left_blinker or right_blinker)) or (show_blindspot and (left_blindspot or right_blindspot))):
return None
interval = 250 if show_blindspot and (left_blindspot or right_blindspot) else 500
flicker_active = (int(rl.get_time() * 1000) % (interval * 2)) < interval
def get_half_border_color(blindspot, turn_signal):
if turn_signal and show_signal:
if blindspot:
return TRAFFIC_COLOR if flicker_active else CEM_OVERRIDE_COLOR
else:
return CEM_OVERRIDE_COLOR if flicker_active else rl.Color(0, 0, 0, 0)
elif blindspot and show_blindspot:
return TRAFFIC_COLOR
else:
return rl.Color(0, 0, 0, 0)
left_color = get_half_border_color(left_blindspot, left_blinker)
right_color = get_half_border_color(right_blindspot, right_blinker)
return left_color, right_color
def render_background_effects(rect: rl.Rectangle, border_width: float):
global _smoothed_steer
sm = ui_state.sm
# 1. Turn Signal and Blind Spot indicators
car_state = sm["carState"] if sm.valid.get("carState", False) else None
if car_state:
params = ui_state.ui_params
show_signal = params.get_bool("SignalMetrics")
show_blindspot = params.get_bool("BlindSpotMetrics")
if show_signal or show_blindspot:
left_blindspot = car_state.leftBlindspot
right_blindspot = car_state.rightBlindspot
if ui_state.starpilot_toggles.get("v_asm_enabled", False):
vasm_left, vasm_right = get_fresh_vasm_state(ui_state.params_memory)
left_blindspot = left_blindspot or vasm_left
right_blindspot = right_blindspot or vasm_right
left_blinker = car_state.leftBlinker
right_blinker = car_state.rightBlinker
if (show_signal and (left_blinker or right_blinker)) or (show_blindspot and (left_blindspot or right_blindspot)):
interval = 250 if show_blindspot and (left_blindspot or right_blindspot) else 500
flicker_active = (int(rl.get_time() * 1000) % (interval * 2)) < interval
def get_half_border_color(blindspot, turn_signal):
if turn_signal and show_signal:
if blindspot:
return TRAFFIC_COLOR if flicker_active else CEM_OVERRIDE_COLOR
else:
return CEM_OVERRIDE_COLOR if flicker_active else rl.Color(0, 0, 0, 0)
elif blindspot and show_blindspot:
return TRAFFIC_COLOR
else:
return rl.Color(0, 0, 0, 0)
left_color = get_half_border_color(left_blindspot, left_blinker)
right_color = get_half_border_color(right_blindspot, right_blinker)
# Draw left side borders
if left_color.a > 0:
rl.begin_scissor_mode(int(rect.x), int(rect.y), int(rect.width // 2), int(rect.height))
rl.draw_rectangle_rounded(rect, 0.12, 10, left_color)
rl.end_scissor_mode()
# Draw right side borders
if right_color.a > 0:
rl.begin_scissor_mode(int(rect.x + rect.width // 2), int(rect.y), int(rect.width // 2), int(rect.height))
rl.draw_rectangle_rounded(rect, 0.12, 10, right_color)
rl.end_scissor_mode()
colors = get_traffic_border_colors()
if colors is not None:
left_color, right_color = colors
if left_color.a > 0:
rl.begin_scissor_mode(int(rect.x), int(rect.y), int(rect.width // 2), int(rect.height))
rl.draw_rectangle_rounded(rect, 0.12, 10, left_color)
rl.end_scissor_mode()
if right_color.a > 0:
rl.begin_scissor_mode(int(rect.x + rect.width // 2), int(rect.y), int(rect.width // 2), int(rect.height))
rl.draw_rectangle_rounded(rect, 0.12, 10, right_color)
rl.end_scissor_mode()
# 2. Steering Torque Border
car_control = sm["carControl"] if sm.valid.get("carControl", False) else None
@@ -39,7 +39,7 @@ class StarPilotOnroadView(AugmentedRoadView):
self._max_fps = 0.0
self._avg_fps = 0.0
self._pip_sidecam = PipSideCamera()
self._pip_sidecam = self._child(PipSideCamera())
self.layout_manager = WidgetLayoutManager(self._content_rect)
+37
View File
@@ -1,6 +1,7 @@
from openpilot.selfdrive.ui.onroad.starpilot.pip_sidecam import (
IMAGE_TO_VEHICLE_SIDE,
PIP_FRAGMENT_SHADER,
PIP_CURVED_FRAGMENT_SHADER,
PipSideCamera,
)
@@ -47,3 +48,39 @@ def test_pip_driver_camera_shader_uses_analytic_bubble_shading_without_new_unifo
assert "rgb = mix(rgb, vec3(1.0)" not in PIP_FRAGMENT_SHADER
assert "uniform sampler2D texture2" not in PIP_FRAGMENT_SHADER
assert "uRefraction" not in PIP_FRAGMENT_SHADER
def test_pip_c4_curved_shader_masks_a_rounded_rectangle_before_sampling():
assert "uRectSize" in PIP_CURVED_FRAGMENT_SHADER
assert "CORNER_RADIUS_FRACTION" in PIP_CURVED_FRAGMENT_SHADER
assert "CURVE_AMOUNT" in PIP_CURVED_FRAGMENT_SHADER
assert "length(max(q, 0.0))" in PIP_CURVED_FRAGMENT_SHADER
assert PIP_CURVED_FRAGMENT_SHADER.index("if (dist > aa)") < PIP_CURVED_FRAGMENT_SHADER.index("texture(texture0")
assert PIP_CURVED_FRAGMENT_SHADER.count("texture(texture0") == 1
assert PIP_CURVED_FRAGMENT_SHADER.count("texture(texture1") == 1
assert "cropCoord.x = 1.0 - cropCoord.x" in PIP_CURVED_FRAGMENT_SHADER
assert "y + 1.402 * c.y" in PIP_CURVED_FRAGMENT_SHADER
assert "uRefraction" not in PIP_CURVED_FRAGMENT_SHADER
def test_pip_sidecam_is_a_widget_with_curved_and_bubble_shapes():
bubble = PipSideCamera.__new__(PipSideCamera)
bubble._closed = True
assert isinstance(bubble, PipSideCamera)
assert hasattr(bubble, "render")
assert hasattr(bubble, "_draw_bubble")
assert hasattr(bubble, "_draw_curved")
curved = PipSideCamera.__new__(PipSideCamera)
curved._closed = True
curved._shape = "curved"
assert curved._shape == "curved"
def test_pip_sidecam_rejects_unknown_shapes():
camera = object.__new__(PipSideCamera)
try:
PipSideCamera.__init__(camera, shape="hexagon")
except ValueError:
pass
else:
raise AssertionError("expected ValueError for unknown shape")
@@ -82,3 +82,34 @@ def test_visible_source_rows_honor_active_only_and_source_order():
assert visible_source_rows(
source_defs, {key: 0.0 for key in values}, "Map Data", ("Map Data",),
) == []
def test_source_label_color_override_and_engagement_states():
from openpilot.selfdrive.ui.onroad.starpilot.slc_speed_limit import _source_label_color
from openpilot.selfdrive.ui.onroad.hud_renderer import COLORS
from openpilot.selfdrive.ui.ui_state import ui_state, UIStatus
# Engaged and not overridden -> Active green
ui_state.status = UIStatus.ENGAGED
color = _source_label_color(255, is_overridden=False)
assert (color.r, color.g, color.b, color.a) == (COLORS.ENGAGED.r, COLORS.ENGAGED.g, COLORS.ENGAGED.b, 255)
# Engaged but overridden -> Disengaged/override gray
color_overridden = _source_label_color(255, is_overridden=True)
assert (color_overridden.r, color_overridden.g, color_overridden.b, color_overridden.a) == (
COLORS.DISENGAGED.r, COLORS.DISENGAGED.g, COLORS.DISENGAGED.b, 255
)
# Disengaged -> Disengaged/override gray
ui_state.status = UIStatus.DISENGAGED
color_disengaged = _source_label_color(255, is_overridden=False)
assert (color_disengaged.r, color_disengaged.g, color_disengaged.b, color_disengaged.a) == (
COLORS.DISENGAGED.r, COLORS.DISENGAGED.g, COLORS.DISENGAGED.b, 255
)
# Override UI status -> Disengaged/override gray
ui_state.status = UIStatus.OVERRIDE
color_ui_override = _source_label_color(255, is_overridden=False)
assert (color_ui_override.r, color_ui_override.g, color_ui_override.b, color_ui_override.a) == (
COLORS.OVERRIDE.r, COLORS.OVERRIDE.g, COLORS.OVERRIDE.b, 255
)
+149
View File
@@ -0,0 +1,149 @@
from types import SimpleNamespace
from openpilot.selfdrive.ui.lib.starpilot_status import TRAFFIC_COLOR, CEM_OVERRIDE_COLOR
from openpilot.selfdrive.ui.onroad.starpilot import starpilot_border
from openpilot.selfdrive.ui.ui_state import ui_state
TRANSPARENT = (0, 0, 0, 0)
def _rgba(color):
return color.r, color.g, color.b, color.a
def _car_state(left_blindspot=False, right_blindspot=False, left_blinker=False, right_blinker=False):
return SimpleNamespace(
leftBlindspot=left_blindspot,
rightBlindspot=right_blindspot,
leftBlinker=left_blinker,
rightBlinker=right_blinker,
)
def _setup(monkeypatch, *, car_state, signal=True, blindspot=True, v_asm_enabled=False, v_asm=(False, False), time=0.0):
class FakeSM(dict):
valid = {"carState": True}
monkeypatch.setattr(ui_state, "sm", FakeSM(carState=car_state))
monkeypatch.setattr(
ui_state,
"ui_params",
SimpleNamespace(get_bool=lambda key: {"SignalMetrics": signal, "BlindSpotMetrics": blindspot}[key]),
)
monkeypatch.setattr(ui_state, "starpilot_toggles", {"v_asm_enabled": v_asm_enabled})
monkeypatch.setattr(ui_state, "params_memory", object())
monkeypatch.setattr(starpilot_border, "get_fresh_vasm_state", lambda _memory: v_asm)
monkeypatch.setattr(starpilot_border.rl, "get_time", lambda: time)
def test_traffic_border_inactive_when_metrics_disabled(monkeypatch):
_setup(monkeypatch, car_state=_car_state(left_blinker=True), signal=False, blindspot=False)
assert starpilot_border.get_traffic_border_colors() is None
def test_traffic_border_inactive_when_nothing_active(monkeypatch):
_setup(monkeypatch, car_state=_car_state())
assert starpilot_border.get_traffic_border_colors() is None
def test_traffic_border_left_blindspot_is_red(monkeypatch):
_setup(monkeypatch, car_state=_car_state(left_blindspot=True))
left, right = starpilot_border.get_traffic_border_colors()
assert _rgba(left) == _rgba(TRAFFIC_COLOR)
assert _rgba(right) == TRANSPARENT
def test_traffic_border_right_blindspot_is_red(monkeypatch):
_setup(monkeypatch, car_state=_car_state(right_blindspot=True))
left, right = starpilot_border.get_traffic_border_colors()
assert _rgba(left) == TRANSPARENT
assert _rgba(right) == _rgba(TRAFFIC_COLOR)
def test_traffic_border_blinker_alone_flickers_amber(monkeypatch):
_setup(monkeypatch, car_state=_car_state(left_blinker=True), blindspot=False, time=0.1)
left, right = starpilot_border.get_traffic_border_colors()
assert _rgba(left) == _rgba(CEM_OVERRIDE_COLOR)
assert _rgba(right) == TRANSPARENT
_setup(monkeypatch, car_state=_car_state(left_blinker=True), blindspot=False, time=0.6)
left, right = starpilot_border.get_traffic_border_colors()
assert _rgba(left) == TRANSPARENT
assert _rgba(right) == TRANSPARENT
def test_traffic_border_blinker_with_blindspot_flickers_red_and_amber(monkeypatch):
_setup(monkeypatch, car_state=_car_state(left_blinker=True, left_blindspot=True), time=0.1)
left, _ = starpilot_border.get_traffic_border_colors()
assert _rgba(left) == _rgba(TRAFFIC_COLOR)
_setup(monkeypatch, car_state=_car_state(left_blinker=True, left_blindspot=True), time=0.3)
left, _ = starpilot_border.get_traffic_border_colors()
assert _rgba(left) == _rgba(CEM_OVERRIDE_COLOR)
def test_traffic_border_v_asm_blindspot_is_red(monkeypatch):
_setup(monkeypatch, car_state=_car_state(), v_asm_enabled=True, v_asm=(True, False))
left, right = starpilot_border.get_traffic_border_colors()
assert _rgba(left) == _rgba(TRAFFIC_COLOR)
assert _rgba(right) == TRANSPARENT
def test_c4_draw_border_paints_traffic_color_on_active_half(monkeypatch):
import pyray as rl
from openpilot.selfdrive.ui.mici.onroad import augmented_road_view as mici_view
view = object.__new__(mici_view.AugmentedRoadView)
view._content_rect = rl.Rectangle(10, 20, 200, 100)
view._get_border_width = lambda: 8
view._closed = True
base_color = rl.Color(0, 0, 0, 255)
calls = []
monkeypatch.setattr(mici_view, "get_border_color", lambda _state: base_color)
monkeypatch.setattr(mici_view, "get_traffic_border_colors", lambda: (TRAFFIC_COLOR, rl.Color(0, 0, 0, 0)))
monkeypatch.setattr(mici_view.rl, "begin_scissor_mode", lambda *args: calls.append(("begin_scissor", args)))
monkeypatch.setattr(mici_view.rl, "end_scissor_mode", lambda: calls.append(("end_scissor",)))
monkeypatch.setattr(mici_view.rl, "draw_rectangle_rounded_lines_ex", lambda *args: calls.append(("line", args)))
view._draw_border()
lines = [c for c in calls if c[0] == "line"]
assert len(lines) == 2
assert _rgba(lines[0][1][4]) == _rgba(base_color)
assert _rgba(lines[1][1][4]) == _rgba(TRAFFIC_COLOR)
scissor = [c for c in calls if c[0] == "begin_scissor"]
assert scissor[0][1] == (10, 20, 200, 100)
assert scissor[1][1] == (14, 24, 96, 92)
def test_c4_draw_border_skips_traffic_colors_when_inactive(monkeypatch):
import pyray as rl
from openpilot.selfdrive.ui.mici.onroad import augmented_road_view as mici_view
view = object.__new__(mici_view.AugmentedRoadView)
view._content_rect = rl.Rectangle(10, 20, 200, 100)
view._get_border_width = lambda: 8
view._closed = True
calls = []
monkeypatch.setattr(mici_view, "get_border_color", lambda _state: rl.Color(0, 0, 0, 255))
monkeypatch.setattr(mici_view, "get_traffic_border_colors", lambda: None)
monkeypatch.setattr(mici_view.rl, "begin_scissor_mode", lambda *args: calls.append(("begin_scissor", args)))
monkeypatch.setattr(mici_view.rl, "end_scissor_mode", lambda: calls.append(("end_scissor",)))
monkeypatch.setattr(mici_view.rl, "draw_rectangle_rounded_lines_ex", lambda *args: calls.append(("line", args)))
view._draw_border()
assert len([c for c in calls if c[0] == "line"]) == 1
assert len([c for c in calls if c[0] == "begin_scissor"]) == 1
+3 -2
View File
@@ -155,8 +155,9 @@ class FordLateralController:
def _lane_change(self) -> tuple[bool, int]:
if self.model is None:
return False, 0
state = int(self.model.meta.laneChangeState)
return state in (1, 2, 3), int(self.model.meta.laneChangeDirection)
state = int(getattr(self.model.meta.laneChangeState, "raw", self.model.meta.laneChangeState))
direction = int(getattr(self.model.meta.laneChangeDirection, "raw", self.model.meta.laneChangeDirection))
return state in (1, 2, 3), direction
@staticmethod
def _current_curvature(CS) -> float:
+9 -1
View File
@@ -49,13 +49,21 @@ def test_curvature_strategy_uses_polynomial_signals(controller):
assert result.ramp_type == 2
def test_lane_change_accepts_capnp_enum_wrappers(controller):
controller.model = SimpleNamespace(meta=SimpleNamespace(
laneChangeState=SimpleNamespace(raw=2),
laneChangeDirection=SimpleNamespace(raw=1),
))
assert controller._lane_change() == (True, 1)
def test_angle_strategy_uses_path_angle_and_shadow(controller):
result = controller.update_angle(
SimpleNamespace(latActive=True), car_state(curvature=0.001), SimpleNamespace(curvature=0.001))
assert result.active
assert result.curvature == 0.0
assert result.path_angle > 0.0
assert result.shadow_curvature == pytest.approx(0.001)
assert result.shadow_curvature == pytest.approx(0.0005)
def test_manual_turn_releases_lateral(controller):
@@ -35,6 +35,97 @@ const state = reactive({
let _loadedImage = null;
let _lastCanvas = null;
let loadedConfig = null;
let deviceType = null;
const C4_ROAD_ASPECT = 476 / 240;
function isC4() {
return (deviceType || "").toLowerCase() === "mici";
}
function drawCurvedRect(ctx, x, y, w, h) {
const r = Math.min(w, h) * 0.22;
ctx.beginPath();
ctx.moveTo(x + r, y);
ctx.arcTo(x + w, y, x + w, y + h, r);
ctx.arcTo(x + w, y + h, x, y + h, r);
ctx.arcTo(x, y + h, x, y, r);
ctx.arcTo(x, y, x + w, y, r);
ctx.closePath();
}
function cropSource(ctx, img, center, zoom) {
const cw = ctx.canvas.width;
const ch = ctx.canvas.height;
const nativeW = img.naturalWidth;
const nativeH = img.naturalHeight;
const [cx, cy] = center;
const nativeCx = (cw - cx) * nativeW / cw;
const nativeCy = cy * nativeH / ch;
const nativeZoom = zoom * nativeW / cw;
return { cw, ch, cx, cy, nativeCx, nativeCy, nativeZoom };
}
function drawC4Preview(ctx, img, center, zoom, color) {
const { cw, ch, cx, cy, nativeCx, nativeCy, nativeZoom } = cropSource(ctx, img, center, zoom);
let w = Math.min(zoom * 1.1, cw * 0.55);
w = Math.max(60, w);
let h = w / C4_ROAD_ASPECT;
if (h > ch * 0.5) {
h = ch * 0.5;
w = h * C4_ROAD_ASPECT;
}
const x = cx - w / 2;
const y = cy - h / 2;
const aspect = w / h;
const sx = nativeCx - nativeZoom / 2;
const sh = nativeZoom / aspect;
const sy = nativeCy - sh / 2;
ctx.save();
drawCurvedRect(ctx, x, y, w, h);
ctx.fillStyle = "#000";
ctx.fill();
ctx.clip();
ctx.translate(x + w, 0);
ctx.scale(-1, 1);
ctx.drawImage(img, sx, sy, nativeZoom, sh, 0, y, w, h);
ctx.restore();
ctx.save();
drawCurvedRect(ctx, x, y, w, h);
ctx.strokeStyle = color;
ctx.lineWidth = 2.5;
ctx.stroke();
ctx.restore();
}
function drawC3Preview(ctx, img, center, zoom, color) {
const { cx, cy, nativeCx, nativeCy, nativeZoom } = cropSource(ctx, img, center, zoom);
const half = zoom / 2;
const sx = nativeCx - nativeZoom / 2;
const sy = nativeCy - nativeZoom / 2;
ctx.save();
ctx.beginPath();
ctx.arc(cx, cy, half, 0, Math.PI * 2);
ctx.fillStyle = "#000";
ctx.fill();
ctx.clip();
ctx.translate(cx + half, 0);
ctx.scale(-1, 1);
ctx.drawImage(img, sx, sy, nativeZoom, nativeZoom, 0, cy - half, zoom, zoom);
ctx.restore();
ctx.save();
ctx.beginPath();
ctx.arc(cx, cy, half, 0, Math.PI * 2);
ctx.strokeStyle = color;
ctx.lineWidth = 2;
ctx.stroke();
ctx.restore();
}
function getCanvas() {
return document.getElementById("pip-sidecam-canvas");
@@ -107,19 +198,11 @@ function redraw() {
const [cx, cy] = side.center;
// Crop square (what gets sampled) + circular bubble overlay.
ctx.strokeStyle = side.color;
ctx.lineWidth = 2;
ctx.setLineDash([]);
ctx.strokeRect(cx - half, cy - half, state.zoom, state.zoom);
ctx.beginPath();
ctx.arc(cx, cy, half, 0, Math.PI * 2);
ctx.fillStyle = side.color + "40";
ctx.fill();
ctx.lineWidth = 2;
ctx.strokeStyle = side.color;
ctx.stroke();
if (isC4()) {
drawC4Preview(ctx, img, side.center, state.zoom, side.color);
} else {
drawC3Preview(ctx, img, side.center, state.zoom, side.color);
}
// Center dot
ctx.beginPath();
@@ -323,8 +406,9 @@ async function loadExistingConfig() {
try {
const resp = await fetch("/api/pip_preview/config");
if (!resp.ok) return;
const config = await resp.json();
loadedConfig = config;
const data = await resp.json();
deviceType = data.device_type || deviceType || null;
loadedConfig = data.mask || null;
applyConfigToCanvas();
} catch (e) {
console.error("PiP Preview config load failed", e);
+2 -1
View File
@@ -8454,7 +8454,8 @@ def setup(app):
def pip_preview_get_config():
if not params.get_bool("GalaxyDeveloperMode"):
return jsonify({"error": "PiP Side Camera is available only with Galaxy Developer Mode enabled."}), 403
return jsonify(_decode_json_object(params.get("PIPPreviewMask")))
mask = _decode_json_object(params.get("PIPPreviewMask"))
return jsonify({"device_type": HARDWARE.get_device_type(), "mask": mask})
@app.route("/api/pip_preview/config", methods=["POST"])
def pip_preview_save_config():
+5 -2
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
import os, ctypes, struct, hashlib, functools, importlib, mmap, errno, array, contextlib, sys, weakref, itertools, collections, atexit, time
assert sys.platform != 'win32'
from dataclasses import dataclass
from tinygrad.runtime.support.hcq import HCQCompiled, HCQAllocator, HCQBuffer, HWQueue, CLikeArgsState, HCQSignal, HCQProgram, FileIOInterface
@@ -45,6 +45,9 @@ 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):
if self.owner is not None and self.owner.is_usb():
self.owner.iface.sleep(1)
return
# 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)
@@ -933,7 +936,7 @@ class USBIface(PCIIface):
# force devmem
return super().alloc(size, host=False, uncached=uncached, cpu_access=cpu_access, contiguous=contiguous, force_devmem=True, **kwargs)
def sleep(self, timeout): pass
def sleep(self, timeout): time.sleep(timeout / 1000)
def _mock(iface, name=None): return type(name or f"MOCK{iface.__name__}", (iface,), {})