Fix turn allowed conditions for lane changes in DesireHelper

Cherry-picked Feature Request 745053 from 371f063362.

Co-authored-by: Prabhaav Pillai <prabhaav@msu.edu>

launch_env.sh

@@ -21,7 +21,7 @@ fi
 export QCOM_PRIORITY=12
 
 if [ -z "$AGNOS_VERSION" ]; then
-  export AGNOS_VERSION="19.6.1"
+  export AGNOS_VERSION="19.6.2"
 fi
 
 if [ -z "$AGNOS_ACCEPTED_VERSIONS" ]; then

selfdrive/controls/lib/desire_helper.py

@@ -224,12 +224,12 @@ class DesireHelper:
         if desired_lane_width >= starpilot_toggles.lane_detection_width and self._nav_torque_applied(carstate, lane_change_direction):
           return log.Desire.keepRight
     elif modifier in ("left", "sharpLeft"):
-      turn_allowed = not carstate.rightBlinker and not carstate.leftBlindspot
+      turn_allowed = carstate.leftBlinker and not carstate.rightBlinker and not carstate.leftBlindspot
       turn_allowed &= carstate.vEgo < starpilot_toggles.minimum_lane_change_speed and not carstate.standstill
       if turn_allowed and self._nav_turn_is_imminent(carstate, maneuver_distance):
         return log.Desire.turnLeft
     elif modifier in ("right", "sharpRight"):
-      turn_allowed = not carstate.leftBlinker and not carstate.rightBlindspot
+      turn_allowed = carstate.rightBlinker and not carstate.leftBlinker and not carstate.rightBlindspot
       turn_allowed &= carstate.vEgo < starpilot_toggles.minimum_lane_change_speed and not carstate.standstill
       if turn_allowed and self._nav_turn_is_imminent(carstate, maneuver_distance):
         return log.Desire.turnRight

selfdrive/controls/lib/latcontrol_torque.py

@@ -111,6 +111,7 @@ class LatControlTorque(LatControl):
     self.is_rav4_tss2 = CP.carFingerprint in RAV4_TSS2_CARS
     self.is_rav4_prime = CP.carFingerprint in RAV4_PRIME_CARS
     self.is_sienna_4th_gen = CP.carFingerprint in SIENNA_4TH_GEN_CARS
+    self.is_toyota_highlander_tss2 = CP.carFingerprint in TOYOTA_HIGHLANDER_TSS2_CARS
     self.is_toyota_corolla_tss2 = CP.carFingerprint in TOYOTA_COROLLA_TSS2_CARS
     self.is_lexus_is = CP.carFingerprint in LEXUS_IS_CARS
     self.is_ioniq_5 = CP.carFingerprint in IONIQ_5_CARS
@@ -310,6 +311,7 @@ class LatControlTorque(LatControl):
       rav4_tss2_active = self.is_rav4_tss2
       rav4_prime_active = self.is_rav4_prime
       sienna_4th_gen_active = self.is_sienna_4th_gen
+      toyota_highlander_tss2_active = self.is_toyota_highlander_tss2
       toyota_corolla_tss2_active = self.is_toyota_corolla_tss2
       lexus_is_active = self.is_lexus_is
       ioniq_5_active = self.is_ioniq_5
@@ -398,6 +400,10 @@ class LatControlTorque(LatControl):
       elif sienna_4th_gen_active:
         ff *= get_sienna_4th_gen_ff_scale(setpoint, desired_lateral_jerk, CS.vEgo)
         friction_threshold = get_sienna_4th_gen_friction_threshold(CS.vEgo, setpoint, desired_lateral_jerk)
+      elif toyota_highlander_tss2_active:
+        ff *= get_toyota_highlander_tss2_ff_scale(setpoint, desired_lateral_jerk, CS.vEgo)
+        friction_threshold = get_toyota_highlander_tss2_friction_threshold(CS.vEgo, setpoint, desired_lateral_jerk)
+        friction_scale = get_toyota_highlander_tss2_friction_scale(CS.vEgo, setpoint, desired_lateral_jerk)
       elif toyota_corolla_tss2_active:
         ff *= get_toyota_corolla_tss2_ff_scale(setpoint, desired_lateral_jerk, CS.vEgo)
       elif lexus_is_active:

