yas

opendbc_repo/opendbc/car/toyota/interface.py

@@ -113,7 +113,9 @@ class CarInterface(CarInterfaceBase):
 
     # No radar dbc for cars without DSU which are not TSS 2.0
     # TODO: make an adas dbc file for dsu-less models
-    ret.radarUnavailable = Bus.radar not in DBC[candidate] or candidate in (NO_DSU_CAR - TSS2_CAR)
+    ret.radarUnavailable = Bus.radar not in DBC[candidate] or candidate in (NO_DSU_CAR - TSS2_CAR - {CAR.TOYOTA_CAMRY})
+    if candidate == CAR.TOYOTA_CAMRY:
+      ret.radarTimeStepDEPRECATED = 0.1
 
     # Since we don't yet parse radar on TSS2/TSS-P radar-based ACC cars, gate
     # longitudinal behind the alpha-long toggle.

opendbc_repo/opendbc/car/toyota/radar_interface.py

@@ -2,9 +2,14 @@
 from opendbc.can import CANParser
 from opendbc.car import Bus
 from opendbc.car.structs import RadarData
-from opendbc.car.toyota.values import DBC, TSS2_CAR
+from opendbc.car.toyota.values import CAR, DBC, TSS2_CAR
 from opendbc.car.interfaces import RadarInterfaceBase
 
+RADAR_ACC_TSSP_CAR = {CAR.TOYOTA_CAMRY}
+TSSP_CLUSTER_MSGS = list(range(0x680, 0x686))
+KPH_TO_MS = 1. / 3.6
+TSSP_RADAR_EGO_SPEED_SCALE = 0.922
+
 
 def _create_radar_can_parser(car_fingerprint):
   if car_fingerprint in TSS2_CAR:
@@ -21,22 +26,37 @@ def _create_radar_can_parser(car_fingerprint):
   return CANParser(DBC[car_fingerprint][Bus.radar], messages, 1)
 
 
+def _create_tssp_radar_can_parser(car_fingerprint):
+  return CANParser(DBC[car_fingerprint][Bus.radar], [(addr, 10) for addr in TSSP_CLUSTER_MSGS], 1)
+
+
+def _create_wheel_speed_can_parser(car_fingerprint):
+  return CANParser(DBC[car_fingerprint][Bus.pt], [("WHEEL_SPEEDS", 80)], 0)
+
+
 class RadarInterface(RadarInterfaceBase):
   def __init__(self, CP):
     super().__init__(CP)
     self.track_id = 0
+    self.radar_acc_tssp = CP.carFingerprint in RADAR_ACC_TSSP_CAR
 
-    if CP.carFingerprint in TSS2_CAR:
-      self.RADAR_A_MSGS = list(range(0x180, 0x190))
-      self.RADAR_B_MSGS = list(range(0x190, 0x1a0))
+    if self.radar_acc_tssp:
+      self.RADAR_MSGS = TSSP_CLUSTER_MSGS
+      self.rcp = None if CP.radarUnavailable else _create_tssp_radar_can_parser(CP.carFingerprint)
+      self.pt_cp = None if CP.radarUnavailable else _create_wheel_speed_can_parser(CP.carFingerprint)
+      self.trigger_msg = self.RADAR_MSGS[-1]
     else:
-      self.RADAR_A_MSGS = list(range(0x210, 0x220))
-      self.RADAR_B_MSGS = list(range(0x220, 0x230))
+      if CP.carFingerprint in TSS2_CAR:
+        self.RADAR_A_MSGS = list(range(0x180, 0x190))
+        self.RADAR_B_MSGS = list(range(0x190, 0x1a0))
+      else:
+        self.RADAR_A_MSGS = list(range(0x210, 0x220))
+        self.RADAR_B_MSGS = list(range(0x220, 0x230))
+      self.valid_cnt = {key: 0 for key in self.RADAR_A_MSGS}
+      self.rcp = None if CP.radarUnavailable else _create_radar_can_parser(CP.carFingerprint)
+      self.pt_cp = None
+      self.trigger_msg = self.RADAR_B_MSGS[-1]
 
-    self.valid_cnt = {key: 0 for key in self.RADAR_A_MSGS}
-
-    self.rcp = None if CP.radarUnavailable else _create_radar_can_parser(CP.carFingerprint)
-    self.trigger_msg = self.RADAR_B_MSGS[-1]
     self.updated_messages = set()
 
   def update(self, can_strings):
@@ -45,16 +65,72 @@ class RadarInterface(RadarInterfaceBase):
 
     vls = self.rcp.update(can_strings)
     self.updated_messages.update(vls)
+    if self.pt_cp is not None:
+      self.pt_cp.update(can_strings)
 
     if self.trigger_msg not in self.updated_messages:
       return None
 
+    if self.pt_cp is not None and not self.pt_cp.can_valid:
+      self.updated_messages.clear()
+      ret = RadarData()
+      ret.errors.canError = True
+      return ret
+
     rr = self._update(self.updated_messages)
     self.updated_messages.clear()
 
     return rr
 
