lactate threshold

selfdrive/controls/lib/latcontrol_torque.py

@@ -439,12 +439,15 @@ class LatControlTorque(LatControl):
         output_torque *= get_ioniq_6_highway_transition_output_taper_scale(setpoint, desired_lateral_jerk, CS.vEgo)
       elif self.is_ram_1500 and output_torque * setpoint > 0.0:
         output_torque *= get_ram_1500_transition_output_scale(setpoint, desired_lateral_jerk, CS.vEgo)
-      elif self.is_kona_non_scc and output_torque * setpoint > 0.0:
-        output_torque *= get_kona_non_scc_highway_transition_output_scale(setpoint, desired_lateral_jerk, CS.vEgo)
+      elif self.is_kona_non_scc:
+        output_torque *= get_kona_non_scc_center_taper_scale(setpoint, CS.vEgo)
+        if output_torque * setpoint > 0.0:
+          output_torque *= get_kona_non_scc_highway_transition_output_scale(setpoint, desired_lateral_jerk, CS.vEgo)
       elif rav4_prime_active:
         output_torque *= get_rav4_prime_output_taper_scale(setpoint, desired_lateral_jerk, CS.vEgo)
       elif sienna_4th_gen_active:
         output_torque *= get_sienna_4th_gen_center_taper_scale(setpoint, CS.vEgo)
+        output_torque *= get_sienna_4th_gen_high_speed_output_taper_scale(CS.vEgo)
       elif prius_active:
         output_torque *= prius_center_taper
       elif volt_standard_test_active:

selfdrive/controls/lib/latcontrol_vehicle_tunes.py

@@ -804,7 +804,7 @@ RAV4_PRIME_SPEED_MAX = 20.0
 RAV4_PRIME_SPEED_MAX_WIDTH = 2.5
 
 SIENNA_4TH_GEN_PHASE_SCALE = 0.12
-SIENNA_4TH_GEN_TURN_IN_FF_BOOST = 0.12
+SIENNA_4TH_GEN_TURN_IN_FF_BOOST = 0.06
 SIENNA_4TH_GEN_TURN_IN_LAT = 0.24
 SIENNA_4TH_GEN_TURN_IN_LAT_WIDTH = 0.08
 SIENNA_4TH_GEN_TURN_IN_SPEED_ONSET = 3.0
@@ -823,11 +823,16 @@ SIENNA_4TH_GEN_CENTER_TAPER_LAT = 0.20
 SIENNA_4TH_GEN_CENTER_TAPER_LAT_WIDTH = 0.06
 SIENNA_4TH_GEN_CENTER_TAPER_SPEED_MAX = 13.0
 SIENNA_4TH_GEN_CENTER_TAPER_SPEED_WIDTH = 2.0
-SIENNA_4TH_GEN_HIGH_SPEED_CENTER_TAPER_MAX = 0.08
+SIENNA_4TH_GEN_HIGH_SPEED_CENTER_TAPER_MAX = 0.16
 SIENNA_4TH_GEN_HIGH_SPEED_CENTER_TAPER_ONSET = 15.0
 SIENNA_4TH_GEN_HIGH_SPEED_CENTER_TAPER_ONSET_WIDTH = 2.0
 SIENNA_4TH_GEN_HIGH_SPEED_CENTER_TAPER_MAX_SPEED = 27.0
 SIENNA_4TH_GEN_HIGH_SPEED_CENTER_TAPER_MAX_SPEED_WIDTH = 3.0
+SIENNA_4TH_GEN_HIGH_SPEED_OUTPUT_TAPER_MAX = 0.10
+SIENNA_4TH_GEN_HIGH_SPEED_OUTPUT_TAPER_ONSET = 18.0
+SIENNA_4TH_GEN_HIGH_SPEED_OUTPUT_TAPER_ONSET_WIDTH = 2.0
+SIENNA_4TH_GEN_HIGH_SPEED_OUTPUT_TAPER_MAX_SPEED = 27.0
+SIENNA_4TH_GEN_HIGH_SPEED_OUTPUT_TAPER_MAX_SPEED_WIDTH = 3.0
 
 LEXUS_IS_PHASE_SCALE = 0.10
 LEXUS_IS_UNWIND_FF_REDUCTION_LEFT = 0.06
