From 72f04816c33aeb3b4a761eaecb9939a1a69b44b2 Mon Sep 17 00:00:00 2001 From: firestar5683 <168790843+firestar5683@users.noreply.github.com> Date: Sat, 31 Oct 2026 12:04:00 -0500 Subject: [PATCH] Add metric-aware extra stopped-lead spacing --- .../lib/longitudinal_mpc_lib/long_mpc.py | 10 +- .../controls/lib/longitudinal_planner.py | 1 + .../starpilot/galaxy/tests/test_settings.py | 2 +- .../longitudinal/profile_document.py | 25 ++- .../starpilot/longitudinal/profile_runtime.py | 6 +- .../tests/test_extra_stopped_gap.py | 192 ++++++++++++++++++ .../tests/test_profile_runtime.py | 3 +- .../tests/test_selected_profiles.py | 1 + .../starpilot/ui/feature_settings_owner.py | 37 +++- prebuilt.json | 2 +- 10 files changed, 264 insertions(+), 15 deletions(-) create mode 100644 openpilot/starpilot/longitudinal/tests/test_extra_stopped_gap.py diff --git a/openpilot/selfdrive/controls/lib/longitudinal_mpc_lib/long_mpc.py b/openpilot/selfdrive/controls/lib/longitudinal_mpc_lib/long_mpc.py index 065138f646..2ebf317f38 100755 --- a/openpilot/selfdrive/controls/lib/longitudinal_mpc_lib/long_mpc.py +++ b/openpilot/selfdrive/controls/lib/longitudinal_mpc_lib/long_mpc.py @@ -538,7 +538,8 @@ class LongitudinalMpc: return lead_xv def update(self, radarstate, personality=log.LongitudinalPersonality.standard, - *, follow_seconds: float | None = None, acceleration_max: float | None = None, stop_line_m: float | None = None, modelV2=None): + *, follow_seconds: float | None = None, acceleration_max: float | None = None, stop_line_m: float | None = None, + modelV2=None, extra_stopped_gap_m: float = 0.0): t_follow = get_T_FOLLOW(personality) if follow_seconds is not None and math.isfinite(follow_seconds) and 0.5 <= follow_seconds <= 3.0: t_follow = follow_seconds @@ -555,6 +556,13 @@ class LongitudinalMpc: lead_0_obstacle = lead_xv_0[:,0] + get_stopped_equivalence_factor(lead_xv_0[:,1]) lead_1_obstacle = lead_xv_1[:,0] + get_stopped_equivalence_factor(lead_xv_1[:,1]) + if type(extra_stopped_gap_m) in (int, float) and math.isfinite(extra_stopped_gap_m) and 0.0 < extra_stopped_gap_m <= 3.048: + # Shift only lead obstacles; physical trajectories still own collision warnings. + if radarstate.leadOne.present: + lead_0_obstacle -= extra_stopped_gap_m * np.clip(1.0 - lead_xv_0[:, 1] / 5.0, 0.0, 1.0) + if radarstate.leadTwo.present: + lead_1_obstacle -= extra_stopped_gap_m * np.clip(1.0 - lead_xv_1[:, 1] / 5.0, 0.0, 1.0) + x_obstacles = np.column_stack([lead_0_obstacle, lead_1_obstacle]) self.source = MPC_SOURCES[np.argmin(x_obstacles[0])] if stop_line_m is not None and math.isfinite(stop_line_m) and 0.0 <= stop_line_m <= 500.0: diff --git a/openpilot/selfdrive/controls/lib/longitudinal_planner.py b/openpilot/selfdrive/controls/lib/longitudinal_planner.py index a174612641..fa96601a55 100755 --- a/openpilot/selfdrive/controls/lib/longitudinal_planner.py +++ b/openpilot/selfdrive/controls/lib/longitudinal_planner.py @@ -271,6 +271,7 @@ class LongitudinalPlanner: follow_seconds=follow if follow is not None else (self.last_profile.follow_seconds if self.last_profile is not None else None), acceleration_max=acceleration_cap, + extra_stopped_gap_m=(selected_profiles.extra_stopped_gap_m if profile_eligible and selected_profiles is not None else 0.0), stop_line_m=self.force_stop_plan.obstacle_m, modelV2=qualified_model_leads(sm, sample_now_ns, self.model_lead_history)) # MPC has selected the actual follow time for this model cycle. Its solve diff --git a/openpilot/starpilot/galaxy/tests/test_settings.py b/openpilot/starpilot/galaxy/tests/test_settings.py index 4eed2170b6..2a12eae913 100644 --- a/openpilot/starpilot/galaxy/tests/test_settings.py +++ b/openpilot/starpilot/galaxy/tests/test_settings.py @@ -902,7 +902,7 @@ class SettingsGatewayTest(unittest.TestCase): self.assertTrue(readable) assert raw is not None document = json.loads(raw) - self.assertEqual(document["schemaVersion"], 5) + self.assertEqual(document["schemaVersion"], 6) self.assertEqual(document["selectedDecelerationProfile"], "sport") self.assertNotIn("globalBrakingResponse", document) self.assertEqual(document["profiles"]["aggressive"]["braking"], previous) diff --git a/openpilot/starpilot/longitudinal/profile_document.py b/openpilot/starpilot/longitudinal/profile_document.py index 9a55255e31..8a41fd3588 100644 --- a/openpilot/starpilot/longitudinal/profile_document.py +++ b/openpilot/starpilot/longitudinal/profile_document.py @@ -7,7 +7,8 @@ import math import numbers PERSONALITY_PROFILES_PARAM = "LongitudinalPersonalityProfiles" -PROFILE_SCHEMA_VERSION = 5 +PROFILE_SCHEMA_VERSION = 6 +MAX_EXTRA_STOPPED_GAP_M = 3.048 GLOBAL_BRAKING_RESPONSES = ("standard", "eco", "sport") DEFAULT_DECELERATION_PROFILE = "eco" PERSONALITY_IDS = ("traffic", "aggressive", "standard", "relaxed") @@ -204,7 +205,8 @@ def default_personality_profiles(ev_tuning: bool, truck_tuning: bool = False) -> def profile_document(profiles: dict[str, dict], *, enabled: bool, global_braking_response: str = DEFAULT_DECELERATION_PROFILE, - selected_acceleration_profile: str = "dom_default", selected_deceleration_profile: str | None = None) -> dict: + selected_acceleration_profile: str = "dom_default", selected_deceleration_profile: str | None = None, + extra_stopped_gap_m: float = 0.0) -> dict: if type(enabled) is not bool: raise ValueError("enabled must be a JSON boolean") if global_braking_response not in GLOBAL_BRAKING_RESPONSES: @@ -215,7 +217,10 @@ def profile_document(profiles: dict[str, dict], *, enabled: bool, global_braking raise ValueError("invalid selected acceleration profile") if selected_deceleration_profile not in ("dom_default", *GLOBAL_BRAKING_RESPONSES): raise ValueError("invalid selected deceleration profile") + if type(extra_stopped_gap_m) not in (int, float) or not math.isfinite(extra_stopped_gap_m) or not 0 <= extra_stopped_gap_m <= MAX_EXTRA_STOPPED_GAP_M: + raise ValueError("invalid extra stopped-lead gap") return { + "extraStoppedGapM": float(extra_stopped_gap_m), "schemaVersion": PROFILE_SCHEMA_VERSION, "enabled": enabled, "selectedAccelerationProfile": selected_acceleration_profile, @@ -331,6 +336,8 @@ def _strict_document( keys.add("globalBrakingResponse") elif schema_version >= 5: keys.update(("selectedAccelerationProfile", "selectedDecelerationProfile")) + if schema_version >= 6: + keys.add("extraStoppedGapM") if not isinstance(decoded, dict) or set(decoded) != keys: return None if type(decoded["schemaVersion"]) is not int or decoded["schemaVersion"] != schema_version: @@ -343,6 +350,10 @@ def _strict_document( if schema_version >= 5 and (decoded["selectedAccelerationProfile"] not in ("dom_default", "standard", "eco", "sport", "sport_plus") or decoded["selectedDecelerationProfile"] not in ("dom_default", *GLOBAL_BRAKING_RESPONSES)): return None + if schema_version >= 6 and (type(decoded["extraStoppedGapM"]) not in (int, float) or + not math.isfinite(decoded["extraStoppedGapM"]) or + not 0 <= decoded["extraStoppedGapM"] <= MAX_EXTRA_STOPPED_GAP_M): + return None raw_profiles = decoded["profiles"] if not isinstance(raw_profiles, dict) or set(raw_profiles) != set(PERSONALITY_IDS): return None @@ -368,6 +379,7 @@ def _strict_document( profile[category] = validated profiles[personality] = profile return { + **({"extraStoppedGapM": float(decoded["extraStoppedGapM"])} if schema_version >= 6 else {}), "schemaVersion": schema_version, "enabled": decoded["enabled"], **({"globalBrakingResponse": decoded["globalBrakingResponse"]} if schema_version == 4 else {}), @@ -389,7 +401,7 @@ def strict_profile_document(raw_document) -> dict | None: # meaning; adopting a speed-dependent preset requires an explicit selection. decoded = _decode_json(raw_document) version = decoded.get("schemaVersion") if isinstance(decoded, dict) else None - if type(version) is not int or version not in (2, 3, 4, PROFILE_SCHEMA_VERSION): + if type(version) is not int or version not in (2, 3, 4, 5, PROFILE_SCHEMA_VERSION): return None document = _strict_document( decoded, @@ -408,6 +420,9 @@ def strict_profile_document(raw_document) -> dict | None: profile[category]["legacyActivation"] = True document = profile_document(document["profiles"], enabled=document["enabled"], global_braking_response=document.get("globalBrakingResponse", "standard")) + if document is not None and version == 5: + document["schemaVersion"] = PROFILE_SCHEMA_VERSION + document["extraStoppedGapM"] = 0.0 return document @@ -482,11 +497,11 @@ def load_personality_profiles(raw_document, ev_tuning: bool, truck_tuning: bool def serialize_personality_profiles(profiles, ev_tuning: bool, truck_tuning: bool = False, *, enabled: bool, global_braking_response: str = DEFAULT_DECELERATION_PROFILE, selected_acceleration_profile: str = "dom_default", - selected_deceleration_profile: str | None = None) -> str: + selected_deceleration_profile: str | None = None, extra_stopped_gap_m: float = 0.0) -> str: del ev_tuning, truck_tuning document = profile_document(profiles, enabled=enabled, global_braking_response=global_braking_response, selected_acceleration_profile=selected_acceleration_profile, - selected_deceleration_profile=selected_deceleration_profile) + selected_deceleration_profile=selected_deceleration_profile, extra_stopped_gap_m=extra_stopped_gap_m) canonical = strict_profile_document(document) if canonical is None: raise ValueError("Longitudinal personality profiles must be complete and valid.") diff --git a/openpilot/starpilot/longitudinal/profile_runtime.py b/openpilot/starpilot/longitudinal/profile_runtime.py index 5e2ed3aa8d..284d6e9a4e 100644 --- a/openpilot/starpilot/longitudinal/profile_runtime.py +++ b/openpilot/starpilot/longitudinal/profile_runtime.py @@ -267,6 +267,7 @@ class SelectedProfileTuning: acceleration_max: float | None = None cruise_brake_magnitude: float | None = None braking_style: str = 'standard' + extra_stopped_gap_m: float = 0.0 @dataclass(frozen=True) @@ -300,7 +301,8 @@ def read_global_powertrain_preset(params, CP) -> GlobalPowertrainPreset | None: def selected_profiles_requested(document: dict | None) -> bool: if document is None: return False - if document['selectedAccelerationProfile'] != 'dom_default' or document['selectedDecelerationProfile'] != 'dom_default': + if (document.get('extraStoppedGapM', 0.) > 0 or document['selectedAccelerationProfile'] != 'dom_default' or + document['selectedDecelerationProfile'] != 'dom_default'): return True return any(config['preset'] not in ('dom_default', 'selected_profile') and not config.get('legacyActivation', False) for profile in document['profiles'].values() for category, config in profile.items() if category != 'following') @@ -353,7 +355,7 @@ def resolve_selected_profiles(document: dict | None, personality, v_ego: float, else: braking = min(max(value, 0.35 if traffic_mode else 0.5), abs(ACCEL_MIN)) style = preset if preset in ('eco', 'sport') else 'standard' - return SelectedProfileTuning(acceleration, braking, style) + return SelectedProfileTuning(acceleration, braking, style, document.get("extraStoppedGapM", 0.)) class ProfileHost: diff --git a/openpilot/starpilot/longitudinal/tests/test_extra_stopped_gap.py b/openpilot/starpilot/longitudinal/tests/test_extra_stopped_gap.py new file mode 100644 index 0000000000..d049174a14 --- /dev/null +++ b/openpilot/starpilot/longitudinal/tests/test_extra_stopped_gap.py @@ -0,0 +1,192 @@ +import copy +from dataclasses import replace +from types import SimpleNamespace as NS + +import numpy as np +import pytest +from openpilot.selfdrive.controls.lib.longitudinal_mpc_lib.long_mpc import LongitudinalMpc +from openpilot.starpilot.longitudinal.profile_document import ( + default_personality_profiles, profile_document, migrate_profile_document, strict_profile_document, +) +from openpilot.starpilot.longitudinal.profile_runtime import ProfileHost, selected_profiles_requested +from openpilot.starpilot.ui.feature_settings_state import row_change +from openpilot.starpilot.ui.tests import test_feature_settings_owner as owner_tests + + +@pytest.fixture +def owner(): + fixture = owner_tests.FeatureSettingsOwnerTests() + fixture.setUp() + try: + yield fixture + finally: + fixture.doCleanups() + + +def document(gap=0.): + return profile_document(default_personality_profiles(False), enabled=False, selected_acceleration_profile='dom_default', + selected_deceleration_profile='dom_default', extra_stopped_gap_m=gap) + + +def test_migration_keeps_all_existing_settings_and_default_output(): + old = document() + old['schemaVersion'] = 5 + del old['extraStoppedGapM'] + before = copy.deepcopy(old) + migrated = migrate_profile_document(old) + assert old == before + assert migrated == {**old, 'schemaVersion': 6, 'extraStoppedGapM': 0.} + assert not selected_profiles_requested(migrated) + assert selected_profiles_requested(document(1.)) + + +@pytest.mark.parametrize('bad', [-1, 3.049, float('inf'), float('nan'), True, '1', None]) +def test_invalid_gap_cannot_activate(bad): + raw = document() + raw['extraStoppedGapM'] = bad + assert strict_profile_document(raw) is None + + +def request(row, value): + gesture = row_change(row, 1) + assert gesture is not None + return replace(gesture, value=value) + + +def test_metric_display_and_edits_preserve_physical_gap_and_other_profiles(owner): + row = owner._row('profiles', 'profile:stopped_gap') + assert row.available and row.value == '0.0' and row.unit == 'ft' and row.maximum == 10. + assert owner.owner.apply(request(row, '5')) + stored = owner.params.get('LongitudinalPersonalityProfiles') + assert stored['extraStoppedGapM'] == pytest.approx(1.524) + owner.params.put_bool('IsMetric', True, block=True) + metric = owner._row('profiles', 'profile:stopped_gap') + assert metric.unit == 'm' and float(metric.value) == pytest.approx(1.524) + assert owner.params.get('LongitudinalPersonalityProfiles') == stored + assert owner.owner.apply(request(metric, '2')) + changed = owner.params.get('LongitudinalPersonalityProfiles') + assert changed == {**stored, 'extraStoppedGapM': 2.} + owner.params.put_bool('IsMetric', False, block=True) + imperial = owner._row('profiles', 'profile:stopped_gap') + assert float(imperial.value) == pytest.approx(2. / .3048, abs=.001) + assert not owner.owner.apply(request(imperial, '-1')) + assert not owner.owner.apply(request(imperial, '10.1')) + assert owner.params.get('LongitudinalPersonalityProfiles') == changed + braking = owner._row('profiles', 'profile:global_braking') + assert owner.owner.apply(request(braking, 'Sport')) + assert owner.params.get('LongitudinalPersonalityProfiles')['extraStoppedGapM'] == 2. + + +def test_changed_units_or_vehicle_or_permission_rejects_old_press(owner): + row = owner._row('profiles', 'profile:stopped_gap') + press = request(row, '5') + owner.params.put_bool('IsMetric', True, block=True) + assert not owner.owner.apply(press) + row = owner._row('profiles', 'profile:stopped_gap') + owner.fingerprint = 'OTHER' + assert not owner.owner.apply(request(row, '2')) + owner.allowed = False + assert not owner.owner.apply(request(owner._row('profiles', 'profile:stopped_gap'), '2')) + + +def test_saved_refresh_uses_existing_profile_snapshot_and_expires_bad_data(owner): + from openpilot.cereal import log + from opendbc.car import gen_empty_fingerprint + from opendbc.car.hyundai.interface import CarInterface + from opendbc.car.hyundai.values import CAR + cp = CarInterface.get_params(CAR.KIA_STINGER_2022, gen_empty_fingerprint(), [], True, False, False) + owner.params.put('LongitudinalPersonalityProfiles', document(2.), block=True) + host = ProfileHost(owner.params) + tuning = host.sample_selected(10_000_000_000, log.LongitudinalPersonality.standard, 1., cp) + assert tuning is not None and tuning.extra_stopped_gap_m == 2. + owner.params.put('LongitudinalPersonalityProfiles', {'invalid': True}, block=True) + assert host.sample_selected(11_000_000_000, log.LongitudinalPersonality.standard, 1., cp) is None + + +def solve(gap=0., *, speed=1., lead_speed=0., present=True, stop_line=None): + mpc = LongitudinalMpc() + mpc.set_cur_state(speed, 0.) + lead = NS(present=present, radar=True, dRel=10., vLead=lead_speed, aLeadK=0., aLeadTau=1.5, modelProb=.99) + radar = NS(leadOne=lead, leadTwo=NS(present=False)) + mpc.update(radar, extra_stopped_gap_m=gap, stop_line_m=stop_line) + assert mpc.solution_status == 0 + return mpc + + +def test_actual_native_mpc_adds_only_bounded_low_speed_lead_space(): + baseline, extra = solve(), solve(3.048) + np.testing.assert_allclose(extra.params[:, 2], baseline.params[:, 2] - 3.048, rtol=0, atol=1e-12) + assert extra.a_solution[1] < baseline.a_solution[1] + np.testing.assert_array_equal(extra.params[:, [0, 1, 3, 4, 5]], baseline.params[:, [0, 1, 3, 4, 5]]) + for kwargs in ({'present': False}, {'speed': 20., 'lead_speed': 20.}, {'stop_line': 0.}): + before, after = solve(**kwargs), solve(3.048, **kwargs) + np.testing.assert_array_equal(before.params, after.params) + np.testing.assert_array_equal(before.a_solution, after.a_solution) + + +@pytest.mark.parametrize('bad', [-1., 3.049, float('nan'), float('inf'), True, None]) +def test_invalid_runtime_input_keeps_original_mpc(bad): + before, after = solve(), solve(bad) + np.testing.assert_array_equal(before.params, after.params) + np.testing.assert_array_equal(before.a_solution, after.a_solution) + + +def test_unconfigured_document_and_zero_reset_are_editable(owner): + from openpilot.starpilot.ui.feature_settings_state import row_default + owner.params.put('LongitudinalPersonalityProfiles', {}, block=True) + assert owner.owner.apply(request(owner._row('profiles', 'profile:stopped_gap'), '5')) + row = owner._row('profiles', 'profile:stopped_gap') + reset = row_default(row) + assert reset is not None and owner.owner.apply(reset) + assert owner.params.get('LongitudinalPersonalityProfiles')['extraStoppedGapM'] == 0. + + +def test_actual_stinger_planner_receives_preference_without_changing_limits(): + from opendbc.car import gen_empty_fingerprint + from opendbc.car.hyundai.interface import CarInterface + from opendbc.car.hyundai.values import CAR + from openpilot.selfdrive.controls.lib.longitudinal_planner import LongitudinalPlanner + from openpilot.starpilot.longitudinal.profile_runtime import SelectedProfileTuning + from openpilot.starpilot.longitudinal.tests.test_lead_approach_planner import Frame, DRIVE_NS + cp = CarInterface.get_params(CAR.KIA_STINGER_2022, gen_empty_fingerprint(), [], True, False, False) + original_cp = cp.to_dict() + assert cp.openpilotLongitudinalControl + baseline, extra = LongitudinalPlanner(cp, init_v=1.), LongitudinalPlanner(cp, init_v=1.) + for index in range(10): + sm = Frame(index) + sm['carState'].vEgo = 1. + sm['carState'].aEgo = 0. + sm['radarState'].leadTwo.present = False + lead = sm['radarState'].leadOne + lead.radar = True + lead.dRel, lead.vLead, lead.vLeadK, lead.aLeadK = 10., 0., 0., 0. + baseline.update(sm, now_ns=sm.now_ns, drive_id=DRIVE_NS) + extra.update(sm, now_ns=sm.now_ns, drive_id=DRIVE_NS, selected_profiles=SelectedProfileTuning(extra_stopped_gap_m=1.524)) + np.testing.assert_allclose(extra.mpc.params[:, 2], baseline.mpc.params[:, 2] - 1.524, rtol=0, atol=1e-12) + np.testing.assert_array_equal(extra.mpc.params[:, [0, 1, 4, 5]], baseline.mpc.params[:, [0, 1, 4, 5]]) + assert extra.output_a_target <= baseline.output_a_target and cp.to_dict() == original_cp + + +def test_galaxy_editor_converts_units_and_rejects_old_unit_press(): + from openpilot.starpilot.galaxy.tests import test_settings as gateway_tests + fixture = gateway_tests.SettingsGatewayTest() + fixture.setUp() + try: + page = fixture.page('profiles') + index = next(i for i, row in enumerate(page['rows']) if row['label'] == 'Extra stopped-lead distance') + assert page['rows'][index]['unit'] == 'ft' + intent = fixture.gateway.preview(page['view'], index, 0, fixture.session, fixture.generation, value=5.) + assert fixture.gateway.confirm(intent['intent'], fixture.session, fixture.generation) + saved = fixture.params.get('LongitudinalPersonalityProfiles') + assert saved['extraStoppedGapM'] == 1.524 + fixture.params.put_bool('IsMetric', True, block=True) + page = fixture.page('profiles') + row = page['rows'][index] + assert row['unit'] == 'm' and float(row['value']) == 1.524 + assert fixture.params.get('LongitudinalPersonalityProfiles') == saved + intent = fixture.gateway.preview(page['view'], index, 0, fixture.session, fixture.generation, value=2.) + fixture.params.put_bool('IsMetric', False, block=True) + assert not fixture.gateway.confirm(intent['intent'], fixture.session, fixture.generation) + assert fixture.params.get('LongitudinalPersonalityProfiles') == saved + finally: + fixture.doCleanups() diff --git a/openpilot/starpilot/longitudinal/tests/test_profile_runtime.py b/openpilot/starpilot/longitudinal/tests/test_profile_runtime.py index 946ec21ef1..f07047588c 100644 --- a/openpilot/starpilot/longitudinal/tests/test_profile_runtime.py +++ b/openpilot/starpilot/longitudinal/tests/test_profile_runtime.py @@ -30,6 +30,7 @@ def legacy_document(profiles, *, enabled, global_braking_response='standard'): profile[category]['preset'] = 'dom_default' document = current_profile_document(profiles, enabled=enabled, global_braking_response=global_braking_response) document['schemaVersion'] = 4 + document.pop('extraStoppedGapM') document['globalBrakingResponse'] = document.pop('selectedDecelerationProfile') document.pop('selectedAccelerationProfile') return document @@ -324,7 +325,7 @@ class ProfileRuntimeTests(unittest.TestCase): self.params.put('LongitudinalPersonalityProfiles', document, block=True) migrated = read_settings(self.params) assert migrated is not None and migrated.document is not None - self.assertEqual(migrated.document['schemaVersion'], 5) + self.assertEqual(migrated.document['schemaVersion'], 6) self.params.put('LongitudinalPersonalityProfiles', document, block=True) Path(self.params.get_param_path('LongitudinalPersonalityProfiles')).write_text('{broken') self.assertIsNone(read_settings(self.params)) diff --git a/openpilot/starpilot/longitudinal/tests/test_selected_profiles.py b/openpilot/starpilot/longitudinal/tests/test_selected_profiles.py index 27d8b99cc3..f73160505a 100644 --- a/openpilot/starpilot/longitudinal/tests/test_selected_profiles.py +++ b/openpilot/starpilot/longitudinal/tests/test_selected_profiles.py @@ -108,6 +108,7 @@ class SelectedProfilesTests(unittest.TestCase): def test_v4_migration_preserves_dormant_custom_and_original_activation(self): document = self.document('dom_default', 'sport', enabled=True) document['schemaVersion'] = 4 + document.pop('extraStoppedGapM') document['globalBrakingResponse'] = document.pop('selectedDecelerationProfile') document.pop('selectedAccelerationProfile') for profile in document['profiles'].values(): diff --git a/openpilot/starpilot/ui/feature_settings_owner.py b/openpilot/starpilot/ui/feature_settings_owner.py index 890b1d611d..db65f2c35d 100644 --- a/openpilot/starpilot/ui/feature_settings_owner.py +++ b/openpilot/starpilot/ui/feature_settings_owner.py @@ -27,10 +27,10 @@ from openpilot.starpilot.longitudinal.planner_selection import KEY as PLANNER_SE from openpilot.starpilot.longitudinal.profile_document import ( ACCELERATION_PRESETS, ACCELERATION_SPEEDS_MPH, BRAKING_PRESETS, FOLLOWING_PRESETS, CURVE_BOUNDS, - DEFAULT_DECELERATION_PROFILE, PERSONALITY_PROFILES_PARAM, default_personality_profiles, initial_custom_curve, + DEFAULT_DECELERATION_PROFILE, MAX_EXTRA_STOPPED_GAP_M, PERSONALITY_PROFILES_PARAM, default_personality_profiles, initial_custom_curve, FOLLOWING_SPEEDS_MPH, is_truck_fingerprint, personality_reference_curves, serialize_personality_profiles, - update_personality_profile, + update_personality_profile, profile_document, synchronise_profile_document_enabled, ) from openpilot.starpilot.ui.feature_settings_state import ( @@ -1202,6 +1202,15 @@ class FeatureSettingsOwner: reason=(CATEGORY_HELP[category] if category == 'braking' else 'Personalities set to Selected Profile follow this choice; explicit overrides stay unchanged.'), default_value=preset_label('dom_default' if category == 'acceleration' else DEFAULT_DECELERATION_PROFILE))) + unit_value, unit_raw, unit_valid = self._value("IsMetric") + unit_valid = unit_valid and unit_value in ("0", "1") and self._readable("IsMetric") + scale = 1.0 if unit_value == "1" else 1.0 / 0.3048 + gap = document.get("extraStoppedGapM", 0.0) if document is not None else 0.0 + rows.append(FeatureRow("profile:stopped_gap", "Extra stopped-lead distance", str(round(gap * scale, 3)), raw, + step=0.1 if unit_value == "1" else 0.5, minimum=0., maximum=round(MAX_EXTRA_STOPPED_GAP_M * scale, 3), + unit="m" if unit_value == "1" else "ft", available=allowed and valid and unit_valid, + dependencies=(("IsMetric", unit_raw),), display_unit="m" if unit_value == "1" else "ft", + default_value="0", reason="Adds space behind slow or stopped leads with the StarPilot planner; zero keeps the normal gap.")) health = read_profile_health(self.params) master = health.values["CustomPersonalities"] master_row = self._bool_row("CustomPersonalities", "Custom Following and Jerk", @@ -1482,6 +1491,11 @@ class FeatureSettingsOwner: not self.authority(group) or self._raw(source_key) != request.expected or not self._readable(source_key)): return False + if key == "profile:stopped_gap": + units, raw_units, units_valid = self._value("IsMetric") + if (not units_valid or units not in ("0", "1") or not self._readable("IsMetric") or + request.dependencies != (("IsMetric", raw_units),) or request.display_unit != ("m" if units == "1" else "ft")): + return False if group in ("torque", "aol", "aol_wheel"): if request.capability is None or request.capability != self._capability(group) or \ any(self._raw(name) != raw for name, raw in request.dependencies): @@ -1752,6 +1766,8 @@ class FeatureSettingsOwner: self.traffic_profiles._sources() == (request.dependencies, True)) and (not require_seed_capability or request.capability is not None and self.long_profiles.curve_capability() == request.capability) and + (request.key != "profile:stopped_gap" or all(self._raw(key) == value and self._readable(key) + for key, value in request.dependencies)) and self._readable("CustomPersonalities") and self._raw("CustomPersonalities") == master_source and (dependencies is None or self.long_profiles._fresh(request, dependencies))) result = commit_exact(self.params, key=PERSONALITY_PROFILES_PARAM, max_bytes=65536, raw=raw, expected=expected, @@ -1760,6 +1776,17 @@ class FeatureSettingsOwner: def _edit_document(self, key: str, value: str, *, seed_capability: tuple | None = None) -> str | None: parts = key.split(":") + if key == "profile:stopped_gap": + document, _, valid = self._document() + if not valid: + return None + if document is None: + document = profile_document(default_personality_profiles(False, is_truck_fingerprint(self.vehicle_fingerprint())), enabled=False) + gap = float(value) * (1.0 if self._value("IsMetric")[0] == "1" else 0.3048) + if not math.isfinite(gap) or not 0. <= gap <= MAX_EXTRA_STOPPED_GAP_M: + return None + document["extraStoppedGapM"] = round(gap, 6) + return json.dumps(document, separators=(",", ":"), sort_keys=True, allow_nan=False) if key in ('profile:global_acceleration', 'profile:global_braking'): category = 'acceleration' if key.endswith('acceleration') else 'braking' selected = preset_value(value) if type(value) is str else None @@ -1778,7 +1805,8 @@ class FeatureSettingsOwner: braking = document['selectedDecelerationProfile'] if document is not None else DEFAULT_DECELERATION_PROFILE return serialize_personality_profiles(profiles, False, truck, enabled=document['enabled'] if document is not None else False, selected_acceleration_profile=selected if category == 'acceleration' else acceleration, - selected_deceleration_profile=selected if category == 'braking' else braking) + selected_deceleration_profile=selected if category == 'braking' else braking, + extra_stopped_gap_m=document.get("extraStoppedGapM", 0.) if document is not None else 0.) if len(parts) not in (3, 4) or parts[1] not in DOCUMENT_PROFILE_NAMES or parts[2] not in PRESETS: return None fingerprint = self.vehicle_fingerprint() @@ -1843,4 +1871,5 @@ class FeatureSettingsOwner: return serialize_personality_profiles(updated, ev, truck, enabled=enabled == "1", selected_acceleration_profile=document["selectedAccelerationProfile"] if document is not None else "dom_default", selected_deceleration_profile=(document["selectedDecelerationProfile"] if document is not None - else DEFAULT_DECELERATION_PROFILE)) + else DEFAULT_DECELERATION_PROFILE), + extra_stopped_gap_m=document.get("extraStoppedGapM", 0.) if document is not None else 0.) diff --git a/prebuilt.json b/prebuilt.json index 9f767dc3f0..d60c63fbab 100644 --- a/prebuilt.json +++ b/prebuilt.json @@ -1 +1 @@ -{"protected_inventory_sha256": "868381b1c309f270f146c1e4d6f8a3a1f02e06c46be1b2f9edaebb4a605dc9cf", "schema_version": 1} +{"protected_inventory_sha256": "50570638434af7b38217a29051f06666683cc974fee8abd0f3070a983bb29ffc", "schema_version": 1}