+  def _get_v_ego(self):
+    ws = self.pt_cp.vl["WHEEL_SPEEDS"]
+    wheel_speed = (ws["WHEEL_SPEED_FL"] + ws["WHEEL_SPEED_FR"] +
+                   ws["WHEEL_SPEED_RL"] + ws["WHEEL_SPEED_RR"]) / 4.
+    return wheel_speed * KPH_TO_MS * self.CP.wheelSpeedFactor
+
+  def _update_tssp(self, updated_messages):
+    ret = RadarData()
+    if not self.rcp.can_valid:
+      ret.errors.canError = True
+
+    v_ego = self._get_v_ego()
+    updated_ids = set()
+    for ii in sorted(updated_messages):
+      if ii not in self.RADAR_MSGS:
+        continue
+
+      cpt = self.rcp.vl[ii]
+      track_id = int(cpt["ID"])
+      if track_id == 0x3f or cpt["LONG_DIST"] <= 0:
+        continue
+
+      updated_ids.add(track_id)
+      if track_id not in self.pts:
+        self.pts[track_id] = RadarData.RadarPoint()
+        self.pts[track_id].trackId = self.track_id
+        self.track_id += 1
+
+      self.pts[track_id].dRel = float(cpt["LONG_DIST"])
+      self.pts[track_id].yRel = -float(cpt["LAT_DIST"])
+      self.pts[track_id].vRel = float(cpt["SPEED"]) - v_ego * TSSP_RADAR_EGO_SPEED_SCALE
+      self.pts[track_id].aRel = float("nan")
+      self.pts[track_id].yvRel = float(cpt["LAT_SPEED"])
+      self.pts[track_id].measured = True
+
+    for track_id in list(self.pts):
+      if track_id not in updated_ids:
+        del self.pts[track_id]
+
+    ret.points = list(self.pts.values())
+    return ret
+
   def _update(self, updated_messages):
+    if self.radar_acc_tssp:
+      return self._update_tssp(updated_messages)
+
+    return self._update_denso(updated_messages)
+
+  def _update_denso(self, updated_messages):
     ret = RadarData()
     if not self.rcp.can_valid:
       ret.errors.canError = True

opendbc_repo/opendbc/car/toyota/tests/test_toyota.py

@@ -1,5 +1,6 @@
 from types import SimpleNamespace
 
+import pytest
 from hypothesis import given, settings, strategies as st
 
 from opendbc.car import Bus, structs
@@ -13,6 +14,7 @@ from opendbc.car.toyota.carcontroller import CarController, get_prius_positive_f
 from opendbc.car.toyota.carstate import calculate_interceptor_gas_pressed
 from opendbc.car.toyota.fingerprints import FW_VERSIONS
 from opendbc.car.toyota.interface import CarInterface
+from opendbc.car.toyota.radar_interface import RadarInterface, TSSP_RADAR_EGO_SPEED_SCALE
 from opendbc.car.toyota.values import CAR, DBC, TSS2_CAR, ANGLE_CONTROL_CAR, RADAR_ACC_CAR, SECOC_CAR, \
                                                   FW_QUERY_CONFIG, PLATFORM_CODE_ECUS, FUZZY_EXCLUDED_PLATFORMS, \
                                                   ToyotaFlags, ToyotaSafetyFlags, get_platform_codes
@@ -80,6 +82,63 @@ class TestToyotaInterfaces:
     assert abs(car_params.vEgoStopping - 0.25) < 1e-6
     assert abs(car_params.vEgoStarting - 0.25) < 1e-6
 
+  def test_camry_continental_radar_keeps_standard_longitudinal_tune(self):
+    fingerprint = {bus: ({0x2FF: 8} if bus == 0 else {}) for bus in range(8)}
+    hybrid_fw = [CarParams.CarFw(ecu=Ecu.hybrid, address=0x7D2, fwVersion=b"test")]
+    car_params = CarInterface.get_params(
+      CAR.TOYOTA_CAMRY,
+      fingerprint,
+      hybrid_fw,
+      alpha_long=True,
+      is_release=False,
+      docs=False,
+      starpilot_toggles=SimpleNamespace(),
+    )
+
+    assert car_params.openpilotLongitudinalControl
+    assert not car_params.radarUnavailable
+    assert abs(car_params.radarTimeStepDEPRECATED - 0.1) < 1e-6
+    assert abs(car_params.longitudinalActuatorDelay - 0.15) < 1e-6
+    assert abs(car_params.vEgoStopping - 0.5) < 1e-6
+    assert abs(car_params.vEgoStarting - 0.5) < 1e-6
+    assert abs(car_params.stoppingDecelRate - 0.8) < 1e-6
+    assert not car_params.flags & ToyotaFlags.NO_STOP_TIMER.value
+
+    radar_interface = RadarInterface(car_params)
+    assert radar_interface.radar_acc_tssp
+    assert radar_interface.rcp is not None
+    assert radar_interface.pt_cp is not None
+
+  def test_camry_continental_radar_converts_absolute_target_speed(self):
+    radar_interface = RadarInterface.__new__(RadarInterface)
+    radar_interface.CP = SimpleNamespace(wheelSpeedFactor=1.0)
+    radar_interface.pts = {}
+    radar_interface.track_id = 0
+    radar_interface.RADAR_MSGS = [0x680]
+    radar_interface.pt_cp = SimpleNamespace(vl={
+      "WHEEL_SPEEDS": {
+        "WHEEL_SPEED_FL": 36.0,
+        "WHEEL_SPEED_FR": 36.0,
+        "WHEEL_SPEED_RL": 36.0,
+        "WHEEL_SPEED_RR": 36.0,
+      },
+    })
+    radar_interface.rcp = SimpleNamespace(can_valid=True, vl={
+      0x680: {
+        "ID": 7,
+        "LONG_DIST": 40.0,
+        "LAT_DIST": -0.2,
+        "SPEED": 11.0,
+        "LAT_SPEED": 0.1,
+      },
+    })
+
+    radar_data = radar_interface._update_tssp({0x680})
+
+    assert len(radar_data.points) == 1
+    assert radar_data.points[0].dRel == 40.0
+    assert radar_data.points[0].vRel == pytest.approx(11.0 - 10.0 * TSSP_RADAR_EGO_SPEED_SCALE)
+
   def test_essential_ecus(self, subtests):
     # Asserts standard ECUs exist for each platform
     common_ecus = {Ecu.fwdRadar, Ecu.fwdCamera}

