Tesla Pre-AP: enforce grade handoff regressions

.github/ci/tesla_preap_longitudinal_mutations.py

@@ -3,18 +3,120 @@
 import sys
 import tempfile
 import xml.etree.ElementTree as ET
+from dataclasses import dataclass
 from pathlib import Path
 
 
 REPO_ROOT = Path(__file__).resolve().parents[2]
-SOURCE_PATH = REPO_ROOT / "opendbc_repo" / "opendbc" / "car" / "tesla" / "preap" / "constants.py"
-ORIGINAL_KI = b"PEDAL_LONG_KI_V = [0.0, 0.0, 0.0, 0.0]\n"
-HISTORICAL_KI = b"PEDAL_LONG_KI_V = [0.05, 0.08, 0.10, 0.15]\n"
-TEST_PATH = "selfdrive/controls/tests/test_tesla_preap_longcontrol.py"
-MUTATION_TEST_NODES = (
-  f"{TEST_PATH}::test_vdas_receives_route_shaped_planner_target_trace_unchanged",
-  f"{TEST_PATH}::test_road_load_history_cannot_reverse_finite_jerk_negative_planner_target",
-  f"{TEST_PATH}::test_negative_planner_target_reaches_regen_side_of_coast_anchor",
+LONGCONTROL_TEST_PATH = "selfdrive/controls/tests/test_tesla_preap_longcontrol.py"
+FOLLOWING_TEST_PATH = "selfdrive/controls/tests/test_tesla_preap_following.py"
+NOISE_GATE_TEST_NODE = (
+  "opendbc_repo/opendbc/car/tesla/preap/tests/test_virtual_das.py::TestInnerPID::" +
+  "test_sub_deadband_sign_changing_noise_does_not_accumulate_residual_authority"
+)
+
+
+@dataclass(frozen=True)
+class HistoricalMutation:
+  name: str
+  source_path: str
+  original: bytes
+  replacement: bytes
+  test_nodes: tuple[str, ...]
+
+
+MUTATIONS = (
+  HistoricalMutation(
+    name="historical-outer-ki",
+    source_path="opendbc_repo/opendbc/car/tesla/preap/constants.py",
+    original=b"PEDAL_LONG_KI_V = [0.0, 0.0, 0.0, 0.0]\n",
+    replacement=b"PEDAL_LONG_KI_V = [0.05, 0.08, 0.10, 0.15]\n",
+    test_nodes=(
+      f"{LONGCONTROL_TEST_PATH}::test_vdas_receives_route_shaped_planner_target_trace_unchanged",
+      f"{LONGCONTROL_TEST_PATH}::test_road_load_history_cannot_reverse_finite_jerk_negative_planner_target",
+      f"{LONGCONTROL_TEST_PATH}::test_negative_planner_target_reaches_regen_side_of_coast_anchor",
+    ),
+  ),
+  HistoricalMutation(
+    name="adaptive-follow-cap-bypassed",
+    source_path="selfdrive/controls/lib/longitudinal_planner.py",
+    original=(
+      b"        cap_strength = get_preap_follow_cap_strength(" +
+      b"v_ego, lead.dRel, lead.vLead, self.t_follow)\n"
+    ),
+    replacement=b"        cap_strength = 0.0\n",
+    test_nodes=(
+      f"{FOLLOWING_TEST_PATH}::" +
+      "test_planner_adaptive_cap_changes_the_delivered_acceleration_for_unequal_speed_lead",
+    ),
+  ),
+  HistoricalMutation(
+    name="longcontrol-feedforward-coupling-bypassed",
+    source_path="selfdrive/controls/lib/longcontrol.py",
+    original=b"                                     feedforward=a_target)\n",
+    replacement=b"                                     feedforward=0.0)\n",
+    test_nodes=(
+      f"{FOLLOWING_TEST_PATH}::test_max_follow_full_closed_loop_recovers_gap_with_production_fallback",
+    ),
+  ),
+  HistoricalMutation(
+    name="hard-inner-error-deadband-restored",
+    source_path="opendbc_repo/opendbc/car/tesla/preap/virtual_das.py",
+    original=b"    error = self._gate_pid_error_noise(error, freeze_integrator)\n",
+    replacement=(
+      b"    if abs(error) < PID_ERROR_DEADBAND:\n" +
+      b"      error = 0.0\n"
+    ),
+    test_nodes=(
+      f"{FOLLOWING_TEST_PATH}::test_max_follow_full_closed_loop_recovers_gap_with_production_fallback",
+    ),
+  ),
+  HistoricalMutation(
+    name="inner-error-noise-gate-call-bypassed",
+    source_path="opendbc_repo/opendbc/car/tesla/preap/virtual_das.py",
+    original=b"    error = self._gate_pid_error_noise(error, freeze_integrator)\n",
+    replacement=b"    error = error\n",
+    test_nodes=(NOISE_GATE_TEST_NODE,),
+  ),
+  HistoricalMutation(
+    name="negative-handoff-integral-slew-regressed",
+    source_path="opendbc_repo/opendbc/car/tesla/preap/virtual_das.py",
+    original=b"NEGATIVE_HANDOFF_INTEGRAL_SLEW = 0.25  # m/s\xc2\xb3\n",
+    replacement=b"NEGATIVE_HANDOFF_INTEGRAL_SLEW = 0.20  # m/s\xc2\xb3\n",
+    test_nodes=(
+      f"{LONGCONTROL_TEST_PATH}::test_negative_planner_target_reaches_regen_side_of_coast_anchor",
+    ),
+  ),
+  HistoricalMutation(
+    name="grade-effort-compensation-removed",
+    source_path="opendbc_repo/opendbc/car/tesla/preap/virtual_das.py",
+    original=b"      a_limited + steady_grade_compensation + transient_pitch_compensation,\n",
+    replacement=b"      a_limited,\n",
+    test_nodes=(
+      f"{FOLLOWING_TEST_PATH}::test_plant_aligned_full_closed_loop_grade_compensation_holds_speed",
+    ),
+  ),
+  HistoricalMutation(
+    name="grade-effort-compensation-sign-flipped",
+    source_path="opendbc_repo/opendbc/car/tesla/preap/virtual_das.py",
+    original=b"      a_limited + steady_grade_compensation + transient_pitch_compensation,\n",
+    replacement=b"      a_limited - steady_grade_compensation - transient_pitch_compensation,\n",
+    test_nodes=(
+      f"{FOLLOWING_TEST_PATH}::test_plant_aligned_full_closed_loop_grade_compensation_holds_speed",
+    ),
+  ),
+  HistoricalMutation(
+    name="grade-effort-compensation-doubled",
+    source_path="opendbc_repo/opendbc/car/tesla/preap/virtual_das.py",
+    original=b"      a_limited + steady_grade_compensation + transient_pitch_compensation,\n",
+    replacement=(
+      b"      a_limited + 2.0 * steady_grade_compensation " +
+      b"+ 2.0 * transient_pitch_compensation,\n"
+    ),
+    test_nodes=(
+      f"{FOLLOWING_TEST_PATH}::test_plant_aligned_full_closed_loop_grade_compensation_holds_speed",
+    ),
+  ),
 )
 
 
@@ -62,11 +164,26 @@ def has_only_assertion_failures(testcases: list[ET.Element]) -> bool:
   )
 
 
