From e0cf4fd988977073dc579e5b66633bc27b216e38 Mon Sep 17 00:00:00 2001 From: firestar5683 <168790843+firestar5683@users.noreply.github.com> Date: Fri, 10 Apr 2026 20:57:30 -0500 Subject: [PATCH] kp / 2018 lat / 2017 lat / 2022 lat / GM long / trailer mode / --- launch_chffrplus.sh | 4 + opendbc_repo/opendbc/car/gm/carcontroller.py | 6 +- opendbc_repo/opendbc/car/gm/interface.py | 2 +- .../opendbc/car/torque_data/override.toml | 2 + .../opendbc/car/torque_data/substitute.toml | 2 - .../opendbc/car/toyota/carcontroller.py | 9 +- .../mine_connect_route_bookmarks.py | 289 ++++++++++++++++++ selfdrive/controls/lib/latcontrol_torque.py | 165 ++++++++-- selfdrive/controls/lib/lead_behavior.py | 4 +- selfdrive/controls/tests/test_latcontrol.py | 38 +++ .../controls/tests/test_lead_behavior.py | 5 + starpilot/common/testing_grounds.py | 8 +- starpilot/ui/qt/offroad/starpilot_settings.cc | 3 +- system/manager/launch_param_migrations.py | 71 +++++ .../test/test_launch_param_migrations.py | 69 +++++ 15 files changed, 645 insertions(+), 32 deletions(-) create mode 100644 scripts/speed_limit_vision/mine_connect_route_bookmarks.py create mode 100644 system/manager/launch_param_migrations.py create mode 100644 system/manager/test/test_launch_param_migrations.py diff --git a/launch_chffrplus.sh b/launch_chffrplus.sh index 81f91610..b7cd3247 100755 --- a/launch_chffrplus.sh +++ b/launch_chffrplus.sh @@ -98,6 +98,10 @@ function launch { # start manager cd system/manager + if ! python3 ./launch_param_migrations.py; then + echo "Launch param migrations failed; continuing boot." + fi + # Bootstrap runtime (e.g. /usr/comma after reset/uninstall) must go straight # to manager/setup flow. Do not run StarPilot prebuilt checks/builds here. if [ "$DIR" = "/usr/comma" ] || [ ! -d "$DIR/.git" ]; then diff --git a/opendbc_repo/opendbc/car/gm/carcontroller.py b/opendbc_repo/opendbc/car/gm/carcontroller.py index d0e38dc9..0b6abafa 100644 --- a/opendbc_repo/opendbc/car/gm/carcontroller.py +++ b/opendbc_repo/opendbc/car/gm/carcontroller.py @@ -66,6 +66,10 @@ def should_spoof_ecm_cruise_status(CP): ) +def get_testing_ground_1_brake_switch_bias(v_ego: float) -> int: + return int(round(np.interp(v_ego, [0.0, 6.0, 15.0, 30.0], [40.0, 85.0, 130.0, 170.0]))) + + class CarController(CarControllerBase): def __init__(self, dbc_names, CP): super().__init__(dbc_names, CP) @@ -458,7 +462,7 @@ class CarController(CarControllerBase): apply_gas_torque = np.clip(scaled_torque, self.params.MAX_ACC_REGEN, gas_max) brake_switch = int(round(np.interp(CS.out.vEgo, self.params.BRAKE_SWITCH_LOOKUP_BP, self.params.BRAKE_SWITCH_LOOKUP_V))) if testing_ground.use_1: - brake_switch_bias = int(round(np.interp(CS.out.vEgo, [0.0, 6.0, 15.0, 30.0], [60.0, 120.0, 180.0, 220.0]))) + brake_switch_bias = get_testing_ground_1_brake_switch_bias(CS.out.vEgo) brake_switch = min(self.params.ZERO_GAS, brake_switch + brake_switch_bias) brake_accel = min((scaled_torque - brake_switch) / (self.tireRadius * self.mass), 0) self.apply_gas = int(round(apply_gas_torque)) diff --git a/opendbc_repo/opendbc/car/gm/interface.py b/opendbc_repo/opendbc/car/gm/interface.py index c7312fcd..673097ee 100755 --- a/opendbc_repo/opendbc/car/gm/interface.py +++ b/opendbc_repo/opendbc/car/gm/interface.py @@ -469,7 +469,7 @@ class CarInterface(CarInterfaceBase): CarInterfaceBase.configure_torque_tune(candidate, ret.lateralTuning) elif candidate in (CAR.CHEVROLET_SUBURBAN, CAR.CHEVROLET_SUBURBAN_CC): - ret.steerActuatorDelay = 0.1 + ret.steerActuatorDelay = 0.2 CarInterfaceBase.configure_torque_tune(candidate, ret.lateralTuning) elif candidate == CAR.GMC_YUKON_CC: diff --git a/opendbc_repo/opendbc/car/torque_data/override.toml b/opendbc_repo/opendbc/car/torque_data/override.toml index c8d32b71..19963e51 100644 --- a/opendbc_repo/opendbc/car/torque_data/override.toml +++ b/opendbc_repo/opendbc/car/torque_data/override.toml @@ -55,6 +55,8 @@ legend = ["LAT_ACCEL_FACTOR", "MAX_LAT_ACCEL_MEASURED", "FRICTION"] "CHEVROLET_BOLT_CC_2018_2021" = [2.0, 2.0, 0.13] "CHEVROLET_BOLT_EUV" = [1.0, 2.0, 0.175] "CHEVROLET_SILVERADO" = [1.9, 1.9, 0.112] +"CHEVROLET_SUBURBAN" = [1.2, 2.5, 0.26] +"CHEVROLET_SUBURBAN_CC" = [1.2, 2.5, 0.26] "CHEVROLET_TRAILBLAZER" = [1.33, 1.9, 0.16] "CHEVROLET_TRAVERSE" = [1.33, 1.33, 0.18] "CHEVROLET_EQUINOX" = [2.5, 2.5, 0.05] diff --git a/opendbc_repo/opendbc/car/torque_data/substitute.toml b/opendbc_repo/opendbc/car/torque_data/substitute.toml index 6880ae9f..df3fa8d4 100644 --- a/opendbc_repo/opendbc/car/torque_data/substitute.toml +++ b/opendbc_repo/opendbc/car/torque_data/substitute.toml @@ -97,8 +97,6 @@ legend = ["LAT_ACCEL_FACTOR", "MAX_LAT_ACCEL_MEASURED", "FRICTION"] "CADILLAC_CT6_CC" = "CHEVROLET_VOLT" "CADILLAC_XT5_CC" = "GMC_ACADIA" "CHEVROLET_EQUINOX_CC" = "CHEVROLET_EQUINOX" -"CHEVROLET_SUBURBAN" = "CHEVROLET_SILVERADO" -"CHEVROLET_SUBURBAN_CC" = "CHEVROLET_SILVERADO" "GMC_YUKON_CC" = "CHEVROLET_SILVERADO" "CHEVROLET_TRAILBLAZER_CC" = "CHEVROLET_TRAILBLAZER" diff --git a/opendbc_repo/opendbc/car/toyota/carcontroller.py b/opendbc_repo/opendbc/car/toyota/carcontroller.py index 20955c90..c79b5e34 100644 --- a/opendbc_repo/opendbc/car/toyota/carcontroller.py +++ b/opendbc_repo/opendbc/car/toyota/carcontroller.py @@ -49,7 +49,7 @@ def get_long_tune(CP, params): k_f = 1.0 if CP.carFingerprint == CAR.TOYOTA_PRIUS: - k_f = 0.0 + k_f = 0.4 elif CP.carFingerprint not in TSS2_CAR: kiBP = [0., 5., 35.] kiV = [3.6, 2.4, 1.5] @@ -265,9 +265,14 @@ class CarController(CarControllerBase): -MAX_PITCH_COMPENSATION, MAX_PITCH_COMPENSATION)) pcm_accel_cmd += pitch_compensation + feedforward = pcm_accel_cmd + if self.CP.carFingerprint == CAR.TOYOTA_PRIUS: + # Preserve the smoother positive handoff, but let braking feedforward pull speed back down. + feedforward = min(feedforward, 0.0) + pcm_accel_cmd = self.long_pid.update(error_future, speed=CS.out.vEgo, - feedforward=pcm_accel_cmd, + feedforward=feedforward, freeze_integrator=actuators.longControlState != LongCtrlState.pid) else: self.long_pid.reset() diff --git a/scripts/speed_limit_vision/mine_connect_route_bookmarks.py b/scripts/speed_limit_vision/mine_connect_route_bookmarks.py new file mode 100644 index 00000000..fcf4e0a2 --- /dev/null +++ b/scripts/speed_limit_vision/mine_connect_route_bookmarks.py @@ -0,0 +1,289 @@ +#!/usr/bin/env python3 +from __future__ import annotations + +import argparse +import bz2 +import csv +import re + +from pathlib import Path + +import cv2 +import zstandard as zstd +from cereal import log +import starpilot.system.speed_limit_vision as slv + +if __package__ in (None, ""): + import sys + sys.path.insert(0, str(Path(__file__).resolve().parent)) + from common import ensure_dir, preferred_clip_root, resolve_workspace # type: ignore + from evaluate_bookmark_leadins import BookmarkWindow # type: ignore + from import_bookmark_leadins import extract_window_frames, write_contact_sheet # type: ignore + from localize_bookmark_signs import configure_models, iter_context_frames, score_frame # type: ignore +else: + from .common import ensure_dir, preferred_clip_root, resolve_workspace + from .evaluate_bookmark_leadins import BookmarkWindow + from .import_bookmark_leadins import extract_window_frames, write_contact_sheet + from .localize_bookmark_signs import configure_models, iter_context_frames, score_frame + + +DEFAULT_WORKSPACE = Path("/Volumes/T5/starpilot_speed_limit/workspace/speed_limit_training_clean") +ROUTE_ID_RE = re.compile(r"([0-9a-f]{16})/([^/]+)") +BOOKMARK_TYPES = ("bookmarkButton", "userBookmark") + + +def parse_args() -> argparse.Namespace: + parser = argparse.ArgumentParser(description="Mine Connect routes with preserved rlog/fcamera into bookmark review sheets and localized sign candidates.") + parser.add_argument("routes", nargs="+", help="Route ids like 'dongle/logid'.") + parser.add_argument("--workspace", type=Path, default=DEFAULT_WORKSPACE, help="Training workspace root.") + parser.add_argument("--clip-root", type=Path, default=preferred_clip_root(), help="Downloaded route clip root.") + parser.add_argument("--models-dir", type=Path, help="Optional directory containing speed_limit_us_detector.onnx and speed_limit_us_value_classifier.onnx.") + parser.add_argument("--lead-in", type=float, default=7.0, help="Seconds before each bookmark to sample into review sheets.") + parser.add_argument("--sample-every", type=float, default=0.5, help="Seconds between sampled lead-in frames.") + parser.add_argument("--max-samples", type=int, default=12, help="Max sampled frames per bookmark for the contact sheet.") + parser.add_argument("--search-before", type=float, default=10.0, help="Seconds before each bookmark to scan for the most sign-like frame.") + parser.add_argument("--search-after", type=float, default=1.0, help="Seconds after each bookmark to scan for the most sign-like frame.") + parser.add_argument("--localize-sample-every", type=float, default=0.25, help="Seconds between frames while searching for the best sign candidate.") + parser.add_argument("--top-k", type=int, default=1, help="Number of localized candidates to keep per bookmark.") + parser.add_argument("--overwrite", action="store_true", help="Overwrite any existing outputs.") + return parser.parse_args() + + +def parse_route_id(text: str) -> tuple[str, str]: + match = ROUTE_ID_RE.fullmatch(text.strip().replace("|", "/")) + if match is None: + raise ValueError(f"Unrecognized route id: {text}") + return match.group(1), match.group(2) + + +def read_log_bytes(path: Path) -> bytes: + if path.suffix == ".zst": + with path.open("rb") as handle: + return zstd.ZstdDecompressor().stream_reader(handle).read() + if path.suffix == ".bz2": + return bz2.decompress(path.read_bytes()) + return path.read_bytes() + + +def load_route_bookmarks(clip_root: Path, log_id: str) -> list[dict]: + segment_dirs = sorted(clip_root.glob(f"{log_id}--*"), key=lambda path: int(path.name.rsplit("--", 1)[-1])) + if not segment_dirs: + raise FileNotFoundError(f"No downloaded segments found for {log_id} under {clip_root}") + + route_start_monotime = None + raw_events: list[tuple[float, str]] = [] + for segment_dir in segment_dirs: + rlog_path = segment_dir / "rlog.zst" + if not rlog_path.exists(): + rlog_path = segment_dir / "rlog.bz2" + if not rlog_path.exists(): + continue + + events = list(log.Event.read_multiple_bytes(read_log_bytes(rlog_path))) + if not events: + continue + if route_start_monotime is None: + route_start_monotime = events[0].logMonoTime + + for event in events: + event_type = event.which() + if event_type not in BOOKMARK_TYPES: + continue + route_time_s = (event.logMonoTime - route_start_monotime) / 1e9 + raw_events.append((route_time_s, event_type)) + + raw_events.sort(key=lambda item: item[0]) + deduped: list[dict] = [] + for route_time_s, event_type in raw_events: + if deduped and abs(route_time_s - deduped[-1]["route_time_s"]) <= 0.5: + if event_type == "userBookmark": + deduped[-1]["event_type"] = event_type + deduped[-1]["route_time_s"] = route_time_s + continue + + segment = max(int(route_time_s // 60.0), 0) + segment_offset_s = route_time_s - segment * 60.0 + deduped.append({ + "event_type": event_type, + "route_time_s": route_time_s, + "segment": segment, + "segment_offset_s": segment_offset_s, + }) + return deduped + + +def write_localized_manifest(path: Path, rows: list[dict]) -> None: + ensure_dir(path.parent) + with path.open("w", encoding="utf-8", newline="") as handle: + writer = csv.DictWriter(handle, fieldnames=[ + "session_id", + "bookmark_number", + "route", + "segment", + "relative_time_s", + "source_video_path", + "score", + "proposal_confidence", + "class_id", + "is_regulatory", + "model_read", + "ocr_read", + "full_detection", + "frame_path", + "crop_path", + "box", + ]) + writer.writeheader() + writer.writerows(rows) + + +def fmt_detection(result) -> str: + if result is None: + return "" + return f"{result[0]}@{result[1]:.3f}" + + +def main() -> int: + args = parse_args() + workspace = resolve_workspace(args.workspace) + clip_root = args.clip_root.expanduser().resolve() + review_root = ensure_dir(workspace / "review" / "connect_route_bookmarks") + frame_dir = ensure_dir(review_root / "frames") + crop_dir = ensure_dir(review_root / "crops") + contact_sheet_dir = ensure_dir(review_root / "contact_sheets") + leadin_manifest_path = review_root / "bookmark_leadins.csv" + localized_manifest_path = review_root / "localized_bookmarks.csv" + + configure_models(args.models_dir) + daemon = slv.SpeedLimitVisionDaemon(use_runtime=False) + localized_rows: list[dict] = [] + leadin_rows: list[dict] = [] + + for raw_route in args.routes: + dongle_id, log_id = parse_route_id(raw_route) + session_id = f"connect_{dongle_id}_{log_id}" + bookmarks = load_route_bookmarks(clip_root, log_id) + if not bookmarks: + print(f"{raw_route}: no bookmark events found in downloaded rlogs") + continue + + print(f"{raw_route}: found {len(bookmarks)} bookmark(s)") + for bookmark_number, bookmark in enumerate(bookmarks, start=1): + window = BookmarkWindow( + bookmark_number=bookmark_number, + route=log_id, + segment=int(bookmark["segment"]), + segment_offset_s=float(bookmark["segment_offset_s"]), + leadin_start_s=float(bookmark["segment_offset_s"]) - args.lead_in, + spans_previous_segment=float(bookmark["segment_offset_s"]) - args.lead_in < 0.0, + ) + + sampled_frames = extract_window_frames({ + "route": log_id, + "segment": window.segment, + "segmentOffsetS": window.segment_offset_s, + "leadinStartS": window.leadin_start_s, + "spansPreviousSegment": window.spans_previous_segment, + }, clip_root, args.sample_every, args.max_samples) + + contact_sheet_frames = [] + contact_sheet_labels = [] + contact_sheet_name = f"{session_id}_bookmark_{bookmark_number:03d}.jpg" + contact_sheet_path = contact_sheet_dir / contact_sheet_name + + for sample_index, sample in enumerate(sampled_frames, start=1): + frame_name = f"{session_id}_bookmark_{bookmark_number:03d}_sample_{sample_index:02d}.jpg" + frame_path = frame_dir / frame_name + if args.overwrite or not frame_path.exists(): + cv2.imwrite(str(frame_path), sample["frame_bgr"], [cv2.IMWRITE_JPEG_QUALITY, 90]) + contact_sheet_frames.append(sample["frame_bgr"]) + contact_sheet_labels.append(f"t={sample['relative_offset_s']:+.2f}s") + leadin_rows.append({ + "session_id": session_id, + "bookmark_number": bookmark_number, + "route": log_id, + "segment": window.segment, + "segment_offset_s": f"{window.segment_offset_s:.3f}", + "sample_offset_s": f"{sample['relative_offset_s']:.3f}", + "frame_path": str(frame_path.relative_to(workspace)), + "contact_sheet_path": str(contact_sheet_path.relative_to(workspace)), + "source_video_path": str(sample["source_video"]), + "event_type": bookmark["event_type"], + "route_time_s": f"{bookmark['route_time_s']:.3f}", + }) + + if contact_sheet_frames: + write_contact_sheet(contact_sheet_path, contact_sheet_frames, contact_sheet_labels, args.overwrite) + + ranked = [] + for relative_time_s, source_video_path, _, frame_bgr in iter_context_frames( + clip_root, + window, + args.search_before, + args.search_after, + args.localize_sample_every, + ): + scored = score_frame(daemon, frame_bgr) + if scored is None: + continue + ranked.append((scored["score"], relative_time_s, source_video_path, frame_bgr, scored)) + + ranked.sort(key=lambda item: item[0], reverse=True) + for rank_index, (_, relative_time_s, source_video_path, frame_bgr, scored) in enumerate(ranked[:max(args.top_k, 1)], start=1): + x1, y1, x2, y2 = scored["box"] + crop = frame_bgr[y1:y2, x1:x2] + frame_name = f"{session_id}_bookmark_{bookmark_number:03d}_rank_{rank_index:02d}.jpg" + crop_name = f"{session_id}_bookmark_{bookmark_number:03d}_rank_{rank_index:02d}_crop.jpg" + frame_path = frame_dir / frame_name + crop_path = crop_dir / crop_name + if args.overwrite or not frame_path.exists(): + cv2.imwrite(str(frame_path), frame_bgr, [cv2.IMWRITE_JPEG_QUALITY, 90]) + if crop.size != 0 and (args.overwrite or not crop_path.exists()): + cv2.imwrite(str(crop_path), crop, [cv2.IMWRITE_JPEG_QUALITY, 90]) + + full_detection = scored["full_detection"] + localized_rows.append({ + "session_id": session_id, + "bookmark_number": bookmark_number, + "route": log_id, + "segment": window.segment, + "relative_time_s": f"{relative_time_s:.3f}", + "source_video_path": str(source_video_path), + "score": f"{scored['score']:.4f}", + "proposal_confidence": f"{scored['proposal_confidence']:.4f}", + "class_id": str(scored["class_id"]), + "is_regulatory": str(bool(scored["is_regulatory"])), + "model_read": fmt_detection(scored["model_read"]), + "ocr_read": fmt_detection(scored["ocr_read"]), + "full_detection": "" if full_detection is None else f"{full_detection.speed_limit_mph}@{full_detection.confidence:.3f}", + "frame_path": str(frame_path), + "crop_path": str(crop_path), + "box": ",".join(str(value) for value in scored["box"]), + }) + + ensure_dir(leadin_manifest_path.parent) + with leadin_manifest_path.open("w", encoding="utf-8", newline="") as handle: + writer = csv.DictWriter(handle, fieldnames=[ + "session_id", + "bookmark_number", + "route", + "segment", + "segment_offset_s", + "sample_offset_s", + "frame_path", + "contact_sheet_path", + "source_video_path", + "event_type", + "route_time_s", + ]) + writer.writeheader() + writer.writerows(leadin_rows) + + write_localized_manifest(localized_manifest_path, localized_rows) + print(f"Wrote {len(leadin_rows)} sampled lead-in frame row(s) to {leadin_manifest_path}") + print(f"Wrote {len(localized_rows)} localized candidate row(s) to {localized_manifest_path}") + return 0 + + +if __name__ == "__main__": + raise SystemExit(main()) diff --git a/selfdrive/controls/lib/latcontrol_torque.py b/selfdrive/controls/lib/latcontrol_torque.py index a637b05e..c253037c 100644 --- a/selfdrive/controls/lib/latcontrol_torque.py +++ b/selfdrive/controls/lib/latcontrol_torque.py @@ -23,7 +23,7 @@ from openpilot.starpilot.common.testing_grounds import testing_ground # Additionally, there is friction in the steering wheel that needs # to be overcome to move it at all, this is compensated for too. -KP = 0.7 +KP = 0.6 KI = 0.35 INTERP_SPEEDS = [1, 1.5, 2.0, 3.0, 5, 7.5, 10, 15, 30] @@ -62,6 +62,9 @@ BOLT_2017_LATERAL_TESTING_GROUND_ID = testing_ground.id_3 BOLT_2017_STEER_RATIO_TEST_SCALE = 1.045 BOLT_2017_STEER_RATIO_ONSET_SPEED = 20.0 * CV.MPH_TO_MS BOLT_2017_STEER_RATIO_ONSET_WIDTH = 4.0 * CV.MPH_TO_MS +BOLT_2017_CENTER_TAPER_LAT = 0.10 +BOLT_2017_CENTER_TAPER_WIDTH = 0.03 +BOLT_2017_CENTER_TAPER_GAIN = 0.055 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] @@ -89,18 +92,18 @@ BOLT_2018_2021_PHASE_SCALE = 0.10 BOLT_2018_2021_TURN_IN_BOOST_LEFT = 0.22 BOLT_2018_2021_TURN_IN_BOOST_RIGHT = 0.12 BOLT_2018_2021_UNWIND_TAPER_GAIN_LEFT = 0.80 -BOLT_2018_2021_UNWIND_TAPER_GAIN_RIGHT = 0.96 +BOLT_2018_2021_UNWIND_TAPER_GAIN_RIGHT = 1.04 BOLT_2018_2021_FRICTION_MULT = 1.01 BOLT_2018_2021_FRICTION_LAT_RISE = 0.24 BOLT_2018_2021_FRICTION_JERK_RISE = 0.28 BOLT_2018_2021_TURN_IN_THRESHOLD_REDUCTION_LEFT = 0.16 -BOLT_2018_2021_TURN_IN_THRESHOLD_REDUCTION_RIGHT = 0.13 +BOLT_2018_2021_TURN_IN_THRESHOLD_REDUCTION_RIGHT = 0.16 BOLT_2018_2021_UNWIND_THRESHOLD_INCREASE_LEFT = 0.15 -BOLT_2018_2021_UNWIND_THRESHOLD_INCREASE_RIGHT = 0.21 +BOLT_2018_2021_UNWIND_THRESHOLD_INCREASE_RIGHT = 0.25 BOLT_2018_2021_TURN_IN_FRICTION_BOOST_LEFT = 0.08 -BOLT_2018_2021_TURN_IN_FRICTION_BOOST_RIGHT = 0.06 +BOLT_2018_2021_TURN_IN_FRICTION_BOOST_RIGHT = 0.08 BOLT_2018_2021_UNWIND_FRICTION_REDUCTION_LEFT = 0.17 -BOLT_2018_2021_UNWIND_FRICTION_REDUCTION_RIGHT = 0.23 +BOLT_2018_2021_UNWIND_FRICTION_REDUCTION_RIGHT = 0.27 BOLT_2022_2023_LATERAL_TESTING_GROUND_ID = testing_ground.id_5 BOLT_2022_2023_FF_GAIN_LEFT = 0.13 @@ -127,6 +130,40 @@ BOLT_2022_2023_TURN_IN_FRICTION_BOOST_RIGHT = 0.04 BOLT_2022_2023_UNWIND_FRICTION_REDUCTION_LEFT = 0.21 BOLT_2022_2023_UNWIND_FRICTION_REDUCTION_RIGHT = 0.17 +SILVERADO_TRAILER_LATERAL_TESTING_GROUND_ID = testing_ground.id_6 +SILVERADO_TRAILER_FF_GAIN_LEFT = 0.11 +SILVERADO_TRAILER_FF_GAIN_RIGHT = 0.03 +SILVERADO_TRAILER_FF_ONSET = 0.18 +SILVERADO_TRAILER_FF_ONSET_WIDTH = 0.10 +SILVERADO_TRAILER_FF_CUTOFF = 1.50 +SILVERADO_TRAILER_FF_CUTOFF_WIDTH = 0.34 +SILVERADO_TRAILER_SPEED_BP = [ + 30.0 * CV.MPH_TO_MS, + 45.0 * CV.MPH_TO_MS, + 55.0 * CV.MPH_TO_MS, + 60.0 * CV.MPH_TO_MS, + 63.0 * CV.MPH_TO_MS, + 68.0 * CV.MPH_TO_MS, + 72.0 * CV.MPH_TO_MS, +] +SILVERADO_TRAILER_SPEED_V = [0.0, 0.35, 0.90, 1.00, 0.55, 0.15, 0.0] +SILVERADO_TRAILER_PHASE_SCALE = 0.18 +SILVERADO_TRAILER_FRICTION_MULT = 1.015 +SILVERADO_TRAILER_FRICTION_LAT_RISE = 0.28 +SILVERADO_TRAILER_FRICTION_JERK_RISE = 0.30 +SILVERADO_TRAILER_TURN_IN_BOOST_LEFT = 0.20 +SILVERADO_TRAILER_TURN_IN_BOOST_RIGHT = 0.06 +SILVERADO_TRAILER_UNWIND_TAPER_LEFT = 0.06 +SILVERADO_TRAILER_UNWIND_TAPER_RIGHT = 0.18 +SILVERADO_TRAILER_TURN_IN_THRESHOLD_REDUCTION_LEFT = 0.12 +SILVERADO_TRAILER_TURN_IN_THRESHOLD_REDUCTION_RIGHT = 0.04 +SILVERADO_TRAILER_UNWIND_THRESHOLD_INCREASE_LEFT = 0.03 +SILVERADO_TRAILER_UNWIND_THRESHOLD_INCREASE_RIGHT = 0.10 +SILVERADO_TRAILER_TURN_IN_FRICTION_BOOST_LEFT = 0.05 +SILVERADO_TRAILER_TURN_IN_FRICTION_BOOST_RIGHT = 0.02 +SILVERADO_TRAILER_UNWIND_FRICTION_REDUCTION_LEFT = 0.03 +SILVERADO_TRAILER_UNWIND_FRICTION_REDUCTION_RIGHT = 0.08 + def get_friction_threshold(v_ego: float) -> float: # Keep the speed-scaled friction threshold behavior. @@ -141,9 +178,17 @@ def _bolt_2017_sigmoid(x: float) -> float: return 1.0 / (1.0 + math.exp(-x)) +def _bolt_2017_high_speed_factor(v_ego: float) -> float: + return _bolt_2017_sigmoid((max(v_ego, 0.0) - BOLT_2017_STEER_RATIO_ONSET_SPEED) / BOLT_2017_STEER_RATIO_ONSET_WIDTH) + + def get_bolt_2017_steer_ratio_scale(v_ego: float) -> float: - onset = _bolt_2017_sigmoid((max(v_ego, 0.0) - BOLT_2017_STEER_RATIO_ONSET_SPEED) / BOLT_2017_STEER_RATIO_ONSET_WIDTH) - return 1.0 + ((BOLT_2017_STEER_RATIO_TEST_SCALE - 1.0) * onset) + return 1.0 + ((BOLT_2017_STEER_RATIO_TEST_SCALE - 1.0) * _bolt_2017_high_speed_factor(v_ego)) + + +def get_bolt_2017_center_taper_scale(desired_lateral_accel: float, v_ego: float) -> float: + center_window = _bolt_2017_sigmoid((BOLT_2017_CENTER_TAPER_LAT - abs(desired_lateral_accel)) / BOLT_2017_CENTER_TAPER_WIDTH) + return 1.0 - (BOLT_2017_CENTER_TAPER_GAIN * _bolt_2017_high_speed_factor(v_ego) * center_window) def _bolt_2017_low_speed_factor(v_ego: float) -> float: @@ -168,18 +213,19 @@ def get_bolt_2017_base_torque_scale(desired_lateral_accel: float) -> float: def get_bolt_2017_torque_scale(desired_lateral_accel: float, desired_lateral_jerk: float = 0.0, v_ego: float = 30.0) -> float: base_scale = get_bolt_2017_base_torque_scale(desired_lateral_accel) - if base_scale <= 1.0 or desired_lateral_jerk == 0.0: - return base_scale + scale = base_scale + if base_scale > 1.0 and desired_lateral_jerk != 0.0: + low_speed_factor = _bolt_2017_low_speed_factor(v_ego) + phase = _bolt_2017_transition_phase(desired_lateral_accel, desired_lateral_jerk) + turn_in_weight = max(phase, 0.0) + unwind_weight = max(-phase, 0.0) + turn_in_boost = 1.0 + (_bolt_2017_side_value(desired_lateral_accel, BOLT_2017_TURN_IN_BOOST_LEFT, BOLT_2017_TURN_IN_BOOST_RIGHT) * + turn_in_weight * (0.35 + 0.65 * low_speed_factor)) + unwind_taper = 1.0 - (_bolt_2017_side_value(desired_lateral_accel, BOLT_2017_UNWIND_TAPER_LEFT, BOLT_2017_UNWIND_TAPER_RIGHT) * + unwind_weight * (0.45 + 0.55 * low_speed_factor)) + scale = 1.0 + ((base_scale - 1.0) * turn_in_boost * max(unwind_taper, 0.0)) - low_speed_factor = _bolt_2017_low_speed_factor(v_ego) - phase = _bolt_2017_transition_phase(desired_lateral_accel, desired_lateral_jerk) - turn_in_weight = max(phase, 0.0) - unwind_weight = max(-phase, 0.0) - turn_in_boost = 1.0 + (_bolt_2017_side_value(desired_lateral_accel, BOLT_2017_TURN_IN_BOOST_LEFT, BOLT_2017_TURN_IN_BOOST_RIGHT) * - turn_in_weight * (0.35 + 0.65 * low_speed_factor)) - unwind_taper = 1.0 - (_bolt_2017_side_value(desired_lateral_accel, BOLT_2017_UNWIND_TAPER_LEFT, BOLT_2017_UNWIND_TAPER_RIGHT) * - unwind_weight * (0.45 + 0.55 * low_speed_factor)) - return 1.0 + ((base_scale - 1.0) * turn_in_boost * max(unwind_taper, 0.0)) + return scale * get_bolt_2017_center_taper_scale(desired_lateral_accel, v_ego) def bolt_2018_2021_lateral_testing_ground_active() -> bool: @@ -338,6 +384,78 @@ def get_bolt_2022_2023_friction_scale(v_ego: float, desired_lateral_accel: float return min(max(friction_scale, 0.92), 1.22) +def silverado_trailer_lateral_testing_ground_active() -> bool: + return testing_ground.use(SILVERADO_TRAILER_LATERAL_TESTING_GROUND_ID) + + +def _silverado_trailer_sigmoid(x: float) -> float: + return 1.0 / (1.0 + math.exp(-x)) + + +def _silverado_trailer_speed_factor(v_ego: float) -> float: + return float(np.interp(v_ego, SILVERADO_TRAILER_SPEED_BP, SILVERADO_TRAILER_SPEED_V)) + + +def _silverado_trailer_transition_phase(desired_lateral_accel: float, desired_lateral_jerk: float) -> float: + return math.tanh((desired_lateral_accel * desired_lateral_jerk) / SILVERADO_TRAILER_PHASE_SCALE) + + +def _silverado_trailer_side_value(desired_lateral_accel: float, left_value: float, right_value: float) -> float: + return left_value if desired_lateral_accel >= 0.0 else right_value + + +def _silverado_trailer_transition_envelope(v_ego: float, desired_lateral_accel: float, desired_lateral_jerk: float) -> float: + lat_factor = 1.0 - math.exp(-abs(desired_lateral_accel) / SILVERADO_TRAILER_FRICTION_LAT_RISE) + jerk_factor = 1.0 - math.exp(-abs(desired_lateral_jerk) / SILVERADO_TRAILER_FRICTION_JERK_RISE) + return _silverado_trailer_speed_factor(v_ego) * lat_factor * jerk_factor + + +def get_silverado_trailer_ff_scale(desired_lateral_accel: float, desired_lateral_jerk: float, v_ego: float) -> float: + if desired_lateral_accel == 0.0: + return 1.0 + + gain = _silverado_trailer_side_value(desired_lateral_accel, SILVERADO_TRAILER_FF_GAIN_LEFT, SILVERADO_TRAILER_FF_GAIN_RIGHT) + abs_lateral_accel = abs(desired_lateral_accel) + onset = _silverado_trailer_sigmoid((abs_lateral_accel - SILVERADO_TRAILER_FF_ONSET) / SILVERADO_TRAILER_FF_ONSET_WIDTH) + cutoff = _silverado_trailer_sigmoid((SILVERADO_TRAILER_FF_CUTOFF - abs_lateral_accel) / SILVERADO_TRAILER_FF_CUTOFF_WIDTH) + speed_factor = _silverado_trailer_speed_factor(v_ego) + extra_scale = gain * speed_factor * onset * cutoff + phase = _silverado_trailer_transition_phase(desired_lateral_accel, desired_lateral_jerk) + turn_in_weight = max(phase, 0.0) + unwind_weight = max(-phase, 0.0) + turn_in_boost = 1.0 + (_silverado_trailer_side_value(desired_lateral_accel, SILVERADO_TRAILER_TURN_IN_BOOST_LEFT, SILVERADO_TRAILER_TURN_IN_BOOST_RIGHT) * + turn_in_weight * speed_factor) + unwind_taper = 1.0 - (_silverado_trailer_side_value(desired_lateral_accel, SILVERADO_TRAILER_UNWIND_TAPER_LEFT, SILVERADO_TRAILER_UNWIND_TAPER_RIGHT) * + unwind_weight * (0.35 + 0.65 * speed_factor)) + return 1.0 + (extra_scale * turn_in_boost * max(unwind_taper, 0.0)) + + +def get_silverado_trailer_friction_threshold(v_ego: float, desired_lateral_accel: float = 0.0, desired_lateral_jerk: float = 0.0) -> float: + base_threshold = get_friction_threshold(v_ego) + transition_envelope = _silverado_trailer_transition_envelope(v_ego, desired_lateral_accel, desired_lateral_jerk) + phase = _silverado_trailer_transition_phase(desired_lateral_accel, desired_lateral_jerk) + turn_in_weight = max(phase, 0.0) + unwind_weight = max(-phase, 0.0) + threshold_scale = 1.0 - (_silverado_trailer_side_value(desired_lateral_accel, SILVERADO_TRAILER_TURN_IN_THRESHOLD_REDUCTION_LEFT, SILVERADO_TRAILER_TURN_IN_THRESHOLD_REDUCTION_RIGHT) * + transition_envelope * turn_in_weight) + threshold_scale += (_silverado_trailer_side_value(desired_lateral_accel, SILVERADO_TRAILER_UNWIND_THRESHOLD_INCREASE_LEFT, SILVERADO_TRAILER_UNWIND_THRESHOLD_INCREASE_RIGHT) * + transition_envelope * unwind_weight) + return base_threshold * min(max(threshold_scale, 0.86), 1.12) + + +def get_silverado_trailer_friction_scale(v_ego: float, desired_lateral_accel: float, desired_lateral_jerk: float) -> float: + transition_envelope = _silverado_trailer_transition_envelope(v_ego, desired_lateral_accel, desired_lateral_jerk) + phase = _silverado_trailer_transition_phase(desired_lateral_accel, desired_lateral_jerk) + turn_in_weight = max(phase, 0.0) + unwind_weight = max(-phase, 0.0) + friction_scale = SILVERADO_TRAILER_FRICTION_MULT + friction_scale += (_silverado_trailer_side_value(desired_lateral_accel, SILVERADO_TRAILER_TURN_IN_FRICTION_BOOST_LEFT, SILVERADO_TRAILER_TURN_IN_FRICTION_BOOST_RIGHT) * + transition_envelope * turn_in_weight) + friction_scale -= (_silverado_trailer_side_value(desired_lateral_accel, SILVERADO_TRAILER_UNWIND_FRICTION_REDUCTION_LEFT, SILVERADO_TRAILER_UNWIND_FRICTION_REDUCTION_RIGHT) * + transition_envelope * unwind_weight) + return min(max(friction_scale, 0.93), 1.12) + + class LatControlTorque(LatControl): def __init__(self, CP, CI, dt): super().__init__(CP, CI, dt) @@ -363,6 +481,7 @@ class LatControlTorque(LatControl): self.is_bolt_2022_2023 = CP.carFingerprint in BOLT_2022_2023_CARS self.is_bolt_2018_2021 = CP.carFingerprint in BOLT_2018_2021_CARS self.is_bolt_2017 = CP.carFingerprint in BOLT_2017_CARS + self.is_silverado = CP.carFingerprint == GM_CAR.CHEVROLET_SILVERADO self.use_bolt_ff_scaling = self.is_bolt_2022_2023 or self.is_bolt_2018_2021 or self.is_bolt_2017 self.use_bolt_ki_multiplier = self.use_bolt_ff_scaling self.torque_ff_scale_pos = 1.0 @@ -445,6 +564,7 @@ class LatControlTorque(LatControl): 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() + 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: @@ -454,6 +574,13 @@ class LatControlTorque(LatControl): elif bolt_2018_2021_test_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: + # Trailer pulls in the reference log showed a repeatable left-turn under-response + # centered around 55-65 mph. Bias feedforward and turn-in friction there, while + # softening right-side unwind so the experimental B tune stays bounded. + ff *= get_silverado_trailer_ff_scale(setpoint, desired_lateral_jerk, CS.vEgo) + friction_threshold = get_silverado_trailer_friction_threshold(CS.vEgo, setpoint, desired_lateral_jerk) + friction_scale = get_silverado_trailer_friction_scale(CS.vEgo, setpoint, desired_lateral_jerk) ff += friction_scale * get_friction(error_with_lsf + JERK_GAIN * desired_lateral_jerk, lateral_accel_deadzone, friction_threshold, self.torque_params) deadzone_boost_active = False if self.torque_deadzone_boost > 0.0 and abs(gravity_adjusted_future_lateral_accel) < DEADZONE_BOOST_LAT_ACCEL: diff --git a/selfdrive/controls/lib/lead_behavior.py b/selfdrive/controls/lib/lead_behavior.py index 3c5cc0e3..aced94b4 100644 --- a/selfdrive/controls/lib/lead_behavior.py +++ b/selfdrive/controls/lib/lead_behavior.py @@ -18,14 +18,14 @@ def get_tracked_lead_catchup_bias(v_ego: float, lead_distance: float, desired_ga # Encourage ACC to treat a tracked lead as the active constraint when we're # hanging far above the requested time gap, but don't override cruise for a # truly distant lead or one we're already closing on decisively. - if actual_hw <= max(desired_hw + 0.35, 1.75): + if actual_hw <= max(desired_hw + 0.3, 1.72): return 0.0 if actual_hw >= max(desired_hw + 1.6, 3.0): return 0.0 if closing_speed > max(2.5, 0.12 * v_ego): return 0.0 - return min(gap_error * 0.5, max(12.0, 0.6 * v_ego)) + return min(gap_error * 0.65, max(14.0, 0.75 * v_ego)) def should_disable_far_lead_throttle(v_ego: float, lead_distance: float, desired_gap: float, diff --git a/selfdrive/controls/tests/test_latcontrol.py b/selfdrive/controls/tests/test_latcontrol.py index a04047d3..939ea414 100644 --- a/selfdrive/controls/tests/test_latcontrol.py +++ b/selfdrive/controls/tests/test_latcontrol.py @@ -14,6 +14,7 @@ from openpilot.selfdrive.controls.lib.latcontrol_angle import LatControlAngle from openpilot.selfdrive.controls.lib.latcontrol_pid import LatControlPID from openpilot.selfdrive.controls.lib.latcontrol_torque import ( LatControlTorque, + get_bolt_2017_center_taper_scale, get_friction_threshold, get_bolt_2017_base_torque_scale, get_bolt_2017_steer_ratio_scale, @@ -25,6 +26,9 @@ from openpilot.selfdrive.controls.lib.latcontrol_torque import ( get_bolt_2018_2021_friction_scale, get_bolt_2018_2021_friction_threshold, get_bolt_2018_2021_torque_scale, + get_silverado_trailer_ff_scale, + get_silverado_trailer_friction_scale, + get_silverado_trailer_friction_threshold, ) @@ -61,6 +65,9 @@ class TestLatControl: assert 1.0 < get_bolt_2017_steer_ratio_scale(10.0 * 0.44704) < get_bolt_2017_steer_ratio_scale(20.0 * 0.44704) < get_bolt_2017_steer_ratio_scale(30.0 * 0.44704) assert get_bolt_2017_steer_ratio_scale(5.0 * 0.44704) < 1.01 assert get_bolt_2017_steer_ratio_scale(35.0 * 0.44704) > 1.04 + assert get_bolt_2017_center_taper_scale(0.0, 30.0 * 0.44704) < get_bolt_2017_center_taper_scale(0.10, 30.0 * 0.44704) < get_bolt_2017_center_taper_scale(0.20, 30.0 * 0.44704) <= 1.0 + assert get_bolt_2017_center_taper_scale(0.0, 30.0 * 0.44704) < get_bolt_2017_center_taper_scale(0.0, 10.0 * 0.44704) + assert get_bolt_2017_torque_scale(0.0, 0.0, 30.0 * 0.44704) < 1.0 assert get_bolt_2017_torque_scale(0.6, 0.6, 8.0) > get_bolt_2017_torque_scale(0.6, 0.0, 8.0) > get_bolt_2017_torque_scale(0.6, -0.6, 8.0) assert get_bolt_2017_torque_scale(-0.6, -0.6, 8.0) > get_bolt_2017_torque_scale(-0.6, 0.0, 8.0) > get_bolt_2017_torque_scale(-0.6, 0.6, 8.0) assert get_bolt_2017_torque_scale(0.6, 0.6, 8.0) > get_bolt_2017_torque_scale(-0.6, -0.6, 8.0) @@ -120,6 +127,29 @@ class TestLatControl: assert left_turn_in > right_turn_in > base assert base > right_unwind > left_unwind + def test_silverado_trailer_ff_scale_curve(self): + assert get_silverado_trailer_ff_scale(0.0, 0.0, 55.0 * 0.44704) == 1.0 + assert get_silverado_trailer_ff_scale(0.7, 0.7, 60.0 * 0.44704) > get_silverado_trailer_ff_scale(-0.7, 0.7, 60.0 * 0.44704) + assert get_silverado_trailer_ff_scale(0.7, 0.7, 60.0 * 0.44704) > get_silverado_trailer_ff_scale(0.7, -0.7, 60.0 * 0.44704) + assert get_silverado_trailer_ff_scale(0.7, 0.7, 70.0 * 0.44704) < get_silverado_trailer_ff_scale(0.7, 0.7, 60.0 * 0.44704) + + def test_silverado_trailer_friction_threshold_curve(self): + base = get_friction_threshold(60.0 * 0.44704) + left_turn_in = get_silverado_trailer_friction_threshold(60.0 * 0.44704, 0.7, 0.8) + right_turn_in = get_silverado_trailer_friction_threshold(60.0 * 0.44704, -0.7, -0.8) + left_unwind = get_silverado_trailer_friction_threshold(60.0 * 0.44704, 0.7, -0.8) + right_unwind = get_silverado_trailer_friction_threshold(60.0 * 0.44704, -0.7, 0.8) + assert left_turn_in < right_turn_in < base < left_unwind < right_unwind + + def test_silverado_trailer_friction_scale_curve(self): + base = get_silverado_trailer_friction_scale(60.0 * 0.44704, 0.7, 0.0) + left_turn_in = get_silverado_trailer_friction_scale(60.0 * 0.44704, 0.7, 0.8) + right_turn_in = get_silverado_trailer_friction_scale(60.0 * 0.44704, -0.7, -0.8) + left_unwind = get_silverado_trailer_friction_scale(60.0 * 0.44704, 0.7, -0.8) + right_unwind = get_silverado_trailer_friction_scale(60.0 * 0.44704, -0.7, 0.8) + assert left_turn_in > right_turn_in > base + assert base > left_unwind > right_unwind + def test_bolt_2017_testing_ground_update_path(self, monkeypatch): 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) @@ -144,6 +174,14 @@ class TestLatControl: assert lac_log.active + def test_silverado_trailer_testing_ground_update_path(self, monkeypatch): + controller, VM, CS, params, starpilot_toggles = self._build_torque_controller(GM.CHEVROLET_SILVERADO) + monkeypatch.setattr(latcontrol_torque, "silverado_trailer_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 + @parameterized.expand([(HONDA.HONDA_CIVIC, LatControlPID), (TOYOTA.TOYOTA_RAV4, LatControlTorque), (NISSAN.NISSAN_LEAF, LatControlAngle), (GM.CHEVROLET_BOLT_ACC_2022_2023, LatControlTorque)]) def test_saturation(self, car_name, controller): diff --git a/selfdrive/controls/tests/test_lead_behavior.py b/selfdrive/controls/tests/test_lead_behavior.py index 4b5e7ba7..69f5189a 100644 --- a/selfdrive/controls/tests/test_lead_behavior.py +++ b/selfdrive/controls/tests/test_lead_behavior.py @@ -19,6 +19,11 @@ def test_tracked_lead_catchup_bias_ignores_very_far_gap(): assert bias == 0.0 +def test_tracked_lead_catchup_bias_applies_to_two_second_highway_gap(): + bias = get_tracked_lead_catchup_bias(30.4, 63.0, 40.0, 0.4) + assert bias > 14.0 + + def test_disable_far_lead_throttle_rejects_two_second_plus_gap(): should_disable = should_disable_far_lead_throttle(31.4, 78.7, 38.0, 0.1, False) assert not should_disable diff --git a/starpilot/common/testing_grounds.py b/starpilot/common/testing_grounds.py index 698e185e..8d40cd24 100644 --- a/starpilot/common/testing_grounds.py +++ b/starpilot/common/testing_grounds.py @@ -68,10 +68,10 @@ TESTING_GROUNDS_SLOT_DEFINITIONS = ( }, { "id": TESTING_GROUND_6, - "name": "Unused", - "description": "", - "aLabel": "A", - "bLabel": "B", + "name": "Silverado Trailer Mode", + "description": "Silverado/Sierra lateral A/B sandbox for trailer-related hugging and turn-in response.", + "aLabel": "A - Installed tune", + "bLabel": "B - Trailer assist tune", }, { "id": TESTING_GROUND_7, diff --git a/starpilot/ui/qt/offroad/starpilot_settings.cc b/starpilot/ui/qt/offroad/starpilot_settings.cc index 61900312..04d84dad 100644 --- a/starpilot/ui/qt/offroad/starpilot_settings.cc +++ b/starpilot/ui/qt/offroad/starpilot_settings.cc @@ -386,7 +386,8 @@ void StarPilotSettingsWindow::updateVariables() { longitudinalActuatorDelay = CP.getLongitudinalActuatorDelay(); startAccel = CP.getStartAccel(); steerActuatorDelay = CP.getSteerActuatorDelay(); - steerKp = 1.0f; + // Keep Qt stock-sync aligned with selfdrive/controls/lib/latcontrol_torque.py::KP. + steerKp = 0.6f; steerRatio = CP.getSteerRatio(); stopAccel = CP.getStopAccel(); stoppingDecelRate = CP.getStoppingDecelRate(); diff --git a/system/manager/launch_param_migrations.py b/system/manager/launch_param_migrations.py new file mode 100644 index 00000000..6ce79d6a --- /dev/null +++ b/system/manager/launch_param_migrations.py @@ -0,0 +1,71 @@ +#!/usr/bin/env python3 +from __future__ import annotations + +import sys + +from pathlib import Path +from typing import Protocol + +LONG_PITCH_KEY = "LongPitch" +STEER_KP_KEY = "SteerKP" +STEER_KP_STOCK_KEY = "SteerKPStock" + +DEFAULT_STEER_KP = 0.6 +LEGACY_STEER_KP = 0.7 +QT_STEER_KP_PLACEHOLDER = 1.0 + +LAUNCH_PARAM_MIGRATION_MARKER = ".starpilot_launch_param_migrations_v1" + + +class ParamsLike(Protocol): + def get_param_path(self, key: str = "") -> str: ... + def get_bool(self, key: str) -> bool: ... + def get_float(self, key: str) -> float: ... + def put_bool(self, key: str, value: bool) -> None: ... + def put_float(self, key: str, value: float) -> None: ... + + +def _approx_equal(lhs: float, rhs: float, tolerance: float = 1e-6) -> bool: + return abs(lhs - rhs) <= tolerance + + +def _default_marker_path(params: ParamsLike) -> Path: + return Path(params.get_param_path()) / LAUNCH_PARAM_MIGRATION_MARKER + + +def apply_launch_param_migrations(params: ParamsLike, marker_path: Path | None = None) -> None: + marker = marker_path or _default_marker_path(params) + if marker.exists(): + return + + marker.parent.mkdir(parents=True, exist_ok=True) + + if not params.get_bool(LONG_PITCH_KEY): + params.put_bool(LONG_PITCH_KEY, True) + + steer_kp = params.get_float(STEER_KP_KEY) + if _approx_equal(steer_kp, 0.0) or _approx_equal(steer_kp, LEGACY_STEER_KP): + params.put_float(STEER_KP_KEY, DEFAULT_STEER_KP) + + steer_kp_stock = params.get_float(STEER_KP_STOCK_KEY) + if (_approx_equal(steer_kp_stock, 0.0) or + _approx_equal(steer_kp_stock, LEGACY_STEER_KP) or + _approx_equal(steer_kp_stock, QT_STEER_KP_PLACEHOLDER)): + params.put_float(STEER_KP_STOCK_KEY, DEFAULT_STEER_KP) + + marker.touch() + + +def main() -> int: + try: + from openpilot.common.params import Params + + apply_launch_param_migrations(Params()) + except Exception as exc: + print(f"launch_param_migrations.py failed: {exc}", file=sys.stderr) + return 1 + return 0 + + +if __name__ == "__main__": + raise SystemExit(main()) diff --git a/system/manager/test/test_launch_param_migrations.py b/system/manager/test/test_launch_param_migrations.py new file mode 100644 index 00000000..3ef4d735 --- /dev/null +++ b/system/manager/test/test_launch_param_migrations.py @@ -0,0 +1,69 @@ +from pathlib import Path + +from openpilot.system.manager.launch_param_migrations import ( + DEFAULT_STEER_KP, + LAUNCH_PARAM_MIGRATION_MARKER, + apply_launch_param_migrations, +) + + +class FileBackedFakeParams: + def __init__(self, root: Path): + self.root = root + self.root.mkdir(parents=True, exist_ok=True) + + def get_param_path(self, key=""): + if key: + return str(self.root / (key.decode() if isinstance(key, bytes) else str(key))) + return str(self.root) + + def get(self, key): + path = Path(self.get_param_path(key)) + if not path.is_file(): + return None + return path.read_text(encoding="utf-8") + + def get_bool(self, key): + value = self.get(key) + return value == "1" + + def get_float(self, key): + value = self.get(key) + return float(value) if value is not None else 0.0 + + def put_bool(self, key, value): + Path(self.get_param_path(key)).write_text("1" if value else "0", encoding="utf-8") + + def put_float(self, key, value): + Path(self.get_param_path(key)).write_text(str(float(value)), encoding="utf-8") + + +def test_apply_launch_param_migrations_sets_branch_defaults_once(tmp_path): + params = FileBackedFakeParams(tmp_path / "params") + + params.put_bool("LongPitch", False) + params.put_float("SteerKP", 0.7) + params.put_float("SteerKPStock", 1.0) + + apply_launch_param_migrations(params) + + assert params.get_bool("LongPitch") + assert params.get_float("SteerKP") == DEFAULT_STEER_KP + assert params.get_float("SteerKPStock") == DEFAULT_STEER_KP + assert (tmp_path / "params" / LAUNCH_PARAM_MIGRATION_MARKER).is_file() + + +def test_apply_launch_param_migrations_does_not_reapply_after_marker(tmp_path): + params = FileBackedFakeParams(tmp_path / "params") + marker = tmp_path / "params" / LAUNCH_PARAM_MIGRATION_MARKER + + params.put_bool("LongPitch", False) + params.put_float("SteerKP", 0.65) + params.put_float("SteerKPStock", DEFAULT_STEER_KP) + marker.touch() + + apply_launch_param_migrations(params, marker) + + assert not params.get_bool("LongPitch") + assert params.get_float("SteerKP") == 0.65 + assert params.get_float("SteerKPStock") == DEFAULT_STEER_KP