From 034992a3e89789c6eb5a12c7e88f8b5dc3a48ad3 Mon Sep 17 00:00:00 2001 From: firestar5683 <168790843+firestar5683@users.noreply.github.com> Date: Mon, 3 Aug 2026 12:21:03 -0500 Subject: [PATCH] lactate threshold --- selfdrive/controls/lib/latcontrol_torque.py | 7 ++- .../controls/lib/latcontrol_vehicle_tunes.py | 27 ++++++++- .../controls/lib/longitudinal_planner.py | 24 +++++++- .../lib/longitudinal_vehicle_tunes.py | 51 +++++++++++++++++ selfdrive/controls/tests/test_latcontrol.py | 10 ++++ .../tests/test_longitudinal_planner.py | 49 ++++++++++++++++ .../ui/mici/onroad/augmented_road_view.py | 2 +- selfdrive/ui/mici/onroad/cameraview.py | 9 ++- .../ui/mici/onroad/driver_camera_dialog.py | 2 +- selfdrive/ui/onroad/cameraview.py | 56 ++++++++++++++++++- selfdrive/ui/tests/test_camera_frame_order.py | 4 +- .../ui/tests/test_ui_state_performance.py | 55 ++++++++++++++++++ selfdrive/ui/ui.py | 2 +- selfdrive/ui/ui_state.py | 38 ++++++++++--- 14 files changed, 317 insertions(+), 19 deletions(-) diff --git a/selfdrive/controls/lib/latcontrol_torque.py b/selfdrive/controls/lib/latcontrol_torque.py index 890b64ea5..d3cd82312 100644 --- a/selfdrive/controls/lib/latcontrol_torque.py +++ b/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: diff --git a/selfdrive/controls/lib/latcontrol_vehicle_tunes.py b/selfdrive/controls/lib/latcontrol_vehicle_tunes.py index 7ea9249d9..643a35b9b 100644 --- a/selfdrive/controls/lib/latcontrol_vehicle_tunes.py +++ b/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") diff --git a/selfdrive/controls/lib/longitudinal_planner.py b/selfdrive/controls/lib/longitudinal_planner.py index b18b8c466..e5c94d483 100755 --- a/selfdrive/controls/lib/longitudinal_planner.py +++ 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) diff --git a/selfdrive/controls/lib/longitudinal_vehicle_tunes.py b/selfdrive/controls/lib/longitudinal_vehicle_tunes.py index b12d7f311..d3424fe69 100644 --- a/selfdrive/controls/lib/longitudinal_vehicle_tunes.py +++ b/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) diff --git a/selfdrive/controls/tests/test_latcontrol.py b/selfdrive/controls/tests/test_latcontrol.py index 64ac47a16..c32cc2316 100644 --- a/selfdrive/controls/tests/test_latcontrol.py +++ b/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 diff --git a/selfdrive/controls/tests/test_longitudinal_planner.py b/selfdrive/controls/tests/test_longitudinal_planner.py index 225b0679e..81a91be58 100644 --- a/selfdrive/controls/tests/test_longitudinal_planner.py +++ b/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) diff --git a/selfdrive/ui/mici/onroad/augmented_road_view.py b/selfdrive/ui/mici/onroad/augmented_road_view.py index 2c75fd948..c77f50d9f 100644 --- a/selfdrive/ui/mici/onroad/augmented_road_view.py +++ b/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 diff --git a/selfdrive/ui/mici/onroad/cameraview.py b/selfdrive/ui/mici/onroad/cameraview.py index 1aa0694ff..82a4c67a0 100644 --- a/selfdrive/ui/mici/onroad/cameraview.py +++ b/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"] diff --git a/selfdrive/ui/mici/onroad/driver_camera_dialog.py b/selfdrive/ui/mici/onroad/driver_camera_dialog.py index 367dc783f..5add1a4b7 100644 --- a/selfdrive/ui/mici/onroad/driver_camera_dialog.py +++ b/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 diff --git a/selfdrive/ui/onroad/cameraview.py b/selfdrive/ui/onroad/cameraview.py index a2d5964b1..8195820f0 100644 --- a/selfdrive/ui/onroad/cameraview.py +++ b/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") diff --git a/selfdrive/ui/tests/test_camera_frame_order.py b/selfdrive/ui/tests/test_camera_frame_order.py index 1f6b3ebfe..19dc4dc97 100644 --- a/selfdrive/ui/tests/test_camera_frame_order.py +++ b/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(): diff --git a/selfdrive/ui/tests/test_ui_state_performance.py b/selfdrive/ui/tests/test_ui_state_performance.py index c36ba31bc..c0e63f9af 100644 --- a/selfdrive/ui/tests/test_ui_state_performance.py +++ b/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.", + "ui.update.after_offroad_callback.", + ] diff --git a/selfdrive/ui/ui.py b/selfdrive/ui/ui.py index 0d2444bcb..dcbba3cc1 100644 --- a/selfdrive/ui/ui.py +++ b/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: diff --git a/selfdrive/ui/ui_state.py b/selfdrive/ui/ui_state.py index e72dd204d..e3839adf9 100644 --- a/selfdrive/ui/ui_state.py +++ b/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