+def apply_mutation(mutation: HistoricalMutation) -> tuple[Path, bytes]:
+  source_path = REPO_ROOT / mutation.source_path
+  original_source = source_path.read_bytes()
+  match_count = original_source.count(mutation.original)
+  if match_count != 1:
+    raise RuntimeError(
+      f"{mutation.name}: expected one source match in {mutation.source_path}, found {match_count}"
+    )
+  source_path.write_bytes(original_source.replace(mutation.original, mutation.replacement, 1))
+  return source_path, original_source
+
+
 def main() -> int:
   with tempfile.TemporaryDirectory(prefix="tesla-preap-parent-mutation-") as temp_dir:
     temp_root = Path(temp_dir)
     baseline_xml = temp_root / "baseline.xml"
-    baseline = run_pytest((TEST_PATH,), baseline_xml)
+    baseline = run_pytest(
+      (LONGCONTROL_TEST_PATH, FOLLOWING_TEST_PATH, NOISE_GATE_TEST_NODE),
+      baseline_xml,
+    )
     if baseline.returncode != 0:
       print("BASELINE FAILED: parent longitudinal regression tests did not pass")
       print(baseline.stdout)
@@ -78,50 +195,59 @@ def main() -> int:
       return 1
     print(f"BASELINE PASS: {len(baseline_testcases)} parent tests")
 
