Qualify split HKG angle EPS publications

.github/workflows/hkg-angle-recovery-check.yaml

@@ -13,6 +13,8 @@ jobs:
       SCALE: '1'
     steps:
       - uses: actions/checkout@v7
+      - name: Fetch exact baseline source
+        run: git -c gc.auto=0 -c maintenance.auto=false fetch --no-tags --depth=1 --filter=blob:none origin 68434cf39c501604fe962e9488a03bde0fed0d73
       - uses: actions/setup-go@v7
         with:
           go-version: '1.25.1'
@@ -24,7 +26,15 @@ jobs:
         run: go mod download
       - run: ./tools/op.sh setup
       - name: Build matching Linux dependencies
-        run: .venv/bin/scons -j2
+        run: >-
+          .venv/bin/scons -j2
+          openpilot/common/libparams_c.so
+          msgq_repo/msgq/_ipc.so
+          msgq_repo/msgq/visionipc/_visionipc.so
+          opendbc_repo/opendbc/can/_raw_value.so
+          opendbc_repo/opendbc/dbc
+          openpilot/selfdrive/controls/lib/longitudinal_mpc_lib/c_generated_code/acados_ocp_solver_pyx.so
+          openpilot/selfdrive/locationd/models/generated/libcar.so
       - name: Require the original split-delivery failure
         shell: bash
         run: |
@@ -33,26 +43,31 @@ jobs:
           import subprocess, xml.etree.ElementTree as ET, json
           path=Path('openpilot/starpilot/aol/temporary_steering.py')
           shipping=path.read_bytes()
+          transport=Path('openpilot/starpilot/aol/transport_pause.py')
+          transport_shipping=transport.read_bytes()
+          transport_original=subprocess.check_output(['git','show','68434cf39c501604fe962e9488a03bde0fed0d73:openpilot/starpilot/aol/transport_pause.py'])
           original=subprocess.check_output(['git','show','68434cf39c501604fe962e9488a03bde0fed0d73:openpilot/starpilot/aol/temporary_steering.py'])
           try:
             path.write_bytes(original)
-            result=subprocess.run(['.venv/bin/python','-m','pytest','-q','openpilot/starpilot/aol/tests/test_hkg_angle_eps_retention.py','-k','split_publications','--junitxml=baseline.xml'])
+            transport.write_bytes(transport_original)
+            result=subprocess.run(['.venv/bin/python','-m','pytest','-q','openpilot/starpilot/aol/tests/test_hkg_angle_eps_retention.py','-k','split_publications or stopped_full_request','--junitxml=baseline.xml'])
             root=ET.parse('baseline.xml').getroot()
             cases=list(root.iter('testcase')); failed=[c for c in cases if c.find('failure') is not None]
-            assert result.returncode==1 and len(failed)==4 and not list(root.iter('error'))
-            assert all('NATIVE_FIRST_EPS_DISARMED' in ''.join(c.find('failure').itertext()) for c in failed)
+            assert result.returncode==1 and len(failed)==8 and not list(root.iter('error'))
+            assert all(any(marker in ''.join(c.find('failure').itertext()) for marker in ('NATIVE_FIRST_EPS_DISARMED','STOPPED_FULL_REQUEST_DISARMED')) for c in failed)
             Path('baseline-result.json').write_text(json.dumps({'cases':len(cases),'expectedFailures':len(failed),'confirmed':True}))
           finally:
             path.write_bytes(shipping)
+            transport.write_bytes(transport_shipping)
           assert path.read_bytes()==shipping
           PYCODE
       - name: Run steering retention and independent safety veto regressions
         run: |
-          .venv/bin/python -m pytest -q --junitxml=qualification.xml             openpilot/starpilot/aol/tests/test_hkg_angle_eps_retention.py             openpilot/starpilot/aol/tests/test_pe_combined_profile_contract.py             openpilot/starpilot/aol/tests/test_stock_eps_recovery.py             openpilot/starpilot/aol/tests/test_runtime.py             openpilot/starpilot/aol/tests/test_source_pause_driver_monitoring.py
+          .venv/bin/python -m pytest -q --junitxml=qualification.xml             openpilot/starpilot/aol/tests/test_hkg_angle_eps_retention.py             openpilot/starpilot/aol/tests/test_pe_combined_profile_contract.py             openpilot/starpilot/aol/tests/test_stock_eps_recovery.py             openpilot/starpilot/aol/tests/test_runtime.py             openpilot/starpilot/aol/tests/test_source_pause_driver_monitoring.py             openpilot/system/hardware/tests             openpilot/starpilot/drive_state/tests/test_hardware_lifecycle.py
       - name: Check owned Python files
         run: |