selfdrive/controls/lib/latcontrol_vehicle_tunes.py

@@ -184,6 +184,10 @@ SIENNA_4TH_GEN_CARS = (
   TOYOTA_CAR.TOYOTA_SIENNA_4TH_GEN,
 )
 
+TOYOTA_HIGHLANDER_TSS2_CARS = (
+  TOYOTA_CAR.TOYOTA_HIGHLANDER_TSS2,
+)
+
 TOYOTA_COROLLA_TSS2_CARS = (
   TOYOTA_CAR.TOYOTA_COROLLA_TSS2,
 )
@@ -235,7 +239,7 @@ GENESIS_G70_FRICTION_CENTER_LAT = 0.28
 GENESIS_G70_FRICTION_CENTER_LAT_WIDTH = 0.10
 GENESIS_G70_FRICTION_CALM_JERK = 0.35
 GENESIS_G70_FRICTION_CALM_JERK_WIDTH = 0.10
-GENESIS_G70_FRICTION_JERK_DEADZONE_MAX = 0.24
+GENESIS_G70_FRICTION_JERK_DEADZONE_MAX = 0.30
 GENESIS_G70_FRICTION_JERK_DEADZONE_LAT = 0.30
 GENESIS_G70_FRICTION_JERK_DEADZONE_LAT_WIDTH = 0.08
 GENESIS_G70_FRICTION_JERK_DEADZONE_SPEED = 12.0
@@ -263,14 +267,14 @@ GENESIS_G70_LOW_SPEED_OUTPUT_LIMIT_LAT = 0.14
 GENESIS_G70_LOW_SPEED_OUTPUT_LIMIT_LAT_WIDTH = 0.05
 GENESIS_G70_LOW_SPEED_OUTPUT_LIMIT_SPEED = 6.0
 GENESIS_G70_LOW_SPEED_OUTPUT_LIMIT_SPEED_WIDTH = 1.5
-GENESIS_G70_CURVE_UNWIND_OUTPUT_BOOST = 0.02
+GENESIS_G70_CURVE_UNWIND_OUTPUT_BOOST = 0.00
 GENESIS_G70_CURVE_UNWIND_SPEED = 18.0
 GENESIS_G70_CURVE_UNWIND_SPEED_WIDTH = 3.0
 GENESIS_G70_CURVE_UNWIND_LAT = 0.25
 GENESIS_G70_CURVE_UNWIND_LAT_WIDTH = 0.12
 GENESIS_G70_CURVE_UNWIND_JERK = 0.08
 GENESIS_G70_CURVE_UNWIND_JERK_WIDTH = 0.08
-GENESIS_G70_UNWIND_FF_REDUCTION_MAX = 0.20
+GENESIS_G70_UNWIND_FF_REDUCTION_MAX = 0.28
 GENESIS_G70_UNWIND_FF_OVERSHOOT = 0.12
 GENESIS_G70_UNWIND_FF_OVERSHOOT_WIDTH = 0.12
 GENESIS_G70_UNWIND_FF_JERK = 0.10
@@ -369,7 +373,7 @@ BOLT_2022_2023_LOW_SPEED_CENTER_OUTPUT_SPEED = 2.5
 BOLT_2022_2023_LOW_SPEED_CENTER_OUTPUT_SPEED_WIDTH = 0.7
 BOLT_2022_2023_LOW_SPEED_CENTER_OUTPUT_SPEED_MAX = 7.2
 BOLT_2022_2023_LOW_SPEED_CENTER_OUTPUT_SPEED_MAX_WIDTH = 0.5
-BOLT_2022_2023_CENTER_FRICTION_THRESHOLD_BUMP = 0.050
+BOLT_2022_2023_CENTER_FRICTION_THRESHOLD_BUMP = 0.080
 BOLT_2022_2023_CENTER_FRICTION_THRESHOLD_LAT = 0.18
 BOLT_2022_2023_CENTER_FRICTION_THRESHOLD_LAT_WIDTH = 0.06
 BOLT_2022_2023_CENTER_FRICTION_THRESHOLD_SPEED = 6.7
