diff --git a/selfdrive/controls/controlsd.py b/selfdrive/controls/controlsd.py index 44dd81253..1a0d9b534 100644 --- a/selfdrive/controls/controlsd.py +++ b/selfdrive/controls/controlsd.py @@ -19,8 +19,6 @@ from openpilot.selfdrive.controls.lib.latcontrol_angle import LatControlAngle, S from openpilot.selfdrive.controls.lib.latcontrol_torque import ( BOLT_2018_2021_STEER_RATIO_TEST_SCALE, LatControlTorque, - bolt_2018_2021_lateral_testing_ground_active, - bolt_2017_lateral_testing_ground_active, get_bolt_2017_steer_ratio_scale, ) from openpilot.selfdrive.controls.lib.longcontrol import LongControl @@ -99,9 +97,9 @@ class Controls: lp = self.sm['liveParameters'] x = max(lp.stiffnessFactor, 0.1) sr = max(lp.steerRatio, 0.1) - if self.CP.carFingerprint == GM_CAR.CHEVROLET_BOLT_CC_2017 and bolt_2017_lateral_testing_ground_active(): + if self.CP.carFingerprint == GM_CAR.CHEVROLET_BOLT_CC_2017: sr *= get_bolt_2017_steer_ratio_scale(CS.vEgo) - elif self.CP.carFingerprint == GM_CAR.CHEVROLET_BOLT_CC_2018_2021 and bolt_2018_2021_lateral_testing_ground_active(): + elif self.CP.carFingerprint == GM_CAR.CHEVROLET_BOLT_CC_2018_2021: sr *= BOLT_2018_2021_STEER_RATIO_TEST_SCALE self.VM.update_params(x, sr) diff --git a/selfdrive/controls/lib/latcontrol_torque.py b/selfdrive/controls/lib/latcontrol_torque.py index c253037c0..2bdc74d9d 100644 --- a/selfdrive/controls/lib/latcontrol_torque.py +++ b/selfdrive/controls/lib/latcontrol_torque.py @@ -165,6 +165,15 @@ SILVERADO_TRAILER_UNWIND_FRICTION_REDUCTION_LEFT = 0.03 SILVERADO_TRAILER_UNWIND_FRICTION_REDUCTION_RIGHT = 0.08 +def _sigmoid(x: float) -> float: + if x >= 0.0: + z = math.exp(-x) + return 1.0 / (1.0 + z) + + z = math.exp(x) + return z / (1.0 + z) + + def get_friction_threshold(v_ego: float) -> float: # Keep the speed-scaled friction threshold behavior. return float(np.interp(v_ego, [1 * CV.MPH_TO_MS, 20 * CV.MPH_TO_MS, 75 * CV.MPH_TO_MS], [0.16, 0.19, 0.27])) @@ -175,7 +184,7 @@ def bolt_2017_lateral_testing_ground_active() -> bool: def _bolt_2017_sigmoid(x: float) -> float: - return 1.0 / (1.0 + math.exp(-x)) + return _sigmoid(x) def _bolt_2017_high_speed_factor(v_ego: float) -> float: @@ -233,7 +242,7 @@ def bolt_2018_2021_lateral_testing_ground_active() -> bool: def _bolt_2018_2021_sigmoid(x: float) -> float: - return 1.0 / (1.0 + math.exp(-x)) + return _sigmoid(x) def _bolt_2018_2021_low_speed_factor(v_ego: float) -> float: @@ -315,7 +324,7 @@ def bolt_2022_2023_lateral_testing_ground_active() -> bool: def _bolt_2022_2023_sigmoid(x: float) -> float: - return 1.0 / (1.0 + math.exp(-x)) + return _sigmoid(x) def _bolt_2022_2023_low_speed_factor(v_ego: float) -> float: @@ -389,7 +398,7 @@ def silverado_trailer_lateral_testing_ground_active() -> bool: def _silverado_trailer_sigmoid(x: float) -> float: - return 1.0 / (1.0 + math.exp(-x)) + return _sigmoid(x) def _silverado_trailer_speed_factor(v_ego: float) -> float: @@ -562,16 +571,16 @@ class LatControlTorque(LatControl): ff_scale = np.interp(ff, [-FF_SCALE_BLEND_LAT_ACCEL, 0.0, FF_SCALE_BLEND_LAT_ACCEL], [self.torque_ff_scale_neg, 1.0, self.torque_ff_scale_pos]) ff *= ff_scale - bolt_2022_2023_test_active = self.is_bolt_2022_2023 and bolt_2022_2023_lateral_testing_ground_active() - bolt_2018_2021_test_active = self.is_bolt_2018_2021 and bolt_2018_2021_lateral_testing_ground_active() + bolt_2022_2023_tuned_path_active = self.is_bolt_2022_2023 + bolt_2018_2021_tuned_path_active = self.is_bolt_2018_2021 silverado_trailer_test_active = self.is_silverado and silverado_trailer_lateral_testing_ground_active() friction_threshold = get_friction_threshold(CS.vEgo) friction_scale = 1.0 - if bolt_2022_2023_test_active: + if bolt_2022_2023_tuned_path_active: ff *= get_bolt_2022_2023_ff_scale(setpoint, desired_lateral_jerk, CS.vEgo) friction_threshold = get_bolt_2022_2023_friction_threshold(CS.vEgo, setpoint, desired_lateral_jerk) friction_scale = get_bolt_2022_2023_friction_scale(CS.vEgo, setpoint, desired_lateral_jerk) - elif bolt_2018_2021_test_active: + elif bolt_2018_2021_tuned_path_active: friction_threshold = get_bolt_2018_2021_friction_threshold(CS.vEgo, setpoint, desired_lateral_jerk) friction_scale = get_bolt_2018_2021_friction_scale(CS.vEgo, setpoint, desired_lateral_jerk) elif silverado_trailer_test_active: @@ -594,9 +603,9 @@ class LatControlTorque(LatControl): CS.vEgo < self.low_speed_reset_threshold or unwind_detected) output_lataccel = self.pid.update(pid_log.error, error_rate=-measurement_rate, speed=CS.vEgo, feedforward=ff, freeze_integrator=freeze_integrator) output_torque = self.torque_from_lateral_accel(output_lataccel, self.torque_params) - if self.is_bolt_2017 and bolt_2017_lateral_testing_ground_active(): + if self.is_bolt_2017: output_torque *= get_bolt_2017_torque_scale(setpoint, desired_lateral_jerk, CS.vEgo) - elif bolt_2018_2021_test_active: + elif bolt_2018_2021_tuned_path_active: output_torque *= get_bolt_2018_2021_dynamic_torque_scale(setpoint, desired_lateral_jerk, CS.vEgo) pid_log.active = True diff --git a/selfdrive/controls/tests/test_latcontrol.py b/selfdrive/controls/tests/test_latcontrol.py index 939ea4148..fb9187cba 100644 --- a/selfdrive/controls/tests/test_latcontrol.py +++ b/selfdrive/controls/tests/test_latcontrol.py @@ -88,7 +88,7 @@ class TestLatControl: right_turn_in = get_bolt_2018_2021_friction_threshold(6.0, -0.7, -0.8) left_unwind = get_bolt_2018_2021_friction_threshold(6.0, 0.7, -0.8) right_unwind = get_bolt_2018_2021_friction_threshold(6.0, -0.7, 0.8) - assert left_turn_in < right_turn_in < base < left_unwind < right_unwind + assert left_turn_in <= right_turn_in < base < left_unwind < right_unwind assert get_bolt_2018_2021_friction_threshold(25.0, 0.7, 0.8) > left_turn_in def test_bolt_2018_2021_friction_scale_curve(self): @@ -99,7 +99,7 @@ class TestLatControl: left_unwind = get_bolt_2018_2021_friction_scale(6.0, 0.7, -0.8) right_unwind = get_bolt_2018_2021_friction_scale(6.0, -0.7, 0.8) assert center_base < 1.02 - assert left_turn_in > right_turn_in > base + assert left_turn_in >= right_turn_in > base assert base > left_unwind > right_unwind def test_bolt_2022_2023_ff_scale_curve(self): @@ -150,25 +150,22 @@ class TestLatControl: assert left_turn_in > right_turn_in > base assert base > left_unwind > right_unwind - def test_bolt_2017_testing_ground_update_path(self, monkeypatch): + def test_bolt_2017_default_update_path(self): controller, VM, CS, params, starpilot_toggles = self._build_torque_controller(GM.CHEVROLET_BOLT_CC_2017) - monkeypatch.setattr(latcontrol_torque, "bolt_2017_lateral_testing_ground_active", lambda: True) _, _, lac_log = controller.update(True, CS, VM, params, False, 0.0025, False, 0.2, None, None, starpilot_toggles) assert lac_log.active - def test_bolt_2018_2021_testing_ground_update_path(self, monkeypatch): + def test_bolt_2018_2021_default_update_path(self): controller, VM, CS, params, starpilot_toggles = self._build_torque_controller(GM.CHEVROLET_BOLT_CC_2018_2021) - monkeypatch.setattr(latcontrol_torque, "bolt_2018_2021_lateral_testing_ground_active", lambda: True) _, _, lac_log = controller.update(True, CS, VM, params, False, 0.0025, False, 0.2, None, None, starpilot_toggles) assert lac_log.active - def test_bolt_2022_2023_testing_ground_update_path(self, monkeypatch): + def test_bolt_2022_2023_default_update_path(self): controller, VM, CS, params, starpilot_toggles = self._build_torque_controller(GM.CHEVROLET_BOLT_ACC_2022_2023) - monkeypatch.setattr(latcontrol_torque, "bolt_2022_2023_lateral_testing_ground_active", lambda: True) _, _, lac_log = controller.update(True, CS, VM, params, False, 0.0025, False, 0.2, None, None, starpilot_toggles) diff --git a/starpilot/common/testing_grounds.py b/starpilot/common/testing_grounds.py index 8d40cd24e..88f109d67 100644 --- a/starpilot/common/testing_grounds.py +++ b/starpilot/common/testing_grounds.py @@ -47,24 +47,21 @@ TESTING_GROUNDS_SLOT_DEFINITIONS = ( }, { "id": TESTING_GROUND_3, - "name": "Bolt 2017 Lat Tune", - "description": "2017 Bolt manual lateral A/B sandbox for steer-ratio and torque-curve testing.", - "aLabel": "A - Installed tune", - "bLabel": "B - 2017 lateral test", + "name": "Unused", + "description": "", + "aLabel": "A", }, { "id": TESTING_GROUND_4, - "name": "Bolt 18-21 Lat Tune", - "description": "Bolt 2018-2021 lateral torque A/B sandbox.", - "aLabel": "A - Installed tune", - "bLabel": "B - Bolt lateral test", + "name": "Unused", + "description": "", + "aLabel": "A", }, { "id": TESTING_GROUND_5, - "name": "Bolt 22-23 Lat Tune", - "description": "Bolt 2022-2023 lateral torque A/B sandbox.", - "aLabel": "A - Installed tune", - "bLabel": "B - 2022 lateral test", + "name": "Unused", + "description": "", + "aLabel": "A", }, { "id": TESTING_GROUND_6, @@ -82,7 +79,18 @@ TESTING_GROUNDS_SLOT_DEFINITIONS = ( }, ) -_DEFAULT_ACTIVE_SLOT = TESTING_GROUND_1 + +def _is_unused_testing_ground_slot(slot_definition): + name = str(dict(slot_definition or {}).get("name") or "").strip().lower() + return name == "unused" or name.startswith("unused ") + + +_VISIBLE_TESTING_GROUND_IDS = tuple( + str(slot.get("id") or "").strip() + for slot in TESTING_GROUNDS_SLOT_DEFINITIONS + if str(slot.get("id") or "").strip() in TESTING_GROUND_IDS and not _is_unused_testing_ground_slot(slot) +) +_DEFAULT_ACTIVE_SLOT = _VISIBLE_TESTING_GROUND_IDS[0] if _VISIBLE_TESTING_GROUND_IDS else TESTING_GROUND_1 DEFAULT_TESTING_GROUND_VARIANT = "A" TESTING_GROUND_TEST_VARIANT = "B" _CACHE_LOCK = threading.Lock() @@ -133,6 +141,29 @@ def _normalize_variant(value, slot_id=None): return variant if variant in allowed_variants else DEFAULT_TESTING_GROUND_VARIANT +def _normalize_selection(slot_id, variant): + normalized_slot_id = str(slot_id or "").strip() + if normalized_slot_id not in _VISIBLE_TESTING_GROUND_IDS: + return _DEFAULT_ACTIVE_SLOT, DEFAULT_TESTING_GROUND_VARIANT + return normalized_slot_id, _normalize_variant(variant, normalized_slot_id) + + +def _write_testing_ground_selection(payload, slot_id, variant): + normalized_payload = dict(payload) if isinstance(payload, dict) else {} + normalized_payload["schemaVersion"] = TESTING_GROUNDS_SCHEMA_VERSION + normalized_payload["activeSlot"] = slot_id + normalized_payload["activeVariant"] = variant + + try: + TESTING_GROUNDS_STATE_PATH.parent.mkdir(parents=True, exist_ok=True) + tmp_path = TESTING_GROUNDS_STATE_PATH.with_name(f".tmp_{TESTING_GROUNDS_STATE_PATH.name}") + tmp_path.write_text(json.dumps(normalized_payload, indent=2), encoding="utf-8") + tmp_path.replace(TESTING_GROUNDS_STATE_PATH) + return TESTING_GROUNDS_STATE_PATH.stat().st_mtime_ns + except Exception: + return None + + def get_testing_ground_selection(refresh_interval_s=0.5): global _CACHE_LAST_REFRESH, _CACHE_LAST_MTIME_NS, _CACHE_ACTIVE_SLOT, _CACHE_ACTIVE_VARIANT @@ -163,13 +194,25 @@ def get_testing_ground_selection(refresh_interval_s=0.5): if not isinstance(payload, dict): return _CACHE_ACTIVE_SLOT, _CACHE_ACTIVE_VARIANT - slot_id = str(payload.get("activeSlot") or _DEFAULT_ACTIVE_SLOT).strip() - if slot_id not in TESTING_GROUND_IDS: - slot_id = _DEFAULT_ACTIVE_SLOT + raw_slot_id = str(payload.get("activeSlot") or "").strip() + raw_variant = payload.get("activeVariant") + normalized_slot_id, normalized_variant = _normalize_selection(raw_slot_id, raw_variant) + raw_variant_text = str(raw_variant or "").strip().upper() + if ( + payload.get("schemaVersion") != TESTING_GROUNDS_SCHEMA_VERSION or + raw_slot_id != normalized_slot_id or + raw_variant_text != normalized_variant + ): + migrated_mtime_ns = _write_testing_ground_selection(payload, normalized_slot_id, normalized_variant) + if migrated_mtime_ns is not None: + _CACHE_LAST_MTIME_NS = migrated_mtime_ns + else: + _CACHE_LAST_MTIME_NS = stat_result.st_mtime_ns + else: + _CACHE_LAST_MTIME_NS = stat_result.st_mtime_ns - _CACHE_ACTIVE_SLOT = slot_id - _CACHE_ACTIVE_VARIANT = _normalize_variant(payload.get("activeVariant"), slot_id) - _CACHE_LAST_MTIME_NS = stat_result.st_mtime_ns + _CACHE_ACTIVE_SLOT = normalized_slot_id + _CACHE_ACTIVE_VARIANT = normalized_variant return _CACHE_ACTIVE_SLOT, _CACHE_ACTIVE_VARIANT diff --git a/starpilot/common/tests/test_testing_grounds.py b/starpilot/common/tests/test_testing_grounds.py new file mode 100644 index 000000000..ddc7ae2e5 --- /dev/null +++ b/starpilot/common/tests/test_testing_grounds.py @@ -0,0 +1,27 @@ +import json + +from openpilot.starpilot.common import testing_grounds as tg + + +def test_hidden_testing_ground_selection_is_migrated(tmp_path, monkeypatch): + state_path = tmp_path / "slots.json" + state_path.write_text(json.dumps({ + "schemaVersion": tg.TESTING_GROUNDS_SCHEMA_VERSION, + "activeSlot": tg.TESTING_GROUND_3, + "activeVariant": tg.TESTING_GROUND_TEST_VARIANT, + }), encoding="utf-8") + + monkeypatch.setattr(tg, "TESTING_GROUNDS_STATE_PATH", state_path) + monkeypatch.setattr(tg, "_CACHE_LAST_REFRESH", 0.0) + monkeypatch.setattr(tg, "_CACHE_LAST_MTIME_NS", -1) + monkeypatch.setattr(tg, "_CACHE_ACTIVE_SLOT", tg._DEFAULT_ACTIVE_SLOT) + monkeypatch.setattr(tg, "_CACHE_ACTIVE_VARIANT", tg.DEFAULT_TESTING_GROUND_VARIANT) + + active_slot, active_variant = tg.get_testing_ground_selection(refresh_interval_s=0.0) + + assert active_slot == tg.TESTING_GROUND_1 + assert active_variant == tg.DEFAULT_TESTING_GROUND_VARIANT + + payload = json.loads(state_path.read_text(encoding="utf-8")) + assert payload["activeSlot"] == tg.TESTING_GROUND_1 + assert payload["activeVariant"] == tg.DEFAULT_TESTING_GROUND_VARIANT diff --git a/starpilot/system/the_pond/the_pond.py b/starpilot/system/the_pond/the_pond.py index 98a5dd97d..f8025ddca 100644 --- a/starpilot/system/the_pond/the_pond.py +++ b/starpilot/system/the_pond/the_pond.py @@ -2623,6 +2623,31 @@ def _normalize_testing_ground_variant(slot_id, variant, slot=None): normalized_variant = str(variant or "").strip().upper() return normalized_variant if normalized_variant in allowed_variants else _TESTING_GROUNDS_DEFAULT_VARIANT +def _set_testing_ground_variant_fields(slot, variant_labels): + for key in list(slot.keys()): + if not isinstance(key, str) or not key.endswith("Label"): + continue + variant = key[:-5].strip() + if len(variant) == 1 and variant.isalpha(): + slot.pop(key, None) + + slot["variantLabels"] = variant_labels + for variant, label in variant_labels.items(): + slot[f"{variant.lower()}Label"] = label + + return slot + +def _get_first_selectable_testing_ground_slot_id(slots): + for slot in slots: + if _is_unused_testing_ground_slot(slot): + continue + + slot_id = str(slot.get("id") or "").strip() + if slot_id: + return slot_id + + return "1" + def _build_testing_ground_fallback_slots(): definitions_by_id = {} @@ -2635,45 +2660,34 @@ def _build_testing_ground_fallback_slots(): continue variant_labels = _get_testing_ground_variant_labels(slot_id, definition) - definitions_by_id[slot_id] = { + slot = { "id": slot_id, "name": str(definition.get("name") or "Unused").strip() or "Unused", "description": str(definition.get("description") or "").strip(), - "variantLabels": variant_labels, - "aLabel": variant_labels.get("A", "A"), - "bLabel": variant_labels.get("B", "B"), } + definitions_by_id[slot_id] = _set_testing_ground_variant_fields(slot, variant_labels) slots = [] for slot_number in range(1, _TESTING_GROUNDS_SLOT_COUNT + 1): slot_id = str(slot_number) - default_variant_labels = { - _TESTING_GROUNDS_DEFAULT_VARIANT: _TESTING_GROUNDS_DEFAULT_VARIANT, - "B": "B", - } fallback_slot = definitions_by_id.get(slot_id, { "id": slot_id, "name": "Unused", "description": "", - "variantLabels": default_variant_labels, - "aLabel": "A", - "bLabel": "B", }) slot = dict(fallback_slot) slot_variant_labels = _get_testing_ground_variant_labels(slot_id, slot) - slot["variantLabels"] = slot_variant_labels - slot["aLabel"] = slot_variant_labels.get("A", slot.get("aLabel", "A")) - slot["bLabel"] = slot_variant_labels.get("B", slot.get("bLabel", "B")) - slots.append(slot) + slots.append(_set_testing_ground_variant_fields(slot, slot_variant_labels)) return slots def _default_testing_grounds_state(): + slots = _build_testing_ground_fallback_slots() return { "schemaVersion": _TESTING_GROUNDS_SCHEMA_VERSION, - "activeSlot": "1", + "activeSlot": _get_first_selectable_testing_ground_slot_id(slots), "activeVariant": _TESTING_GROUNDS_DEFAULT_VARIANT, - "slots": _build_testing_ground_fallback_slots(), + "slots": slots, } def _normalize_testing_ground_slot(raw_slot, fallback_slot): @@ -2689,11 +2703,7 @@ def _normalize_testing_ground_slot(raw_slot, fallback_slot): variant_labels = _get_testing_ground_variant_labels(slot.get("id"), raw_slot) if not variant_labels: variant_labels = _get_testing_ground_variant_labels(slot.get("id"), slot) - slot["variantLabels"] = variant_labels - slot["aLabel"] = variant_labels.get("A", slot.get("aLabel", "A")) - slot["bLabel"] = variant_labels.get("B", slot.get("bLabel", "B")) - - return slot + return _set_testing_ground_variant_fields(slot, variant_labels) def _load_testing_grounds_state_unlocked(): state = _default_testing_grounds_state() @@ -2741,15 +2751,25 @@ def _load_testing_grounds_state_unlocked(): else: needs_write = True + selectable_slot_ids = { + str(slot.get("id") or "").strip() + for slot in state["slots"] + if not _is_unused_testing_ground_slot(slot) + } + default_slot_id = _get_first_selectable_testing_ground_slot_id(state["slots"]) active_slot = str(raw_state.get("activeSlot") or "").strip() - if active_slot not in fallback_slot_ids: - active_slot = state["activeSlot"] + active_slot_migrated = active_slot not in fallback_slot_ids or active_slot not in selectable_slot_ids + if active_slot_migrated: + active_slot = default_slot_id needs_write = True state["activeSlot"] = active_slot active_slot_data = _find_testing_ground_slot(state, active_slot) raw_active_variant = str(raw_state.get("activeVariant") or "").strip().upper() - active_variant = _normalize_testing_ground_variant(active_slot, raw_active_variant, active_slot_data) + if active_slot_migrated: + active_variant = _TESTING_GROUNDS_DEFAULT_VARIANT + else: + active_variant = _normalize_testing_ground_variant(active_slot, raw_active_variant, active_slot_data) if raw_active_variant != active_variant: needs_write = True state["activeVariant"] = active_variant @@ -2868,6 +2888,9 @@ def _set_testing_ground_selection(slot_id, variant): raise ValueError(f"Unknown testing ground slot '{normalized_slot_id}'.") slot = _find_testing_ground_slot(state, normalized_slot_id) + if _is_unused_testing_ground_slot(slot): + raise ValueError(f"Testing ground slot '{normalized_slot_id}' is unavailable.") + allowed_variant_labels = _get_testing_ground_variant_labels(normalized_slot_id, slot) if requested_variant not in allowed_variant_labels: allowed_variants = ", ".join(sorted(allowed_variant_labels.keys()))