IQ.Pilot Release Commit @ 9ba2ea7

2026-08-31 10:53:00 -05:00
parent 4b46753a27
commit 67a2b4955e
22 changed files with 2011 additions and 65 deletions
--- a/iqpilot/selfdrive/controls/tests/test_scan_radar_leads.py
+++ b/iqpilot/selfdrive/controls/tests/test_scan_radar_leads.py
@@ -0,0 +1,191 @@
+from types import SimpleNamespace
+
+import pytest
+
+from iqdbc.car.honda.values import CAR
+from iqpilot.selfdrive.controls.radard import (SCAN_CHALLENGER_STALE_CYCLES, SCAN_DISTANCE_STALE_CYCLES,
+                                               SCAN_LEAD_MIN_CYCLES, SCAN_LEAD_PROB, SCAN_SWEEP_DT,
+                                               KalmanParams, RadarD, Track, get_lead,
+                                               scan_low_speed_candidate, track_agrees_with_model,
+                                               uses_scan_radar)
+
+DT_MDL = 0.05
+
+
+def honda_cp(fingerprint=CAR.HONDA_CIVIC_BOSCH, radar_unavailable=False, brand="honda"):
+  return SimpleNamespace(brand=brand, carFingerprint=fingerprint, radarUnavailable=radar_unavailable, flags=0)
+
+
+def model_lead(x=30.0, y=0.0, v=10.0, prob=0.9, x_std=2.0, y_std=0.5, v_std=1.0):
+  return SimpleNamespace(x=[x], y=[y], v=[v], a=[0.0], prob=prob, xStd=[x_std], yStd=[y_std], vStd=[v_std])
+
+
+def make_track(identifier, d_rel, y_rel=0.0, v_rel=0.0, v_ego=10.0, cycles=SCAN_LEAD_MIN_CYCLES, measured=True):
+  track = Track(identifier, v_rel + v_ego, KalmanParams(SCAN_SWEEP_DT))
+  for _ in range(cycles):
+    track.update(d_rel, y_rel, v_rel, v_rel + v_ego, measured)
+  return track
+
+
+class TestScanRadarGating:
+  def test_capable_verified_car_uses_scan(self):
+    assert uses_scan_radar(honda_cp())
+
+  def test_radar_unavailable_disables(self):
+    assert not uses_scan_radar(honda_cp(radar_unavailable=True))
+
+  def test_non_family_car_never_scans(self):
+    assert not uses_scan_radar(honda_cp(fingerprint=CAR.HONDA_CIVIC))
+    assert not uses_scan_radar(honda_cp(fingerprint=CAR.HONDA_CIVIC_2022))
+
+  def test_other_brand_never_scans(self):
+    assert not uses_scan_radar(honda_cp(brand="toyota"))
+
+
+class TestTrackMeasurementGating:
+  def test_repeated_payload_not_absorbed_twice(self):
+    kp = KalmanParams(SCAN_SWEEP_DT)
+    absorbed = Track(1, 10.0, kp)
+    starved = Track(1, 10.0, kp)
+    absorbed.update(30.0, 0.0, 5.0, 15.0, True)
+    starved.update(30.0, 0.0, 5.0, 15.0, True)
+    v_after_first = starved.vLeadK
+
+    for _ in range(5):
+      absorbed.update(30.0, 0.0, 5.0, 15.0, True, absorb_measurement=True)
+      starved.update(30.0, 0.0, 5.0, 15.0, False, absorb_measurement=False)
+
+    assert starved.vLeadK == v_after_first
+    assert absorbed.vLeadK != v_after_first
+    assert starved.cnt == absorbed.cnt
+
+
+class TestLowSpeedCandidate:
+  def test_needs_minimum_cycles(self):
+    young = make_track(1, 10.0, v_ego=2.0, cycles=SCAN_LEAD_MIN_CYCLES - 1)
+    mature = make_track(2, 10.0, v_ego=2.0)
+    assert not scan_low_speed_candidate(young, 2.0)
+    assert scan_low_speed_candidate(mature, 2.0)
+
+  def test_geometry_still_applies(self):
+    offset = make_track(3, 10.0, y_rel=2.0, v_ego=2.0)
+    assert not scan_low_speed_candidate(offset, 2.0)
+
+
+class TestModelAgreement:
+  def test_strict_tighter_than_relaxed(self):
+    track = make_track(1, 40.0, v_ego=10.0)
+    # ~15m disagreement: beyond the strict 25% envelope, inside the relaxed 40% one
+    lead = model_lead(x=56.5)
+    assert not track_agrees_with_model(track, lead, 10.0, strict=True)
+    assert track_agrees_with_model(track, lead, 10.0, strict=False)
+
+
+class TestScanLeadSelection:
+  def make_cp(self):
+    return honda_cp(), SimpleNamespace()
+
+  def test_held_lead_survives_probability_dip(self):
+    CP, CP_IQ = self.make_cp()
+    track = make_track(7, 10.0, v_ego=2.0)
+    lead = model_lead(x=11.5, v=2.0, prob=0.05)
+    # raw prob is below threshold, the filtered prob is passed in above it
+    result = get_lead(2.0, True, {7: track}, lead, 2.0, CP, CP_IQ, low_speed_override=True,
+                      scan_radar=True, filtered_prob=0.6, held_track_id=7)
+    assert result['status'] and result['radarTrackId'] == 7
+
+  def test_unrelated_closer_point_cannot_usurp_without_model_evidence(self):
+    CP, CP_IQ = self.make_cp()
+    held = make_track(7, 10.0, v_ego=2.0)
+    interloper = make_track(9, 4.0, y_rel=0.5, v_ego=2.0)
+    lead = model_lead(x=11.5, v=2.0, prob=0.9)
+    result = get_lead(2.0, True, {7: held, 9: interloper}, lead, 2.0, CP, CP_IQ, low_speed_override=True,
+                      scan_radar=True, filtered_prob=0.9, held_track_id=7)
+    assert result['radarTrackId'] == 7
+
+  def test_matching_closer_point_takes_over(self):
+    CP, CP_IQ = self.make_cp()
+    held = make_track(7, 10.0, v_ego=2.0)
+    closer = make_track(9, 4.0, v_ego=2.0)
+    lead = model_lead(x=5.5, v=2.0, prob=0.9)  # model agrees with the closer car
+    result = get_lead(2.0, True, {7: held, 9: closer}, lead, 2.0, CP, CP_IQ, low_speed_override=True,
+                      scan_radar=True, filtered_prob=0.9, held_track_id=7)
+    assert result['radarTrackId'] == 9
+
+  def test_radar_only_takeover_when_no_model_lead(self):
+    CP, CP_IQ = self.make_cp()
+    track = make_track(4, 8.0, v_ego=2.0)
+    lead = model_lead(prob=0.0)
+    result = get_lead(2.0, True, {4: track}, lead, 2.0, CP, CP_IQ, low_speed_override=True,
+                      scan_radar=True, filtered_prob=0.0, held_track_id=-1)
+    assert result['status'] and result['radarTrackId'] == 4
+
+  def test_young_track_cannot_lead_alone(self):
+    CP, CP_IQ = self.make_cp()
+    track = make_track(4, 8.0, v_ego=2.0, cycles=1)
+    lead = model_lead(prob=0.0)
+    result = get_lead(2.0, True, {4: track}, lead, 2.0, CP, CP_IQ, low_speed_override=True,
+                      scan_radar=True, filtered_prob=0.0, held_track_id=-1)
+    assert not result['status']
+
+  def test_non_scan_behavior_unchanged(self):
+    CP, CP_IQ = self.make_cp()
+    track = make_track(4, 8.0, v_ego=2.0, cycles=1)
+    lead = model_lead(prob=0.0)
+    result = get_lead(2.0, True, {4: track}, lead, 2.0, CP, CP_IQ, low_speed_override=True)
+    assert result['status']  # legacy path has no maturity gate
+
+
+class TestHeldLeadStaleness:
+  def make_radard(self, tracks):
+    rd = object.__new__(RadarD)
+    rd.tracks = tracks
+    rd.ready = True
+    rd.v_ego = 10.0
+    rd.held_lead_ids = [7, -1]
+    rd._held_evidence_ids = [7, -1]
+    rd._challenger_stale_counts = [0, 0]
+    rd._distance_stale_counts = [0, 0]
+    return rd
+
+  def test_relaxed_match_keeps_hold(self):
+    held = make_track(7, 30.0, v_ego=10.0)
+    rd = self.make_radard({7: held})
+    for _ in range(5):
+      rd._refresh_held_lead_evidence(0, model_lead(x=32.0), 0.9)
+    assert rd.held_lead_ids[0] == 7
+
+  def test_better_challenger_releases_hold(self):
+    held = make_track(7, 80.0, v_ego=10.0)
+    challenger = make_track(9, 30.0, v_ego=10.0)
+    rd = self.make_radard({7: held, 9: challenger})
+    lead = model_lead(x=31.5)
+    for _ in range(SCAN_CHALLENGER_STALE_CYCLES):
+      rd._refresh_held_lead_evidence(0, lead, 0.9)
+    assert rd.held_lead_ids[0] == -1
+
+  def test_gross_distance_disagreement_releases_hold(self):
+    held = make_track(7, 80.0, v_ego=10.0)
+    rd = self.make_radard({7: held})
+    lead = model_lead(x=31.5, x_std=60.0, y_std=30.0, v_std=30.0)  # huge model uncertainty
+    for _ in range(SCAN_DISTANCE_STALE_CYCLES):
+      rd._refresh_held_lead_evidence(0, lead, 0.9)
+    assert rd.held_lead_ids[0] == -1
+
+  def test_low_probability_resets_evidence_without_release(self):
+    held = make_track(7, 80.0, v_ego=10.0)
+    rd = self.make_radard({7: held})
+    for _ in range(10):
+      rd._refresh_held_lead_evidence(0, model_lead(x=31.5), SCAN_LEAD_PROB)
+    assert rd.held_lead_ids[0] == 7
+    assert rd._distance_stale_counts[0] == 0
+
+
+class TestProbFilterTiming:
+  def test_rises_instantly_decays_slowly(self):
+    from iqpilot.common.filter_simple import FirstOrderFilter
+    f = FirstOrderFilter(0.0, 0.2, DT_MDL)
+    f.x = max(f.x, 0.9)
+    assert f.x == pytest.approx(0.9)
+    f.update(0.0)
+    assert f.x > 0.6