@@ -1108,6 +1112,17 @@ TOYOTA_COROLLA_TSS2_CENTER_OUTPUT_TAPER_LAT_WIDTH = 0.08
 TOYOTA_COROLLA_TSS2_CENTER_OUTPUT_TAPER_SPEED = 4.5
 TOYOTA_COROLLA_TSS2_CENTER_OUTPUT_TAPER_SPEED_WIDTH = 1.5
 
+TOYOTA_HIGHLANDER_TSS2_PHASE_SCALE = 0.12
+TOYOTA_HIGHLANDER_TSS2_UNWIND_FF_REDUCTION = 0.10
+TOYOTA_HIGHLANDER_TSS2_UNWIND_FRICTION_THRESHOLD_GAIN = 0.16
+TOYOTA_HIGHLANDER_TSS2_UNWIND_FRICTION_SCALE_REDUCTION = 0.10
+TOYOTA_HIGHLANDER_TSS2_UNWIND_LAT_ONSET = 0.20
+TOYOTA_HIGHLANDER_TSS2_UNWIND_LAT_WIDTH = 0.08
+TOYOTA_HIGHLANDER_TSS2_UNWIND_SPEED_ONSET = 3.0
+TOYOTA_HIGHLANDER_TSS2_UNWIND_SPEED_WIDTH = 1.5
+TOYOTA_HIGHLANDER_TSS2_UNWIND_SPEED_MAX = 15.0
+TOYOTA_HIGHLANDER_TSS2_UNWIND_SPEED_MAX_WIDTH = 2.0
+
 LEXUS_IS_PHASE_SCALE = 0.10
 LEXUS_IS_TURN_IN_FF_BOOST_LEFT = 0.06
 LEXUS_IS_TURN_IN_FF_BOOST_RIGHT = 0.06
