From 04e199cf7b77ffcecef7780e289018e7c3eb160f Mon Sep 17 00:00:00 2001 From: firestar5683 <168790843+firestar5683@users.noreply.github.com> Date: Wed, 8 Apr 2026 19:10:29 -0500 Subject: [PATCH] 2017 test --- selfdrive/controls/lib/latcontrol_torque.py | 16 ++--- starpilot/system/the_pond/the_pond.py | 70 ++++++++++++++++++++- 2 files changed, 75 insertions(+), 11 deletions(-) diff --git a/selfdrive/controls/lib/latcontrol_torque.py b/selfdrive/controls/lib/latcontrol_torque.py index 34137e03..00f2891a 100644 --- a/selfdrive/controls/lib/latcontrol_torque.py +++ b/selfdrive/controls/lib/latcontrol_torque.py @@ -61,14 +61,14 @@ BOLT_CARS = BOLT_2022_2023_CARS + BOLT_2018_2021_CARS + BOLT_2017_CARS BOLT_2017_LATERAL_TESTING_GROUND_ID = testing_ground.id_3 BOLT_2017_STEER_RATIO_TEST_SCALE = 1.045 BOLT_2017_TORQUE_SCALE_BP = [0.0, 0.2, 0.5, 1.0, 1.5, 2.5] -BOLT_2017_TORQUE_SCALE_LEFT = [1.0, 1.0, 1.065, 1.060, 1.055, 1.045] -BOLT_2017_TORQUE_SCALE_RIGHT = [1.0, 1.0, 1.035, 1.020, 0.995, 0.985] -BOLT_2017_TRANSITION_SPEED = 10.0 -BOLT_2017_PHASE_SCALE = 0.12 -BOLT_2017_TURN_IN_BOOST_LEFT = 0.28 -BOLT_2017_TURN_IN_BOOST_RIGHT = 0.18 -BOLT_2017_UNWIND_TAPER_LEFT = 0.08 -BOLT_2017_UNWIND_TAPER_RIGHT = 0.28 +BOLT_2017_TORQUE_SCALE_LEFT = [1.0, 1.0, 1.078, 1.073, 1.068, 1.052] +BOLT_2017_TORQUE_SCALE_RIGHT = [1.0, 1.0, 1.045, 1.035, 1.010, 0.995] +BOLT_2017_TRANSITION_SPEED = 12.0 +BOLT_2017_PHASE_SCALE = 0.10 +BOLT_2017_TURN_IN_BOOST_LEFT = 0.40 +BOLT_2017_TURN_IN_BOOST_RIGHT = 0.28 +BOLT_2017_UNWIND_TAPER_LEFT = 0.22 +BOLT_2017_UNWIND_TAPER_RIGHT = 0.32 BOLT_2018_2021_LATERAL_TESTING_GROUND_ID = testing_ground.id_4 BOLT_2018_2021_STEER_RATIO_TEST_SCALE = 1.01 diff --git a/starpilot/system/the_pond/the_pond.py b/starpilot/system/the_pond/the_pond.py index d4cd520a..76bac075 100644 --- a/starpilot/system/the_pond/the_pond.py +++ b/starpilot/system/the_pond/the_pond.py @@ -99,6 +99,12 @@ send_from_directory = None _POND_WEB_DEPS_READY = False _POND_WEB_DEPS_ERROR = None +_TESTING_GROUND_CUSTOM_RESERVED_SERVICE = "customReservedRawData0" +_TESTING_GROUND_CUSTOM_RESERVED_INTERVAL_S = 15.0 +_TESTING_GROUND_CUSTOM_RESERVED_PM = None +_TESTING_GROUND_CUSTOM_RESERVED_LOCK = threading.Lock() +_TESTING_GROUND_CUSTOM_RESERVED_LAST_PUBLISH_MONO = 0.0 + def _is_comma_device_runtime() -> bool: """Robust runtime device check. @@ -2790,12 +2796,64 @@ def _serialize_testing_grounds_state(state): "selectableSlots": [slot for slot in slots if not _is_unused_testing_ground_slot(slot)], } +def _get_testing_ground_custom_reserved_pm(): + global _TESTING_GROUND_CUSTOM_RESERVED_PM + + with _TESTING_GROUND_CUSTOM_RESERVED_LOCK: + if _TESTING_GROUND_CUSTOM_RESERVED_PM is None: + _TESTING_GROUND_CUSTOM_RESERVED_PM = messaging.PubMaster([_TESTING_GROUND_CUSTOM_RESERVED_SERVICE]) + return _TESTING_GROUND_CUSTOM_RESERVED_PM + +def _build_testing_ground_custom_reserved_payload(state, reason): + serialized = _serialize_testing_grounds_state(state) + return { + "type": "testingGroundState", + "reason": reason, + "slotId": serialized["activeSlot"], + "slotName": serialized["activeSlotName"], + "variant": serialized["activeVariant"], + "variantLabel": serialized["activeVariantLabel"], + "publishedAt": datetime.now(timezone.utc).isoformat(timespec="seconds").replace("+00:00", "Z"), + } + +def _publish_testing_ground_custom_reserved(state, reason): + global _TESTING_GROUND_CUSTOM_RESERVED_LAST_PUBLISH_MONO + + payload = _build_testing_ground_custom_reserved_payload(state, reason) + + try: + message = messaging.new_message(_TESTING_GROUND_CUSTOM_RESERVED_SERVICE) + message.customReservedRawData0 = json.dumps(payload, separators=(",", ":"), allow_nan=False).encode("utf-8") + _get_testing_ground_custom_reserved_pm().send(_TESTING_GROUND_CUSTOM_RESERVED_SERVICE, message) + except Exception: + return + + with _TESTING_GROUND_CUSTOM_RESERVED_LOCK: + _TESTING_GROUND_CUSTOM_RESERVED_LAST_PUBLISH_MONO = time.monotonic() + +def _testing_ground_custom_reserved_worker(): + while True: + with _TESTING_GROUND_CUSTOM_RESERVED_LOCK: + last_publish_mono = _TESTING_GROUND_CUSTOM_RESERVED_LAST_PUBLISH_MONO + + sleep_s = _TESTING_GROUND_CUSTOM_RESERVED_INTERVAL_S - (time.monotonic() - last_publish_mono) + if sleep_s > 0: + time.sleep(min(sleep_s, 1.0)) + continue + + try: + _publish_testing_ground_custom_reserved(_get_testing_grounds_state(), "heartbeat") + except Exception: + time.sleep(1.0) + def _set_testing_ground_selection(slot_id, variant): normalized_slot_id = str(slot_id or "").strip() requested_variant = str(variant or "").strip().upper() with _TESTING_GROUNDS_LOCK: state, _ = _load_testing_grounds_state_unlocked() + previous_slot_id = str(state.get("activeSlot") or "").strip() + previous_variant = _normalize_testing_ground_variant(previous_slot_id, state.get("activeVariant"), _find_testing_ground_slot(state, previous_slot_id)) slot_ids = {slot["id"] for slot in state["slots"]} if normalized_slot_id not in slot_ids: raise ValueError(f"Unknown testing ground slot '{normalized_slot_id}'.") @@ -2807,10 +2865,12 @@ def _set_testing_ground_selection(slot_id, variant): raise ValueError(f"Variant must be one of: {allowed_variants}.") normalized_variant = _normalize_testing_ground_variant(normalized_slot_id, requested_variant, slot) + changed = normalized_slot_id != previous_slot_id or normalized_variant != previous_variant state["activeSlot"] = normalized_slot_id state["activeVariant"] = normalized_variant - _write_testing_grounds_state_unlocked(state) - return state + if changed: + _write_testing_grounds_state_unlocked(state) + return state, changed def _default_longitudinal_maneuver_status(): return { @@ -4609,12 +4669,15 @@ def setup(app): return jsonify({"error": "Missing 'slotId' in request body."}), 400 try: - state = _set_testing_ground_selection(slot_id, variant) + state, changed = _set_testing_ground_selection(slot_id, variant) except ValueError as exception: return jsonify({"error": str(exception)}), 400 except Exception as exception: return jsonify({"error": str(exception)}), 500 + if changed: + _publish_testing_ground_custom_reserved(state, "manual_change") + slot = _find_testing_ground_slot(state, slot_id) slot_name = slot.get("name", f"Testing Ground {slot_id}") selected_variant = str(state.get("activeVariant") or _TESTING_GROUNDS_DEFAULT_VARIANT) @@ -5927,6 +5990,7 @@ def main(): app = Flask(__name__, static_folder="assets", static_url_path="/assets") setup(app) + threading.Thread(target=_testing_ground_custom_reserved_worker, daemon=True).start() # Desktop-only debug mode. On-device must stay on 8082 to match Galaxy FRP routing. debug = not _is_comma_device_runtime()