diff --git a/opendbc_repo/opendbc/car/gm/carcontroller.py b/opendbc_repo/opendbc/car/gm/carcontroller.py index 0b6abafa7..9bfb815dd 100644 --- a/opendbc_repo/opendbc/car/gm/carcontroller.py +++ b/opendbc_repo/opendbc/car/gm/carcontroller.py @@ -5,7 +5,10 @@ from opendbc.car import ACCELERATION_DUE_TO_GRAVITY, Bus, DT_CTRL, create_gas_in from opendbc.car.lateral import apply_driver_steer_torque_limits from opendbc.car.gm import gmcan from opendbc.car.common.conversions import Conversions as CV -from opendbc.car.gm.values import ASCM_INT, CAR, CC_ONLY_CAR, CC_REGEN_PADDLE_CAR, DBC, EV_CAR, SDGM_CAR, AccState, CanBus, CarControllerParams, CruiseButtons, GMFlags +from opendbc.car.gm.values import ( + ASCM_INT, CAR, CC_ONLY_CAR, CC_REGEN_PADDLE_CAR, DBC, EV_CAR, SDGM_CAR, AccState, CanBus, CarControllerParams, + CruiseButtons, GMFlags, +) from opendbc.car.interfaces import CarControllerBase from openpilot.common.params import Params, UnknownKeyName from openpilot.starpilot.common.testing_grounds import testing_ground @@ -66,6 +69,14 @@ def should_spoof_ecm_cruise_status(CP): ) +def should_send_cc_button_spam(CP, CC, CS): + return ( + bool(CP.flags & GMFlags.CC_LONG.value) and + CC.longActive and + CS.out.vEgo > CP.minEnableSpeed + ) + + def get_testing_ground_1_brake_switch_bias(v_ego: float) -> int: return int(round(np.interp(v_ego, [0.0, 6.0, 15.0, 30.0], [40.0, 85.0, 130.0, 170.0]))) @@ -478,7 +489,11 @@ class CarController(CarControllerBase): interceptor_gas_cmd, press_regen_paddle = self.calc_pedal_command(actuators.accel, CC.longActive, CS.out.vEgo) maneuver_sng_launch = self.longitudinal_maneuver_mode and self.is_volt - if self.CP.enableGasInterceptorDEPRECATED and self.apply_gas > self.params.INACTIVE_REGEN and use_interceptor_sng_launch(self.CP, CS, maneuver_sng_launch): + if ( + self.CP.enableGasInterceptorDEPRECATED and + self.apply_gas > self.params.INACTIVE_REGEN and + use_interceptor_sng_launch(self.CP, CS, maneuver_sng_launch) + ): interceptor_gas_cmd = self.params.SNG_INTERCEPTOR_GAS if maneuver_sng_launch: interceptor_gas_cmd = max(interceptor_gas_cmd, float(np.interp(actuators.accel, [0.0, 1.0, 2.0], [self.params.SNG_INTERCEPTOR_GAS, 0.11, 0.16]))) @@ -501,7 +516,7 @@ class CarController(CarControllerBase): can_sends.append(gmcan.create_regen_paddle_command(self.packer_pt, CanBus.POWERTRAIN, paddle_spoof_pressed)) if self.CP.flags & GMFlags.CC_LONG.value: - if CC.longActive and CS.out.cruiseState.enabled and CS.out.vEgo > self.CP.minEnableSpeed: + if should_send_cc_button_spam(self.CP, CC, CS): # Using extend instead of append since the message is only sent intermittently can_sends.extend(gmcan.create_gm_cc_spam_command(self.packer_pt, self, CS, actuators, starpilot_toggles)) elif (CS.out.cruiseState.enabled and CC.enabled and self.frame % 52 == 0 and diff --git a/opendbc_repo/opendbc/car/gm/gmcan.py b/opendbc_repo/opendbc/car/gm/gmcan.py index 0a49bf969..b60e0bcbb 100644 --- a/opendbc_repo/opendbc/car/gm/gmcan.py +++ b/opendbc_repo/opendbc/car/gm/gmcan.py @@ -1,7 +1,7 @@ from opendbc.car import DT_CTRL from opendbc.car.can_definitions import CanData from opendbc.car.common.conversions import Conversions as CV -from opendbc.car.gm.values import CAR, CanBus, CruiseButtons +from opendbc.car.gm.values import CAR, CanBus, CruiseButtons, GMFlags MALIBU_BUTTON_TABLE = { 0: [0x10FF, 0x15EE, 0x1ADD, 0x1FCC], @@ -314,7 +314,13 @@ def create_gm_cc_spam_command(packer, controller, CS, actuators, starpilot_toggl controller.malibu_button_phase = (controller.malibu_button_phase + 1) % 4 return msgs idx = (CS.buttons_counter + 1) % 4 # Need to predict the next idx for '22-23 EUV - return [create_buttons(packer, CanBus.POWERTRAIN, idx, cruise_btn)] + msgs = [create_buttons(packer, CanBus.POWERTRAIN, idx, cruise_btn)] + + # Flashed camera-forward Volt CC installs also need the button spoof on the + # camera side. Removed-camera installs set NO_CAMERA and keep this PT-only. + if CS.CP.carFingerprint == CAR.CHEVROLET_VOLT_CC and not (CS.CP.flags & GMFlags.NO_CAMERA.value): + msgs.append(create_buttons(packer, CanBus.CAMERA, idx, cruise_btn)) + return msgs else: return [] diff --git a/opendbc_repo/opendbc/car/gm/interface.py b/opendbc_repo/opendbc/car/gm/interface.py index e6e2fc2ea..3ca01219b 100755 --- a/opendbc_repo/opendbc/car/gm/interface.py +++ b/opendbc_repo/opendbc/car/gm/interface.py @@ -612,7 +612,9 @@ class CarInterface(CarInterfaceBase): ret.flags |= GMFlags.FORCE_BRAKE_C9.value ret.safetyConfigs[0].safetyParam |= GMSafetyFlags.FLAG_GM_FORCE_BRAKE_C9.value - if (ret.networkLocation == NetworkLocation.fwdCamera or candidate in CC_ONLY_CAR) and CAM_MSG not in fingerprint[CanBus.CAMERA] and candidate not in SDGM_CAR: + # Exception for flashed cars, or cars whose camera was removed. + missing_camera_msg = CAM_MSG not in fingerprint.get(CanBus.CAMERA, {}) + if (ret.networkLocation == NetworkLocation.fwdCamera or candidate in CC_ONLY_CAR) and missing_camera_msg and candidate not in SDGM_CAR: ret.flags |= GMFlags.NO_CAMERA.value ret.safetyConfigs[0].safetyParam |= GMSafetyFlags.FLAG_GM_NO_CAMERA.value diff --git a/opendbc_repo/opendbc/car/gm/tests/test_gm.py b/opendbc_repo/opendbc/car/gm/tests/test_gm.py index 9ad923a75..ec93ae927 100644 --- a/opendbc_repo/opendbc/car/gm/tests/test_gm.py +++ b/opendbc_repo/opendbc/car/gm/tests/test_gm.py @@ -2,12 +2,15 @@ import pytest from types import SimpleNamespace from parameterized import parameterized +from opendbc.can import CANPacker +from opendbc.car import Bus, DT_CTRL from opendbc.car.car_helpers import interfaces -from opendbc.car.gm.carcontroller import should_spoof_dash_speed +from opendbc.car.gm import gmcan +from opendbc.car.gm.carcontroller import should_send_cc_button_spam, should_spoof_dash_speed import opendbc.car.gm.interface as gm_interface from opendbc.car.common.conversions import Conversions as CV from opendbc.car.gm.fingerprints import FINGERPRINTS -from opendbc.car.gm.values import CAMERA_ACC_CAR, CAR, CC_ONLY_CAR, GM_RX_OFFSET +from opendbc.car.gm.values import CAMERA_ACC_CAR, CAR, CC_ONLY_CAR, DBC, GM_RX_OFFSET, GMFlags, GMSafetyFlags CAMERA_DIAGNOSTIC_ADDRESS = 0x24b VOLT_CARS = ( @@ -78,6 +81,19 @@ class TestGMInterface: assert car_params.startingState assert car_params.startAccel == pytest.approx(1.15) + def test_volt_cc_sparse_fingerprint_without_camera_sets_no_camera(self): + CarInterface = interfaces[CAR.CHEVROLET_VOLT_CC] + fingerprint = { + 0: FINGERPRINTS[CAR.CHEVROLET_VOLT][0].copy(), + 1: {}, + } + + car_params = CarInterface.get_params(CAR.CHEVROLET_VOLT_CC, fingerprint, [], alpha_long=False, is_release=False, docs=False, + starpilot_toggles=_test_starpilot_toggles()) + + assert car_params.flags & GMFlags.NO_CAMERA.value + assert car_params.safetyConfigs[0].safetyParam & GMSafetyFlags.FLAG_GM_NO_CAMERA.value + class TestGMCarController: def test_dash_speed_spoof_respects_live_stock_acc_toggles(self): @@ -91,3 +107,80 @@ class TestGMCarController: cp = SimpleNamespace(openpilotLongitudinalControl=True, enableGasInterceptorDEPRECATED=False) assert should_spoof_dash_speed(cp, SimpleNamespace(disable_openpilot_long=False)) + + def test_cc_button_spam_does_not_require_stock_cruise_enabled(self): + cp = SimpleNamespace(flags=GMFlags.CC_LONG.value, minEnableSpeed=10.0) + cc = SimpleNamespace(longActive=True) + cs = SimpleNamespace(out=SimpleNamespace(vEgo=11.0, cruiseState=SimpleNamespace(enabled=False))) + + assert should_send_cc_button_spam(cp, cc, cs) + + def test_cc_button_spam_requires_cc_long_and_speed(self): + cc = SimpleNamespace(longActive=True) + cs = SimpleNamespace(out=SimpleNamespace(vEgo=9.0, cruiseState=SimpleNamespace(enabled=True))) + + assert not should_send_cc_button_spam(SimpleNamespace(flags=GMFlags.CC_LONG.value, minEnableSpeed=10.0), cc, cs) + assert not should_send_cc_button_spam(SimpleNamespace(flags=0, minEnableSpeed=10.0), cc, cs) + + def test_volt_cc_redneck_spam_is_mirrored_to_camera_bus(self): + packer = CANPacker(DBC[CAR.CHEVROLET_VOLT_CC][Bus.pt]) + controller = SimpleNamespace(frame=int(0.3 / DT_CTRL), last_button_frame=0, apply_speed=0, malibu_button_phase=0) + cs = SimpleNamespace( + CP=SimpleNamespace( + carFingerprint=CAR.CHEVROLET_VOLT_CC, + flags=0, + minEnableSpeed=24 * CV.MPH_TO_MS, + ), + buttons_counter=2, + out=SimpleNamespace( + vEgo=25.0, + cruiseState=SimpleNamespace(speed=20.0), + ), + ) + actuators = SimpleNamespace(accel=1.0) + + msgs = gmcan.create_gm_cc_spam_command(packer, controller, cs, actuators, SimpleNamespace(is_metric=False)) + + assert [msg[2] for msg in msgs] == [0, 2] + + def test_volt_cc_no_camera_redneck_spam_stays_on_powertrain_bus(self): + packer = CANPacker(DBC[CAR.CHEVROLET_VOLT_CC][Bus.pt]) + controller = SimpleNamespace(frame=int(0.3 / DT_CTRL), last_button_frame=0, apply_speed=0, malibu_button_phase=0) + cs = SimpleNamespace( + CP=SimpleNamespace( + carFingerprint=CAR.CHEVROLET_VOLT_CC, + flags=GMFlags.NO_CAMERA.value, + minEnableSpeed=24 * CV.MPH_TO_MS, + ), + buttons_counter=2, + out=SimpleNamespace( + vEgo=25.0, + cruiseState=SimpleNamespace(speed=20.0), + ), + ) + actuators = SimpleNamespace(accel=1.0) + + msgs = gmcan.create_gm_cc_spam_command(packer, controller, cs, actuators, SimpleNamespace(is_metric=False)) + + assert [msg[2] for msg in msgs] == [0] + + def test_non_volt_cc_redneck_spam_stays_on_powertrain_bus(self): + packer = CANPacker(DBC[CAR.CHEVROLET_BOLT_CC_2018_2021][Bus.pt]) + controller = SimpleNamespace(frame=int(0.3 / DT_CTRL), last_button_frame=0, apply_speed=0, malibu_button_phase=0) + cs = SimpleNamespace( + CP=SimpleNamespace( + carFingerprint=CAR.CHEVROLET_BOLT_CC_2018_2021, + flags=GMFlags.CC_LONG.value, + minEnableSpeed=24 * CV.MPH_TO_MS, + ), + buttons_counter=2, + out=SimpleNamespace( + vEgo=25.0, + cruiseState=SimpleNamespace(speed=20.0), + ), + ) + actuators = SimpleNamespace(accel=1.0) + + msgs = gmcan.create_gm_cc_spam_command(packer, controller, cs, actuators, SimpleNamespace(is_metric=False)) + + assert [msg[2] for msg in msgs] == [0] diff --git a/selfdrive/controls/tests/test_conditional_experimental_mode.py b/selfdrive/controls/tests/test_conditional_experimental_mode.py index 72c9bc3f8..35e415a72 100644 --- a/selfdrive/controls/tests/test_conditional_experimental_mode.py +++ b/selfdrive/controls/tests/test_conditional_experimental_mode.py @@ -7,13 +7,15 @@ import openpilot.starpilot.controls.starpilot_planner as starpilot_planner_modul from openpilot.starpilot.controls.lib.conditional_experimental_mode import ConditionalExperimentalMode -def make_cem(*, model_length: float, model_stopped: bool = False, tracking_lead: bool = False): +def make_cem(*, model_length: float, model_stopped: bool = False, tracking_lead: bool = False, + lead_status: bool = False, lead_d_rel: float = float("inf")): planner = SimpleNamespace( params=None, params_memory=None, model_length=model_length, model_stopped=model_stopped, tracking_lead=tracking_lead, + lead_one=SimpleNamespace(status=lead_status, dRel=lead_d_rel), ) return ConditionalExperimentalMode(planner) @@ -24,6 +26,12 @@ def make_sm(traffic_mode_enabled: bool = False): } +def run_stop_light_detector(cem, v_ego, *, steps: int, tracking_lead: bool = False): + for _ in range(steps): + cem.starpilot_planner.tracking_lead = tracking_lead + cem.stop_sign_and_light(v_ego, make_sm(), model_time=7.0) + + def test_low_speed_cruise_does_not_trigger_stop_light_from_model_stopped(): v_ego = 10 * CV.MPH_TO_MS model_length = v_ego * 10.0 @@ -39,7 +47,41 @@ def test_predicted_stop_within_threshold_triggers_stop_light(): model_length = v_ego * 4.0 cem = make_cem(model_length=model_length) - cem.stop_sign_and_light(v_ego, make_sm(), model_time=7.0) + run_stop_light_detector(cem, v_ego, steps=20) + + assert cem.stop_light_detected + + +def test_chattering_lead_does_not_trigger_stop_light(): + v_ego = 22 * CV.MPH_TO_MS + model_length = v_ego * 4.0 + + cem = make_cem(model_length=model_length) + for i in range(30): + cem.starpilot_planner.tracking_lead = (i % 2 == 0) + cem.starpilot_planner.lead_one.status = (i % 2 == 0) + cem.starpilot_planner.lead_one.dRel = model_length + 5.0 + cem.stop_sign_and_light(v_ego, make_sm(), model_time=7.0) + + assert not cem.stop_light_detected + + +def test_close_visible_but_untracked_lead_blocks_stop_light(): + v_ego = 22 * CV.MPH_TO_MS + model_length = v_ego * 4.0 + + cem = make_cem(model_length=model_length, lead_status=True, lead_d_rel=model_length + 5.0) + run_stop_light_detector(cem, v_ego, steps=30) + + assert not cem.stop_light_detected + + +def test_far_visible_lead_does_not_block_stop_light(): + v_ego = 22 * CV.MPH_TO_MS + model_length = v_ego * 4.0 + + cem = make_cem(model_length=model_length, lead_status=True, lead_d_rel=v_ego * 7.0 + 30.0) + run_stop_light_detector(cem, v_ego, steps=30) assert cem.stop_light_detected @@ -110,6 +152,7 @@ def test_starpilot_planner_updates_cem_with_current_frame_state(monkeypatch): planner.gps_location_service: SimpleNamespace(latitude=1.0, longitude=1.0, bearingDeg=90.0), } + planner.tracking_lead_filter.x = 1.0 planner.update(0.0, False, sm, starpilot_toggles) assert seen == { diff --git a/selfdrive/ui/mici/onroad/model_renderer.py b/selfdrive/ui/mici/onroad/model_renderer.py index a993bb409..56252ab71 100644 --- a/selfdrive/ui/mici/onroad/model_renderer.py +++ b/selfdrive/ui/mici/onroad/model_renderer.py @@ -21,9 +21,10 @@ MIN_DRAW_DISTANCE = 10.0 MAX_DRAW_DISTANCE = 100.0 RAINBOW_GRADIENT_COLOR_COUNT = 19 RAINBOW_SCROLL_SPEED_DEG_PER_SEC = 60.0 -ACCEL_PATH_MIN_LIGHTNESS = 0.78 -ACCEL_PATH_MIN_SATURATION = 0.50 STOCK_LANE_LINES_COLOR = rl.Color(255, 255, 255, 255) +DEFAULT_LANE_LINES_WIDTH = 4.0 +DEFAULT_PATH_WIDTH = 6.1 +DEFAULT_ROAD_EDGES_WIDTH = 2.0 LANE_LINE_COLORS = { UIStatus.DISENGAGED: rl.Color(200, 200, 200, 255), UIStatus.OVERRIDE: rl.Color(255, 255, 255, 255), @@ -199,14 +200,13 @@ class ModelRenderer(Widget): def _update_model(self, lead, path_x_array): """Update model visualization data based on model message""" model_ui_enabled = self._params.get_bool("ModelUI", default=True) - if model_ui_enabled: - path_width = self._path_width_to_half_m(self._params.get_float("PathWidth", return_default=True, default=6.1)) - lane_line_width = self._small_distance_to_half_m(self._params.get_float("LaneLinesWidth", return_default=True, default=4.0)) - road_edge_width = self._small_distance_to_half_m(self._params.get_float("RoadEdgesWidth", return_default=True, default=2.0)) - else: - path_width = 0.9 - lane_line_width = 0.025 - road_edge_width = 0.025 + custom_path_width = model_ui_enabled and self._param_float_changed("PathWidth", DEFAULT_PATH_WIDTH) + custom_lane_line_width = model_ui_enabled and self._param_float_changed("LaneLinesWidth", DEFAULT_LANE_LINES_WIDTH) + custom_road_edge_width = model_ui_enabled and self._param_float_changed("RoadEdgesWidth", DEFAULT_ROAD_EDGES_WIDTH) + + path_width = self._path_width_to_half_m(self._params.get_float("PathWidth", default=DEFAULT_PATH_WIDTH)) if custom_path_width else 0.9 + lane_line_width = self._small_distance_to_half_m(self._params.get_float("LaneLinesWidth", default=DEFAULT_LANE_LINES_WIDTH)) if custom_lane_line_width else None + road_edge_width = self._small_distance_to_half_m(self._params.get_float("RoadEdgesWidth", default=DEFAULT_ROAD_EDGES_WIDTH)) if custom_road_edge_width else None if model_ui_enabled and self._params.get_bool("DynamicPathWidth", default=False): if ui_state.status == UIStatus.ENGAGED: @@ -220,14 +220,19 @@ class ModelRenderer(Widget): max_idx = self._get_path_length_idx(self._lane_lines[0].raw_points[:, 0], max_distance) # Update lane lines using raw points + line_width_factor = 0.12 for i, lane_line in enumerate(self._lane_lines): + if i in (1, 2): + line_width_factor = 0.16 + line_width = lane_line_width if lane_line_width is not None else line_width_factor lane_line.projected_points = self._map_line_to_polygon( - lane_line.raw_points, lane_line_width * self._lane_line_probs[i], 0.0, max_idx + lane_line.raw_points, line_width * self._lane_line_probs[i], 0.0, max_idx ) # Update road edges using raw points + edge_width = road_edge_width if road_edge_width is not None else line_width_factor for road_edge in self._road_edges: - road_edge.projected_points = self._map_line_to_polygon(road_edge.raw_points, road_edge_width, 0.0, max_idx) + road_edge.projected_points = self._map_line_to_polygon(road_edge.raw_points, edge_width, 0.0, max_idx) # Update path using raw points if lead and lead.status: @@ -255,17 +260,15 @@ class ModelRenderer(Widget): def _update_experimental_gradient(self): """Pre-calculate experimental mode gradient colors""" use_rainbow = self._params.get_bool("RainbowPath", default=False) - use_acceleration = not use_rainbow and (self._experimental_mode or self._params.get_bool("AccelerationPath", default=True)) - - if not (use_rainbow or use_acceleration): - return - - max_len = min(len(self._path.projected_points) // 2, len(self._acceleration_x)) if use_rainbow: gradient_bottom, gradient_top = self._get_visible_gradient_bounds() self._exp_gradient = self._build_rainbow_gradient(gradient_bottom, gradient_top) return + if not self._experimental_mode or not self._params.get_bool("AccelerationPath", default=True): + return + + max_len = min(len(self._path.projected_points) // 2, len(self._acceleration_x)) segment_colors = [] gradient_stops = [] @@ -282,9 +285,9 @@ class ModelRenderer(Widget): # speed up: 120, slow down: 0 path_hue = np.clip(60 + self._acceleration_x[i] * 35, 0, 120) - accel_magnitude = np.clip(abs(self._acceleration_x[i]) * 1.5, 0.0, 1.0) - saturation = np.interp(accel_magnitude, [0.0, 1.0], [ACCEL_PATH_MIN_SATURATION, 1.0]) - lightness = np.interp(accel_magnitude, [0.0, 1.0], [ACCEL_PATH_MIN_LIGHTNESS, 0.62]) + + saturation = min(abs(self._acceleration_x[i] * 1.5), 1) + lightness = np.interp(saturation, [0.0, 1.0], [0.95, 0.62]) alpha = np.interp(lin_grad_point, [0.75 / 2.0, 0.75], [0.4, 0.0]) # Use HSL to RGB conversion @@ -361,12 +364,13 @@ class ModelRenderer(Widget): def _get_ll_color(self, prob: float, adjacent: bool, left: bool): alpha = np.clip(prob, 0.0, 0.7) stock_scheme = is_stock_color_scheme(self._params) + line_status = UIStatus.ENGAGED if ui_state.status == UIStatus.DISENGAGED and ui_state.always_on_lateral_active else ui_state.status if adjacent: override = get_param_color(self._params, "PathEdgesColor", 255) if override is not None: color = with_alpha(override, int(alpha * override.a)) elif stock_scheme: - base_color = LANE_LINE_COLORS.get(ui_state.status, LANE_LINE_COLORS[UIStatus.DISENGAGED]) + base_color = LANE_LINE_COLORS.get(line_status, LANE_LINE_COLORS[UIStatus.DISENGAGED]) color = rl.Color(base_color.r, base_color.g, base_color.b, int(alpha * 255)) else: base_color = get_theme_color("PathEdge", rl.Color(0, 255, 64, 255)) @@ -391,7 +395,7 @@ class ModelRenderer(Widget): lane_lines_color = get_theme_color("LaneLines", STOCK_LANE_LINES_COLOR) color = with_alpha(lane_lines_color, int(alpha * lane_lines_color.a)) - if stock_scheme and ui_state.status == UIStatus.DISENGAGED: + if stock_scheme and ui_state.status == UIStatus.DISENGAGED and not ui_state.always_on_lateral_active: color = rl.Color(0, 0, 0, int(alpha * 255)) return color @@ -424,17 +428,36 @@ class ModelRenderer(Widget): self._blend_filter.update(int(allow_throttle)) use_rainbow = self._params.get_bool("RainbowPath", default=False) use_accel_path = not use_rainbow and self._params.get_bool("AccelerationPath", default=True) - path_override = get_param_color(self._params, "PathColor", 255) - custom_theme_selected = (self._params.get("ColorScheme", encoding="utf-8", default="stock") or "stock").lower() != "stock" - if use_rainbow or self._experimental_mode or use_accel_path: - # Draw with acceleration coloring + if use_rainbow: if len(self._exp_gradient.colors) > 1: draw_polygon(self._rect, self._path.projected_points, gradient=self._exp_gradient) else: fallback = get_border_color(ui_state) draw_polygon(self._rect, self._path.projected_points, rl.Color(fallback.r, fallback.g, fallback.b, 90)) - elif path_override is not None or custom_theme_selected: + elif use_accel_path: + if self._experimental_mode: + if len(self._exp_gradient.colors) > 1: + draw_polygon(self._rect, self._path.projected_points, gradient=self._exp_gradient) + else: + fallback = get_border_color(ui_state) + draw_polygon(self._rect, self._path.projected_points, rl.Color(fallback.r, fallback.g, fallback.b, 90)) + else: + blend_factor = round(self._blend_filter.x * 100) / 100 + blended_colors = self._blend_colors(NO_THROTTLE_COLORS, THROTTLE_COLORS, blend_factor) + if lateral_ui_active and blend_factor < 1.0: + blended_colors = self._blend_colors(blended_colors, THROTTLE_COLORS, 0.65) + gradient = Gradient( + start=(0.0, 1.0), + end=(0.0, 0.0), + colors=blended_colors, + stops=[0.0, 0.5, 1.0], + ) + if ui_state.status == UIStatus.DISENGAGED and not ui_state.always_on_lateral_active: + draw_polygon(self._rect, self._path.projected_points, rl.Color(0, 0, 0, 90)) + else: + draw_polygon(self._rect, self._path.projected_points, gradient=gradient) + else: path_color = get_visual_color(self._params, "PathColor", "Path", rl.Color(48, 255, 156, 255)) gradient = Gradient( start=(0.0, 1.0), @@ -447,19 +470,6 @@ class ModelRenderer(Widget): stops=[0.0, 0.5, 1.0], ) draw_polygon(self._rect, self._path.projected_points, gradient=gradient) - else: - # Blend throttle/no throttle colors based on transition - blend_factor = round(self._blend_filter.x * 100) / 100 - blended_colors = self._blend_colors(NO_THROTTLE_COLORS, THROTTLE_COLORS, blend_factor) - if lateral_ui_active and blend_factor < 1.0: - blended_colors = self._blend_colors(blended_colors, THROTTLE_COLORS, 0.65) - gradient = Gradient( - start=(0.0, 1.0), # Bottom of path - end=(0.0, 0.0), # Top of path - colors=blended_colors, - stops=[0.0, 0.5, 1.0], - ) - draw_polygon(self._rect, self._path.projected_points, gradient=gradient) def _draw_lead_indicator(self): # Draw lead vehicles if available @@ -599,3 +609,12 @@ class ModelRenderer(Widget): if self._params.get_bool("IsMetric"): return value / 2.0 return value * CV.FOOT_TO_METER / 2.0 + + def _param_float_changed(self, key: str, default: float) -> bool: + value = self._params.get(key, encoding="utf-8") + if value in (None, ""): + return False + try: + return not np.isclose(float(value), default) + except (TypeError, ValueError): + return False diff --git a/selfdrive/ui/onroad/model_renderer.py b/selfdrive/ui/onroad/model_renderer.py index 62938deb7..5c959cd8b 100644 --- a/selfdrive/ui/onroad/model_renderer.py +++ b/selfdrive/ui/onroad/model_renderer.py @@ -19,9 +19,11 @@ MIN_DRAW_DISTANCE = 10.0 MAX_DRAW_DISTANCE = 100.0 RAINBOW_GRADIENT_COLOR_COUNT = 19 RAINBOW_SCROLL_SPEED_DEG_PER_SEC = 60.0 -ACCEL_PATH_MIN_LIGHTNESS = 0.78 -ACCEL_PATH_MIN_SATURATION = 0.50 STOCK_LANE_LINES_COLOR = rl.Color(255, 255, 255, 255) +DEFAULT_LANE_LINES_WIDTH = 4.0 +DEFAULT_PATH_EDGE_WIDTH = 20.0 +DEFAULT_PATH_WIDTH = 6.1 +DEFAULT_ROAD_EDGES_WIDTH = 2.0 THROTTLE_COLORS = [ rl.Color(13, 248, 122, 102), # HSLF(148/360, 0.94, 0.51, 0.4) @@ -35,7 +37,6 @@ NO_THROTTLE_COLORS = [ rl.Color(242, 242, 242, 0), # HSLF(112/360, 0.0, 0.95, 0.0) ] - @dataclass class ModelPoints: raw_points: np.ndarray = field(default_factory=lambda: np.empty((0, 3), dtype=np.float32)) @@ -189,16 +190,15 @@ class ModelRenderer(Widget): def _update_model(self, lead, path_x_array): """Update model visualization data based on model message""" model_ui_enabled = self._params.get_bool('ModelUI', default=True) - if model_ui_enabled: - path_width = self._path_width_to_half_m(self._params.get_float('PathWidth', return_default=True, default=6.1)) - lane_line_width_m = self._small_distance_to_half_m(self._params.get_float('LaneLinesWidth', return_default=True, default=4.0)) - road_edge_width_m = self._small_distance_to_half_m(self._params.get_float('RoadEdgesWidth', return_default=True, default=2.0)) - path_edge_width_pct = np.clip(self._params.get_float('PathEdgeWidth', return_default=True, default=20.0) / 100.0, 0.0, 1.0) - else: - path_width = 0.9 - lane_line_width_m = 0.025 - road_edge_width_m = 0.025 - path_edge_width_pct = 0.0 + custom_path_width = model_ui_enabled and self._param_float_changed('PathWidth', DEFAULT_PATH_WIDTH) + custom_lane_line_width = model_ui_enabled and self._param_float_changed('LaneLinesWidth', DEFAULT_LANE_LINES_WIDTH) + custom_road_edge_width = model_ui_enabled and self._param_float_changed('RoadEdgesWidth', DEFAULT_ROAD_EDGES_WIDTH) + custom_path_edge_width = model_ui_enabled and self._param_float_changed('PathEdgeWidth', DEFAULT_PATH_EDGE_WIDTH) + + path_width = self._path_width_to_half_m(self._params.get_float('PathWidth', default=DEFAULT_PATH_WIDTH)) if custom_path_width else 0.9 + lane_line_width_m = self._small_distance_to_half_m(self._params.get_float('LaneLinesWidth', default=DEFAULT_LANE_LINES_WIDTH)) if custom_lane_line_width else 0.025 + road_edge_width_m = self._small_distance_to_half_m(self._params.get_float('RoadEdgesWidth', default=DEFAULT_ROAD_EDGES_WIDTH)) if custom_road_edge_width else 0.025 + path_edge_width_pct = np.clip(self._params.get_float('PathEdgeWidth', default=DEFAULT_PATH_EDGE_WIDTH) / 100.0, 0.0, 1.0) if custom_path_edge_width else 0.0 # Dynamic path width if model_ui_enabled and self._params.get_bool('DynamicPathWidth', default=False): @@ -246,17 +246,15 @@ class ModelRenderer(Widget): def _update_experimental_gradient(self): """Pre-calculate experimental mode gradient colors""" use_rainbow = self._params.get_bool('RainbowPath', default=False) - use_acceleration = not use_rainbow and (self._experimental_mode or self._params.get_bool('AccelerationPath', default=True)) - - if not use_acceleration and not use_rainbow: - return - - max_len = min(len(self._path.projected_points) // 2, len(self._acceleration_x)) if use_rainbow: gradient_bottom, gradient_top = self._get_visible_gradient_bounds() self._exp_gradient = self._build_rainbow_gradient(gradient_bottom, gradient_top) return + if not self._experimental_mode or not self._params.get_bool('AccelerationPath', default=True): + return + + max_len = min(len(self._path.projected_points) // 2, len(self._acceleration_x)) segment_colors = [] gradient_stops = [] @@ -273,9 +271,9 @@ class ModelRenderer(Widget): # speed up: 120, slow down: 0 path_hue = np.clip(60 + self._acceleration_x[i] * 35, 0, 120) - accel_magnitude = np.clip(abs(self._acceleration_x[i]) * 1.5, 0.0, 1.0) - saturation = np.interp(accel_magnitude, [0.0, 1.0], [ACCEL_PATH_MIN_SATURATION, 1.0]) - lightness = np.interp(accel_magnitude, [0.0, 1.0], [ACCEL_PATH_MIN_LIGHTNESS, 0.62]) + + saturation = min(abs(self._acceleration_x[i] * 1.5), 1) + lightness = np.interp(saturation, [0.0, 1.0], [0.95, 0.62]) alpha = np.interp(lin_grad_point, [0.75 / 2.0, 0.75], [0.4, 0.0]) # Use HSL to RGB conversion @@ -387,14 +385,28 @@ class ModelRenderer(Widget): use_rainbow = self._params.get_bool('RainbowPath', default=False) use_accel_path = not use_rainbow and self._params.get_bool('AccelerationPath', default=True) - path_override = get_param_color(self._params, 'PathColor', 255) - custom_theme_selected = (self._params.get('ColorScheme', encoding='utf-8', default='stock') or 'stock').lower() != 'stock' - if use_rainbow or self._experimental_mode or use_accel_path: + if use_rainbow: if len(self._exp_gradient.colors) > 1: draw_polygon(self._rect, self._path.projected_points, gradient=self._exp_gradient) else: - draw_polygon(self._rect, self._path.projected_points, rl.Color(255, 255, 255, 30)) - elif path_override is not None or custom_theme_selected: + draw_polygon(self._rect, self._path.projected_points, rl.Color(48, 255, 156, 90)) + elif use_accel_path: + if self._experimental_mode: + if len(self._exp_gradient.colors) > 1: + draw_polygon(self._rect, self._path.projected_points, gradient=self._exp_gradient) + else: + draw_polygon(self._rect, self._path.projected_points, rl.Color(255, 255, 255, 30)) + else: + blend_factor = round(self._blend_filter.x * 100) / 100 + blended_colors = self._blend_colors(NO_THROTTLE_COLORS, THROTTLE_COLORS, blend_factor) + gradient = Gradient( + start=(0.0, 1.0), + end=(0.0, 0.0), + colors=blended_colors, + stops=[0.0, 0.5, 1.0], + ) + draw_polygon(self._rect, self._path.projected_points, gradient=gradient) + else: path_color = get_visual_color(self._params, "PathColor", "Path", rl.Color(48, 255, 156, 255)) gradient = Gradient( start=(0.0, 1.0), @@ -407,17 +419,6 @@ class ModelRenderer(Widget): stops=[0.0, 0.5, 1.0], ) draw_polygon(self._rect, self._path.projected_points, gradient=gradient) - else: - # Blend throttle/no throttle colors based on transition - blend_factor = round(self._blend_filter.x * 100) / 100 - blended_colors = self._blend_colors(NO_THROTTLE_COLORS, THROTTLE_COLORS, blend_factor) - gradient = Gradient( - start=(0.0, 1.0), # Bottom of path - end=(0.0, 0.0), # Top of path - colors=blended_colors, - stops=[0.0, 0.5, 1.0], - ) - draw_polygon(self._rect, self._path.projected_points, gradient=gradient) def _draw_lead_indicator(self): # Draw lead vehicles if available @@ -683,6 +684,15 @@ class ModelRenderer(Widget): return value / 2.0 return value * CV.FOOT_TO_METER / 2.0 + def _param_float_changed(self, key: str, default: float) -> bool: + value = self._params.get(key, encoding="utf-8") + if value in (None, ""): + return False + try: + return not np.isclose(float(value), default) + except (TypeError, ValueError): + return False + @staticmethod def _blend_colors(begin_colors, end_colors, t): if t >= 1.0: diff --git a/starpilot/controls/lib/conditional_experimental_mode.py b/starpilot/controls/lib/conditional_experimental_mode.py index 78d51e32a..9a5714319 100644 --- a/starpilot/controls/lib/conditional_experimental_mode.py +++ b/starpilot/controls/lib/conditional_experimental_mode.py @@ -38,6 +38,12 @@ class ConditionalExperimentalMode: LIGHT_SPEED_LOW = 50 * CV.MPH_TO_MS # 50 mph threshold LIGHT_SPEED_HIGH = 60 * CV.MPH_TO_MS # 60 mph threshold LIGHT_MAX_TIME = 9 # Balanced max time preserving city performance + LOW_SPEED_LIGHT_FILTER_TIME = 0.35 + LEAD_CLEAR_FILTER_TIME_LOW = 0.6 + LEAD_CLEAR_FILTER_TIME_HIGH = 0.35 + STOP_LIGHT_ON_MARGIN = 2.5 + STOP_LIGHT_OFF_MARGIN = 4.0 + STOP_LIGHT_LEAD_BLOCK_MARGIN = 15.0 # ===== END TUNING PARAMETERS ===== @@ -66,11 +72,13 @@ class ConditionalExperimentalMode: self.curvature_filter = FirstOrderFilter(0, self.FILTER_TIME_CURVE, DT_MDL) self.slow_lead_filter = FirstOrderFilter(0, self.FILTER_TIME_LEAD, DT_MDL) self.stop_light_filter = FirstOrderFilter(0, self.FILTER_TIME_LIGHT, DT_MDL) + self.lead_clear_filter = FirstOrderFilter(0, self.LEAD_CLEAR_FILTER_TIME_LOW, DT_MDL) self.curve_detected = False self.slow_lead_detected = False self.experimental_mode = False self.stop_light_detected = False + self.stop_light_model_detected = False self.prev_experimental_mode = False # For hysteresis self.mode_hold_until = 0.0 self.mode_false_since = 0.0 @@ -177,7 +185,8 @@ class ConditionalExperimentalMode: filter_time_curves = interp(speed_mph, bp, [low_filter_time, low_filter_time, tuned_filter_time_curves]) filter_time_leads = interp(speed_mph, bp, [low_filter_time, low_filter_time, tuned_filter_time_leads]) - filter_time_lights = interp(speed_mph, bp, [low_filter_time, low_filter_time, tuned_filter_time_lights]) + filter_time_lights = interp(speed_mph, bp, [self.LOW_SPEED_LIGHT_FILTER_TIME, self.LOW_SPEED_LIGHT_FILTER_TIME, tuned_filter_time_lights]) + lead_clear_filter_time = interp(speed_mph, bp, [self.LEAD_CLEAR_FILTER_TIME_LOW, self.LEAD_CLEAR_FILTER_TIME_LOW, self.LEAD_CLEAR_FILTER_TIME_HIGH]) light_boost = interp(speed_mph, bp, [low_boost, low_boost, tuned_boost]) cap_factor = interp(speed_mph, bp, [low_cap_factor, low_cap_factor, tuned_cap_factor]) @@ -185,11 +194,14 @@ class ConditionalExperimentalMode: self.curvature_filter = FirstOrderFilter(self.curvature_filter.x, filter_time_curves, DT_MDL) self.slow_lead_filter = FirstOrderFilter(self.slow_lead_filter.x, filter_time_leads, DT_MDL) self.stop_light_filter = FirstOrderFilter(self.stop_light_filter.x, filter_time_lights, DT_MDL) + self.lead_clear_filter.update_alpha(lead_clear_filter_time) # Disable stoplight detection at very high speeds to prevent false positives if speed_mph > 75: # Disable above 75 mph self.stop_light_filter.x = 0 self.stop_light_detected = False + self.stop_light_model_detected = False + self.lead_clear_filter.x = 0 return # Adjust model time with interp boost and gradual cap @@ -197,15 +209,30 @@ class ConditionalExperimentalMode: if cap_factor > 0: adjusted_model_time = min(adjusted_model_time, self.LIGHT_MAX_TIME * cap_factor + model_time * (1 - cap_factor)) # Gradual cap - model_stopping = self.starpilot_planner.model_length < v_ego * adjusted_model_time + stop_threshold = max(v_ego * adjusted_model_time, 0.0) + if self.stop_light_model_detected: + model_stopping = self.starpilot_planner.model_length < stop_threshold + self.STOP_LIGHT_OFF_MARGIN + else: + model_stopping = self.starpilot_planner.model_length < max(stop_threshold - self.STOP_LIGHT_ON_MARGIN, 0.0) + self.stop_light_model_detected = model_stopping # `model_stopped` is a coarse horizon-length check (< 50 m with current constants) # used elsewhere for force-stop/green-light behavior. Reusing it here causes # ordinary low-speed cruising to look like a stop prediction and can latch the # STOP_LIGHT CEM trigger. For the CEM detector, key strictly off the configured # "predicted stop within N seconds" threshold. - self.stop_light_filter.update(model_stopping) - self.stop_light_detected = bool(self.stop_light_filter.x >= THRESHOLD**2 and not self.starpilot_planner.tracking_lead) + # Key off relevant raw lead presence, not trackingLead. Vision-only GM can + # flap trackingLead around the model-length threshold while leadOne remains + # present; far/stale leads should not suppress true stop-light detection. + lead = getattr(self.starpilot_planner, "lead_one", None) + lead_distance = float(getattr(lead, "dRel", float("inf"))) + lead_relevant = bool(getattr(lead, "status", False)) and lead_distance < stop_threshold + self.STOP_LIGHT_LEAD_BLOCK_MARGIN + self.lead_clear_filter.update(not lead_relevant) + lead_cleared = self.lead_clear_filter.x >= THRESHOLD + self.stop_light_filter.update(model_stopping and lead_cleared) + self.stop_light_detected = bool(self.stop_light_filter.x >= THRESHOLD**2 and lead_cleared) else: self.stop_light_filter.x = 0 self.stop_light_detected = False + self.stop_light_model_detected = False + self.lead_clear_filter.x = 0