-    original_source = SOURCE_PATH.read_bytes()
-    mutation_result = None
-    mutation_error = None
-    restored = False
-    try:
-      match_count = original_source.count(ORIGINAL_KI)
-      if match_count != 1:
-        raise RuntimeError(f"expected one outer-KI source match, found {match_count}")
-      SOURCE_PATH.write_bytes(original_source.replace(ORIGINAL_KI, HISTORICAL_KI, 1))
-      mutation_result = run_pytest(MUTATION_TEST_NODES, temp_root / "historical-outer-ki.xml")
-    except Exception as exc:  # pragma: no cover - failure reporting path
-      mutation_error = exc
-    finally:
-      SOURCE_PATH.write_bytes(original_source)
-      restored = SOURCE_PATH.read_bytes() == original_source
-
-    if not restored:
-      print("INVALID: source restoration did not reproduce the original bytes")
-      return 1
-    if mutation_error is not None:
-      print(f"INVALID: historical outer-KI mutation could not run: {mutation_error}")
-      return 1
-    if mutation_result is None:
-      print("INVALID: historical outer-KI mutation produced no pytest result")
-      return 1
-
-    mutation_xml = temp_root / "historical-outer-ki.xml"
-    try:
-      mutation_testcases = junit_testcases(mutation_xml)
-    except JUnitReportError as exc:
-      print(f"INVALID: historical-outer-ki {exc}")
+    survivors = []
+    for mutation in MUTATIONS:
+      source_path = None
+      original_source = None
+      mutation_result = None
+      mutation_error = None
+      restored = False
+      try:
+        source_path, original_source = apply_mutation(mutation)
+        mutation_result = run_pytest(
+          mutation.test_nodes,
+          temp_root / f"{mutation.name}.xml",
+        )
+      except Exception as exc:  # pragma: no cover - failure reporting path
+        mutation_error = exc
+      finally:
+        if source_path is not None and original_source is not None:
+          source_path.write_bytes(original_source)
+          restored = source_path.read_bytes() == original_source
+
+      if not restored:
+        print(f"INVALID: {mutation.name} source restoration was not byte-identical")
+        return 1
+      if mutation_error is not None:
+        print(f"INVALID: {mutation.name} could not run: {mutation_error}")
+        return 1
+      if mutation_result is None:
+        print(f"INVALID: {mutation.name} produced no pytest result")
+        return 1
+
+      mutation_xml = temp_root / f"{mutation.name}.xml"
+      try:
+        mutation_testcases = junit_testcases(mutation_xml)
+      except JUnitReportError as exc:
+        print(f"INVALID: {mutation.name} {exc}")
+        return 1
+      if mutation_result.returncode == 1 and has_only_assertion_failures(mutation_testcases):
+        print(f"KILLED: {mutation.name} [{', '.join(mutation.test_nodes)}]")
+      elif mutation_result.returncode == 0:
+        survivors.append(mutation.name)
+        print(f"SURVIVED: {mutation.name} [{', '.join(mutation.test_nodes)}]")
+      else:
+        print(f"INVALID: {mutation.name} exited without assertion-only test failures "
+              + f"(pytest status {mutation_result.returncode})")
+        print(mutation_result.stdout)
+        return 1
+
+    if survivors:
+      print(f"Historical mutations survived: {', '.join(survivors)}")
       return 1
-    if mutation_result.returncode == 1 and has_only_assertion_failures(mutation_testcases):
-      print(f"KILLED: historical-outer-ki [{', '.join(MUTATION_TEST_NODES)}]")
-      print("RESTORED: outer-KI source is byte-identical")
-      return 0
-    if mutation_result.returncode == 0:
-      print("SURVIVED: historical-outer-ki")
-      return 1
-
-    print("INVALID: historical-outer-ki exited without assertion-only test failures "
-          + f"(pytest status {mutation_result.returncode})")
-    print(mutation_result.stdout)
-    return 1
+    print(f"ALL KILLED: {len(MUTATIONS)} parent longitudinal mutations")
+    print("RESTORED: every mutated source is byte-identical")
+    return 0
 
 
 if __name__ == "__main__":

.github/ci/test_tesla_preap_longitudinal_workflow.py

@@ -3,6 +3,7 @@
 
 
 WORKFLOW_PATH = Path(__file__).resolve().parents[1] / "workflows" / "tests.yaml"
+PROCESS_REPLAY_PATH = Path(__file__).resolve().parents[2] / "selfdrive" / "test" / "process_replay" / "process_replay.py"
 
 
 def indented_block(document: str, header: str) -> str:
@@ -41,9 +42,12 @@ def test_focused_tests_and_mutations_are_pinned():
   workflow = WORKFLOW_PATH.read_text()
   focused_job = indented_block(workflow, "  tesla_preap_longitudinal_regression:")
   required_commands = (
+    "selfdrive/controls/tests/test_following_distance.py",
+    "selfdrive/controls/tests/test_tesla_preap_following.py",
     "selfdrive/controls/tests/test_tesla_preap_longcontrol.py",
     "opendbc_repo/opendbc/car/tesla/preap/tests/test_longitudinal_tuning.py",
     "opendbc_repo/opendbc/car/tesla/preap/tests/test_virtual_das.py",
+    "opendbc_repo/opendbc/car/tesla/preap/tests/test_vdas_grade_control.py",
   )
 
   for command in required_commands:
@@ -58,11 +62,21 @@ def test_focused_job_cannot_be_skipped_or_soft_failed():
   assert not re.search(r"^\s*(?:if|continue-on-error)\s*:", focused_job, re.MULTILINE)
 
 
+def test_additive_follow_telemetry_is_ignored_by_process_replay():
+  process_replay = PROCESS_REPLAY_PATH.read_text()
+  plannerd_config = re.search(r'proc_name="plannerd",(?P<body>.*?)\n  \),', process_replay, re.DOTALL)
+  assert plannerd_config is not None
+
+  assert '"longitudinalPlan.napFollowDistance"' in plannerd_config.group("body")
+  assert '"longitudinalPlan.tFollow"' in plannerd_config.group("body")
+
+
 def main():
   test_nap_branches_run_on_push()
   test_named_focused_job_is_present()
   test_focused_tests_and_mutations_are_pinned()
   test_focused_job_cannot_be_skipped_or_soft_failed()
+  test_additive_follow_telemetry_is_ignored_by_process_replay()
   print("Tesla Pre-AP longitudinal workflow contract passed")
 
 

.github/workflows/tests.yaml

@@ -40,9 +40,12 @@ jobs:
     - name: Run focused longitudinal tests
       run: |
         pytest -q -n 0 \
+          selfdrive/controls/tests/test_following_distance.py \
+          selfdrive/controls/tests/test_tesla_preap_following.py \
           selfdrive/controls/tests/test_tesla_preap_longcontrol.py \
           opendbc_repo/opendbc/car/tesla/preap/tests/test_longitudinal_tuning.py \
-          opendbc_repo/opendbc/car/tesla/preap/tests/test_virtual_das.py
+          opendbc_repo/opendbc/car/tesla/preap/tests/test_virtual_das.py \
+          opendbc_repo/opendbc/car/tesla/preap/tests/test_vdas_grade_control.py
     - name: Run historical mutation checks
       run: python .github/ci/tesla_preap_longitudinal_mutations.py
 

cereal/log.capnp

@@ -1266,6 +1266,8 @@ struct LongitudinalPlan @0xe00b5b3eba12876c {
   shouldStop @37: Bool;
   allowThrottle @38: Bool;
   allowBrake @39: Bool;
+  napFollowDistance @40 :UInt8;
+  tFollow @41 :Float32;
 
 
   solverExecutionTime @35 :Float32;

selfdrive/controls/lib/longitudinal_mpc_lib/long_mpc.py

@@ -58,6 +58,7 @@
 CRUISE_MIN_ACCEL = -1.2
 CRUISE_MAX_ACCEL = 1.6
 MIN_X_LEAD_FACTOR = 0.5
+NAP_T_FOLLOW = (0.7, 0.9, 1.1, 1.3, 1.5, 1.7, 1.9)
 
 def get_jerk_factor(personality=log.LongitudinalPersonality.standard):
   if personality==log.LongitudinalPersonality.relaxed:
@@ -71,9 +72,8 @@ def get_jerk_factor(personality=log.LongitudinalPersonality.standard):
 
 
 def get_T_FOLLOW(personality=log.LongitudinalPersonality.standard, nap_follow_dist=None):
-  # NAP configurable follow distance: 1-7 maps to 0.7s - 1.9s in 0.2s steps
-  if nap_follow_dist is not None and 1 <= nap_follow_dist <= 7:
-    return 0.7 + (nap_follow_dist - 1) * 0.2
+  if nap_follow_dist in range(1, len(NAP_T_FOLLOW) + 1):
+    return NAP_T_FOLLOW[nap_follow_dist - 1]
 
   if personality==log.LongitudinalPersonality.relaxed:
     return 1.75
@@ -317,8 +317,7 @@ def process_lead(self, lead):
     lead_xv = self.extrapolate_lead(x_lead, v_lead, a_lead, a_lead_tau)
     return lead_xv
 
-  def update(self, radarstate, v_cruise, personality=log.LongitudinalPersonality.standard, nap_follow_dist=None):
-    t_follow = get_T_FOLLOW(personality, nap_follow_dist)
+  def update(self, radarstate, v_cruise, t_follow):
     v_ego = self.x0[1]
     self.status = radarstate.leadOne.status or radarstate.leadTwo.status