diff --git a/openpilot/starpilot/speed_limits/README.md b/openpilot/starpilot/speed_limits/README.md index f675e9ee1b..ff33b71453 100644 --- a/openpilot/starpilot/speed_limits/README.md +++ b/openpilot/starpilot/speed_limits/README.md @@ -201,6 +201,7 @@ the gas above the accepted limit plus offset lets cruise hold the fastest speed reach, up to SET. **Use your SET speed** targets SET. **Resume the adjusted limit** returns to the accepted limit plus offset on release. The override clears when cruise disengages, the adjusted limit catches up, or the driver explicitly accepts or adopts a limit. +Stopping while cruise stays engaged preserves the override. After an explicit choice, the current accelerator press cannot start another override. Releasing and pressing again can start one. SET and the accepted limit are separate from the override. Lead, curve and diff --git a/openpilot/starpilot/speed_limits/SPEED_DOMAIN.md b/openpilot/starpilot/speed_limits/SPEED_DOMAIN.md index 4077a97eee..c9830c0880 100644 --- a/openpilot/starpilot/speed_limits/SPEED_DOMAIN.md +++ b/openpilot/starpilot/speed_limits/SPEED_DOMAIN.md @@ -12,6 +12,11 @@ unavailable target/pair produces `UNAVAILABLE`; malformed values produce speed; resolution clamps each delta to `max(cluster - raw, 0)`. Only a `VALID` resolution supplies a planner coordinate. +Before entering this domain, the runtime floors finite negative ego-speed estimates +to zero when fresh `carState` evidence reports standstill. The speed filter can +undershoot zero after the wheels stop. Negative estimates while moving, nonfinite +values and stale vehicle data remain unavailable; the original messages are unchanged. + A finite negative offset that would make the target nonpositive in cluster or planner coordinates is ignored for that limit. The unadjusted accepted limit is used instead; the result stays within the ordinary cruise-speed bounds. diff --git a/openpilot/starpilot/speed_limits/runtime.py b/openpilot/starpilot/speed_limits/runtime.py index 3160aa9cbf..98ce95d4aa 100644 --- a/openpilot/starpilot/speed_limits/runtime.py +++ b/openpilot/starpilot/speed_limits/runtime.py @@ -352,8 +352,11 @@ class Runtime: authority = replace(authority, accelerator_override=bool(CP.openpilotLongitudinalControl and long_enabled and car_state.gasPressed)) selected = float(car_state.vCruise) if car_fresh and _number(car_state.vCruise) else 0.0 selected_cluster = float(car_state.vCruiseCluster) if car_fresh and _number(car_state.vCruiseCluster) else None - ego_valid = car_fresh and _number(car_state.vEgo) and _number(car_state.vEgoCluster) - ego = (sd.SpeedPair(sd.DomainStatus.VALID, float(car_state.vEgo), float(car_state.vEgoCluster), self.session_id, now_ns) + # Filtered ego speed can undershoot zero after the wheels stop. + ego_speeds = (car_state.vEgo, car_state.vEgoCluster) + ego_valid = car_fresh and all(type(speed) in (int, float) and math.isfinite(speed) and + (car_state.standstill or speed >= 0.) for speed in ego_speeds) + ego = (sd.SpeedPair(sd.DomainStatus.VALID, *(max(0., speed) for speed in ego_speeds), self.session_id, now_ns) if ego_valid else sd.SpeedPair(sd.DomainStatus.STALE if sm.logMonoTime.get("carState", 0) else sd.DomainStatus.UNKNOWN)) host = comp.HostState(bool(CP.openpilotLongitudinalControl), host_long, bool(selfdrive.enabled and selfdrive_fresh), bool(controls.forceDecel) if controls_fresh else True, selected, selected_cluster) diff --git a/openpilot/starpilot/speed_limits/tests/test_core_flow.py b/openpilot/starpilot/speed_limits/tests/test_core_flow.py index b863ffe925..5a4f08e946 100644 --- a/openpilot/starpilot/speed_limits/tests/test_core_flow.py +++ b/openpilot/starpilot/speed_limits/tests/test_core_flow.py @@ -14,6 +14,8 @@ from openpilot.starpilot.speed_limits.speed_domain import OffsetBand, OffsetSche from openpilot.starpilot.speed_limits.tests import test_runtime_replay as replay START = 2_000_000_000 +# Observed stop jitter, plus a larger normal filter undershoot. +STOPPED_SPEEDS = (0., -0.0005728941, 0.0000374307, -0.0000785884, -.08, 0.) @pytest.fixture @@ -182,15 +184,19 @@ def test_wheel_override_survives_engaged_stop(drive, cruise_standstill): stopped = sm['carState'].cruiseState if cruise_standstill else sm['carState'] stopped.standstill = True - sm['carState'].vEgo = sm['carState'].vEgoCluster = 0. sm['controlsState'].longControlState = 'stopping' - held = tick(runtime, sm, cp, 2) - assert held.message.slcState.overrideBasis == 'wheel' - assert held.message.slcState.wheelOverrideSince == 0 + for step, speed in enumerate((0.,) if cruise_standstill else STOPPED_SPEEDS, 2): + sm['carState'].vEgo = sm['carState'].vEgoCluster = speed + held = tick(runtime, sm, cp, step) + assert held.message.slcState.overrideBasis == 'wheel' + assert held.message.slcState.effectiveClusterTarget == pytest.approx(91. / 3.6) + assert held.message.slcState.wheelOverrideSince == 0 + assert held.result.ceiling is None stopped.standstill = False + sm['carState'].vEgo = sm['carState'].vEgoCluster = 1. sm['controlsState'].longControlState = 'pid' - assert tick(runtime, sm, cp, 3).message.slcState.overrideBasis == 'wheel' + assert tick(runtime, sm, cp, step + 1).message.slcState.overrideBasis == 'wheel' def test_wheel_override_survives_gas_handoff_without_latching_pedal_speed(drive): @@ -213,11 +219,15 @@ def test_wheel_override_survives_gas_handoff_without_latching_pedal_speed(drive) @pytest.mark.parametrize('missing', ['absent', 'unknown', 'stale']) -def test_source_gap_uses_the_previous_limit_and_current_offset(drive, missing): +@pytest.mark.parametrize('stopped', [False, True]) +def test_source_gap_uses_the_previous_limit_and_current_offset(drive, missing, stopped): sm, cp = drive runtime = controller(2.) first = tick(runtime, sm, cp, 0) assert first.result.ceiling.speed_mps == 27. + if stopped: + sm['carState'].standstill = True + sm['carState'].vEgo = sm['carState'].vEgoCluster = -.08 sm['slcDashboardObservation'].status = missing runtime.update_offsets(OffsetSchedule((OffsetBand(0., None, 4.),))) gap = tick(runtime, sm, cp, 1) @@ -324,16 +334,31 @@ def test_gas_above_limit_holds_the_selected_override_after_release(drive, mode, planner.update(sm, cruise_ceiling=released.result.ceiling) assert planner.last_cruise_ceiling_status == ('applied' if mode is PedalOverride.SPEED_REACHED else 'above_driver') + sm['carState'].standstill = True + sm['controlsState'].longControlState = 'stopping' + for step, speed in enumerate(STOPPED_SPEEDS, 5): + sm['carState'].vEgo = sm['carState'].vEgoCluster = speed + held = tick(runtime, sm, cp, step) + assert held.message.slcState.overrideBasis == 'persistent' + assert held.result.ceiling == released.result.ceiling + assert held.message.slcState.effectiveClusterTarget == pytest.approx(target) + assert sm['carState'].vCruise == sm['carState'].vCruiseCluster == 100. + + sm['carState'].standstill = False + sm['carState'].vEgo = sm['carState'].vEgoCluster = 1. + sm['controlsState'].longControlState = 'pid' + assert tick(runtime, sm, cp, step + 1).result.ceiling == released.result.ceiling sm['carControl'].enabled = sm['carControl'].longActive = sm['selfdriveState'].enabled = False - assert tick(runtime, sm, cp, 5).result.ceiling is None + assert tick(runtime, sm, cp, step + 2).result.ceiling is None sm['carControl'].enabled = sm['carControl'].longActive = sm['selfdriveState'].enabled = True - resumed = tick(runtime, sm, cp, 6) + resumed = tick(runtime, sm, cp, step + 3) assert resumed.result.ceiling.speed_mps == 25. assert not resumed.message.slcState.driverOverrideActive -@pytest.mark.parametrize('clear_with', ['lower SET', 'higher limit', 'explicit accept']) -def test_override_clears_when_the_driver_or_limit_takes_back_control(drive, clear_with): +@pytest.mark.parametrize('change', ['lower SET', 'higher limit', 'accept', 'reject', 'adopt']) +@pytest.mark.parametrize('stopped', [False, True]) +def test_driver_and_limit_changes_preserve_override_rules(drive, change, stopped): sm, cp = drive runtime = controller() tick(runtime, sm, cp, 0) @@ -341,21 +366,30 @@ def test_override_clears_when_the_driver_or_limit_takes_back_control(drive, clea sm['carState'].gasPressed = True assert tick(runtime, sm, cp, 1).result.ceiling.speed_mps == pytest.approx(100. / 3.6) sm['carState'].gasPressed = False + if stopped: + sm['carState'].standstill = True + sm['carState'].vEgo = sm['carState'].vEgoCluster = -.08 action = None expected = 25. - if clear_with == 'lower SET': + if change == 'lower SET': sm['carState'].vCruise = sm['carState'].vCruiseCluster = 90. - elif clear_with == 'higher limit': + elif change == 'higher limit': sm['slcDashboardObservation'].speedMps = expected = 28. else: runtime.settings = replace(runtime.settings, acceptance=acc.Policy()) sm['slcDashboardObservation'].speedMps = expected = 20. pending = tick(runtime, sm, cp, 2).message.slcState assert pending.hasPending and pending.driverOverrideActive - action = Action(runtime.session_id, 1, pending.decisionId, pending.presentationId, 'accept') - cleared = tick(runtime, sm, cp, 3, request=action) - assert cleared.result.ceiling.speed_mps == expected - assert not cleared.message.slcState.driverOverrideActive + action = Action(runtime.session_id, 1, pending.decisionId, pending.presentationId, change) + if change == 'reject': + expected = 100. / 3.6 + changed = tick(runtime, sm, cp, 3, request=action) + assert changed.result.ceiling.speed_mps == expected + assert changed.message.slcState.driverOverrideActive == (change == 'reject') + assert not changed.message.slcState.hasPending + if change == 'reject': + assert changed.message.slcState.acceptedSpeedLimit == 25. + assert changed.message.slcState.speedLimit == 20. sm['carState'].vCruise = sm['carState'].vCruiseCluster = 100. assert tick(runtime, sm, cp, 4).result.ceiling.speed_mps == expected @@ -442,6 +476,60 @@ def test_raw_cluster_conversion_is_applied_once_including_during_source_loss(dri assert tick(runtime, sm, cp, 1).result.ceiling.speed_mps == 26.5 +@pytest.mark.parametrize('stopped,raw,cluster,cap', [ + (True, 0., 0., 25.), + (True, -.08, -.08, 25.), + (True, -.08, .1, 24.9), + (True, .1, -.08, 25.), + (True, .1, .2, 24.9), + (False, -.08, 0., None), + (False, 0., -.08, None), + (True, float('nan'), 0., None), + (True, 0., float('nan'), None), + (True, float('inf'), 0., None), + (True, 0., float('inf'), None), + (True, float('-inf'), 0., None), + (True, 0., float('-inf'), None), +]) +def test_ego_coordinates_are_normalized_only_at_standstill(drive, stopped, raw, cluster, cap): + sm, cp = drive + runtime = controller() + tick(runtime, sm, cp, 0) + sm['carState'].standstill = stopped + sm['carState'].vEgo, sm['carState'].vEgoCluster = raw, cluster + output = tick(runtime, sm, cp, 1) + assert not output.message.slcState.driverOverrideActive + if cap is None: + assert output.result.ceiling is None + else: + assert output.result.ceiling.speed_mps == pytest.approx(cap) + assert (sm['carState'].vEgo, sm['carState'].vEgoCluster) == pytest.approx((raw, cluster), nan_ok=True) + + +@pytest.mark.parametrize('unavailable', ['stale', 'invalid', 'not alive', 'CAN timeout', 'cruise standstill only']) +def test_standstill_normalization_requires_fresh_vehicle_evidence(drive, unavailable): + sm, cp = drive + runtime = controller() + tick(runtime, sm, cp, 0) + sm['carState'].standstill = unavailable != 'cruise standstill only' + sm['carState'].cruiseState.standstill = True + sm['carState'].vEgo = sm['carState'].vEgoCluster = -.08 + now = START + 50_000_000 + sm.advance(now) + if unavailable == 'stale': + sm.logMonoTime['carState'] = now - 1_000_000_000 + elif unavailable == 'invalid': + sm.valid['carState'] = False + elif unavailable == 'not alive': + sm.alive['carState'] = False + elif unavailable == 'CAN timeout': + sm['carState'].canTimeout = True + output = runtime.step(sm, cp, now_ns=now) + assert not output.result.errors + assert output.result.ceiling is None + assert not output.message.slcState.driverOverrideActive + + @pytest.mark.parametrize('pcm', [False, True]) def test_factory_longitudinal_never_gets_an_slc_cap(drive, pcm): sm, cp = drive