@@ -1645,6 +1660,52 @@ def get_toyota_corolla_tss2_center_output_scale(desired_lateral_accel: float, v_
   return max(1.0 - reduction, 0.65)
 
 
+def _toyota_highlander_tss2_unwind_weight(desired_lateral_accel: float,
+                                           desired_lateral_jerk: float,
+                                           v_ego: float) -> float:
+  phase = math.tanh((desired_lateral_accel * desired_lateral_jerk) /
+                    TOYOTA_HIGHLANDER_TSS2_PHASE_SCALE)
+  curve_weight = _sigmoid((abs(desired_lateral_accel) - TOYOTA_HIGHLANDER_TSS2_UNWIND_LAT_ONSET) /
+                          TOYOTA_HIGHLANDER_TSS2_UNWIND_LAT_WIDTH)
+  speed_weight = (_sigmoid((v_ego - TOYOTA_HIGHLANDER_TSS2_UNWIND_SPEED_ONSET) /
+                           TOYOTA_HIGHLANDER_TSS2_UNWIND_SPEED_WIDTH) *
+                  _sigmoid((TOYOTA_HIGHLANDER_TSS2_UNWIND_SPEED_MAX - v_ego) /
+                           TOYOTA_HIGHLANDER_TSS2_UNWIND_SPEED_MAX_WIDTH))
+  return max(-phase, 0.0) * curve_weight * speed_weight
+
+
+def get_toyota_highlander_tss2_ff_scale(desired_lateral_accel: float,
+                                         desired_lateral_jerk: float,
+                                         v_ego: float) -> float:
+  reduction = _flm_vehicle_knob("toyota_highlander_tss2.unwind_ff_reduction",
+                                TOYOTA_HIGHLANDER_TSS2_UNWIND_FF_REDUCTION)
+  return 1.0 - reduction * _toyota_highlander_tss2_unwind_weight(
+    desired_lateral_accel, desired_lateral_jerk, v_ego,
+  )
+
+
+def get_toyota_highlander_tss2_friction_threshold(v_ego: float,
+                                                  desired_lateral_accel: float = 0.0,
+                                                  desired_lateral_jerk: float = 0.0) -> float:
+  gain = _flm_vehicle_knob("toyota_highlander_tss2.unwind_friction_threshold_gain",
+                           TOYOTA_HIGHLANDER_TSS2_UNWIND_FRICTION_THRESHOLD_GAIN)
+  return get_standard_friction_threshold(v_ego) * (
+    1.0 + gain * _toyota_highlander_tss2_unwind_weight(
+      desired_lateral_accel, desired_lateral_jerk, v_ego,
+    )
+  )
+
+
+def get_toyota_highlander_tss2_friction_scale(v_ego: float,
+                                              desired_lateral_accel: float,
+                                              desired_lateral_jerk: float) -> float:
+  reduction = _flm_vehicle_knob("toyota_highlander_tss2.unwind_friction_scale_reduction",
+                                TOYOTA_HIGHLANDER_TSS2_UNWIND_FRICTION_SCALE_REDUCTION)
+  return 1.0 - reduction * _toyota_highlander_tss2_unwind_weight(
+    desired_lateral_accel, desired_lateral_jerk, v_ego,
+  )
+
+
 def get_lexus_is_ff_scale(desired_lateral_accel: float, desired_lateral_jerk: float, v_ego: float) -> float:
   if desired_lateral_accel == 0.0:
     return 1.0

selfdrive/controls/tests/test_latcontrol.py

@@ -109,6 +109,9 @@ from openpilot.selfdrive.controls.lib.latcontrol_torque import (
   get_sienna_4th_gen_ff_scale,
   get_sienna_4th_gen_friction_threshold,
   get_sienna_4th_gen_high_speed_output_taper_scale,
+  get_toyota_highlander_tss2_ff_scale,
+  get_toyota_highlander_tss2_friction_scale,
+  get_toyota_highlander_tss2_friction_threshold,
   get_toyota_corolla_tss2_center_output_scale,
   get_toyota_corolla_tss2_ff_scale,
   get_lexus_is_ff_scale,
@@ -881,9 +884,10 @@ class TestLatControl:
     assert get_genesis_g70_low_speed_output_limit(0.0, 2.0) < 0.30
     assert get_genesis_g70_low_speed_angle_damping(0.0, -20.0, 0.0, 2.0) < 0.0
     assert get_genesis_g70_low_speed_angle_damping(0.0, 20.0, 0.0, 2.0) > 0.0
-    assert get_genesis_g70_curve_unwind_output_scale(0.7, -0.5, 25.0) > 1.0
+    assert get_genesis_g70_curve_unwind_output_scale(0.7, -0.5, 25.0) == pytest.approx(1.0)
     assert get_genesis_g70_curve_unwind_output_scale(0.7, 0.5, 25.0) == 1.0
-    assert get_genesis_g70_unwind_ff_scale(-0.7, -0.95, 0.5, 25.0) < 1.0
+    assert get_genesis_g70_friction_jerk_deadzone(25.0, 0.0) > 0.25
+    assert get_genesis_g70_unwind_ff_scale(-0.7, -0.95, 0.5, 25.0) < 0.90
     assert get_genesis_g70_unwind_ff_scale(-0.7, -0.95, -0.5, 25.0) == 1.0
     assert get_genesis_g70_unwind_ff_scale(-0.7, 0.2, 0.5, 25.0) == 1.0
 
@@ -1009,6 +1013,26 @@ class TestLatControl:
     assert get_sienna_4th_gen_high_speed_output_taper_scale(10.0) == pytest.approx(1.0, abs=0.002)
     assert get_sienna_4th_gen_high_speed_output_taper_scale(22.0) < 1.0
 
+  def test_toyota_highlander_tss2_unwind_shaping_is_low_speed_only(self):
+    base = get_standard_friction_threshold(9.0)
+    steady = get_toyota_highlander_tss2_ff_scale(0.8, 0.0, 9.0)
+    unwind = get_toyota_highlander_tss2_ff_scale(0.8, -0.8, 9.0)
+    highway_unwind = get_toyota_highlander_tss2_ff_scale(0.8, -0.8, 28.0)
+
+    assert steady == pytest.approx(1.0)
+    assert 0.90 < unwind < 1.0
+    assert highway_unwind > unwind
+
+    unwind_threshold = get_toyota_highlander_tss2_friction_threshold(9.0, 0.8, -0.8)
+    turn_threshold = get_toyota_highlander_tss2_friction_threshold(9.0, 0.8, 0.8)
+    assert unwind_threshold > base
+    assert turn_threshold == pytest.approx(base, rel=0.01)
+
+    unwind_scale = get_toyota_highlander_tss2_friction_scale(9.0, 0.8, -0.8)
+    turn_scale = get_toyota_highlander_tss2_friction_scale(9.0, 0.8, 0.8)
+    assert 0.90 < unwind_scale < 1.0
+    assert turn_scale == pytest.approx(1.0)
+
   def test_rav4_prime_forced_torque_update_path(self, monkeypatch):
     controller, VM, CS, params, starpilot_toggles = self._build_torque_controller(TOYOTA.TOYOTA_RAV4_PRIME, force_torque=True)
     CS.vEgo = 13.0

selfdrive/modeld/dmonitoringmodeld.py

@@ -16,7 +16,8 @@ from cereal import messaging
 from cereal.messaging import PubMaster, SubMaster
 from msgq.visionipc import VisionBuf, VisionIpcClient, VisionStreamType
 from openpilot.common.file_chunker import read_file_chunked
-from openpilot.common.realtime import config_realtime_process
+from openpilot.common.params import Params
+from openpilot.common.realtime import config_realtime_process, set_core_affinity
 from openpilot.common.swaglog import cloudlog
 from openpilot.common.transformations.camera import _ar_ox_fisheye, _os_fisheye
 from openpilot.common.transformations.model import dmonitoringmodel_intrinsics
@@ -29,6 +30,19 @@ SEND_RAW_PRED = os.getenv("SEND_RAW_PRED")
 MODELS_DIR = Path(__file__).parent / "models"
 MODEL_PKL_PATH = MODELS_DIR / "dmonitoring_model_tinygrad.pkl"
 METADATA_PATH = MODELS_DIR / "dmonitoring_model_metadata.pkl"
+AFFINITY_CHECK_INTERVAL_SECONDS = 0.5
+DEFAULT_AFFINITY_CORES = [7]
+EXTERNAL_GPU_AFFINITY_CORES = [6, 7]
+
+
+def update_external_gpu_affinity(params: Params, external_gpu_affinity: bool) -> bool:
+  """Let driver monitoring use the otherwise-idle camera core only while the big model is active."""
+  external_gpu_active = params.get_bool("UsbGpuActive")
+  if external_gpu_active != external_gpu_affinity:
+    cores = EXTERNAL_GPU_AFFINITY_CORES if external_gpu_active else DEFAULT_AFFINITY_CORES
+    set_core_affinity(cores)
+    cloudlog.warning(f"dmonitoringmodeld affinity set to {cores}; external GPU active: {external_gpu_active}")
+  return external_gpu_active
 
 
 class ModelState:
@@ -132,6 +146,9 @@ def get_driverstate_packet(model_output, frame_id: int, exec_time: float, gpu_ex
 
 def main():
   config_realtime_process(7, 5)
+  params = Params()
+  external_gpu_affinity = False
+  next_affinity_check = 0.0
   cloudlog.warning("connecting to driver stream")
   vipc_client = VisionIpcClient("camerad", VisionStreamType.VISION_STREAM_DRIVER, True)
   while not vipc_client.connect(False):
@@ -156,6 +173,12 @@ def main():
     buf = vipc_client.recv()
     if buf is None:
       continue
+
+    now = time.monotonic()
+    if now >= next_affinity_check:
+      external_gpu_affinity = update_external_gpu_affinity(params, external_gpu_affinity)
+      next_affinity_check = now + AFFINITY_CHECK_INTERVAL_SECONDS
+
     if model_transform is None:
       camera = _os_fisheye if buf.width == _os_fisheye.width else _ar_ox_fisheye
       model_transform = np.linalg.inv(

selfdrive/modeld/tests/test_dmonitoring_affinity.py

@@ -0,0 +1,38 @@
+from openpilot.selfdrive.modeld import dmonitoringmodeld
+
+
+class FakeParams:
+  def __init__(self, active: bool):
+    self.active = active
+
+  def get_bool(self, key: str) -> bool:
+    assert key == "UsbGpuActive"
+    return self.active
+
+
+def test_non_gpu_affinity_is_unchanged(monkeypatch):
+  calls = []
+  monkeypatch.setattr(dmonitoringmodeld, "set_core_affinity", calls.append)
+
+  assert not dmonitoringmodeld.update_external_gpu_affinity(FakeParams(False), False)
+  assert calls == []
+
+
+def test_external_gpu_adds_idle_camera_core(monkeypatch):
+  calls = []
+  monkeypatch.setattr(dmonitoringmodeld, "set_core_affinity", calls.append)
+
+  assert dmonitoringmodeld.update_external_gpu_affinity(FakeParams(True), False)
+  assert calls == [dmonitoringmodeld.EXTERNAL_GPU_AFFINITY_CORES]
+
+  calls.clear()
+  assert dmonitoringmodeld.update_external_gpu_affinity(FakeParams(True), True)
+  assert calls == []
+
+
+def test_affinity_returns_to_upstream_default_after_gpu_fallback(monkeypatch):
+  calls = []
+  monkeypatch.setattr(dmonitoringmodeld, "set_core_affinity", calls.append)
+
+  assert not dmonitoringmodeld.update_external_gpu_affinity(FakeParams(False), True)
+  assert calls == [dmonitoringmodeld.DEFAULT_AFFINITY_CORES]

selfdrive/pandad/spi.cc

@@ -350,13 +350,13 @@ int PandaSpiHandle::spi_transfer(uint8_t endpoint, uint8_t *tx_data, uint16_t tx
     SPILOG(LOGE, "SPI: failed to send header");
     goto fail;
   }
-  wait_for_spi_turnaround(nanos_since_boot());
 
   // Wait for (N)ACK
   ret = wait_for_ack(SPI_HACK, 0x11, timeout, 1);
   if (ret < 0) {
     goto fail;
   }
+  wait_for_spi_turnaround(nanos_since_boot());
 
   // Send data
   if (tx_data != NULL) {

selfdrive/ui/mici/onroad/augmented_road_view.py

@@ -21,6 +21,7 @@ from openpilot.selfdrive.ui.mici.onroad.starpilot_status import (
 )
 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
@@ -825,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:

selfdrive/ui/onroad/starpilot/starpilot_border.py

@@ -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

selfdrive/ui/tests/test_traffic_border.py

@@ -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

system/hardware/tici/agnos.json

@@ -67,13 +67,13 @@
   },
   {
     "name": "system",
-    "url": "https://www.dropbox.com/scl/fi/3gtw78kxn6xs6dil10fmc/system9.img.xz?rlkey=uwj6tss7lw3j1di09xvtdugcy&st=ghnm9teb&dl=1",
-    "hash": "b6f1e5ade7baa0078990e881b9f392b6a29164347df8f54be20c81e9c9dbabfe",
-    "hash_raw": "b6f1e5ade7baa0078990e881b9f392b6a29164347df8f54be20c81e9c9dbabfe",
+    "url": "https://www.dropbox.com/scl/fi/pewhzpqzi3aewuiaffc6m/system10.img.xz?rlkey=olzrzulhs93zzghnjrskmdwxt&st=exnfk2oz&dl=1",
+    "hash": "ab395d4c963a908ab86709f1a6580a62dd24cb34ee71cf5fd5ec29d7d48d0e10",
+    "hash_raw": "ab395d4c963a908ab86709f1a6580a62dd24cb34ee71cf5fd5ec29d7d48d0e10",
     "size": 4718592000,
     "sparse": false,
     "full_check": false,
     "has_ab": true,
-    "ondevice_hash": "b6f1e5ade7baa0078990e881b9f392b6a29164347df8f54be20c81e9c9dbabfe"
+    "ondevice_hash": "ab395d4c963a908ab86709f1a6580a62dd24cb34ee71cf5fd5ec29d7d48d0e10"
   }
 ]