@@ -860,6 +865,10 @@ KONA_NON_SCC_TRANSITION_JERK_ONSET = 0.45
 KONA_NON_SCC_TRANSITION_JERK_FULL = 1.25
 KONA_NON_SCC_TRANSITION_LAT_FADE_START = 0.55
 KONA_NON_SCC_TRANSITION_LAT_FADE_END = 1.65
+KONA_NON_SCC_CENTER_TAPER_MAX = 0.14
+KONA_NON_SCC_CENTER_TAPER_LAT = 0.28
+KONA_NON_SCC_CENTER_TAPER_SPEED_ONSET = 12.0
+KONA_NON_SCC_CENTER_TAPER_SPEED_FULL = 24.0
 
 TRAILER_LOAD_FULL_ASSIST_KG = 15000.0 * CV.LB_TO_KG
 TRAILER_LATERAL_MIN_SPEED = 15.0 * CV.MPH_TO_MS
@@ -1229,6 +1238,14 @@ def get_sienna_4th_gen_center_taper_scale(desired_lateral_accel: float, v_ego: f
   return 1.0 - (reduction * center_weight)
 
 
+def get_sienna_4th_gen_high_speed_output_taper_scale(v_ego: float) -> float:
+  onset = _sigmoid((v_ego - SIENNA_4TH_GEN_HIGH_SPEED_OUTPUT_TAPER_ONSET) /
+                   SIENNA_4TH_GEN_HIGH_SPEED_OUTPUT_TAPER_ONSET_WIDTH)
+  cutoff = _sigmoid((SIENNA_4TH_GEN_HIGH_SPEED_OUTPUT_TAPER_MAX_SPEED - v_ego) /
+                    SIENNA_4TH_GEN_HIGH_SPEED_OUTPUT_TAPER_MAX_SPEED_WIDTH)
+  return 1.0 - SIENNA_4TH_GEN_HIGH_SPEED_OUTPUT_TAPER_MAX * onset * cutoff
+
+
 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
@@ -1266,6 +1283,12 @@ def get_kona_non_scc_highway_transition_output_scale(desired_lateral_accel: floa
   return 1.0 - (KONA_NON_SCC_TRANSITION_TAPER_MAX * speed_weight * jerk_weight * lat_weight)
 
 
+def get_kona_non_scc_center_taper_scale(desired_lateral_accel: float, v_ego: float) -> float:
+  speed_weight = float(np.interp(v_ego, [KONA_NON_SCC_CENTER_TAPER_SPEED_ONSET, KONA_NON_SCC_CENTER_TAPER_SPEED_FULL], [0.0, 1.0]))
+  center_weight = float(np.interp(abs(desired_lateral_accel), [0.0, KONA_NON_SCC_CENTER_TAPER_LAT], [1.0, 0.0]))
+  return 1.0 - (KONA_NON_SCC_CENTER_TAPER_MAX * speed_weight * center_weight)
+
+
 def civic_bosch_modified_lateral_testing_ground_active() -> bool:
   return testing_ground.use("8", "B")
 

selfdrive/controls/lib/longitudinal_planner.py

@@ -16,7 +16,11 @@ from openpilot.selfdrive.controls.lib.longitudinal_mpc_lib.long_mpc import shoul
 from openpilot.selfdrive.controls.lib.longitudinal_mpc_lib.long_mpc import STOP_DISTANCE
 from openpilot.selfdrive.controls.lib.longitudinal_mpc_lib.long_mpc import T_IDXS as T_IDXS_MPC
 from openpilot.selfdrive.controls.lib.lead_behavior import is_radarless_matched_follow_window
-from openpilot.selfdrive.controls.lib.longitudinal_vehicle_tunes import get_far_follow_output_slew_rates, get_untracked_slow_lead_decel_scale
+from openpilot.selfdrive.controls.lib.longitudinal_vehicle_tunes import (
+  get_far_follow_output_slew_rates,
+  get_toyota_sienna_post_departure_restop_cap,
+  get_untracked_slow_lead_decel_scale,
+)
 from openpilot.selfdrive.controls.lib.drive_helpers import CONTROL_N
 from openpilot.selfdrive.car.cruise import V_CRUISE_UNSET
 from openpilot.common.swaglog import cloudlog
@@ -3855,6 +3859,24 @@ class LongitudinalPlanner:
       output_a_target = RADAR_STANDSTILL_GAP_SETTLE_ACCEL
       output_should_stop = False
 
+    sienna_restop_caps = [
+      cap for cap in (
+        get_toyota_sienna_post_departure_restop_cap(
+          self.CP, self.lead_one, scene_v_ego, output_accel_min,
+          standstill_nudge_gap, now_t, self.post_departure_follow_settle_until,
+        ),
+        get_toyota_sienna_post_departure_restop_cap(
+          self.CP, self.lead_two, scene_v_ego, output_accel_min,
+          standstill_nudge_gap, now_t, self.post_departure_follow_settle_until,
+        ),
+      ) if cap is not None
+    ]
+    if sienna_restop_caps:
+      sienna_restop_cap = min(sienna_restop_caps)
+      self.a_desired = min(self.a_desired, sienna_restop_cap)
+      output_a_target = min(output_a_target, sienna_restop_cap)
+      output_should_stop = True
+
     self.output_a_target = output_a_target
     self.output_should_stop = bool(output_should_stop or vision_low_speed_stop_active)
 

selfdrive/controls/lib/longitudinal_vehicle_tunes.py

@@ -1,6 +1,17 @@
+import numpy as np
+
+
 HONDA_HRV_3G_FAR_FOLLOW_BRAKE_SLEW_RATE = 3.0
 HONDA_HRV_3G_FAR_FOLLOW_RELEASE_SLEW_RATE = 2.0
 HONDA_HRV_3G_UNTRACKED_SLOW_LEAD_DECEL_SCALE = 1.35
+TOYOTA_SIENNA_POST_DEPARTURE_RESTOP_MAX_EGO_SPEED = 2.0
+TOYOTA_SIENNA_POST_DEPARTURE_RESTOP_MAX_LEAD_SPEED = 0.45
+TOYOTA_SIENNA_POST_DEPARTURE_RESTOP_MAX_LEAD_DELTA = 0.35
+TOYOTA_SIENNA_POST_DEPARTURE_RESTOP_MAX_LEAD_ACCEL = 0.35
+TOYOTA_SIENNA_POST_DEPARTURE_RESTOP_MIN_MODEL_PROB = 0.95
+TOYOTA_SIENNA_POST_DEPARTURE_RESTOP_MAX_LATERAL_OFFSET = 1.75
+TOYOTA_SIENNA_POST_DEPARTURE_RESTOP_MIN_BRAKE = 0.18
+TOYOTA_SIENNA_POST_DEPARTURE_RESTOP_MAX_BRAKE = 0.32
 
 
 def get_far_follow_output_slew_rates(CP):
@@ -16,3 +27,43 @@ def get_untracked_slow_lead_decel_scale(CP):
   if CP.brand == "honda" and str(CP.carFingerprint) == "HONDA_HRV_3G":
     return HONDA_HRV_3G_UNTRACKED_SLOW_LEAD_DECEL_SCALE
   return 1.0
+
+
+def get_toyota_sienna_post_departure_restop_cap(CP, lead, v_ego, accel_min,
+                                                stop_distance, now_t, departure_latch_until):
+  """Re-arm a stop if a Sienna's lead twitches forward and stops again."""
+  if (
+    CP.brand != "toyota" or str(CP.carFingerprint) != "TOYOTA_SIENNA_4TH_GEN" or
+    now_t >= departure_latch_until or lead is None or not lead.status or
+    float(v_ego) > TOYOTA_SIENNA_POST_DEPARTURE_RESTOP_MAX_EGO_SPEED
+  ):
+    return None
+
+  lead_radar = bool(getattr(lead, "radar", False))
+  lead_prob = float(getattr(lead, "modelProb", 1.0 if lead_radar else 0.0))
+  if not lead_radar and lead_prob < TOYOTA_SIENNA_POST_DEPARTURE_RESTOP_MIN_MODEL_PROB:
+    return None
+  if abs(float(getattr(lead, "yRel", 0.0))) > TOYOTA_SIENNA_POST_DEPARTURE_RESTOP_MAX_LATERAL_OFFSET:
+    return None
+
+  lead_speed = max(float(getattr(lead, "vLead", 0.0)), 0.0)
+  lead_delta = lead_speed - float(v_ego)
+  lead_accel = float(getattr(lead, "aLeadK", 0.0))
+  max_distance = max(float(stop_distance) + 3.0, 4.5)
+  if (
+    float(getattr(lead, "dRel", float("inf"))) > max_distance or
+    lead_speed > TOYOTA_SIENNA_POST_DEPARTURE_RESTOP_MAX_LEAD_SPEED or
+    lead_delta > TOYOTA_SIENNA_POST_DEPARTURE_RESTOP_MAX_LEAD_DELTA or
+    lead_accel > TOYOTA_SIENNA_POST_DEPARTURE_RESTOP_MAX_LEAD_ACCEL
+  ):
+    return None
+
+  speed_factor = float(np.clip(float(v_ego) / TOYOTA_SIENNA_POST_DEPARTURE_RESTOP_MAX_EGO_SPEED, 0.0, 1.0))
+  closing_factor = float(np.clip((float(v_ego) - lead_speed) / 1.5, 0.0, 1.0))
+  hold_brake = TOYOTA_SIENNA_POST_DEPARTURE_RESTOP_MIN_BRAKE + 0.08 * speed_factor + 0.06 * closing_factor
+  brake_floor = -float(np.clip(
+    hold_brake,
+    TOYOTA_SIENNA_POST_DEPARTURE_RESTOP_MIN_BRAKE,
+    TOYOTA_SIENNA_POST_DEPARTURE_RESTOP_MAX_BRAKE,
+  ))
+  return brake_floor if accel_min >= 0.0 else max(float(accel_min), brake_floor)

selfdrive/controls/tests/test_latcontrol.py

@@ -27,6 +27,7 @@ from openpilot.selfdrive.controls.lib.latcontrol_vehicle_tunes import (
   clear_flm_runtime_overrides,
   get_flm_runtime_overrides,
   get_hkg_canfd_base_friction_threshold,
+  get_kona_non_scc_center_taper_scale,
   get_kona_non_scc_highway_transition_output_scale,
   KIA_FORTE_BASE_LAT_ACCEL_FACTOR_MULT,
   RAM_1500_BASE_LAT_ACCEL_FACTOR_MULT,
@@ -73,6 +74,7 @@ from openpilot.selfdrive.controls.lib.latcontrol_torque import (
   get_sienna_4th_gen_center_taper_scale,
   get_sienna_4th_gen_ff_scale,
   get_sienna_4th_gen_friction_threshold,
+  get_sienna_4th_gen_high_speed_output_taper_scale,
   get_lexus_is_ff_scale,
   get_ioniq_5_ff_scale,
   get_ioniq_5_friction_scale,
@@ -733,6 +735,8 @@ class TestLatControl:
     assert calm < turn_taper <= 1.0
     assert calm < highway_calm < 1.0
     assert fast > calm
+    assert get_sienna_4th_gen_high_speed_output_taper_scale(10.0) == pytest.approx(1.0)
+    assert get_sienna_4th_gen_high_speed_output_taper_scale(22.0) < 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)
@@ -809,6 +813,12 @@ class TestLatControl:
     assert center_transition < medium_transition < 1.0
     assert get_kona_non_scc_highway_transition_output_scale(1.65, 2.5, 30.0) == pytest.approx(1.0)
 
+  def test_kona_non_scc_center_taper_curve(self):
+    assert get_kona_non_scc_center_taper_scale(0.0, 10.0) == pytest.approx(1.0)
+    assert get_kona_non_scc_center_taper_scale(0.0, 25.0) == pytest.approx(0.86)
+    assert get_kona_non_scc_center_taper_scale(0.28, 25.0) == pytest.approx(1.0)
+    assert get_kona_non_scc_center_taper_scale(0.10, 25.0) < get_kona_non_scc_center_taper_scale(0.10, 15.0)
+
   def test_kona_non_scc_highway_transition_taper_update_path(self, monkeypatch):
     controller, VM, CS, params, starpilot_toggles = self._build_torque_controller(HYUNDAI.HYUNDAI_KONA_NON_SCC)
     CS.vEgo = 30.0

selfdrive/controls/tests/test_longitudinal_planner.py

@@ -10,12 +10,15 @@ from cereal import log
 from opendbc.car.honda.interface import CarInterface
 from opendbc.car.honda.values import CAR
 from opendbc.car.gm.values import CAR as GM_CAR, GMFlags
+from opendbc.car.toyota.interface import CarInterface as ToyotaCarInterface
+from opendbc.car.toyota.values import CAR as TOYOTA_CAR
 import openpilot.selfdrive.controls.lib.longitudinal_planner as longitudinal_planner_module
 from openpilot.selfdrive.controls.lib.longcontrol import LongCtrlState
 from openpilot.selfdrive.controls.lib.drive_helpers import CONTROL_N
 from openpilot.selfdrive.controls.lib.longitudinal_planner import LongitudinalPlanner, get_coast_accel, get_vehicle_min_accel, should_publish_planner_fcw
 from openpilot.selfdrive.controls.lib.longitudinal_mpc_lib.long_mpc import LongitudinalMpc, soften_far_radar_lead_accel, should_trigger_planner_fcw
 from openpilot.selfdrive.controls.lib.longitudinal_mpc_lib.long_mpc import T_IDXS as T_IDXS_MPC
+from openpilot.selfdrive.controls.lib.longitudinal_vehicle_tunes import get_toyota_sienna_post_departure_restop_cap
 from openpilot.selfdrive.modeld.constants import ModelConstants, Plan
 from openpilot.selfdrive.modeld import modeld
 
@@ -2078,6 +2081,52 @@ def test_standstill_stopped_lead_guard_blocks_false_release_during_creep_frame(m
   assert planner.output_a_target <= 0.0
 
 
+def test_toyota_sienna_post_departure_restop_blocks_lead_that_stops_again():
+  CP = ToyotaCarInterface.get_non_essential_params(TOYOTA_CAR.TOYOTA_SIENNA_4TH_GEN)
+  now_t = time.monotonic()
+  lead = make_lead(status=True, d_rel=5.0, v_lead=0.10, a_lead=0.0, model_prob=0.99)
+
+  cap = get_toyota_sienna_post_departure_restop_cap(
+    CP, lead, v_ego=1.2, accel_min=-1.0, stop_distance=4.0,
+    now_t=now_t, departure_latch_until=now_t + 5.0,
+  )
+
+  assert cap is not None
+  assert cap < -0.18
+
+
+def test_toyota_sienna_post_departure_restop_allows_confirmed_departure():
+  CP = ToyotaCarInterface.get_non_essential_params(TOYOTA_CAR.TOYOTA_SIENNA_4TH_GEN)
+  now_t = time.monotonic()
+  lead = make_lead(status=True, d_rel=6.5, v_lead=1.0, a_lead=0.45, model_prob=0.99)
+
+  cap = get_toyota_sienna_post_departure_restop_cap(
+    CP, lead, v_ego=0.6, accel_min=-1.0, stop_distance=4.0,
+    now_t=now_t, departure_latch_until=now_t + 5.0,
+  )
+
+  assert cap is None
+
+
+def test_toyota_sienna_post_departure_restop_reasserts_should_stop_in_planner():
+  CP = ToyotaCarInterface.get_non_essential_params(TOYOTA_CAR.TOYOTA_SIENNA_4TH_GEN)
+  planner = LongitudinalPlanner(CP, init_v=1.2)
+  sm = make_sm(
+    1.2,
+    desired_accel=1.0,
+    min_accel=-1.0,
+    experimental_mode=False,
+    tracking_lead=True,
+    lead_one=make_lead(status=True, d_rel=5.0, v_lead=0.10, a_lead=0.0, model_prob=0.99),
+  )
+  planner.post_departure_follow_settle_until = time.monotonic() + 5.0
+
+  planner.update(sm, make_toggles())
+
+  assert planner.output_should_stop
+  assert planner.output_a_target < 0.0
+
+
 @pytest.mark.parametrize("model_version", ["v11", "v12", "v13", "v14", "v15"])
 def test_standstill_confident_departing_lead_clears_stop_without_waiting_for_model_accel(model_version):
   CP = CarInterface.get_non_essential_params(CAR.HONDA_CIVIC)

selfdrive/ui/mici/onroad/augmented_road_view.py

@@ -19,7 +19,7 @@ from openpilot.selfdrive.ui.mici.onroad.starpilot_status import (
   TRAFFIC_COLOR,
   get_border_color,
 )
-from openpilot.selfdrive.ui.onroad.cameraview import CameraView
+from openpilot.selfdrive.ui.mici.onroad.cameraview import CameraView
 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

selfdrive/ui/mici/onroad/cameraview.py

@@ -1,5 +1,10 @@
-"""Compatibility import for the shared CameraView implementation."""
+"""Mici CameraView using upstream's engaged color treatment."""
+
+from openpilot.selfdrive.ui.onroad.cameraview import CameraView as SharedCameraView
+
+
+class CameraView(SharedCameraView):
+  _use_upstream_engaged_color = True
 
-from openpilot.selfdrive.ui.onroad.cameraview import CameraView
 
 __all__ = ["CameraView"]

selfdrive/ui/mici/onroad/driver_camera_dialog.py

@@ -1,7 +1,7 @@
 import pyray as rl
 from cereal import log, messaging
 from msgq.visionipc import VisionStreamType
-from openpilot.selfdrive.ui.onroad.cameraview import CameraView
+from openpilot.selfdrive.ui.mici.onroad.cameraview import CameraView
 from openpilot.selfdrive.ui.mici.onroad.driver_state import DriverStateRenderer
 from openpilot.selfdrive.ui.ui_state import ui_state, device
 from openpilot.system.ui.lib.application import gui_app, FontWeight

selfdrive/ui/onroad/cameraview.py

@@ -88,8 +88,59 @@ FRAME_FRAGMENT_SHADER_YUV = VERSION + """
   }
   """
 
+FRAME_FRAGMENT_SHADER_EXTERNAL_MICI = """
+  #version 300 es
+  #extension GL_OES_EGL_image_external_essl3 : enable
+  precision mediump float;
+  in vec2 fragTexCoord;
+  uniform samplerExternalOES texture0;
+  uniform int enhance_driver;
+  out vec4 fragColor;
+  void main() {
+    vec4 color = texture(texture0, fragTexCoord);
+    float gray = dot(color.rgb, vec3(0.299, 0.587, 0.114));
+    color.rgb = mix(vec3(gray), color.rgb, 0.2);
+    color.rgb = clamp((color.rgb - 0.5) * 1.2 + 0.5, 0.0, 1.0);
+    color.rgb = pow(color.rgb, vec3(1.0/1.28));
+    if (enhance_driver == 1) {
+      float brightness = 1.1;
+      color.rgb = color.rgb + 0.15;
+      color.rgb = clamp((color.rgb - 0.5) * (brightness * 0.8) + 0.5, 0.0, 1.0);
+      color.rgb = color.rgb * color.rgb * (3.0 - 2.0 * color.rgb);
+      color.rgb = pow(color.rgb, vec3(0.8));
+    }
+    fragColor = vec4(color.rgb, color.a);
+  }
+  """
+
+FRAME_FRAGMENT_SHADER_YUV_MICI = VERSION + """
+  in vec2 fragTexCoord;
+  uniform sampler2D texture0;
+  uniform sampler2D texture1;
+  uniform int enhance_driver;
+  out vec4 fragColor;
+  void main() {
+    float y = texture(texture0, fragTexCoord).r;
+    vec2 uv = texture(texture1, fragTexCoord).ra - 0.5;
+    vec3 rgb = vec3(y + 1.402*uv.y, y - 0.344*uv.x - 0.714*uv.y, y + 1.772*uv.x);
+    float gray = dot(rgb, vec3(0.299, 0.587, 0.114));
+    rgb = mix(vec3(gray), rgb, 0.2);
+    rgb = clamp((rgb - 0.5) * 1.2 + 0.5, 0.0, 1.0);
+    if (enhance_driver == 1) {
+      float brightness = 1.1;
+      rgb = rgb + 0.15;
+      rgb = clamp((rgb - 0.5) * (brightness * 0.8) + 0.5, 0.0, 1.0);
+      rgb = rgb * rgb * (3.0 - 2.0 * rgb);
+      rgb = pow(rgb, vec3(0.8));
+    }
+    fragColor = vec4(rgb, 1.0);
+  }
+  """
+
 
 class CameraView(Widget):
+  _use_upstream_engaged_color = False
+
   def __init__(self, name: str, stream_type: VisionStreamType):
     super().__init__()
     self._name = name
@@ -343,7 +394,10 @@ class CameraView(Widget):
       rl.draw_rectangle_rec(rect, self._placeholder_color)
 
   def _load_frame_shader(self) -> None:
-    frame_shader = FRAME_FRAGMENT_SHADER_EXTERNAL if self._use_egl else FRAME_FRAGMENT_SHADER_YUV
+    if self._use_upstream_engaged_color:
+      frame_shader = FRAME_FRAGMENT_SHADER_EXTERNAL_MICI if self._use_egl else FRAME_FRAGMENT_SHADER_YUV_MICI
+    else:
+      frame_shader = FRAME_FRAGMENT_SHADER_EXTERNAL if self._use_egl else FRAME_FRAGMENT_SHADER_YUV
     self.shader = rl.load_shader_from_memory(VERTEX_SHADER, frame_shader)
     self._texture1_loc = -1 if self._use_egl else rl.get_shader_location(self.shader, "texture1")
     self._enhance_driver_loc = rl.get_shader_location(self.shader, "enhance_driver")

selfdrive/ui/tests/test_camera_frame_order.py

@@ -26,7 +26,9 @@ def _camera_view():
 
 
 def test_mici_uses_shared_camera_view():
-  assert mici_cameraview.CameraView is big_cameraview.CameraView
+  assert issubclass(mici_cameraview.CameraView, big_cameraview.CameraView)
+  assert mici_cameraview.CameraView._use_upstream_engaged_color
+  assert not big_cameraview.CameraView._use_upstream_engaged_color
 
 
 def test_pending_switch_is_cancelled_when_requested_stream_is_current():

selfdrive/ui/tests/test_ui_state_performance.py

@@ -1,4 +1,6 @@
 import threading
+import time
+from types import SimpleNamespace
 
 from openpilot.selfdrive.ui.lib.ui_param_cache import UIParamCache
 from openpilot.selfdrive.ui import ui_state as ui_state_module
@@ -33,3 +35,56 @@ def test_usbgpu_poll_does_not_block_ui_thread(monkeypatch):
   release.set()
   polling_thread.join(timeout=1.0)
   assert state.usbgpu is True
+
+
+def test_ui_update_reports_subphases(monkeypatch):
+  phases = []
+  calls = []
+  state = object.__new__(ui_state_module.UIState)
+  state.prime_state = SimpleNamespace(start=lambda: calls.append("prime"))
+  state.sm = SimpleNamespace(update=lambda timeout: calls.append(("submaster", timeout)))
+  def update_state(progress_hook):
+    progress_hook("ui.update.before_state_params")
+    calls.append("state")
+    progress_hook("ui.update.after_state_params")
+
+  state._update_state = update_state
+  state._update_status = lambda progress_hook: calls.append("status")
+  state._param_update_time = time.monotonic()
+  monkeypatch.setattr(ui_state_module, "device", SimpleNamespace(update=lambda: calls.append("device")))
+
+  state.update(progress_hook=phases.append)
+
+  assert calls == ["prime", ("submaster", 0), "state", "status", "device"]
+  assert phases == [
+    "ui.update.before_prime_state",
+    "ui.update.before_submaster",
+    "ui.update.before_state",
+    "ui.update.before_state_params",
+    "ui.update.after_state_params",
+    "ui.update.before_status",
+    "ui.update.before_params",
+    "ui.update.before_device",
+    "ui.update.after_device",
+  ]
+
+
+def test_ui_update_reports_offroad_callback(monkeypatch):
+  phases = []
+  callback_calls = []
+  state = object.__new__(ui_state_module.UIState)
+  state.started = False
+  state._started_prev = True
+  state._engaged_prev = False
+  state.status = ui_state_module.UIStatus.DISENGAGED
+  state.sm = SimpleNamespace(frame=2)
+  state._offroad_transition_callbacks = [lambda: callback_calls.append("offroad")]
+  state._engaged_transition_callbacks = []
+
+  state._update_status(phases.append)
+
+  assert callback_calls == ["offroad"]
+  assert phases == [
+    "ui.update.before_offroad_callback.<lambda>",
+    "ui.update.after_offroad_callback.<lambda>",
+  ]

selfdrive/ui/ui.py

@@ -72,7 +72,7 @@ def main():
       stall_monitor.progress("ui.loop_iteration")
       kick_watchdog()
       stall_monitor.progress("ui.after_watchdog")
-      ui_state.update()
+      ui_state.update(progress_hook=stall_monitor.progress)
       stall_monitor.progress("ui.after_state_update")
       now = time.monotonic()
       if now - context_update_time >= 1.0:

selfdrive/ui/ui_state.py

@@ -19,6 +19,10 @@ BACKLIGHT_OFFROAD = 65 if HARDWARE.get_device_type() == "mici" else 50
 USBGPU_POLL_INTERVAL = 1.0
 
 
+def _noop_progress(_phase: str) -> None:
+  pass
+
+
 class UIStatus(Enum):
   DISENGAGED = "disengaged"
   ENGAGED = "engaged"
@@ -166,16 +170,27 @@ class UIState:
   def is_offroad(self) -> bool:
     return not self.started
 
-  def update(self) -> None:
+  def update(self, progress_hook: Callable[[str], None] | None = None) -> None:
+    mark_progress = progress_hook or _noop_progress
+
+    mark_progress("ui.update.before_prime_state")
     self.prime_state.start()  # start thread after manager forks ui
+    mark_progress("ui.update.before_submaster")
     self.sm.update(0)
-    self._update_state()
-    self._update_status()
+    mark_progress("ui.update.before_state")
+    self._update_state(mark_progress)
+    mark_progress("ui.update.before_status")
+    self._update_status(mark_progress)
+    mark_progress("ui.update.before_params")
     if time.monotonic() - self._param_update_time > 5.0:
       self.update_params()
+    mark_progress("ui.update.before_device")
     device.update()
+    mark_progress("ui.update.after_device")
+
+  def _update_state(self, progress_hook: Callable[[str], None] | None = None) -> None:
+    mark_progress = progress_hook or _noop_progress
 
-  def _update_state(self) -> None:
     # Handle panda states updates
     if self.sm.updated["pandaStates"]:
       panda_states = self.sm["pandaStates"]
@@ -197,6 +212,7 @@ class UIState:
       self.light_sensor = -1
 
     # Trust hardwared's filtered started state; raw ignition can flap on Toyota.
+    mark_progress("ui.update.before_state_params")
     params = self.ui_params
     force_onroad = params.get_bool("ForceOnroad")
     force_offroad = params.get_bool("ForceOffroad")
@@ -215,6 +231,8 @@ class UIState:
     self.usbgpu_compiled = params.get_bool("UsbGpuCompiled")
     self.usbgpu_active = params.get_bool("UsbGpuActive")
     self.switchback_mode_enabled = self.params_memory.get_bool("SwitchbackModeEnabled") if self.started else False
+    self.conditional_status = self.params_memory.get_int("CEStatus", default=0) if self.started else 0
+    mark_progress("ui.update.after_state_params")
     if self.sm.valid.get("starpilotCarState", False):
       starpilot_car_state = self.sm["starpilotCarState"]
       self.always_on_lateral_active = (not self.sm["selfdriveState"].enabled and
@@ -224,8 +242,6 @@ class UIState:
       self.always_on_lateral_active = False
       self.traffic_mode_enabled = False
 
-    self.conditional_status = self.params_memory.get_int("CEStatus", default=0) if self.started else 0
-
     if self.sm.updated["starpilotPlan"]:
       plan = self.sm["starpilotPlan"]
       toggles_str = plan.starpilotToggles
@@ -240,7 +256,9 @@ class UIState:
     self.starpilot_toggles["force_offroad"] = force_offroad
     self.starpilot_toggles["force_onroad"] = force_onroad
 
-  def _update_status(self) -> None:
+  def _update_status(self, progress_hook: Callable[[str], None] | None = None) -> None:
+    mark_progress = progress_hook or _noop_progress
+
     if self.started and self.sm.updated["selfdriveState"]:
       ss = self.sm["selfdriveState"]
       state = ss.state
@@ -253,7 +271,10 @@ class UIState:
     # Check for engagement state changes
     if self.engaged != self._engaged_prev:
       for callback in self._engaged_transition_callbacks:
+        callback_name = getattr(callback, "__name__", type(callback).__name__)
+        mark_progress(f"ui.update.before_engaged_callback.{callback_name}")
         callback()
+        mark_progress(f"ui.update.after_engaged_callback.{callback_name}")
       self._engaged_prev = self.engaged
 
     # Handle onroad/offroad transition
@@ -264,7 +285,10 @@ class UIState:
         self.started_time = time.monotonic()
 
       for callback in self._offroad_transition_callbacks:
+        callback_name = getattr(callback, "__name__", type(callback).__name__)
+        mark_progress(f"ui.update.before_offroad_callback.{callback_name}")
         callback()
+        mark_progress(f"ui.update.after_offroad_callback.{callback_name}")
 
       self._started_prev = self.started