opendbc_repo/opendbc/car/toyota/values.py

@@ -171,7 +171,7 @@ class CAR(Platforms):
       ToyotaCarDocs("Toyota Camry Hybrid 2018-20", video="https://www.youtube.com/watch?v=Q2DYY0AWKgk"),
     ],
     CarSpecs(mass=3400. * CV.LB_TO_KG, wheelbase=2.82448, steerRatio=13.7, tireStiffnessFactor=0.7933),
-    dbc_dict('toyota_nodsu_pt_generated', 'toyota_adas'),
+    dbc_dict('toyota_nodsu_pt_generated', 'toyota_radar_dsu_tssp'),
     flags=ToyotaFlags.NO_DSU,
   )
   TOYOTA_CAMRY_TSS2 = ToyotaTSS2PlatformConfig( # TSS 2.5

scripts/speed_limit_vision/build_track_classifier_dataset.py

@@ -11,6 +11,10 @@ import shutil
 from collections import Counter
 from pathlib import Path
 
+import cv2
+
+from starpilot.system.speed_limit_vision import DETECTOR_CLASSIFIER_EXPANSIONS
+
 
 SPEED_VALUES = frozenset((15, 20, 25, 30, 35, 40, 45, 50, 55, 60, 65, 70, 75))
 
@@ -22,9 +26,18 @@ def parse_args() -> argparse.Namespace:
   parser.add_argument("--output", type=Path, required=True)
   parser.add_argument("--train-ratio", type=float, default=0.85)
   parser.add_argument("--min-growth", type=float, default=1.10)
+  parser.add_argument("--max-growth", type=float, default=float("inf"))
   parser.add_argument("--min-exact-confidence", type=float, default=0.80)
   parser.add_argument("--min-detector-confidence", type=float, default=0.30)
+  parser.add_argument("--min-tracking-confidence", type=float, default=1.01)
   parser.add_argument("--max-track-rank", type=int, default=3)
+  parser.add_argument("--hard-example-repeat", type=int, default=1, help="Train repeats for rejected or low-confidence track crops.")
+  parser.add_argument("--hard-example-min-confidence", type=float, default=0.90)
+  parser.add_argument(
+    "--runtime-expansions",
+    action="store_true",
+    help="Build crops from each source frame using the live detector/classifier expansion geometry.",
+  )
   return parser.parse_args()
 
 
@@ -52,23 +65,78 @@ def remove_appledouble_files(root: Path) -> int:
   return removed
 
 
+def parse_bbox(value: str) -> tuple[int, int, int, int] | None:
+  try:
+    bbox = tuple(int(round(float(part.strip()))) for part in value.split(","))
+  except ValueError:
+    return None
+  if len(bbox) != 4:
+    return None
+  x1, y1, x2, y2 = bbox
+  return (x1, y1, x2, y2) if x2 > x1 and y2 > y1 else None
+
+
+def stage_runtime_expansions(row: dict[str, str], destination_dir: Path, repeat_count: int) -> int:
+  frame_path = Path(row.get("frame_path", "")).expanduser().resolve()
+  bbox = parse_bbox(row.get("bbox", ""))
+  if not frame_path.is_file() or bbox is None:
+    return 0
+  frame = cv2.imread(str(frame_path))
+  if frame is None:
+    return 0
+
+  frame_height, frame_width = frame.shape[:2]
+  x1, y1, x2, y2 = bbox
+  box_width = x2 - x1
+  box_height = y2 - y1
+  destination_dir.mkdir(parents=True, exist_ok=True)
+  staged = 0
+  track_key = row.get("track_key", "")
+  rank = row.get("rank", "")
+  for repeat_index in range(repeat_count):
+    repeat_suffix = f"_r{repeat_index:02d}" if repeat_count > 1 else ""
+    for expansion_index, (left, top, right, bottom, _weight) in enumerate(DETECTOR_CLASSIFIER_EXPANSIONS):
+      crop_x1 = max(int(x1 - box_width * left), 0)
+      crop_y1 = max(int(y1 - box_height * top), 0)
+      crop_x2 = min(int(x2 + box_width * right), frame_width)
+      crop_y2 = min(int(y2 + box_height * bottom), frame_height)
+      crop = frame[crop_y1:crop_y2, crop_x1:crop_x2]
+      if crop.size == 0:
+        continue
+      destination = destination_dir / f"track_{track_key}_{rank}_e{expansion_index:02d}{repeat_suffix}.jpg"
+      if destination.exists() or cv2.imwrite(str(destination), crop, (cv2.IMWRITE_JPEG_QUALITY, 95)):
+        staged += 1
+  return staged
+
+
 def trusted_track_row(row: dict[str, str], args: argparse.Namespace) -> bool:
   try:
     expected = int(row.get("expected_speed_limit_mph", ""))
     predicted = int(row.get("predicted_speed_limit_mph", "") or 0)
     read_confidence = float(row.get("read_confidence", "") or 0.0)
     detector_confidence = float(row.get("detector_confidence", "") or 0.0)
+    tracking_confidence = float(row.get("tracking_confidence", "") or 0.0)
     growth = float(row.get("area_ratio_to_anchor", "") or 0.0)
     rank = int(row.get("rank", "") or 999)
   except ValueError:
     return False
   exact = predicted == expected and read_confidence >= args.min_exact_confidence
   detector_snap = detector_confidence >= args.min_detector_confidence
-  return expected in SPEED_VALUES and growth >= args.min_growth and rank <= args.max_track_rank and (exact or detector_snap)
+  optical_flow_track = tracking_confidence >= args.min_tracking_confidence
+  return (
+    expected in SPEED_VALUES and
+    args.min_growth <= growth <= args.max_growth and
+    rank <= args.max_track_rank and
+    (exact or detector_snap or optical_flow_track)
+  )
 
 
 def main() -> int:
   args = parse_args()
+  if args.hard_example_repeat < 1:
+    raise ValueError("--hard-example-repeat must be at least 1")
+  if args.max_growth < args.min_growth:
+    raise ValueError("--max-growth must be at least --min-growth")
   base = args.base.expanduser().resolve()
   output = args.output.expanduser().resolve()
   counts: Counter[str] = Counter()
@@ -96,10 +164,26 @@ def main() -> int:
       split = split_for_key(row.get("route") or row.get("track_key", ""), args.train_ratio)
       if split == "val" and row.get("route"):
         validation_routes.add(row["route"])
-      name = f"track_{row.get('track_key', '')}_{row.get('rank', '')}{source.suffix.lower()}"
-      link_or_copy(source, output / split / str(speed) / name)
-      counts[f"track_{split}"] += 1
-      counts[f"speed_{speed}"] += 1
+      predicted = int(row.get("predicted_speed_limit_mph", "") or 0)
+      read_confidence = float(row.get("read_confidence", "") or 0.0)
+      hard_example = predicted != speed or read_confidence < args.hard_example_min_confidence
+      repeat_count = args.hard_example_repeat if split == "train" and hard_example else 1
+      if args.runtime_expansions:
+        staged = stage_runtime_expansions(row, output / split / str(speed), repeat_count)
+      else:
+        staged = 0
+        for repeat_index in range(repeat_count):
+          repeat_suffix = f"_r{repeat_index:02d}" if repeat_count > 1 else ""
+          name = f"track_{row.get('track_key', '')}_{row.get('rank', '')}{repeat_suffix}{source.suffix.lower()}"
+          link_or_copy(source, output / split / str(speed) / name)
+          staged += 1
+      if staged == 0:
+        counts["track_rejected"] += 1
+        continue
+      if hard_example:
+        counts[f"hard_track_{split}"] += staged
+      counts[f"track_{split}"] += staged
+      counts[f"speed_{speed}"] += staged
 
   counts["appledouble_removed"] = remove_appledouble_files(output)
   (output / "track_validation_routes.txt").write_text("\n".join(sorted(validation_routes)) + "\n", encoding="ascii")

scripts/speed_limit_vision/test_track_classifier_dataset.py

@@ -0,0 +1,67 @@
+from __future__ import annotations
+
+import argparse
+import importlib.util
+
+from pathlib import Path
+
+import cv2
+import numpy as np
+
+
+def load_local_module(name: str):
+  path = Path(__file__).resolve().with_name(f"{name}.py")
+  spec = importlib.util.spec_from_file_location(f"test_local_{name}", path)
+  assert spec is not None and spec.loader is not None
+  module = importlib.util.module_from_spec(spec)
+  spec.loader.exec_module(module)
+  return module
+
+
+dataset = load_local_module("build_track_classifier_dataset")
+
+
+def options(**overrides) -> argparse.Namespace:
+  values = {
+    "min_exact_confidence": 0.80,
+    "min_detector_confidence": 0.30,
+    "min_tracking_confidence": 0.75,
+    "min_growth": 0.30,
+    "max_growth": 8.0,
+    "max_track_rank": 6,
+  }
+  values.update(overrides)
+  return argparse.Namespace(**values)
+
+
+def test_trusted_track_row_accepts_high_confidence_optical_flow() -> None:
+  row = {
+    "expected_speed_limit_mph": "35",
+    "predicted_speed_limit_mph": "",
+    "read_confidence": "0",
+    "detector_confidence": "0",
+    "tracking_confidence": "1.0",
+    "area_ratio_to_anchor": "5.1",
+    "rank": "6",
+  }
+
+  assert dataset.trusted_track_row(row, options())
+  assert not dataset.trusted_track_row(row, options(min_tracking_confidence=1.01))
+  assert not dataset.trusted_track_row(row, options(max_growth=5.0))
+
+
+def test_stage_runtime_expansions_writes_each_view_and_repeat(tmp_path: Path) -> None:
+  frame = np.zeros((100, 200, 3), dtype=np.uint8)
+  frame[20:60, 50:70] = 255
+  frame_path = tmp_path / "frame.jpg"
+  assert cv2.imwrite(str(frame_path), frame)
+  row = {
+    "frame_path": str(frame_path),
+    "bbox": "50,20,70,60",
+    "track_key": "track",
+    "rank": "2",
+  }
+
+  output = tmp_path / "crops"
+  assert dataset.stage_runtime_expansions(row, output, repeat_count=2) == 6
+  assert len(list(output.glob("*.jpg"))) == 6

selfdrive/controls/controlsd.py

@@ -212,7 +212,9 @@ class Controls:
     # accel PID loop
     pid_accel_limits = self.CI.get_pid_accel_limits(self.CP, CS.vEgo, CS.vCruise * CV.KPH_TO_MS)
     self.LoC.experimental_mode = bool(self.sm['selfdriveState'].experimentalMode)
-    actuators.accel = float(min(self.LoC.update(CC.longActive, CS, long_plan.aTarget, long_plan.shouldStop, pid_accel_limits, self.starpilot_toggles), self.starpilot_toggles.max_desired_acceleration))
+    actuators.accel = float(min(self.LoC.update(CC.longActive, CS, long_plan.aTarget, long_plan.shouldStop, pid_accel_limits,
+                                                self.starpilot_toggles, has_lead=long_plan.hasLead),
+                                self.starpilot_toggles.max_desired_acceleration))
 
     # Steering PID loop and lateral MPC
     # Reset desired curvature to current to avoid violating the limits on engage

selfdrive/controls/lib/longcontrol.py

@@ -229,7 +229,7 @@ class LongControl:
     if not preserve_stop_release:
       self.stop_release_counter = 0
 
-  def _stop_release_ready(self, CS, a_target, should_stop, starpilot_toggles):
+  def _stop_release_ready(self, CS, a_target, should_stop, has_lead, starpilot_toggles):
     if self.long_control_state != LongCtrlState.stopping:
       self.stop_release_counter = 0
       return True
@@ -242,6 +242,10 @@ class LongControl:
       self.stop_release_counter = int(round(STOPPING_RELEASE_HYSTERESIS / DT_CTRL))
       return True
 
+    if has_lead and a_target > STOPPING_RELEASE_MIN_ACCEL:
+      self.stop_release_counter = int(round(STOPPING_RELEASE_HYSTERESIS / DT_CTRL))
+      return True
+
     if a_target >= STOPPING_RELEASE_STRONG_ACCEL and not CS.cruiseState.standstill:
       self.stop_release_counter = int(round(STOPPING_RELEASE_HYSTERESIS / DT_CTRL))
       return True
@@ -392,12 +396,12 @@ class LongControl:
     )
     return a_target * effective_gain
 
