From bdf7bca569f7663250572850fe7611cd36ce8783 Mon Sep 17 00:00:00 2001 From: firestar5683 <168790843+firestar5683@users.noreply.github.com> Date: Thu, 25 Jun 2026 10:54:58 -0500 Subject: [PATCH] Lite Brite --- opendbc_repo/opendbc/car/gm/carcontroller.py | 3 +- opendbc_repo/opendbc/car/gm/gmcan.py | 2 +- opendbc_repo/opendbc/car/gm/interface.py | 4 +- .../car/gm/tests/test_carcontroller.py | 14 +++ opendbc_repo/opendbc/car/gm/tests/test_gm.py | 6 +- selfdrive/controls/lib/latcontrol_torque.py | 92 +++++++++---------- .../tests/test_conditional_chill_mode.py | 3 +- .../test_conditional_experimental_mode.py | 81 +++++++++++++++- .../controls/lib/conditional_chill_mode.py | 14 +++ .../lib/conditional_experimental_mode.py | 89 +++++++++--------- 10 files changed, 212 insertions(+), 96 deletions(-) diff --git a/opendbc_repo/opendbc/car/gm/carcontroller.py b/opendbc_repo/opendbc/car/gm/carcontroller.py index 946feba92..dce5fad74 100644 --- a/opendbc_repo/opendbc/car/gm/carcontroller.py +++ b/opendbc_repo/opendbc/car/gm/carcontroller.py @@ -995,7 +995,8 @@ class CarController(CarControllerBase): # cannot linger after a disengage or main-off event. if should_send_bolt_acc_pedal_friction: can_sends.append(gmcan.create_friction_brake_command( - self.packer_ch, friction_brake_bus, experiment_brake, idx, False, near_stop, at_full_stop, self.CP)) + self.packer_ch, friction_brake_bus, experiment_brake, idx, bolt_acc_pedal_friction_main_on, + near_stop, at_full_stop, self.CP)) if self.CP.carFingerprint not in CC_ONLY_CAR: friction_brake_bus = get_friction_brake_bus(self.CP) # GM Camera exceptions diff --git a/opendbc_repo/opendbc/car/gm/gmcan.py b/opendbc_repo/opendbc/car/gm/gmcan.py index ab2fab317..fedd4765d 100644 --- a/opendbc_repo/opendbc/car/gm/gmcan.py +++ b/opendbc_repo/opendbc/car/gm/gmcan.py @@ -183,7 +183,7 @@ def get_friction_brake_mode(apply_brake, enabled, near_stop, at_full_stop, CP, a mode = 0x1 # TODO: Understand this better. Volts and ICE Camera ACC cars are 0x1 when enabled with no brake - if enabled and CP.carFingerprint in (CAR.CHEVROLET_BOLT_ACC_2022_2023,): + if enabled and CP.carFingerprint in (CAR.CHEVROLET_BOLT_ACC_2022_2023, CAR.CHEVROLET_BOLT_ACC_2022_2023_PEDAL): mode = 0x9 if apply_brake > 0: diff --git a/opendbc_repo/opendbc/car/gm/interface.py b/opendbc_repo/opendbc/car/gm/interface.py index 974297a22..bda812c42 100755 --- a/opendbc_repo/opendbc/car/gm/interface.py +++ b/opendbc_repo/opendbc/car/gm/interface.py @@ -532,10 +532,10 @@ class CarInterface(CarInterfaceBase): ret.longitudinalTuning.kiBP = [0.0, 4.0, 12.0, 35.0] ret.longitudinalTuning.kiV = [0.03, 0.04, 0.055, 0.07] ret.minEnableSpeed = 5 * CV.KPH_TO_MS - ret.stoppingDecelRate = 1.2 + ret.stoppingDecelRate = 1.0 ret.vEgoStopping = 0.35 ret.vEgoStarting = 0.35 - ret.stopAccel = -0.40 + ret.stopAccel = -0.30 CarInterfaceBase.configure_torque_tune(candidate, ret.lateralTuning) elif candidate == CAR.BUICK_BABYENCLAVE: diff --git a/opendbc_repo/opendbc/car/gm/tests/test_carcontroller.py b/opendbc_repo/opendbc/car/gm/tests/test_carcontroller.py index 6bef21d9d..fa44d9af7 100644 --- a/opendbc_repo/opendbc/car/gm/tests/test_carcontroller.py +++ b/opendbc_repo/opendbc/car/gm/tests/test_carcontroller.py @@ -683,6 +683,20 @@ def test_friction_brake_mode_uses_near_stop_hold_mode_for_volt_auto_hold(): assert get_friction_brake_mode(120, False, True, True, CP, allow_near_stop_mode=True) == 0xd +def test_friction_brake_mode_uses_stock_bolt_unwind_for_pedal_print_when_enabled(): + CP = SimpleNamespace(carFingerprint=CAR.CHEVROLET_BOLT_ACC_2022_2023_PEDAL) + + assert get_friction_brake_mode(0, False, True, False, CP) == 0x1 + assert get_friction_brake_mode(0, True, True, False, CP) == 0x9 + + +def test_friction_brake_mode_keeps_bolt_pedal_braking_mode_unchanged(): + CP = SimpleNamespace(carFingerprint=CAR.CHEVROLET_BOLT_ACC_2022_2023_PEDAL) + + assert get_friction_brake_mode(120, True, False, False, CP) == 0xa + assert get_friction_brake_mode(120, True, True, True, CP) == 0xd + + def test_calc_pedal_command_small_accel_deadband_keeps_creep_target_stable(): pos_controller = _controller() neg_controller = _controller() diff --git a/opendbc_repo/opendbc/car/gm/tests/test_gm.py b/opendbc_repo/opendbc/car/gm/tests/test_gm.py index 30fe9c455..b8b8e5688 100644 --- a/opendbc_repo/opendbc/car/gm/tests/test_gm.py +++ b/opendbc_repo/opendbc/car/gm/tests/test_gm.py @@ -166,7 +166,7 @@ class TestGMInterface: assert list(car_params.longitudinalTuning.kiBP) == pytest.approx([0.0, 5.0, 15.0, 35.0]) assert list(car_params.longitudinalTuning.kiV) == pytest.approx([0.28, 0.26, 0.20, 0.16]) - def test_blazer_uses_earlier_stronger_low_speed_stop_tune(self): + def test_blazer_uses_softer_low_speed_stop_hold_tune(self): CarInterface = interfaces[CAR.CHEVROLET_BLAZER] fingerprint = _empty_fingerprint() fingerprint[0] = FINGERPRINTS[CAR.CHEVROLET_BLAZER][0].copy() @@ -181,10 +181,10 @@ class TestGMInterface: assert list(car_params.longitudinalTuning.kiBP) == pytest.approx([0.0, 4.0, 12.0, 35.0]) assert list(car_params.longitudinalTuning.kiV) == pytest.approx([0.03, 0.04, 0.055, 0.07]) assert car_params.minEnableSpeed == pytest.approx(5 * CV.KPH_TO_MS) - assert car_params.stoppingDecelRate == pytest.approx(1.2) + assert car_params.stoppingDecelRate == pytest.approx(1.0) assert car_params.vEgoStopping == pytest.approx(0.35) assert car_params.vEgoStarting == pytest.approx(0.35) - assert car_params.stopAccel == pytest.approx(-0.40) + assert car_params.stopAccel == pytest.approx(-0.30) def test_volt_gateway_without_accel_pos_uses_brake_pedal_message(self): CarInterface = interfaces[CAR.CHEVROLET_VOLT] diff --git a/selfdrive/controls/lib/latcontrol_torque.py b/selfdrive/controls/lib/latcontrol_torque.py index 3b37dcb70..edcf6fe8d 100644 --- a/selfdrive/controls/lib/latcontrol_torque.py +++ b/selfdrive/controls/lib/latcontrol_torque.py @@ -522,21 +522,21 @@ IONIQ_6_CENTER_TAPER_LAT = 0.24 IONIQ_6_CENTER_TAPER_LAT_WIDTH = 0.025 IONIQ_6_CENTER_TAPER_SPEED = 18.0 IONIQ_6_CENTER_TAPER_SPEED_WIDTH = 2.5 -IONIQ_6_HIGHWAY_CENTER_TAPER_MAX = 0.038 -IONIQ_6_HIGHWAY_CENTER_TAPER_LAT = 0.09 -IONIQ_6_HIGHWAY_CENTER_TAPER_LAT_WIDTH = 0.03 +IONIQ_6_HIGHWAY_CENTER_TAPER_MAX = 0.046 +IONIQ_6_HIGHWAY_CENTER_TAPER_LAT = 0.10 +IONIQ_6_HIGHWAY_CENTER_TAPER_LAT_WIDTH = 0.035 IONIQ_6_HIGHWAY_CENTER_TAPER_SPEED = 24.5 IONIQ_6_HIGHWAY_CENTER_TAPER_SPEED_WIDTH = 1.8 -IONIQ_6_HIGHWAY_OUTPUT_TAPER_MAX = 0.09 +IONIQ_6_HIGHWAY_OUTPUT_TAPER_MAX = 0.10 IONIQ_6_HIGHWAY_OUTPUT_TAPER_LAT = 0.14 IONIQ_6_HIGHWAY_OUTPUT_TAPER_LAT_WIDTH = 0.04 IONIQ_6_HIGHWAY_OUTPUT_TAPER_SPEED = 23.5 IONIQ_6_HIGHWAY_OUTPUT_TAPER_SPEED_WIDTH = 2.0 -IONIQ_6_HIGHWAY_TRANSITION_OUTPUT_TAPER_MAX = 0.12 -IONIQ_6_HIGHWAY_TRANSITION_OUTPUT_TAPER_LAT = 0.12 +IONIQ_6_HIGHWAY_TRANSITION_OUTPUT_TAPER_MAX = 0.14 +IONIQ_6_HIGHWAY_TRANSITION_OUTPUT_TAPER_LAT = 0.13 IONIQ_6_HIGHWAY_TRANSITION_OUTPUT_TAPER_LAT_WIDTH = 0.035 -IONIQ_6_HIGHWAY_TRANSITION_OUTPUT_TAPER_JERK = 0.18 -IONIQ_6_HIGHWAY_TRANSITION_OUTPUT_TAPER_JERK_WIDTH = 0.09 +IONIQ_6_HIGHWAY_TRANSITION_OUTPUT_TAPER_JERK = 0.16 +IONIQ_6_HIGHWAY_TRANSITION_OUTPUT_TAPER_JERK_WIDTH = 0.10 IONIQ_6_LOW_MID_CENTER_TAPER_MAX = 0.088 IONIQ_6_LOW_MID_CENTER_TAPER_LAT = 0.28 IONIQ_6_LOW_MID_CENTER_TAPER_LAT_WIDTH = 0.06 @@ -561,32 +561,32 @@ IONIQ_6_DIRECTIONAL_TAPER_LOW_SPEED_RELIEF_SPEED = 11.2 IONIQ_6_DIRECTIONAL_TAPER_LOW_SPEED_RELIEF_SPEED_WIDTH = 1.5 IONIQ_6_DIRECTIONAL_TAPER_LOW_SPEED_RELIEF_LAT = 0.10 IONIQ_6_DIRECTIONAL_TAPER_LOW_SPEED_RELIEF_LAT_WIDTH = 0.06 -IONIQ_6_CRAWL_TURN_IN_FF_BOOST_LEFT = 0.14 -IONIQ_6_CRAWL_TURN_IN_FF_BOOST_RIGHT = 0.19 -IONIQ_6_CRAWL_TURN_IN_FF_SPEED = 5.0 -IONIQ_6_CRAWL_TURN_IN_FF_SPEED_WIDTH = 0.9 -IONIQ_6_CRAWL_TURN_IN_FF_LAT = 0.08 -IONIQ_6_CRAWL_TURN_IN_FF_LAT_WIDTH = 0.045 -IONIQ_6_LOW_SPEED_ANGLE_ASSIST_MAX_TORQUE = 0.42 -IONIQ_6_LOW_SPEED_ANGLE_ASSIST_SPEED = 2.95 -IONIQ_6_LOW_SPEED_ANGLE_ASSIST_SPEED_WIDTH = 0.35 -IONIQ_6_LOW_SPEED_ANGLE_ASSIST_ERROR = 2.4 -IONIQ_6_LOW_SPEED_ANGLE_ASSIST_ERROR_WIDTH = 1.35 -IONIQ_6_LOW_SPEED_ANGLE_ASSIST_DESIRED_ANGLE = 6.5 -IONIQ_6_LOW_SPEED_ANGLE_ASSIST_DESIRED_ANGLE_WIDTH = 2.6 -IONIQ_6_LOW_SPEED_ANGLE_ASSIST_TRACK_RATIO_START = 0.58 -IONIQ_6_LOW_SPEED_ANGLE_ASSIST_TRACK_RATIO_WIDTH = 0.14 -IONIQ_6_LOW_SPEED_ANGLE_ASSIST_TRACK_RATIO_FLOOR = 0.18 +IONIQ_6_CRAWL_TURN_IN_FF_BOOST_LEFT = 0.18 +IONIQ_6_CRAWL_TURN_IN_FF_BOOST_RIGHT = 0.24 +IONIQ_6_CRAWL_TURN_IN_FF_SPEED = 5.3 +IONIQ_6_CRAWL_TURN_IN_FF_SPEED_WIDTH = 1.0 +IONIQ_6_CRAWL_TURN_IN_FF_LAT = 0.06 +IONIQ_6_CRAWL_TURN_IN_FF_LAT_WIDTH = 0.035 +IONIQ_6_LOW_SPEED_ANGLE_ASSIST_MAX_TORQUE = 0.46 +IONIQ_6_LOW_SPEED_ANGLE_ASSIST_SPEED = 3.25 +IONIQ_6_LOW_SPEED_ANGLE_ASSIST_SPEED_WIDTH = 0.45 +IONIQ_6_LOW_SPEED_ANGLE_ASSIST_ERROR = 1.9 +IONIQ_6_LOW_SPEED_ANGLE_ASSIST_ERROR_WIDTH = 1.20 +IONIQ_6_LOW_SPEED_ANGLE_ASSIST_DESIRED_ANGLE = 5.5 +IONIQ_6_LOW_SPEED_ANGLE_ASSIST_DESIRED_ANGLE_WIDTH = 2.4 +IONIQ_6_LOW_SPEED_ANGLE_ASSIST_TRACK_RATIO_START = 0.66 +IONIQ_6_LOW_SPEED_ANGLE_ASSIST_TRACK_RATIO_WIDTH = 0.12 +IONIQ_6_LOW_SPEED_ANGLE_ASSIST_TRACK_RATIO_FLOOR = 0.26 IONIQ_6_LOW_SPEED_ANGLE_ASSIST_ADD_BP = [0.0, 0.35, 0.65, 1.0] -IONIQ_6_LOW_SPEED_ANGLE_ASSIST_ADD_V = [1.0, 1.0, 0.82, 0.0] -IONIQ_6_LOW_SPEED_UNWIND_ASSIST_MAX_TORQUE = 0.26 -IONIQ_6_LOW_SPEED_UNWIND_ASSIST_SPEED = 3.05 -IONIQ_6_LOW_SPEED_UNWIND_ASSIST_SPEED_WIDTH = 0.35 -IONIQ_6_LOW_SPEED_UNWIND_ASSIST_ERROR = 1.8 -IONIQ_6_LOW_SPEED_UNWIND_ASSIST_ERROR_WIDTH = 1.0 -IONIQ_6_LOW_SPEED_UNWIND_ASSIST_ACTUAL_ANGLE = 12.0 -IONIQ_6_LOW_SPEED_UNWIND_ASSIST_ACTUAL_ANGLE_WIDTH = 5.0 -IONIQ_6_LOW_SPEED_UNWIND_ASSIST_BLEND = 0.45 +IONIQ_6_LOW_SPEED_ANGLE_ASSIST_ADD_V = [1.0, 1.0, 0.88, 0.08] +IONIQ_6_LOW_SPEED_UNWIND_ASSIST_MAX_TORQUE = 0.30 +IONIQ_6_LOW_SPEED_UNWIND_ASSIST_SPEED = 3.35 +IONIQ_6_LOW_SPEED_UNWIND_ASSIST_SPEED_WIDTH = 0.50 +IONIQ_6_LOW_SPEED_UNWIND_ASSIST_ERROR = 1.6 +IONIQ_6_LOW_SPEED_UNWIND_ASSIST_ERROR_WIDTH = 0.95 +IONIQ_6_LOW_SPEED_UNWIND_ASSIST_ACTUAL_ANGLE = 10.5 +IONIQ_6_LOW_SPEED_UNWIND_ASSIST_ACTUAL_ANGLE_WIDTH = 4.0 +IONIQ_6_LOW_SPEED_UNWIND_ASSIST_BLEND = 0.52 IONIQ_6_HIGH_SPEED_RIGHT_TURN_IN_FF_BOOST = 0.10 IONIQ_6_HIGH_SPEED_RIGHT_TURN_IN_FF_SPEED = 18.0 IONIQ_6_HIGH_SPEED_RIGHT_TURN_IN_FF_SPEED_WIDTH = 2.5 @@ -607,27 +607,27 @@ IONIQ_6_OUTPUT_DIRECTIONAL_TAPER_BLEND = 0.97 KIA_EV6_LATERAL_TESTING_GROUND_ID = testing_ground.id_6 KIA_EV6_LATERAL_TESTING_GROUND_VARIANT = "C" -KIA_EV6_FF_GAIN_LEFT = 0.08 -KIA_EV6_FF_GAIN_RIGHT = 0.12 +KIA_EV6_FF_GAIN_LEFT = 0.10 +KIA_EV6_FF_GAIN_RIGHT = 0.14 KIA_EV6_FF_ONSET = 0.08 KIA_EV6_FF_ONSET_WIDTH = 0.04 -KIA_EV6_FF_CUTOFF = 0.82 -KIA_EV6_FF_CUTOFF_WIDTH = 0.18 -KIA_EV6_TRANSITION_SPEED = 11.0 +KIA_EV6_FF_CUTOFF = 1.55 +KIA_EV6_FF_CUTOFF_WIDTH = 0.34 +KIA_EV6_TRANSITION_SPEED = 12.5 KIA_EV6_PHASE_SCALE = 0.09 -KIA_EV6_TURN_IN_BOOST_LEFT = 0.22 -KIA_EV6_TURN_IN_BOOST_RIGHT = 0.34 -KIA_EV6_UNWIND_TAPER_LEFT = 0.48 -KIA_EV6_UNWIND_TAPER_RIGHT = 0.46 +KIA_EV6_TURN_IN_BOOST_LEFT = 0.34 +KIA_EV6_TURN_IN_BOOST_RIGHT = 0.42 +KIA_EV6_UNWIND_TAPER_LEFT = 0.56 +KIA_EV6_UNWIND_TAPER_RIGHT = 0.54 KIA_EV6_FRICTION_MULT = 1.01 KIA_EV6_FRICTION_LAT_RISE = 0.18 KIA_EV6_FRICTION_JERK_RISE = 0.22 -KIA_EV6_TURN_IN_THRESHOLD_REDUCTION_LEFT = 0.10 -KIA_EV6_TURN_IN_THRESHOLD_REDUCTION_RIGHT = 0.24 +KIA_EV6_TURN_IN_THRESHOLD_REDUCTION_LEFT = 0.16 +KIA_EV6_TURN_IN_THRESHOLD_REDUCTION_RIGHT = 0.30 KIA_EV6_UNWIND_THRESHOLD_INCREASE_LEFT = 0.28 KIA_EV6_UNWIND_THRESHOLD_INCREASE_RIGHT = 0.24 -KIA_EV6_TURN_IN_FRICTION_BOOST_LEFT = 0.04 -KIA_EV6_TURN_IN_FRICTION_BOOST_RIGHT = 0.12 +KIA_EV6_TURN_IN_FRICTION_BOOST_LEFT = 0.08 +KIA_EV6_TURN_IN_FRICTION_BOOST_RIGHT = 0.16 KIA_EV6_UNWIND_FRICTION_REDUCTION_LEFT = 0.28 KIA_EV6_UNWIND_FRICTION_REDUCTION_RIGHT = 0.22 KIA_EV6_CENTER_TAPER_MAX = 0.12 diff --git a/selfdrive/controls/tests/test_conditional_chill_mode.py b/selfdrive/controls/tests/test_conditional_chill_mode.py index 720309a48..a4635d3d6 100644 --- a/selfdrive/controls/tests/test_conditional_chill_mode.py +++ b/selfdrive/controls/tests/test_conditional_chill_mode.py @@ -249,7 +249,7 @@ def test_ccm_immediately_exits_chill_when_scene_turns_into_slow_lead(monkeypatch planner, detector, ccm = make_ccm() sm = make_sm() toggles = make_toggles() - monotonic_values = iter([10.0, 10.5, 10.6]) + monotonic_values = iter([10.0, 11.1, 11.2]) monkeypatch.setattr("openpilot.starpilot.controls.lib.conditional_chill_mode.time.monotonic", lambda: next(monotonic_values)) planner.tracking_lead = True @@ -392,6 +392,7 @@ def test_ccm_launch_assist_exits_immediately_if_lead_slows_again(monkeypatch): planner.lead_one.dRel = 18.0 planner.lead_one.vLead = 2.0 planner.lead_one.vRel = 2.0 + planner.lead_one.aLeadK = 0.2 planner.lead_one.radar = True ccm.update(0.0, 25 * CV.MPH_TO_MS, sm, toggles) diff --git a/selfdrive/controls/tests/test_conditional_experimental_mode.py b/selfdrive/controls/tests/test_conditional_experimental_mode.py index 81030502c..78894a615 100644 --- a/selfdrive/controls/tests/test_conditional_experimental_mode.py +++ b/selfdrive/controls/tests/test_conditional_experimental_mode.py @@ -53,6 +53,7 @@ def make_cem(*, model_length: float, model_stopped: bool = False, tracking_lead: def make_sm(traffic_mode_enabled: bool = False): return { + "carState": SimpleNamespace(standstill=False, leftBlinker=False, rightBlinker=False, steeringAngleDeg=0.0), "starpilotCarState": SimpleNamespace(trafficModeEnabled=traffic_mode_enabled), } @@ -65,7 +66,7 @@ def run_stop_light_detector(cem, v_ego, *, steps: int, tracking_lead: bool = Fal def make_update_sm(*, standstill: bool): return { - "carState": SimpleNamespace(standstill=standstill, leftBlinker=False, rightBlinker=False, gasPressed=False), + "carState": SimpleNamespace(standstill=standstill, leftBlinker=False, rightBlinker=False, gasPressed=False, steeringAngleDeg=0.0), "starpilotCarState": SimpleNamespace(trafficModeEnabled=False, dashboardStopSign=0, accelPressed=False), } @@ -360,6 +361,26 @@ def test_standstill_red_light_keeps_exp_on_even_when_model_stopped_clears(monkey assert cem.status_value == conditional_experimental_mode_module.CEStatus["STOP_LIGHT"] +def test_standstill_close_stopped_lead_does_not_drop_red_light_exp(monkeypatch): + cem = make_cem( + model_length=80.0, + model_stopped=False, + lead_status=True, + lead_d_rel=8.0, + lead_v_lead=0.0, + ) + toggles = make_update_toggles() + sm = make_update_sm(standstill=True) + + cem.stop_light_detected = True + monkeypatch.setattr(cem, "stop_sign_and_light", lambda *args, **kwargs: None) + + cem.update(0.0, sm, toggles) + + assert cem.experimental_mode + assert cem.status_value == conditional_experimental_mode_module.CEStatus["STOP_LIGHT"] + + def test_standstill_update_can_activate_exp_from_red_light_detection(monkeypatch): cem = make_cem(model_length=80.0, model_stopped=False) toggles = make_update_toggles() @@ -405,6 +426,42 @@ def test_first_post_standstill_pullaway_frame_does_not_blip_into_exp(monkeypatch assert cem.status_value == conditional_experimental_mode_module.CEStatus["OFF"] +def test_post_stop_speed_trigger_is_suppressed_after_red_light_release(monkeypatch): + cem = make_cem(model_length=80.0, model_stopped=False) + toggles = make_update_toggles() + toggles.conditional_limit = 5.0 * CV.MPH_TO_MS + standstill_sm = make_update_sm(standstill=True) + moving_sm = make_update_sm(standstill=False) + + now = [100.0] + monkeypatch.setattr(conditional_experimental_mode_module.time, "monotonic", lambda: now[0]) + + def hold_red_light(*args, **kwargs): + cem.stop_light_detected = True + + monkeypatch.setattr(cem, "stop_sign_and_light", hold_red_light) + cem.update(0.0, standstill_sm, toggles) + assert cem.experimental_mode + + def clear_red_light(*args, **kwargs): + cem.stop_light_detected = False + cem.stop_light_model_detected = False + + monkeypatch.setattr(cem, "stop_sign_and_light", clear_red_light) + + low_speed_trigger_v_ego = 4.0 * CV.MPH_TO_MS + + now[0] = 100.1 + cem.update(low_speed_trigger_v_ego, moving_sm, toggles) + assert not cem.experimental_mode + assert cem.params_memory.get_int("CEStatus") == conditional_experimental_mode_module.CEStatus["OFF"] + + now[0] = 102.5 + cem.update(low_speed_trigger_v_ego, moving_sm, toggles) + assert cem.experimental_mode + assert cem.status_value == conditional_experimental_mode_module.CEStatus["SPEED"] + + def test_standstill_update_can_activate_exp_from_dashboard_stop_sign(monkeypatch): cem = make_cem(model_length=80.0, model_stopped=False) toggles = make_update_toggles() @@ -473,6 +530,28 @@ def test_standstill_force_stop_keeps_exp_on_even_if_red_light_latch_clears(monke assert cem.status_value == conditional_experimental_mode_module.CEStatus["STOP_LIGHT"] +def test_committed_turn_veto_blocks_stop_light_detection(): + v_ego = 10 * CV.MPH_TO_MS + model_length = v_ego * 4.0 + + cem = make_cem(model_length=model_length) + sm = make_sm() + sm["carState"].rightBlinker = True + sm["carState"].steeringAngleDeg = 70.0 + + run_stop_light_detector(cem, v_ego, steps=20) + assert cem.stop_light_detected + + cem.stop_light_detected = False + cem.stop_light_model_detected = False + cem.stop_light_filter.x = 0.0 + + for _ in range(20): + cem.stop_sign_and_light(v_ego, sm, model_time=7.0) + + assert not cem.stop_light_detected + + def test_standstill_stop_sign_latches_until_pedal_even_after_force_stop_ends(monkeypatch): cem = make_cem(model_length=80.0, model_stopped=False) toggles = make_update_toggles() diff --git a/starpilot/controls/lib/conditional_chill_mode.py b/starpilot/controls/lib/conditional_chill_mode.py index 7f8144201..9fa91e768 100644 --- a/starpilot/controls/lib/conditional_chill_mode.py +++ b/starpilot/controls/lib/conditional_chill_mode.py @@ -56,6 +56,7 @@ class ConditionalChillMode: self._chill_hold_until = 0.0 self._prev_cc_status = None self._launch_active = False + self._launch_forced_exit = False def update(self, v_ego, v_cruise, sm, starpilot_toggles): now = time.monotonic() @@ -95,6 +96,16 @@ class ConditionalChillMode: else: self._candidate_since = 0.0 if not self.experimental_mode: + if self._launch_forced_exit: + self.experimental_mode = True + self._active_auto_status = CCStatus["OFF"] + self.status_value = CCStatus["OFF"] + self._soft_exit_since = 0.0 + self._chill_hold_until = 0.0 + self._launch_forced_exit = False + self._write_status(CCStatus["OFF"]) + return + if self._soft_exit_since == 0.0: self._soft_exit_since = now @@ -118,6 +129,7 @@ class ConditionalChillMode: self._soft_exit_since = 0.0 self._chill_hold_until = 0.0 self._launch_active = False + self._launch_forced_exit = False def _refresh_detector(self, v_ego, sm): detector_toggles = type("DetectorToggles", (), { @@ -216,6 +228,7 @@ class ConditionalChillMode: def _get_launch_status(self, v_ego, sm): if self._launch_active and self._launch_exit_required(v_ego, sm): self._launch_active = False + self._launch_forced_exit = True return CCStatus["OFF"] if self._launch_active: @@ -224,6 +237,7 @@ class ConditionalChillMode: if not self._launch_scene_eligible(v_ego, sm): return CCStatus["OFF"] + self._launch_forced_exit = False self._launch_active = True return self._get_launch_cc_status() diff --git a/starpilot/controls/lib/conditional_experimental_mode.py b/starpilot/controls/lib/conditional_experimental_mode.py index 1bbbef871..ee66e189c 100644 --- a/starpilot/controls/lib/conditional_experimental_mode.py +++ b/starpilot/controls/lib/conditional_experimental_mode.py @@ -57,6 +57,9 @@ class ConditionalExperimentalMode: SLOW_LEAD_FORCE_CLEAR_TIME = 0.75 SLOW_LEAD_MIN_CLOSING_SPEED = 0.75 SLOW_LEAD_CLEAR_FASTER_FACTOR = 0.5 + POST_STOP_SPEED_TRIGGER_SUPPRESS_TIME = 2.0 + TURN_STOP_LIGHT_VETO_MAX_SPEED = 15 * CV.MPH_TO_MS + TURN_STOP_LIGHT_VETO_STEERING_ANGLE = 45.0 # ===== END TUNING PARAMETERS ===== @@ -102,11 +105,21 @@ class ConditionalExperimentalMode: self.mode_hold_until = 0.0 self.mode_false_since = 0.0 self._prev_ce_status = None - self.close_stopped_lead_since = 0.0 + self.prev_standstill = False + self.prev_standstill_stop_hold = False + self.post_stop_speed_trigger_suppress_until = 0.0 def update(self, v_ego, sm, starpilot_toggles): now = time.monotonic() standstill = bool(sm["carState"].standstill) + current_standstill_stop_hold = False + released_standstill_stop_hold = self.prev_standstill and self.prev_standstill_stop_hold and not standstill + + if released_standstill_stop_hold: + self.post_stop_speed_trigger_suppress_until = now + self.POST_STOP_SPEED_TRIGGER_SUPPRESS_TIME + self.mode_hold_until = 0.0 + self.mode_false_since = 0.0 + self.prev_experimental_mode = False if not standstill: self.standstill_stop_reason = None @@ -143,6 +156,7 @@ class ConditionalExperimentalMode: # immediately release to CHILL when the model clears the stop. self.stop_sign_and_light(v_ego, sm, starpilot_toggles.conditional_model_stop_time) standstill_stop_hold = self.get_standstill_stop_hold(sm) + current_standstill_stop_hold = standstill_stop_hold self.experimental_mode = standstill_stop_hold self.prev_experimental_mode = self.experimental_mode @@ -160,18 +174,10 @@ class ConditionalExperimentalMode: self.stop_light_detected &= not is_manual_ce_status(self.status_value) self.stop_light_filter.x = 0 - # At standstill behind a close, stopped lead, prefer Chill over CEM. - # Why: CEM is slow to release when the lead pulls away (waits on stop-light - # filter + STOP_LIGHT_DETECTED_HOLD_TIME). Chill reacts to lead departure faster. - STANDSTILL_LEAD_OVERRIDE_MAX_DISTANCE = 15.0 # meters - STANDSTILL_LEAD_OVERRIDE_MAX_SPEED = 1.0 # m/s - # Persistence required before handing off CEM->Chill. Cross-traffic (cars passing - # perpendicular in front) briefly registers as a close, stopped lead and would - # otherwise flap CEM out of EXP. Real queue-mates persist much longer than this. - STANDSTILL_LEAD_OVERRIDE_PERSIST_TIME = 0.5 # seconds + self.prev_standstill = standstill + self.prev_standstill_stop_hold = current_standstill_stop_hold def get_standstill_stop_hold(self, sm): - now = time.monotonic() dash_stop_sign = ( bool(getattr(self.starpilot_planner.starpilot_vcruise, "stop_sign_confirmed", False)) or bool(getattr(sm["starpilotCarState"], "dashboardStopSign", 0) > 0) @@ -182,7 +188,6 @@ class ConditionalExperimentalMode: if pedal_override or not bool(sm["carState"].standstill): self.standstill_stop_reason = None - self.close_stopped_lead_since = 0.0 return False if dash_stop_sign: @@ -194,29 +199,14 @@ class ConditionalExperimentalMode: self.standstill_stop_reason = None if self.standstill_stop_reason == "sign": - self.close_stopped_lead_since = 0.0 return True - lead = getattr(self.starpilot_planner, "lead_one", None) - close_stopped_lead = bool( - lead is not None and - getattr(lead, "status", False) and - float(getattr(lead, "dRel", float("inf"))) < self.STANDSTILL_LEAD_OVERRIDE_MAX_DISTANCE and - float(getattr(lead, "vLead", float("inf"))) < self.STANDSTILL_LEAD_OVERRIDE_MAX_SPEED - ) - if close_stopped_lead: - if self.close_stopped_lead_since == 0.0: - self.close_stopped_lead_since = now - if (now - self.close_stopped_lead_since) >= self.STANDSTILL_LEAD_OVERRIDE_PERSIST_TIME: - return False - else: - self.close_stopped_lead_since = 0.0 - return bool(self.stop_light_detected or force_stop_active or model_stopped) def check_conditions(self, v_ego, sm, starpilot_toggles): - below_speed = starpilot_toggles.conditional_limit > v_ego >= 1 and not self.starpilot_planner.starpilot_following.following_lead - below_speed_with_lead = starpilot_toggles.conditional_limit_lead > v_ego >= 1 and self.starpilot_planner.starpilot_following.following_lead + speed_trigger_suppressed = time.monotonic() < self.post_stop_speed_trigger_suppress_until + below_speed = not speed_trigger_suppressed and starpilot_toggles.conditional_limit > v_ego >= 1 and not self.starpilot_planner.starpilot_following.following_lead + below_speed_with_lead = not speed_trigger_suppressed and starpilot_toggles.conditional_limit_lead > v_ego >= 1 and self.starpilot_planner.starpilot_following.following_lead if below_speed or below_speed_with_lead: self.status_value = CEStatus["SPEED"] return True @@ -329,6 +319,28 @@ class ConditionalExperimentalMode: self.slow_lead_continuity_until = 0.0 self.prev_tracking_lead = tracking_lead + def reset_stop_light_state(self): + self.stop_light_filter.x = 0 + self.stop_light_detected = False + self.stop_light_model_detected = False + self.stop_light_detected_hold_until = 0.0 + self.lead_clear_filter.x = 0 + self.stop_approach_hold_until = 0.0 + + def in_committed_turn_scene(self, v_ego, sm): + car_state = sm["carState"] + if bool(getattr(car_state, "standstill", False)) or v_ego > self.TURN_STOP_LIGHT_VETO_MAX_SPEED: + return False + + if not (bool(getattr(car_state, "leftBlinker", False)) or bool(getattr(car_state, "rightBlinker", False))): + return False + + steering_angle = abs(float(getattr(car_state, "steeringAngleDeg", 0.0))) + return bool( + steering_angle >= self.TURN_STOP_LIGHT_VETO_STEERING_ANGLE or + getattr(self.starpilot_planner, "driving_in_curve", False) + ) + def stop_sign_and_light(self, v_ego, sm, model_time): now = time.monotonic() @@ -341,6 +353,10 @@ class ConditionalExperimentalMode: self.stop_light_detected = True return + if self.in_committed_turn_scene(v_ego, sm): + self.reset_stop_light_state() + return + if not sm["starpilotCarState"].trafficModeEnabled: speed_mph = v_ego * CV.MS_TO_MPH # Convert m/s to mph @@ -370,11 +386,7 @@ class ConditionalExperimentalMode: # 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.stop_light_detected_hold_until = 0.0 - self.lead_clear_filter.x = 0 + self.reset_stop_light_state() return # Adjust model time with interp boost and gradual cap @@ -444,9 +456,4 @@ class ConditionalExperimentalMode: (hold_context_ok and now < self.stop_light_detected_hold_until) ) else: - self.stop_light_filter.x = 0 - self.stop_light_detected = False - self.stop_light_model_detected = False - self.stop_light_detected_hold_until = 0.0 - self.lead_clear_filter.x = 0 - self.stop_approach_hold_until = 0.0 + self.reset_stop_light_state()