-          .venv/bin/ruff check openpilot/starpilot/aol/temporary_steering.py openpilot/starpilot/aol/tests/test_hkg_angle_eps_retention.py
-          .venv/bin/ty check openpilot/starpilot/aol/temporary_steering.py openpilot/starpilot/aol/tests/test_hkg_angle_eps_retention.py
+          .venv/bin/ruff check openpilot/starpilot/aol/temporary_steering.py openpilot/starpilot/aol/tests/test_hkg_angle_eps_retention.py openpilot/starpilot/aol/transport_pause.py openpilot/system/hardware/hardwared.py openpilot/system/hardware/hardware_timing.py openpilot/system/hardware/tests/test_hardware_timing.py
+          .venv/bin/ty check openpilot/starpilot/aol/temporary_steering.py openpilot/starpilot/aol/tests/test_hkg_angle_eps_retention.py openpilot/starpilot/aol/transport_pause.py openpilot/system/hardware/hardwared.py openpilot/system/hardware/hardware_timing.py openpilot/system/hardware/tests/test_hardware_timing.py
       - name: Save exact results
         if: always()
         uses: actions/upload-artifact@v4

openpilot/starpilot/aol/tests/test_hkg_angle_eps_retention.py

@@ -164,3 +164,44 @@ def test_angle_eps_split_publications_preserve_only_armed_intent(identity, full,
       assert not feedback.failed
       if fault:
         assert not axis.lateralActive and not command.latActive
+
+
+@pytest.mark.parametrize('identity', ('pe_long', 'ev9_long'))
+@pytest.mark.parametrize('native_long_pending', (False, True))
+def test_stopped_full_request_keeps_existing_lateral_intent_without_granting_commands(identity, native_long_pending):
+  from opendbc.car import structs
+  case = fixture.IoniqAvailabilityTests(methodName='runTest')
+  with ExitStack() as cleanup:
+    cleanup.callback(case.doCleanups)
+    cleanup.enter_context(OpenpilotPrefix())
+    cp = profile(identity)
+    assert cp is not None
+    sd, _, drive = case._authority_callers(False, cp=cp, genuine_events=True)
+    feedback = TransportPauseFeedback(sd.CP)
+    sm = messaging.SubMaster(['aolAxisState', 'onroadEvents', 'aolSafetyWire', 'pandaStates'])
+    state = fixture.car_state()
+    state.vEgoRaw = state.vEgo
+    for tick in range(7):
+      state.standstill = tick in (3, 4, 5)
+      state.vEgo = state.vEgoRaw = 0. if state.standstill else 5.
+      state.buttonEnable = tick == 4
+      state.buttonEvents = ([structs.CarState.ButtonEvent(type='accelCruise', pressed=False)] if tick == 4 else [])
+      requested_long = tick >= 5 or tick == 4 and native_long_pending
+      allowed_long = tick >= 5
+      axis, _, command = drive(tick, observed_state=state, native_request_lat=True,
+        allowed=not state.standstill, native_request_long=requested_long, long_allowed=allowed_long,
+        panda_controls_allowed=allowed_long)
+      now = 10_000_000_000 + tick * 10_000_000
+      frames = [sd.pm.messages['aolAxisState'], sd.pm.messages['onroadEvents']]
+      for name in ('aolSafetyWire', 'pandaStates'):
+        message = messaging.new_message(name, len(sd.sm[name]), valid=sd.sm.valid[name],
+          logMonoTime=sd.sm.logMonoTime[name])
+        setattr(message, name, sd.sm[name])
+        frames.append(message.as_reader())
+      sm.update_msgs(now / 1e9, frames)
+      if tick == 4:
+        assert axis.desiredLongitudinal and not axis.longitudinalActive
+        assert not axis.lateralActive and not command.latActive and not command.longActive
+      assert not feedback.observe(sm, sd.CP, state, now, latched=True,
+        intent_producer_session='authority-card'), 'STOPPED_FULL_REQUEST_DISARMED'
+      assert not feedback.failed

openpilot/starpilot/aol/transport_pause.py

@@ -1177,6 +1177,7 @@ class TransportPauseFeedback:
     ccnc_stop = bool((cs.standstill or 0 <= cs.vEgoRaw <= .3) and cp.openpilotLongitudinalControl and
       cp.carFingerprint in (CAR.KIA_EV9, CAR.HYUNDAI_IONIQ_5_PE) and not cs.accFaulted and not cs.steeringDisengage)
     combined_stop = bool(ccnc_stop and axis.desiredLongitudinal and axis.longitudinalActive)
+    pending_stop = bool(ccnc_stop and axis.desiredLongitudinal and not axis.longitudinalActive and not axis.lateralActive)
     subaru_stop = cs.standstill and subaru_restriction(cp, cs)
     if (not (qualified(cp, marked_only=True) or mazda_qualified(cp, marked_only=True) or subaru_stop or ccnc_stop) or
       cs.gearShifter != car.CarState.GearShifter.drive or
@@ -1187,7 +1188,7 @@ class TransportPauseFeedback:
       not producer or producer != self.availability_producer or
       axis.intentProducerSessionId != producer or axis.intentIntegrityEpoch != self.availability_epoch or
       not (axis.desiredLateral or subaru_stop and not axis.lateralActive) or not axis.nativeAcknowledged or
-      (axis.desiredLongitudinal or axis.longitudinalActive) and not combined_stop or
+      (axis.desiredLongitudinal or axis.longitudinalActive) and not (combined_stop or pending_stop) or
       not 0 < axis.sourceCarStateMonoTime <= stamp <= now_ns or
       now_ns - axis.sourceCarStateMonoTime > AXIS_MAX_AGE_NS):
       return False
@@ -1198,10 +1199,11 @@ class TransportPauseFeedback:
             event.name not in (log.OnroadEvent.EventName.pcmDisable, log.OnroadEvent.EventName.pedalPressed)
             for event in sm['onroadEvents'])):
       return False
