diff --git a/openpilot/starpilot/curve_speed/host.py b/openpilot/starpilot/curve_speed/host.py index 3163d59982..dbce5faf81 100644 --- a/openpilot/starpilot/curve_speed/host.py +++ b/openpilot/starpilot/curve_speed/host.py @@ -5,11 +5,13 @@ import math import time from openpilot.cereal.services import SERVICE_LIST +from openpilot.common.realtime import DT_MDL from openpilot.starpilot.curve_speed.runtime import Frame, INPUT_MAX_AGE_NS, MODEL_MAX_AGE_NS, Runtime from openpilot.starpilot.curve_speed.target import CurveProfile -from openpilot.starpilot.lead_tracking import LeadDetector +from openpilot.starpilot.lead_tracking import FILTER_THRESHOLD, FILTER_TIME_S, LeadDetector CLOCK_PAIR_MAX_SKEW_NS = 5_000_000 +LEAD_WARMUP_FRAMES = math.ceil(math.log(1 - FILTER_THRESHOLD) / math.log(1 - DT_MDL / (FILTER_TIME_S + DT_MDL))) def clock_pair_ns() -> tuple[int, int] | None: @@ -67,12 +69,14 @@ class CurveHost: self.boot_offset_ns: int | None = None self.source_floor_ns = 0 self.leads = LeadDetector() + self.lead_samples = 0 self.lead_drive_id = 0 self.previous_long_active = False self.previous_active_headway: float | None = None def _reset_leads(self) -> None: self.leads.reset() + self.lead_samples = 0 self.previous_long_active = False self.previous_active_headway = None @@ -101,22 +105,20 @@ class CurveHost: required = ('modelV2', 'carState', 'carControl', 'controlsState', 'selfdriveState', 'radarState') try: camera_eof = sm['modelV2'].timestampEof - camera_current = (type(camera_eof) is int and 0 < camera_eof <= pair[1] and - pair[1] - camera_eof <= MODEL_MAX_AGE_NS) + camera_age = pair[1] - camera_eof if type(camera_eof) is int and camera_eof > 0 else -1 + camera_current = (0 <= camera_age <= MODEL_MAX_AGE_NS and camera_eof <= now_ns + offset) except (KeyError, AttributeError, TypeError, ValueError, OverflowError): camera_current = False # A newly published model envelope cannot renew the age of its camera # input. EOF uses BOOTTIME; only the bracketed clock pair may compare it. # Offline replay intentionally keeps its separate recorded timeline. - if (not camera_current or not 0 <= pair[0] - now_ns <= INPUT_MAX_AGE_NS or - not all(_fresh(sm, name, pair[0]) for name in required)): + if (not camera_current or not 0 < frame.model_ns <= now_ns or + pair[0] - frame.model_ns > MODEL_MAX_AGE_NS or not 0 <= pair[0] - now_ns <= INPUT_MAX_AGE_NS): frame = replace(frame, inputs_fresh=False) if rearmed or any(type(sm.logMonoTime.get(name, 0)) is not int or sm.logMonoTime[name] <= self.source_floor_ns for name in required): frame = replace(frame, inputs_fresh=False) - if not frame.inputs_fresh or frame.profile is None: - self._reset_leads() - else: + if frame.inputs_fresh and frame.profile is not None: try: # Tracking precedes headway reset: the first inactive tick retains the prior headway, # then uses zero. Active control requires this cycle's MPC headway. @@ -131,9 +133,16 @@ class CurveHost: observed_mono_s=frame.model_ns / 1e9, now_mono_s=now_ns / 1e9, radar_fresh=_fresh(sm, 'radarState', now_ns), model_fresh=_fresh(sm, 'modelV2', now_ns), car_fresh=_fresh(sm, 'carState', now_ns), headway_fresh=headway_fresh) + if self.leads.last_observed_mono_s is None: + self.lead_samples = 0 + elif observed.tracked is not None and not sm['carState'].standstill: + self.lead_samples += 1 + tracked = observed.tracked following = (observed.following if frame.long_active else False if observed.tracked is not None else None) - frame = replace(frame, tracking_lead=observed.tracked, following_lead=following) + if self.lead_samples < LEAD_WARMUP_FRAMES and not tracked and observed.raw_present: + tracked = following = None + frame = replace(frame, tracking_lead=tracked, following_lead=following) self.previous_long_active = frame.long_active if observed.tracked is not None else False self.previous_active_headway = (float(detector_headway) if detector_headway is not None and frame.long_active and headway_fresh and observed.tracked is not None else None) diff --git a/openpilot/starpilot/curve_speed/runtime.py b/openpilot/starpilot/curve_speed/runtime.py index fcf1db91f2..071202f050 100644 --- a/openpilot/starpilot/curve_speed/runtime.py +++ b/openpilot/starpilot/curve_speed/runtime.py @@ -76,6 +76,7 @@ class Runtime: self.filter = TargetFilter() self.last_ns = 0 self.last_model_ns = 0 + self.last_valid_ns = 0 self.drive_id = 0 self.was_controlling = False self.was_long_active = False @@ -94,6 +95,7 @@ class Runtime: self.pending_ns = 0 self.pending_target: float | None = None self.pending_cap = False + self.pending_hold = False self.pending_cruise = 0.0 self.pending_ego = 0.0 self.glow = False @@ -110,6 +112,7 @@ class Runtime: self.filter.reset() self.last_ns = 0 self.last_model_ns = 0 + self.last_valid_ns = 0 self.drive_id = drive_id self.was_controlling = False self.was_long_active = False @@ -127,6 +130,7 @@ class Runtime: self.pending_ns = 0 self.pending_target = None self.pending_cap = False + self.pending_hold = False self.pending_cruise = 0.0 self.pending_ego = 0.0 self.glow = False @@ -149,6 +153,8 @@ class Runtime: self.glow = False return False self.confirmed_ns = frame_ns + if self.pending_hold: + return True target = self.pending_target selected = applied and self.pending_cap and target is not None self.was_controlling = bool(selected and target < self.pending_cruise - 1.0 and @@ -230,24 +236,31 @@ class Runtime: self.pending_ns = now self.pending_target = None self.pending_cap = False + self.pending_hold = False valid_profile = (frame.inputs_fresh and frame.profile is not None and type(frame.model_ns) is int and 0 < frame.model_ns <= now and now - frame.model_ns <= MODEL_MAX_AGE_NS and frame.profile.observed_ns == frame.model_ns and frame.model_ns > self.last_model_ns) if not valid_profile: - self.filter.reset() self.watch_until_ns = 0 self.last_training_ns = 0 - self.nudged = False - self.override = False - self.was_controlling = False - self.glow = False - self.was_long_active = frame.long_active - self.was_gas = frame.gas_pressed - self.was_brake = frame.brake_pressed + self.pending_hold = (self.filter.target is not None and + now - self.last_valid_ns <= int(MAX_STEP_SECONDS * 1e9)) + if self.pending_hold: + self.pending_target = self.filter.target + else: + self.filter.reset() + self.nudged = False + self.override = False + self.was_controlling = False + self.glow = False + self.was_long_active = frame.long_active + self.was_gas = frame.gas_pressed + self.was_brake = frame.brake_pressed return self._result(None, None, 0.0, 'stale_or_invalid_evidence', False, now) self.last_model_ns = frame.model_ns + self.last_valid_ns = now if (not finite_number(frame.cruise_mps) or frame.cruise_mps <= 0 or not finite_number(frame.ego_mps) or frame.ego_mps < 0 or frame.ego_mps > 100.0 or not finite_number(frame.measured_curvature) or diff --git a/openpilot/starpilot/curve_speed/status.py b/openpilot/starpilot/curve_speed/status.py index 1f1c1d0e16..213f5ff139 100644 --- a/openpilot/starpilot/curve_speed/status.py +++ b/openpilot/starpilot/curve_speed/status.py @@ -50,7 +50,8 @@ class StatusPublisher: 'documentValid': bool(runtime.document_valid), 'hasCandidate': result.candidate_mps is not None, 'candidateMps': float(result.candidate_mps or 0.0), 'hasCeiling': result.ceiling_mps is not None, 'ceilingMps': float(result.ceiling_mps or 0.0), - 'applied': bool(planner.last_curve_ceiling_applied), 'controlling': bool(runtime.was_controlling), + 'applied': bool(planner.last_curve_ceiling_applied), + 'controlling': bool(runtime.was_controlling and planner.last_curve_ceiling_applied), 'training': bool(result.training), 'calibrationProgress': float(result.progress), 'comfortAccel': float(runtime.curve.average_comfort), 'bindingDistance': float(result.binding_distance_m), 'reason': result.reason if result.ceiling_mps is not None else result.learning_reason, diff --git a/openpilot/starpilot/curve_speed/tests/test_host.py b/openpilot/starpilot/curve_speed/tests/test_host.py index 50d38ae620..3e119a9f4a 100644 --- a/openpilot/starpilot/curve_speed/tests/test_host.py +++ b/openpilot/starpilot/curve_speed/tests/test_host.py @@ -101,6 +101,50 @@ class TestCurveHost(unittest.TestCase): self.assertFalse(seen[-1][1].following) self.assertFalse(seen[-1][1].tracked) + def capture_frames(self, host): + frames = [] + original = host.runtime.step + def runtime_capture(frame, **kwargs): + frames.append(frame) + return original(frame, **kwargs) + self.enterContext(patch.object(host.runtime, 'step', side_effect=runtime_capture)) + return frames + + def present_lead(self, d_rel=30.0): + radar = self.sm.messages['radarState'].leadOne + radar.present = True + radar.dRel = d_rel + + def test_stale_frame_keeps_lead_consensus(self): + host = CurveHost(enabled=True, no_lead=True, replay=True) + frames = self.capture_frames(host) + self.present_lead() + for n in range(1, 17): + stamp = NOW + n * 50_000_000 + self.sm.logMonoTime = dict.fromkeys(self.sm.messages, stamp) + host.sample(self.sm, self.cp, stamp, follow_time_s=1.45) + self.assertTrue(frames[-1].following_lead) + stamp += 45_000_000 + self.sm.logMonoTime = dict.fromkeys(self.sm.messages, stamp) + self.sm.logMonoTime['carState'] = stamp - 30_000_000 + host.sample(self.sm, self.cp, stamp, follow_time_s=1.45) + stamp += 45_000_000 + self.sm.logMonoTime = dict.fromkeys(self.sm.messages, stamp) + host.sample(self.sm, self.cp, stamp, follow_time_s=1.45) + self.assertTrue(frames[-1].tracking_lead) + self.assertTrue(frames[-1].following_lead) + + def test_present_lead_is_unknown_until_detector_warms_up(self): + host = CurveHost(enabled=True, no_lead=True, replay=True) + frames = self.capture_frames(host) + self.present_lead() + for n in range(1, 21): + stamp = NOW + n * 50_000_000 + self.sm.logMonoTime = dict.fromkeys(self.sm.messages, stamp) + self.assertIsNone(host.sample(self.sm, self.cp, stamp, follow_time_s=1.45).ceiling_mps) + self.assertNotIn(False, [frame.following_lead for frame in frames]) + self.assertTrue(frames[-1].following_lead) + def test_actual_serialized_schema_projection(self): events = {name: messaging.new_message(name) for name in self.sm.messages} events['modelV2'].modelV2.orientationRate.z = [0.25] * 33 @@ -178,6 +222,33 @@ class TestCurveHost(unittest.TestCase): self.assertFalse(result.training) self.assertEqual(result.reason, 'stale_or_invalid_evidence') + def test_live_planner_latency_does_not_age_out_loop_inputs(self): + offset = 2_000_000_000 + for latency, input_age, camera_age, expected in ( + (35_000_000, 5_000_000, 60_000_000, True), + (35_000_000, 21_000_000, 60_000_000, False), + (35_000_000, 5_000_000, 115_000_000, True), + (35_000_000, 5_000_000, 115_000_001, False), + (35_000_000, 5_000_000, 140_000_000, False), + (35_000_000, 5_000_000, -1, False), + (35_000_000, 5_000_000, 150_000_001, False), + (150_000_001, 5_000_000, 60_000_000, False), + ): + with self.subTest(latency=latency, input_age=input_age, camera_age=camera_age): + tick = [NOW] + host = CurveHost(enabled=True, clock_pair=lambda tick=tick, latency=latency: (tick[0] + latency, tick[0] + latency + offset)) + result = None + for _ in range(4): + tick[0] += 50_000_000 + self.sm.logMonoTime = dict.fromkeys(self.sm.messages, tick[0] - input_age) + self.sm.logMonoTime['modelV2'] = tick[0] - 3_000_000 + self.sm.logMonoTime['radarState'] = tick[0] - 40_000_000 + self.sm.messages['modelV2'].timestampEof = tick[0] + offset - camera_age + result = host.sample(self.sm, self.cp, tick[0]) + self.assertEqual(result.ceiling_mps is not None, expected) + if not expected: + self.assertEqual(result.reason, 'stale_or_invalid_evidence') + if __name__ == '__main__': unittest.main() diff --git a/openpilot/starpilot/curve_speed/tests/test_runtime.py b/openpilot/starpilot/curve_speed/tests/test_runtime.py index 12b3a61924..ca4e630496 100644 --- a/openpilot/starpilot/curve_speed/tests/test_runtime.py +++ b/openpilot/starpilot/curve_speed/tests/test_runtime.py @@ -1,7 +1,9 @@ import unittest from dataclasses import replace +from types import SimpleNamespace as NS from openpilot.starpilot.curve_speed.runtime import DriverEvent, Frame, Runtime +from openpilot.starpilot.curve_speed.status import StatusPublisher, observation from openpilot.starpilot.curve_speed.target import CurveProfile @@ -84,6 +86,58 @@ class TestCurveRuntime(unittest.TestCase): self.assertIsNotNone(rt.step(frame(4)).ceiling_mps) self.assertIsNotNone(rt.step(frame(20)).ceiling_mps) # discontinuity resets filter, then fresh frame can rearm + def test_isolated_stale_frame_holds_target_without_ceiling(self): + rt = Runtime(enabled=True) + arm_selected(rt) + held = rt.filter.target + stale = frame(27, inputs_fresh=False) + self.assertIsNone(rt.step(stale).ceiling_mps) + rt.confirm_applied(stale.now_ns, applied=False) + self.assertTrue(rt.was_controlling) + self.assertTrue(rt.glow) + self.assertEqual(rt.filter.target, held) + resumed = rt.step(frame(28)).ceiling_mps + assert resumed is not None + self.assertLessEqual(resumed, held) + + def test_stale_hold_publishes_valid_glowing_status(self): + rt = Runtime(enabled=True) + arm_selected(rt) + stale = frame(27, inputs_fresh=False) + result = rt.step(stale) + rt.confirm_applied(stale.now_ns, applied=False) + planner = NS(last_curve_ceiling_applied=False, last_curve_ceiling_status='absent') + event = StatusPublisher().attach(None, NS(runtime=rt), result, planner, now_ns=stale.now_ns, + model_ns=frame(26).now_ns, persistence_status='idle') + status = observation(event.slcState, stale.now_ns) + assert status is not None + self.assertTrue(status.glow) + self.assertFalse(status.controlling) + + def test_stale_hold_expires_after_max_step(self): + rt = Runtime(enabled=True) + arm_selected(rt) + for n in range(27, 31): + item = frame(n, inputs_fresh=False) + rt.step(item) + rt.confirm_applied(item.now_ns, applied=False) + self.assertIsNotNone(rt.filter.target) + item = frame(31, inputs_fresh=False) + rt.step(item) + rt.confirm_applied(item.now_ns, applied=False) + self.assertIsNone(rt.filter.target) + self.assertFalse(rt.glow) + self.assertFalse(rt.was_controlling) + + def test_gas_override_during_stale_hold_still_latches(self): + rt = Runtime(enabled=True) + arm_selected(rt) + stale = frame(27, inputs_fresh=False, gas_pressed=True) + rt.step(stale) + rt.confirm_applied(stale.now_ns, applied=False) + self.assertIsNone(rt.step(frame(28, gas_pressed=True)).ceiling_mps) + self.assertTrue(rt.override) + def test_invalid_document_never_learns_or_persists(self): rt = Runtime({'version': 1, 'buckets': {'nan': {'average': 2.0, 'count': 1}}}, enabled=True) self.assertFalse(rt.document_valid) diff --git a/prebuilt.json b/prebuilt.json index cb16652e9c..934c6bddca 100644 --- a/prebuilt.json +++ b/prebuilt.json @@ -1 +1 @@ -{"protected_inventory_sha256": "f30473215dcb0bdcd968347b35851fff4fc8b1606785c4b126cd181dbf9a06e5", "schema_version": 1} +{"protected_inventory_sha256": "c63cb2675c4d7c9ad1e0e64eb6e1205e157b3d11c359a303af7395dd5a148128", "schema_version": 1}