From 7848a0090a395ea99c15bd9f2e0e627fc4c299e8 Mon Sep 17 00:00:00 2001 From: firestar5683 <168790843+firestar5683@users.noreply.github.com> Date: Thu, 1 Oct 2026 21:08:43 -0500 Subject: [PATCH] Soup --- common/params_keys.h | 1 + .../controls/lib/latcontrol_vehicle_tunes.py | 2 +- selfdrive/selfdrived/selfdrived.py | 3 +- .../common/assets/device_settings_layout.json | 12 ++ starpilot/system/stinger_object_shadow.py | 104 ++++++++++++++++++ system/manager/process_config.py | 7 ++ 6 files changed, 127 insertions(+), 2 deletions(-) create mode 100644 starpilot/system/stinger_object_shadow.py diff --git a/common/params_keys.h b/common/params_keys.h index 2719e0436a..8f6b7adcc0 100644 --- a/common/params_keys.h +++ b/common/params_keys.h @@ -728,6 +728,7 @@ inline static std::unordered_map keys = { {"SteerRatio", {PERSISTENT, FLOAT, "0.0", "0.0", 3}}, {"SteerRatioStock", {PERSISTENT, FLOAT, "0.0", "0.0", 3}}, {"EnableTorqueBarWidget", {PERSISTENT, BOOL, "1", "0", 0}}, + {"StingerObjectShadow", {PERSISTENT, BOOL, "0", "0", 3}}, {"StockConfidenceBallWidget", {PERSISTENT, BOOL, "0", "0", 0, SETTINGS_SIMPLE}}, {"StockDongleId", {PERSISTENT, STRING, "", ""}}, {"StopAccel", {PERSISTENT, FLOAT, "0.0", "0.0", 3}}, diff --git a/selfdrive/controls/lib/latcontrol_vehicle_tunes.py b/selfdrive/controls/lib/latcontrol_vehicle_tunes.py index c1e2f6f1dc..32476d3cb3 100644 --- a/selfdrive/controls/lib/latcontrol_vehicle_tunes.py +++ b/selfdrive/controls/lib/latcontrol_vehicle_tunes.py @@ -364,7 +364,7 @@ GENESIS_G70_OUTPUT_SMOOTHING_CENTER_RC = 0.18 GENESIS_G70_OUTPUT_SMOOTHING_CURVE_RC = 0.10 GENESIS_G70_OUTPUT_SMOOTHING_RELEASE_RC = 0.03 GENESIS_G70_OUTPUT_SMOOTHING_OVERSHOOT = 0.08 -GENESIS_G70_HIGHWAY_STABILIZER_CENTER_LAT_BP = [1.0, 1.4] +GENESIS_G70_HIGHWAY_STABILIZER_CENTER_LAT_BP = [1.4, 1.8] GENESIS_G70_HIGHWAY_STABILIZER_CURVE_EXIT_LAT = 0.15 GENESIS_G70_ANGLE_OUTPUT_TAPER_MIN = 0.45 GENESIS_G70_ANGLE_OUTPUT_TAPER_START = 70.0 diff --git a/selfdrive/selfdrived/selfdrived.py b/selfdrive/selfdrived/selfdrived.py index aea14c1a93..ab07cafd65 100644 --- a/selfdrive/selfdrived/selfdrived.py +++ b/selfdrive/selfdrived/selfdrived.py @@ -664,7 +664,8 @@ class SelfdriveD: if self.big_model_active and big_failed: self.events.add(EventName.bigModelFailed) - not_running = {p.name for p in self.sm['managerState'].processes if not p.running and p.shouldBeRunning} + not_running = {p.name for p in self.sm['managerState'].processes + if not p.running and p.shouldBeRunning and p.name != 'stinger_object_shadow'} if self.sm.recv_frame['managerState'] and len(not_running): if not_running != self.not_running_prev: cloudlog.event("process_not_running", not_running=not_running, error=True) diff --git a/starpilot/common/assets/device_settings_layout.json b/starpilot/common/assets/device_settings_layout.json index 8d67a60e4b..d4f1eb2457 100644 --- a/starpilot/common/assets/device_settings_layout.json +++ b/starpilot/common/assets/device_settings_layout.json @@ -4981,6 +4981,18 @@ "is_parent_toggle": true, "settings_tier": "simple" }, + { + "key": "StingerObjectShadow", + "label": "Stinger Object Logging Beta", + "description": "Collect candidate object data on the 2022-2023 Kia Stinger for validation. Logging only: this does not enable radar-based braking or change steering. Default off. Keep driving normally, stay attentive, and upload full rlogs with bookmarked observations.", + "picker_description": "Read-only Stinger object validation; no braking or steering changes.", + "data_type": "bool", + "ui_type": "toggle", + "galaxy_only": true, + "vehicle_makes": ["Kia"], + "parent_key": "GalaxyDeveloperMode", + "settings_tier": "advanced" + }, { "key": "ClusterOffset", "label": "Dashboard Speed Offset", diff --git a/starpilot/system/stinger_object_shadow.py b/starpilot/system/stinger_object_shadow.py new file mode 100644 index 0000000000..6c85066919 --- /dev/null +++ b/starpilot/system/stinger_object_shadow.py @@ -0,0 +1,104 @@ +"""Opt-in, read-only Stinger object logging. Never publishes control or radar data.""" + +import math +import time +from collections import deque +from dataclasses import asdict + +from openpilot.tools.car_porting.stinger_object_tracks import MAX_EGO_AGE_NS, SLOT_COUNT, StingerObjectDecoder + + +PLATFORM = "KIA_STINGER_2022" +LOG_INTERVAL_NS = 500_000_000 +MAX_OBJECT_AGE_NS = 100_000_000 + + +class StingerObjectShadow: + def __init__(self, openpilot_longitudinal: bool): + self.openpilot_longitudinal = openpilot_longitudinal + self.decoder = StingerObjectDecoder() + self.ego_history = deque(maxlen=128) + self.objects: dict[int, dict] = {} + self.scc = None + self.object_count = 0 + self.last_log_ns = None + + def update_ego(self, timestamp_ns: int, speed: float, cruise_enabled: bool, brake_pressed: bool, steering_angle: float): + if not math.isfinite(speed) or not math.isfinite(steering_angle): + return + if self.ego_history and timestamp_ns <= self.ego_history[-1]["timestamp_ns"]: + return + self.ego_history.append({"timestamp_ns": timestamp_ns, "speed": speed, "cruise_enabled": cruise_enabled, + "brake_pressed": brake_pressed, "steering_angle": steering_angle}) + + def update_can(self, timestamp_ns: int, address: int, dat: bytes, bus: int): + if bus == 0 and address == 0x420 and len(dat) == 8: + raw = int.from_bytes(dat, "little") + self.scc = {"timestamp_ns": timestamp_ns, "payload": dat.hex(), "main_mode": bool(raw & 1), + "object_valid": bool((raw >> 16) & 1), "object_status": (raw >> 22) & 3, + "distance": ((raw >> 33) & 0x7ff) * 0.1, + "relative_speed": ((raw >> 44) & 0xfff) * 0.1 - 170.0} + + obj = self.decoder.update(timestamp_ns, address, dat, bus) + if obj is None: + return + ego = next((sample for sample in reversed(self.ego_history) + if 0 <= timestamp_ns - sample["timestamp_ns"] <= MAX_EGO_AGE_NS), None) + self.objects[obj.slot] = {"timestamp_ns": timestamp_ns, **asdict(obj), + "ego_timestamp_ns": ego["timestamp_ns"] if ego else None, + "ego_speed": ego["speed"] if ego else None, + "relative_speed": obj.relative_speed(ego["speed"]) if ego else None} + self.object_count += 1 + + def snapshot(self, timestamp_ns: int) -> dict | None: + if self.last_log_ns is not None and timestamp_ns - self.last_log_ns < LOG_INTERVAL_NS: + return None + self.last_log_ns = timestamp_ns + self.objects = {slot: obj for slot, obj in self.objects.items() + if 0 <= timestamp_ns - obj["timestamp_ns"] <= MAX_OBJECT_AGE_NS} + assert len(self.objects) <= SLOT_COUNT + scc = self.scc if self.scc and 0 <= timestamp_ns - self.scc["timestamp_ns"] <= MAX_OBJECT_AGE_NS else None + ego = self.ego_history[-1] if self.ego_history else None + if ego and not 0 <= timestamp_ns - ego["timestamp_ns"] <= MAX_EGO_AGE_NS: + ego = None + return {"timestamp_ns": timestamp_ns, "platform": PLATFORM, "shadow_only": True, + "openpilot_longitudinal": self.openpilot_longitudinal, "decoded_objects_total": self.object_count, + "objects": [self.objects[slot] for slot in sorted(self.objects)], "car_state": ego, + "scc11": scc, "scc11_independent_reference": scc is not None and not self.openpilot_longitudinal} + + +def main(): + from cereal import car, messaging + from openpilot.common.params import Params + from openpilot.common.swaglog import cloudlog + + params = Params() + cp_bytes = params.get("CarParams") + if cp_bytes is None or not params.get_bool("StingerObjectShadow") or params.get_bool("DisableLogging"): + return + with car.CarParams.from_bytes(cp_bytes) as CP: + if CP.carFingerprint != PLATFORM or CP.notCar: + return + collector = StingerObjectShadow(CP.openpilotLongitudinalControl) + + can_sock = messaging.sub_sock("can", timeout=100) + sm = messaging.SubMaster(["carState"]) + cloudlog.event("stinger_object_shadow_start", platform=PLATFORM, shadow_only=True) + while True: + sm.update(0) + if sm.updated["carState"] and sm.valid["carState"]: + CS = sm["carState"] + collector.update_ego(sm.logMonoTime["carState"], CS.vEgo, CS.cruiseState.enabled, CS.brakePressed, CS.steeringAngleDeg) + for event in messaging.drain_sock(can_sock, wait_for_one=True): + if event.which() == "can" and event.valid: + for msg in event.can: + collector.update_can(event.logMonoTime, msg.address, bytes(msg.dat), msg.src) + snapshot = collector.snapshot(time.monotonic_ns()) + if snapshot is not None: + if not params.get_bool("StingerObjectShadow") or params.get_bool("DisableLogging"): + return + cloudlog.event("stinger_object_shadow", **snapshot) + + +if __name__ == "__main__": + main() diff --git a/system/manager/process_config.py b/system/manager/process_config.py index c2324867bb..ef0cfc98fd 100644 --- a/system/manager/process_config.py +++ b/system/manager/process_config.py @@ -143,6 +143,12 @@ def run_v_asm(started: bool, params: Params, CP: car.CarParams, starpilot_toggle return started and getattr(starpilot_toggles, "v_asm_enabled", False) +def run_stinger_object_shadow(started: bool, params: Params, CP: car.CarParams, starpilot_toggles: SimpleNamespace) -> bool: + return (started and not CP.notCar and CP.carFingerprint == "KIA_STINGER_2022" and + params.get_bool("StingerObjectShadow") and not params.get_bool("DisableLogging") and + not getattr(starpilot_toggles, "no_logging", False)) + + def big_device_ui_process() -> NativeProcess: return NativeProcess( "ui", @@ -231,6 +237,7 @@ procs += [ PythonProcess("speed_limit_filler", "starpilot.system.speed_limit_filler", run_speed_limit_filler, nice=19), PythonProcess("speed_limit_vision", "starpilot.system.speed_limit_vision", run_speed_limit_vision, nice=19), PythonProcess("adj_spot_monitor_vision", "starpilot.system.adj_spot_monitor_vision", run_v_asm, nice=19), + PythonProcess("stinger_object_shadow", "starpilot.system.stinger_object_shadow", run_stinger_object_shadow, nice=19), ] managed_processes = {p.name: p for p in procs}