-  def update(self, active, CS, a_target, should_stop, accel_limits, starpilot_toggles):
+  def update(self, active, CS, a_target, should_stop, accel_limits, starpilot_toggles, has_lead=False):
     """Update longitudinal control. This updates the state machine and runs a PID loop"""
     self.pid.neg_limit = accel_limits[0]
     self.pid.pos_limit = accel_limits[1]
 
-    allow_stopping_release = self._stop_release_ready(CS, a_target, should_stop, starpilot_toggles)
+    allow_stopping_release = self._stop_release_ready(CS, a_target, should_stop, has_lead, starpilot_toggles)
     self.long_control_state = long_control_state_trans(self.CP, active, self.long_control_state, CS.vEgo,
                                                        should_stop, CS.brakePressed,
                                                        CS.cruiseState.standstill, starpilot_toggles,

selfdrive/controls/lib/longitudinal_planner.py

@@ -30,6 +30,10 @@ ALLOW_THROTTLE_HYSTERESIS = 0.05
 ALLOW_THROTTLE_ENABLE_THRESHOLD = ALLOW_THROTTLE_THRESHOLD + ALLOW_THROTTLE_HYSTERESIS
 ALLOW_THROTTLE_DISABLE_THRESHOLD = ALLOW_THROTTLE_THRESHOLD - ALLOW_THROTTLE_HYSTERESIS
 MIN_ALLOW_THROTTLE_SPEED = 5.0
+MODEL_LAUNCH_DISARM_SPEED = 2.0
+MODEL_LAUNCH_COMMIT_TIME = 3.5
+MODEL_LAUNCH_MOVING_SPEED = 1.2
+MODEL_LAUNCH_MAX_ACCEL = 1.5
 RAW_LEAD_SAFETY_MIN_CLOSING_SPEED = 0.5
 RAW_LEAD_SAFETY_TTC = 7.0
 RAW_LEAD_SAFETY_DISTANCE = 40.0
@@ -572,6 +576,8 @@ class LongitudinalPlanner:
     self.v_model_error = 0.0
     self.output_a_target = 0.0
     self.output_should_stop = False
+    self.model_launch_armed = False
+    self.model_launch_stop_seen = False
     self.confident_lead_depart_elapsed = 0.0
     self.slow_creep_lead_depart_elapsed = 0.0
 
@@ -663,6 +669,31 @@ class LongitudinalPlanner:
       throttle_prob = 1.0
     return x, v, a, j, throttle_prob
 
+  @staticmethod
+  def get_model_launch_accel(model_v, model_a, action_t, v_ego):
+    if len(model_v) != len(T_IDXS_MPC) or len(model_a) != len(T_IDXS_MPC):
+      return None
+    if float(np.interp(MODEL_LAUNCH_COMMIT_TIME, T_IDXS_MPC, model_v)) <= MODEL_LAUNCH_DISARM_SPEED:
+      return None
+
+    moving_idxs = np.flatnonzero(np.asarray(model_v) > MODEL_LAUNCH_MOVING_SPEED)
+    if len(moving_idxs) == 0:
+      return None
+
+    t_cut = min(float(T_IDXS_MPC[int(moving_idxs[0])]), MODEL_LAUNCH_COMMIT_TIME)
+    shifted_t = T_IDXS_MPC + t_cut
+    shifted_v = np.interp(shifted_t, T_IDXS_MPC, model_v)
+    shifted_a = np.interp(shifted_t, T_IDXS_MPC, model_a)
+    safe_action_t = max(float(action_t), 1e-3)
+    v_target = float(np.interp(safe_action_t, T_IDXS_MPC, shifted_v))
+    a_launch = 2.0 * (v_target - float(shifted_v[0])) / safe_action_t - float(shifted_a[0])
+    accel_cap = float(np.interp(
+      float(v_ego),
+      [MODEL_LAUNCH_MOVING_SPEED, MODEL_LAUNCH_DISARM_SPEED],
+      [MODEL_LAUNCH_MAX_ACCEL, 0.0],
+    ))
+    return float(np.clip(a_launch, 0.0, accel_cap))
+
   def get_close_lead_brake_cap(self, lead, v_ego, accel_min):
     if lead is None or not lead.status:
       return None
@@ -2308,6 +2339,18 @@ class LongitudinalPlanner:
     # Compute model v_ego error
     self.v_model_error = self.get_model_speed_error(sm['modelV2'], v_ego)
     x, v, a, j, throttle_prob = self.parse_model(sm['modelV2'], self.v_model_error, v_ego, starpilot_toggles)
+    if bool(sm['carState'].standstill):
+      self.model_launch_armed = True
+      self.model_launch_stop_seen |= bool(
+        sm['modelV2'].action.shouldStop or
+        getattr(sm['starpilotPlan'], 'redLight', False) or
+        getattr(sm['starpilotPlan'], 'forcingStop', False)
+      )
+    elif scene_v_ego > MODEL_LAUNCH_DISARM_SPEED:
+      self.model_launch_armed = False
+      self.model_launch_stop_seen = False
+    model_launch_v = np.array(v, copy=True)
+    model_launch_a = np.array(a, copy=True)
     # Don't clip at low speeds since throttle_prob doesn't account for creep. Use
     # hysteresis here because raw gasPressProb noise can chatter the throttle gate.
     if v_ego <= MIN_ALLOW_THROTTLE_SPEED:
@@ -2577,6 +2620,9 @@ class LongitudinalPlanner:
     experimental_mlsim = bool(tinygrad_model and self.mlsim and self.mode != 'acc')
     action_t = self.CP.longitudinalActuatorDelay + DT_MDL
     prev_output_a_target = float(self.output_a_target)
+    model_launch_accel = None
+    if self.model_launch_armed and not bool(sm['modelV2'].action.shouldStop):
+      model_launch_accel = self.get_model_launch_accel(model_launch_v, model_launch_a, action_t, scene_v_ego)
 
     if classic_model:
       output_a_target, output_should_stop = get_accel_from_plan_classic(
@@ -2795,6 +2841,24 @@ class LongitudinalPlanner:
       output_a_target = max(output_a_target, STANDSTILL_LEAD_DEPART_MIN_ACCEL)
       self.post_departure_follow_settle_until = now_t + POST_DEPARTURE_FOLLOW_SETTLE_LATCH_TIME
 
+    lead_present = any(bool(getattr(lead, "status", False)) for lead in (self.lead_one, self.lead_two))
+    confirmed_lead_release = bool(confident_depart_ready or lead_depart_ready or slow_creep_depart_ready)
+    model_launch_allowed = bool(
+      model_launch_accel is not None and
+      not output_should_stop and
+      not vision_low_speed_stop_active and
+      not bool(getattr(sm['carState'], 'brakePressed', False)) and
+      not bool(getattr(sm['starpilotPlan'], 'forcingStop', False)) and
+      not bool(getattr(sm['starpilotPlan'], 'redLight', False)) and
+      not depart_safety_veto and
+      (
+        (lead_present and lead_control_active and confirmed_lead_release) or
+        (not lead_present and (self.mode != 'acc' or self.model_launch_stop_seen))
+      )
+    )
+    if model_launch_allowed:
+      output_a_target = max(output_a_target, model_launch_accel)
+
     if depart_safety_veto or output_should_stop or bool(getattr(sm['starpilotPlan'], 'forcingStop', False)) or bool(getattr(sm['starpilotPlan'], 'redLight', False)):
       self.lead_depart_accel_hold_until = 0.0
       self.lead_depart_accel_hold_floor = None

selfdrive/controls/tests/test_longcontrol.py

@@ -269,6 +269,59 @@ def test_update_releases_stopping_on_small_sustained_positive_target():
   assert lc.long_control_state == LongCtrlState.starting
 
 
+def test_update_releases_stopping_immediately_after_confirmed_lead_departure():
+  CP = car.CarParams.new_message(startingState=True, vEgoStarting=0.5)
+  CP.longitudinalTuning.kpBP = [0.0]
+  CP.longitudinalTuning.kpV = [0.1]
+  CP.longitudinalTuning.kiBP = [0.0]
+  CP.longitudinalTuning.kiV = [0.03]
+
+  lc = LongControl(CP)
+  lc.long_control_state = LongCtrlState.stopping
+  CS = car.CarState.new_message(vEgo=0.0, aEgo=0.0, brakePressed=False)
+  CS.cruiseState.standstill = True
+
+  output_accel = lc.update(
+    active=True,
+    CS=CS,
+    a_target=0.16,
+    should_stop=False,
+    accel_limits=(-3.0, 2.0),
+    starpilot_toggles=make_toggles(startAccel=1.5),
+    has_lead=True,
+  )
+
+  assert lc.long_control_state == LongCtrlState.starting
+  assert output_accel > 0.0
+
+
+@pytest.mark.parametrize(("should_stop", "brake_pressed"), [(True, False), (False, True)])
+def test_confirmed_lead_departure_does_not_override_stop_or_driver_brake(should_stop, brake_pressed):
+  CP = car.CarParams.new_message(startingState=True, vEgoStarting=0.5)
+  CP.longitudinalTuning.kpBP = [0.0]
+  CP.longitudinalTuning.kpV = [0.1]
+  CP.longitudinalTuning.kiBP = [0.0]
+  CP.longitudinalTuning.kiV = [0.03]
+
+  lc = LongControl(CP)
+  lc.long_control_state = LongCtrlState.stopping
+  CS = car.CarState.new_message(vEgo=0.0, aEgo=0.0, brakePressed=brake_pressed)
+  CS.cruiseState.standstill = True
+
+  output_accel = lc.update(
+    active=True,
+    CS=CS,
+    a_target=0.5,
+    should_stop=should_stop,
+    accel_limits=(-3.0, 2.0),
+    starpilot_toggles=make_toggles(startAccel=1.5),
+    has_lead=True,
+  )
+
+  assert lc.long_control_state == LongCtrlState.stopping
+  assert output_accel <= 0.0
+
+
 def test_update_releases_stopping_with_cruise_standstill_latched():
   CP = car.CarParams.new_message(vEgoStarting=0.5)
   CP.longitudinalTuning.kpBP = [0.0]

selfdrive/controls/tests/test_longitudinal_planner.py

@@ -15,6 +15,7 @@ from openpilot.selfdrive.controls.lib.longcontrol import LongCtrlState
 from openpilot.selfdrive.controls.lib.drive_helpers import CONTROL_N
 from openpilot.selfdrive.controls.lib.longitudinal_planner import LongitudinalPlanner, get_coast_accel, get_vehicle_min_accel, should_publish_planner_fcw
 from openpilot.selfdrive.controls.lib.longitudinal_mpc_lib.long_mpc import soften_far_radar_lead_accel, should_trigger_planner_fcw
+from openpilot.selfdrive.controls.lib.longitudinal_mpc_lib.long_mpc import T_IDXS as T_IDXS_MPC
 from openpilot.selfdrive.modeld.constants import ModelConstants, Plan
 
 
@@ -70,6 +71,14 @@ def set_model_lead(model, idx: int, *, prob: float, x0: float, y0: float, v0: fl
   lead.a = [float(a0)]
 
 
+def set_model_launch_trajectory(model, *, wait_time: float = 0.6, accel: float = 1.0):
+  times = np.asarray(ModelConstants.T_IDXS, dtype=float)
+  moving_time = np.maximum(times - wait_time, 0.0)
+  model.position.x = (0.5 * accel * moving_time ** 2).tolist()
+  model.velocity.x = (accel * moving_time).tolist()
+  model.acceleration.x = np.where(times >= wait_time, accel, 0.0).tolist()
+
+
 def make_sm(v_ego: float, desired_accel: float, min_accel: float, *, experimental_mode: bool = True,
             tracking_lead: bool = False, lead_one=None, lead_two=None,
             gas_press_prob: float = 1.0, brake_press_prob: float = 0.0, disable_throttle: bool = False):
@@ -1101,6 +1110,144 @@ def test_tracked_vision_model_brake_cap_does_not_relax_strong_model_brake(model_
   assert cap is None
 
 
+def test_model_launch_accel_skips_hesitant_start_of_trajectory():
+  model_v = np.maximum(T_IDXS_MPC - 0.6, 0.0)
+  model_a = np.where(T_IDXS_MPC >= 0.6, 1.0, 0.0)
+
+  launch_accel = LongitudinalPlanner.get_model_launch_accel(model_v, model_a, action_t=0.2, v_ego=0.0)
+
+  assert launch_accel is not None
+  assert launch_accel >= 0.8
+
+
+def test_green_light_model_launch_boosts_no_lead_experimental_takeoff():
+  CP = CarInterface.get_non_essential_params(CAR.HONDA_CIVIC)
+  planner = LongitudinalPlanner(CP, init_v=0.0)
+  sm = make_sm(0.0, desired_accel=0.0, min_accel=-0.5, experimental_mode=True)
+  sm["carState"].standstill = True
+  sm["controlsState"].longControlState = LongCtrlState.stopping
+  set_model_launch_trajectory(sm["modelV2"])
+
+  planner.update(sm, make_toggles())
+
+  assert not planner.output_should_stop
+  assert planner.output_a_target >= 0.8
+
+
+def test_green_light_model_launch_survives_cem_switch_back_to_chill():
+  CP = CarInterface.get_non_essential_params(CAR.HONDA_CIVIC)
+  planner = LongitudinalPlanner(CP, init_v=0.0)
+
+  sm_red = make_sm(0.0, desired_accel=0.0, min_accel=-0.5, experimental_mode=True)
+  sm_red["carState"].standstill = True
+  sm_red["controlsState"].longControlState = LongCtrlState.stopping
+  sm_red["modelV2"].action.shouldStop = True
+  sm_red["starpilotPlan"].redLight = True
+  planner.update(sm_red, make_toggles())
+
+  sm_green = make_sm(0.0, desired_accel=0.0, min_accel=-0.5, experimental_mode=False)
+  sm_green["carState"].standstill = True
+  sm_green["controlsState"].longControlState = LongCtrlState.stopping
+  set_model_launch_trajectory(sm_green["modelV2"])
+  planner.update(sm_green, make_toggles())
+
+  assert planner.model_launch_stop_seen
+  assert not planner.output_should_stop
+  assert planner.output_a_target >= 0.8
+
+
+@pytest.mark.parametrize(("veto", "brake_pressed"), [("redLight", False), ("forcingStop", False), (None, True)])
+def test_green_light_model_launch_respects_stop_and_driver_vetoes(veto, brake_pressed):
+  CP = CarInterface.get_non_essential_params(CAR.HONDA_CIVIC)
+  planner = LongitudinalPlanner(CP, init_v=0.0)
+  sm = make_sm(0.0, desired_accel=0.0, min_accel=-0.5, experimental_mode=True)
+  sm["carState"].standstill = True
+  sm["carState"].brakePressed = brake_pressed
+  sm["controlsState"].longControlState = LongCtrlState.stopping
+  if veto is not None:
+    setattr(sm["starpilotPlan"], veto, True)
+  set_model_launch_trajectory(sm["modelV2"])
+
+  planner.update(sm, make_toggles())
+
+  assert planner.output_a_target < 0.3
+
+
+def test_model_launch_does_not_override_stationary_lead_guard():
+  CP = CarInterface.get_non_essential_params(CAR.HONDA_CIVIC)
+  planner = LongitudinalPlanner(CP, init_v=0.0)
+  sm = make_sm(
+    0.0,
+    desired_accel=0.0,
+    min_accel=-0.5,
+    experimental_mode=True,
+    tracking_lead=True,
+    lead_one=make_lead(status=True, d_rel=4.0, v_lead=0.0, a_lead=0.0, radar=True, model_prob=1.0),
+  )
+  sm["carState"].standstill = True
+  sm["controlsState"].longControlState = LongCtrlState.stopping
+  set_model_launch_trajectory(sm["modelV2"])
+
+  planner.update(sm, make_toggles())
+
+  assert planner.output_should_stop
+  assert planner.output_a_target <= 0.0
+
+
+def test_model_launch_boosts_only_after_lead_departure_is_confirmed():
+  CP = CarInterface.get_non_essential_params(CAR.HONDA_CIVIC)
+  planner = LongitudinalPlanner(CP, init_v=0.0)
+  sm = make_sm(
+    0.0,
+    desired_accel=0.1,
+    min_accel=-0.5,
+    experimental_mode=True,
+    tracking_lead=True,
+    lead_one=make_lead(status=True, d_rel=7.0, v_lead=1.5, a_lead=0.8, radar=True, model_prob=1.0),
+  )
+  sm["carState"].standstill = True
+  sm["controlsState"].longControlState = LongCtrlState.stopping
+  set_model_launch_trajectory(sm["modelV2"])
+
+  planner.update(sm, make_toggles())
+
+  assert not planner.output_should_stop
+  assert planner.output_a_target >= 0.8
+
+
+def test_model_launch_is_cancelled_when_departing_lead_stops_again():
+  CP = CarInterface.get_non_essential_params(CAR.HONDA_CIVIC)
+  planner = LongitudinalPlanner(CP, init_v=0.0)
+  sm_depart = make_sm(
+    0.0,
+    desired_accel=0.1,
+    min_accel=-0.5,
+    experimental_mode=True,
+    tracking_lead=True,
+    lead_one=make_lead(status=True, d_rel=7.0, v_lead=1.5, a_lead=0.8, radar=True, model_prob=1.0),
+  )
+  sm_depart["carState"].standstill = True
+  sm_depart["controlsState"].longControlState = LongCtrlState.stopping
+  set_model_launch_trajectory(sm_depart["modelV2"])
+  planner.update(sm_depart, make_toggles())
+
+  sm_stop = make_sm(
+    0.2,
+    desired_accel=0.1,
+    min_accel=-0.5,
+    experimental_mode=True,
+    tracking_lead=True,
+    lead_one=make_lead(status=True, d_rel=3.8, v_lead=0.0, a_lead=-0.6, radar=True, model_prob=1.0),
+  )
+  sm_stop["controlsState"].longControlState = LongCtrlState.pid
+  set_model_launch_trajectory(sm_stop["modelV2"])
+
+  planner.update(sm_stop, make_toggles())
+
+  assert planner.output_should_stop
+  assert planner.output_a_target <= 0.0
+
+
 @pytest.mark.parametrize("model_version", ["v11", "v12", "v13", "v14", "v15"])
 def test_manual_resume_override_clears_no_lead_model_stop_at_standstill(model_version):
   CP = CarInterface.get_non_essential_params(CAR.HONDA_CIVIC)

tools/lib/route.py

@@ -6,7 +6,7 @@ from urllib.parse import urlparse
 from collections import defaultdict
 from itertools import chain
 
-from openpilot.tools.lib.auth_config import get_token
+from openpilot.tools.lib.auth_config import KONIK_API_HOST, get_token
 from openpilot.tools.lib.api import APIError, CommaApi, route_api_hosts
 from openpilot.tools.lib.helpers import RE
 
@@ -107,7 +107,12 @@ class Route:
     return sorted(segments.values(), key=lambda seg: seg.name.segment_num)
 
   def _get_route_metadata(self):
-    return self._get_route_endpoint('v1/route/' + self.name.canonical_name)
+    try:
+      return self._get_route_endpoint('v1/route/' + self.name.canonical_name)
+    except APIError as e:
+      if self._api_host == KONIK_API_HOST and e.status_code in (400, 404):
+        return {"url": f"https://connect.konik.ai/{self.name.dongle_id}/{self.name.log_id}"}
+      raise
 
   def _get_route_files(self):
     return self._get_route_endpoint('v1/route/' + self.name.canonical_name + '/files')

tools/lib/tests/test_route_library.py

@@ -78,3 +78,26 @@ class TestRouteLibrary:
       Route(route_name)
 
     assert calls == [(DEFAULT_API_HOST, f"v1/route/{route_name.replace('/', '|')}/files")]
+
+  def test_route_synthesizes_konik_metadata_when_endpoint_is_unavailable(self, mocker):
+    route_name = "59679e5e40b60ce0/0000091b--316e931f07"
+    file_url = "https://api.konik.ai/connectdata/59679e5e40b60ce0/0000091b--316e931f07/0/qlog.zst"
+
+    class FakeApi:
+      def __init__(self, token=None, host=None):
+        self.host = host
+
+      def get(self, endpoint):
+        if endpoint.endswith("/files"):
+          return {"qlogs": [file_url]}
+        raise APIError("400:metadata unavailable", 400)
+
+    mocker.patch("openpilot.tools.lib.route.route_api_hosts", return_value=[KONIK_API_HOST])
+    mocker.patch("openpilot.tools.lib.route.get_token", return_value=None)
+    mocker.patch("openpilot.tools.lib.route.CommaApi", FakeApi)
+
+    route = Route(route_name)
+
+    assert route.qlog_paths() == [file_url]
+    assert route.metadata == {"url": "https://connect.konik.ai/59679e5e40b60ce0/0000091b--316e931f07"}
+    assert route.segments[0].url == "https://connect.konik.ai/59679e5e40b60ce0/0000091b--316e931f07/0"