-    native = _configured_transport_native(sm, cp, self.session, now_ns, allow_restricted_lateral=True)
+    native = _configured_transport_native(sm, cp, self.session, now_ns,
+      require_longitudinal=not pending_stop, allow_restricted_lateral=True)
     return bool(native is not None and native.pandaInventory and native.requestedLateral and
       (not native.lateralAllowed or subaru_stop and not axis.desiredLateral and not axis.lateralActive) and
-      (not native.requestedLongitudinal and not combined_stop or
+      (pending_stop or not native.requestedLongitudinal and not combined_stop or
        combined_stop and native.requestedLongitudinal and native.longitudinalAllowed) and
       native.observedMonoTime >= self.lateral_native_ns and
       (native.pandaSerial, tuple(p.hardwareSerial for p in native.pandaInventory)) == self.lateral_inventory)

openpilot/system/hardware/hardware_timing.py

@@ -0,0 +1,20 @@
+import time
+from contextlib import suppress
+
+from openpilot.common.swaglog import cloudlog
+
+
+class HardwareLoopTiming:
+  def __init__(self):
+    self.phase = ''
+    self.wall_ns = 0
+    self.cpu_ns = 0
+
+  def mark(self, phase: str) -> None:
+    wall_ns, cpu_ns = time.monotonic_ns(), time.thread_time_ns()
+    elapsed_ns = wall_ns - self.wall_ns
+    if self.phase and elapsed_ns >= 500_000_000:
+      with suppress(OSError):
+        cloudlog.event('hardwared.slow_phase', phase=self.phase,
+          elapsedMs=elapsed_ns / 1e6, threadCpuMs=(cpu_ns - self.cpu_ns) / 1e6, error=True)
+    self.phase, self.wall_ns, self.cpu_ns = phase, wall_ns, cpu_ns

openpilot/system/hardware/hardwared.py

@@ -30,6 +30,7 @@ from openpilot.common.linux import LinuxSystemStats
 from openpilot.system.loggerd.config import get_available_percent
 from openpilot.common.swaglog import cloudlog
 from openpilot.system.hardware.power_monitoring import PowerMonitoring
+from openpilot.system.hardware.hardware_timing import HardwareLoopTiming
 from openpilot.system.hardware.fan_controller import FanController
 from openpilot.system.hardware.chestnut.monitoring import chestnut_state_thread
 from openpilot.system.hardware.chestnut.status import ChestnutStatus
@@ -224,6 +225,7 @@ def hw_state_thread(end_event, hw_queue):
 
 def hardware_thread(end_event, hw_queue) -> None:
   system_stats = LinuxSystemStats() if sys.platform == "linux" else None
+  timing = HardwareLoopTiming()
   pm = messaging.PubMaster(['deviceState'])
   sm = messaging.SubMaster(["peripheralState", "gpsLocationExternal", "selfdriveState", "pandaStates", "chestnutState"], poll="pandaStates")
 
@@ -283,8 +285,10 @@ def hardware_thread(end_event, hw_queue) -> None:
   branch = get_short_branch()
 
   while not end_event.is_set():
+    timing.mark("panda_poll")
     sm.update(PANDA_STATES_TIMEOUT)
 
+    timing.mark("drive_mode")
     pandaStates = sm['pandaStates']
     peripheralState = sm['peripheralState']
 
@@ -324,6 +328,7 @@ def hardware_thread(end_event, hw_queue) -> None:
     if (sm.frame % round(SERVICE_LIST['pandaStates'].frequency * DT_HW) != 0) and not ign_edge:
       continue
 
+    timing.mark("thermal_read")
     msg = messaging.new_message('deviceState', valid=True)
     msg.deviceState = thermal_config.get_msg()
     msg.deviceState.deviceType = HARDWARE.get_device_type()
@@ -333,6 +338,7 @@ def hardware_thread(end_event, hw_queue) -> None:
     except queue.Empty:
       pass
 
+    timing.mark("system_stats")
     memory_usage = system_stats.memory_usage_percent() if system_stats is not None else 0.
     cpu_usage = system_stats.cpu_usage_percent() if system_stats is not None else []
 
@@ -352,8 +358,10 @@ def hardware_thread(end_event, hw_queue) -> None:
 
     msg.deviceState.modemTempC = last_hw_state.modem_temps
 
+    timing.mark("brightness")
     msg.deviceState.screenBrightnessPercent = HARDWARE.get_screen_brightness()
 
+    timing.mark("usb_chestnut")
     set_usb_state(msg.deviceState, last_hw_state.usb_state)
     chestnut.update(started_ts is None, last_hw_state.usb_state)
     chestnut_state = sm["chestnutState"]
@@ -374,6 +382,7 @@ def hardware_thread(end_event, hw_queue) -> None:
     all_comp_temp = all_temp_filter.update(max(temp_sources))
     msg.deviceState.maxTempC = all_comp_temp
 
+    timing.mark("fan_control")
     msg.deviceState.fanSpeedPercentDesired = fan_controller.update(all_comp_temp, onroad_conditions["ignition"])
 
     is_offroad_for_5_min = (started_ts is None) and ((not started_seen) or (off_ts is None) or (time.monotonic() - off_ts > 60 * 5))
@@ -391,6 +400,7 @@ def hardware_thread(end_event, hw_queue) -> None:
 
     # **** starting logic ****
 
+    timing.mark("startup_conditions")
     startup_conditions["up_to_date"] = params.get("Offroad_ConnectivityNeeded") is None or params.get_bool("DisableUpdates") or params.get_bool("SnoozeUpdate")
     startup_conditions["no_excessive_actuation"] = params.get("Offroad_ExcessiveActuation") is None
     startup_conditions["not_uninstalling"] = not params.get_bool("DoUninstall")
@@ -413,6 +423,7 @@ def hardware_thread(end_event, hw_queue) -> None:
     onroad_conditions["device_temp_good"] = thermal_status < ThermalStatus.critical
     extra_text = f"{offroad_comp_temp:.1f}C"
     show_alert = (not onroad_conditions["device_temp_good"] or not startup_conditions["device_temp_engageable"]) and onroad_conditions["ignition"]
+    timing.mark("alerts")
     set_offroad_alert_if_changed("Offroad_TemperatureTooHigh", show_alert, extra_text=extra_text)
     update_jetlink_alert()
 
@@ -426,6 +437,7 @@ def hardware_thread(end_event, hw_queue) -> None:
       startup_conditions["registered_device"] = params.get("DongleId") != UNREGISTERED_DONGLE_ID
 
     # Handle offroad/onroad transition
+    timing.mark("engagement")
     should_start = resolve_start(drive_mode, onroad_conditions, startup_conditions, already_started=started_ts is not None)
 
     if should_start != should_start_prev or (count == 0):
@@ -444,6 +456,7 @@ def hardware_thread(end_event, hw_queue) -> None:
       except Exception:
         pass
 
+    timing.mark("power_save")
     should_pwrsave = not (onroad_conditions["ignition"] or should_start) and msg.deviceState.screenBrightnessPercent < 1e-3
     if should_pwrsave != pwrsave or (count == 0):
       HARDWARE.set_power_save(should_pwrsave)
@@ -479,12 +492,15 @@ def hardware_thread(end_event, hw_queue) -> None:
 
     # Offroad power monitoring
     voltage = None if peripheralState.pandaType == log.PandaState.PandaType.unknown else peripheralState.voltage
+    timing.mark("power_read")
     power_monitor.calculate(voltage, onroad_conditions["ignition"])
     msg.deviceState.offroadPowerUsageUwh = power_monitor.get_power_used()
     msg.deviceState.carBatteryCapacityUwh = max(0, power_monitor.get_car_battery_capacity())
+    timing.mark("power_draw")
     current_power_draw = HARDWARE.get_current_power_draw()
     msg.deviceState.powerDrawW = current_power_draw
 
+    timing.mark("som_power")
     som_power_draw = HARDWARE.get_som_power_draw()
     msg.deviceState.somPowerDrawW = som_power_draw
 
@@ -494,6 +510,7 @@ def hardware_thread(end_event, hw_queue) -> None:
       cloudlog.warning(f"shutting device down, offroad since {shutdown_off_ts}")
       params.put_bool("DoShutdown", True, block=True)
 
+    timing.mark("publish")
     msg.deviceState.started = started_ts is not None
     msg.deviceState.startedMonoTime = int(1e9*(started_ts or 0))
 
@@ -524,6 +541,7 @@ def hardware_thread(end_event, hw_queue) -> None:
         except Exception:
           cloudlog.exception("failed to save offroad status")
 
+    timing.mark("bookkeeping")
     params.put_bool("NetworkMetered", msg.deviceState.networkMetered)
 
     now_ts = time.monotonic()

openpilot/system/hardware/tests/test_hardware_timing.py

@@ -0,0 +1,31 @@
+from unittest import TestCase, mock
+
+from openpilot.system.hardware import hardware_timing as timing
+
+
+class TestHardwareTiming(TestCase):
+  def test_slow_phase_preserves_original_wall_and_thread_cpu_clocks(self):
+    for elapsed_ns, reported in ((499_999_999, False), (500_000_000, True), (5_100_019_763, True)):
+      with self.subTest(elapsed_ns=elapsed_ns), \
+           mock.patch.object(timing.time, 'monotonic_ns', side_effect=[1_000_000_000, 1_000_000_000 + elapsed_ns]), \
+           mock.patch.object(timing.time, 'thread_time_ns', side_effect=[80_000_000, 90_000_000]), \
+           mock.patch.object(timing.cloudlog, 'event') as report:
+        watch = timing.HardwareLoopTiming()
+        watch.mark('thermal_read')
+        report.assert_not_called()
+        watch.mark('system_stats')
+        self.assertEqual(bool(report.call_count), reported)
+        if reported:
+          report.assert_called_once_with('hardwared.slow_phase', phase='thermal_read', elapsedMs=elapsed_ns / 1e6,
+            threadCpuMs=10., error=True)
+        self.assertEqual(watch.phase, 'system_stats')
+
+  def test_logging_failure_does_not_interrupt_hardware_work(self):
+    with mock.patch.object(timing.time, 'monotonic_ns', side_effect=[1, 5_100_000_001]), \
+         mock.patch.object(timing.time, 'thread_time_ns', side_effect=[1, 10_000_001]), \
+         mock.patch.object(timing.cloudlog, 'event', side_effect=OSError('logging unavailable')):
+      watch = timing.HardwareLoopTiming()
+      watch.mark('power_read')
+      watch.mark('publish')
+      self.assertEqual(watch.phase, 'publish')
+      self.assertEqual(watch.wall_ns, 5_100_000_001)