mirror of
https://github.com/firestar5683/StarPilot.git
synced 2026-06-27 17:42:04 +08:00
Lite Brite
This commit is contained in:
firestar5683
@@ -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
|
||||
|
||||
@@ -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:
|
||||
|
||||
@@ -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:
|
||||
|
||||
@@ -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()
|
||||
|
||||
@@ -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]
|
||||
|
||||
@@ -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
|
||||
|
||||
@@ -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)
|
||||
|
||||
@@ -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()
|
||||
|
||||
@@ -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()
|
||||
|
||||
|
||||
@@ -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()
|
||||
|
||||
Reference in New Issue
Block a user