Lateral authority

opendbc_repo/opendbc/car/gm/carcontroller.py

@@ -568,7 +568,7 @@ class CarController(CarControllerBase):
       if self.CP.openpilotLongitudinalControl:
         stock_steer_ready = (stock_steer_ready and CS.pedal_sensor_healthy and
                             0 < CS.pedal_sensor_ts_nanos <= now_nanos <= CS.pedal_sensor_ts_nanos + PEDAL_SENSOR_TIMEOUT_NS)
-    lat_active = CC.latActive and stock_steer_ready
+    lat_active = CC.latActive and stock_steer_ready and not CS.steering_authority.latched
 
     # Steering (Active: 50Hz, inactive: 10Hz)
     steer_step = self.params.STEER_STEP if lat_active else self.params.INACTIVE_STEER_STEP
@@ -605,6 +605,7 @@ class CarController(CarControllerBase):
       self.apply_torque_last = apply_torque
       idx = self.lka_steering_cmd_counter % 4
       can_sends.append(gmcan.create_steering_control(self.packer_pt, CanBus.POWERTRAIN, apply_torque, idx, lat_active))
+      CS.steering_authority.command(now_nanos, lat_active)
 
     status_bolt = self.CP.carFingerprint in NO_ACC_BOLT_CAR and (
       is_bolt_present_no_acc_pedal_profile(self.CP) or is_bolt_pedal_removed_profile(self.CP))

opendbc_repo/opendbc/car/gm/carstate.py

@@ -1,3 +1,4 @@
+from opendbc.car.gm.steering_authority import SteeringAuthority
 from opendbc.car.gps import CarGpsTracker, get_car_gps_config
 from opendbc.car.gm.values import gm_control_word, is_volt_one_pedal, camera_acc_pedal_profile, BrakeSource, volt_cc_pedal_profile
 from opendbc.car.gm.values import is_volt_longitudinal, is_gm_auto_hold
@@ -35,6 +36,7 @@ BUTTONS_DICT = {CruiseButtons.RES_ACCEL: ButtonType.accelCruise, CruiseButtons.D
 class CarState(CarStateBase):
   def __init__(self, CP):
     super().__init__(CP)
+    self.steering_authority = SteeringAuthority(CP)
     self.car_gps_tracker = CarGpsTracker(CP)
     self.car_gps_supported = self.car_gps_tracker.config is not None
     self.hybrid_profile = malibu_hybrid_profile(CP)

opendbc_repo/opendbc/car/gm/interface.py

@@ -2,7 +2,7 @@
 from math import fabs, exp
 import numpy as np
 
-from opendbc.car import get_safety_config, structs
+from opendbc.car import Bus, get_safety_config, structs
 from opendbc.car.pedal import supported_pedal_detected
 from opendbc.car.gm.pedal_capability import pedal_candidate
 from opendbc.car.common.conversions import Conversions as CV
@@ -50,6 +50,7 @@ class CarInterface(CarInterfaceBase):
     return super().apply(c, now_nanos)
 
   def update(self, can_packets):
+    self.CS.steering_authority.observe(can_packets)
     if malibu_hybrid_profile(self.CP) is not None:
       self.CS.hybrid_buttons.observe_packets(can_packets)
     if (is_bolt_pedal_removed_profile(self.CP) or is_bolt_pedal_removed_profile(self.CP, stock_only=True)):
@@ -63,7 +64,9 @@ class CarInterface(CarInterfaceBase):
     if is_conventional_cc_pedal_profile(self.CP):
       self.CS.conventional_cancel_credit.observe(can_packets)
     if not is_bolt_cc_profile(self.CP):
-      return super().update(can_packets)
+      ret = super().update(can_packets)
+      self.CS.steering_authority.update(ret, self.can_parsers[Bus.pt]._last_update_nanos)
+      return ret
     adaptive = (self.CP.carFingerprint == CAR.CHEVROLET_BOLT_ACC_2022_2023_PEDAL and
                 self.CP.safetyConfigs[0].safetyParam == 0xC140)
     camera_source = getattr(self, "bolt_cc_camera_observed", (0, b""))

opendbc_repo/opendbc/car/gm/steering_authority.py

@@ -0,0 +1,137 @@
+from opendbc.car import structs
+from opendbc.car.gm.values import GMFlags, camera_acc_pedal_profile, is_volt_ascm_longitudinal
+
+# PSCMStatus is expected at 10 Hz. Three periods bound first acquisition and missing feedback.
+EPS_STATUS_TIMEOUT_NS = 300_000_000
+STEERING_COMMAND_TIMEOUT_NS = 100_000_000
+ButtonType = structs.CarState.ButtonEvent.Type
+
+
+def monitored_profile(CP: structs.CarParams) -> bool:
+  return (is_volt_ascm_longitudinal(CP) and camera_acc_pedal_profile(CP) is None and
+          not CP.flags & GMFlags.PEDAL_LONG)
+
+
+class SteeringAuthority:
+  def __init__(self, CP: structs.CarParams):
+    self.enabled = monitored_profile(CP)
+    self.min_speed = CP.minSteerSpeed
+    self.latched = False
+    self.command_active = False
+    self.seen_active = False
+    self.last_active_ns = 0
+    self.active_since_ns = 0
+    self.status_ns = 0
+    self.status = -1
+    self.bad_since_ns = 0
+    self.bad_samples = 0
+    self.last_sample_ns = 0
+    self.disable_ns = 0
+    self.neutral_ns = 0
+    self.main_off = False
+    self.source_ns: dict[int, int] = {}
+
+  def observe(self, can_packets):
+    if not self.enabled:
+      return
+    self.source_ns.clear()
+    malformed = any(bus == 0 and address == 0x184 and (stamp <= 0 or len(data) != 8)
+                    for stamp, packets in can_packets for address, data, bus in packets)
+    lengths = {0xC9: 8, 0xBE: 6, 0xBD: 7, 0x1E1: 7}
+    invalid = {address for stamp, packets in can_packets for address, data, bus in packets
+               if bus == 0 and address in lengths and (stamp <= 0 or len(data) != lengths[address])}
+    for address in invalid:
+      self.source_ns.pop(address, None)
+    for stamp, packets in can_packets:
+      for address, data, bus in packets:
+        if bus != 0 or stamp <= 0:
+          continue
+        if address in lengths and address not in invalid:
+          self.source_ns[address] = max(stamp, self.source_ns.get(address, 0))
+        if address == 0x184 and stamp > self.status_ns:
+          if self.active_since_ns and stamp - max(self.status_ns, self.active_since_ns) > EPS_STATUS_TIMEOUT_NS:
+            self.latched = True
+          self.status_ns = stamp
+          self.status = -1 if malformed else (data[0] >> 3) & 7
+
+  def command(self, now_ns: int, active: bool):
+    if not self.enabled:
+      return
+    self.command_active = active
+    if active:
+      self.last_active_ns = now_ns
+      if not self.active_since_ns:
+        self.active_since_ns = now_ns
+    elif self.disable_ns and now_ns > self.disable_ns:
+      self.neutral_ns = now_ns
+
+  def update(self, CS: structs.CarState, now_ns: int):
+    if not self.enabled:
+      return
+    healthy = CS.canValid and not CS.canTimeout
+
+    def fresh(address):
+      stamp = self.source_ns.get(address, 0)
+      return 0 < stamp <= now_ns and now_ns - stamp <= STEERING_COMMAND_TIMEOUT_NS
+
+    main_fresh = fresh(0xC9)
+    buttons_fresh = fresh(0x1E1)
+    pedal_fresh = (CS.brakePressed and (main_fresh or fresh(0xBE)) or
+                   CS.regenBraking and fresh(0xBD))
+    cancel = buttons_fresh and any(b.type == ButtonType.cancel and b.pressed for b in CS.buttonEvents)
+    if self.latched and healthy:
+      if pedal_fresh or cancel or main_fresh and not CS.cruiseState.available:
+        if not self.disable_ns:
+          self.disable_ns = now_ns
+        self.main_off |= main_fresh and not CS.cruiseState.available
+      physical_enable = buttons_fresh and self.source_ns[0x1E1] > self.neutral_ns and CS.buttonEnable
+      main_enable = self.main_off and main_fresh and self.source_ns[0xC9] > self.neutral_ns and CS.cruiseState.available
+      if (self.neutral_ns > self.disable_ns > 0 and (physical_enable or main_enable) and
+          CS.cruiseState.available and not CS.brakePressed and not CS.regenBraking):
+        self.latched = False
+        self.disable_ns = self.neutral_ns = 0
+        self.main_off = False
+        self.last_active_ns = self.active_since_ns = 0
+        self.seen_active = False
+        self.bad_since_ns = self.bad_samples = 0
+
+    deliberate_pause = (pedal_fresh or cancel or main_fresh and not CS.cruiseState.available or
+                        CS.standstill or CS.vEgo < self.min_speed or CS.steeringPressed or
+                        self.status in (2, 3) and 0 < self.status_ns <= now_ns and
+                        now_ns - self.status_ns <= EPS_STATUS_TIMEOUT_NS)
+    if healthy and deliberate_pause and not self.latched:
+      self.seen_active = False
+      self.command_active = False
+      self.last_active_ns = self.active_since_ns = 0
+      self.bad_since_ns = self.bad_samples = 0
+      self.last_sample_ns = self.status_ns
+    # A missing EPS feed must retain a fault even after host health gates emit neutral.
+    if (self.active_since_ns and not self.latched and
+        now_ns - max(self.status_ns, self.active_since_ns) > EPS_STATUS_TIMEOUT_NS):
+      self.latched = True
+
+    recent_command = 0 < self.last_active_ns <= now_ns and now_ns - self.last_active_ns <= STEERING_COMMAND_TIMEOUT_NS
+    eligible = ((healthy or self.status == -1) and self.command_active and recent_command and CS.cruiseState.available and
+                not CS.brakePressed and not CS.regenBraking and not cancel and
+                not CS.standstill and CS.vEgo >= self.min_speed and not CS.steeringPressed)
+    fresh_status = 0 < self.status_ns <= now_ns and now_ns - self.status_ns <= EPS_STATUS_TIMEOUT_NS
+    if eligible and fresh_status and self.status_ns > self.active_since_ns and self.status_ns != self.last_sample_ns:
+      self.last_sample_ns = self.status_ns
+      if self.status == 1:
+        self.seen_active = True
+        self.bad_since_ns = self.bad_samples = 0
+      elif self.status not in (2, 3):
+        if not self.bad_since_ns:
+          self.bad_since_ns = self.status_ns
+        self.bad_samples += 1
+        if self.seen_active:
+          self.latched = True
+      else:
+        self.bad_since_ns = self.bad_samples = 0
+    if (eligible and self.bad_samples >= 2 and self.bad_since_ns and
+        now_ns - self.active_since_ns >= EPS_STATUS_TIMEOUT_NS):
+      self.latched = True
+    if not recent_command or CS.steeringPressed or CS.standstill or CS.vEgo < self.min_speed:
+      self.bad_since_ns = self.bad_samples = 0
+    if self.latched:
+      CS.steerFaultTemporary = True

opendbc_repo/opendbc/car/gm/tests/test_ascm_intercept.py

@@ -1,9 +1,11 @@
 import unittest
 
 from opendbc.can import CANPacker
-from opendbc.car import Bus
+from opendbc.car import Bus, structs
 from opendbc.car import gen_empty_fingerprint
 from opendbc.car.gm.carstate import CarState
+from opendbc.car.gm import gmcan
+from opendbc.car.gm.values import CruiseButtons
 from opendbc.car.gm.interface import CarInterface
 from opendbc.car.gm.fingerprints import FINGERPRINTS, FW_VERSIONS
 from opendbc.car.gm.values import ASCM_INTERCEPT_CAR, CAR, DBC, GMSafetyFlags, CarControllerParams
@@ -22,6 +24,183 @@ def params(car, *, sascm=False, accelerator=True, radar=False, alpha=False, rele
 
 
 class TestAscmIntercept(unittest.TestCase):
+  def authority_frame(self, ci, packer, tick, *, status=1, active=True, main=True,
+                      regen=False, brake=False, button=CruiseButtons.UNPRESS, driver=0., speed=8., torque=0.02,
+                      malformed=False, physical_delay_ns=0, eps_missing=False):
+    now = 1_000_000_000 + tick * 10_000_000
+    values = {
+      'ECMCruiseControl': {'CruiseActive': 0},
+      'ECMEngineStatus': {'CruiseMainOn': int(main), 'BrakePressed': int(brake)},
+      'AcceleratorPedal2': {'CruiseState': 0},
+      'ECMPRDNL2': {'PRNDL2': 4},
+      'EBCMWheelSpdRear': {'RLWheelSpd': speed * 3.6, 'RRWheelSpd': speed * 3.6, 'RLWheelDir': 1, 'RRWheelDir': 1},
+      'EBCMWheelSpdFront': {'FLWheelSpd': speed * 3.6, 'FRWheelSpd': speed * 3.6},
+      'ESPStatus': {'TractionControlOn': 1},
+      'BCMDoorBeltStatus': {'LeftSeatBelt': 1},
+      'BCMGeneralPlatformStatus': {}, 'BCMTurnSignals': {}, 'PSCMSteeringAngle': {},
+      'ECMAcceleratorPos': {'BrakePedalPos': 30 if brake else 0},
+      'EBCMFrictionBrakeStatus': {}, 'EBCMRegenPaddle': {'RegenPaddle': int(regen)},
+    }
+    packets = [packer.make_can_msg(name, 0, signals) for name, signals in values.items()]
+    packets += [gmcan.create_buttons(packer, 0, tick % 4, button),
+                packer.make_can_msg('ASCMLKASteeringCmd', 2, {}),
+                packer.make_can_msg('ASCMActiveCruiseControlStatus', 2, {}),
+                packer.make_can_msg('AEBCmd', 2, {})]
+    if tick % 10 == 0 and not eps_missing:
+      pscm = packer.make_can_msg('PSCMStatus', 0, {'LKATorqueDeliveredStatus': status,
+                                                'LKADriverAppldTrq': driver, 'LKATorqueDelivered': 0})
+      if malformed:
+        packets.append((0x184, b'\x00', 0))
+      packets.append(pscm)
+    physical = [packet for packet in packets if packet[0] in (0xC9, 0xBE, 0xBD, 0x1E1)]
+    other = [packet for packet in packets if packet[0] not in (0xC9, 0xBE, 0xBD, 0x1E1)]
+    cs = ci.update([(now - physical_delay_ns, physical), (now, other)])
+    if tick >= 10 and not eps_missing:
+      self.assertTrue(cs.canValid, (tick, status))
+      self.assertFalse(cs.canTimeout, tick)
+    cc = structs.CarControl()
+    cc.enabled = active
+    cc.latActive = active
+    cc.actuators.torque = torque
+    cc.hudControl.setSpeed = 30
+    _, sends = ci.apply(cc.as_reader(), now)
+    steering = [data for address, data, bus in sends if address == 0x180 and bus == 0]
+    if steering:
+      self.assertEqual(bool((steering[-1][0] >> 3) & 1), active and not ci.CS.steering_authority.latched, tick)
+    return cs
+
+  def test_volt_steering_loss_requires_driver_rearm(self):
+    for reset in ('regen', 'brake', 'cancel', 'set', 'main'):
+      with self.subTest(reset=reset):
+        cp = params(CAR.CHEVROLET_VOLT_ASCM, sascm=True, alpha=True, radar=True)
+        ci, packer = CarInterface(cp), CANPacker(DBC[cp.carFingerprint][Bus.pt])
+        for tick in range(30):
+          self.authority_frame(ci, packer, tick)
+        self.assertTrue(ci.CS.steering_authority.seen_active)
+        # One emitted neutral frame must not erase the confirmed EPS episode.
+        for tick in range(30, 34):
+          self.authority_frame(ci, packer, tick, active=False)
+        for tick in range(34, 41):
+          cs = self.authority_frame(ci, packer, tick, status=0)
+        self.assertTrue(cs.steerFaultTemporary)
+        self.assertTrue(ci.CS.steering_authority.latched)
+        for tick in range(41, 60):
+          cs = self.authority_frame(ci, packer, tick, active=False)
+        self.assertTrue(cs.steerFaultTemporary)  # EPS recovery/automatic CC off cannot rearm.
+        for tick in range(60, 70):
+          self.authority_frame(ci, packer, tick, active=False, main=reset != 'main',
+                               regen=reset == 'regen', brake=reset == 'brake',
+                               button=CruiseButtons.CANCEL if reset == 'cancel' else CruiseButtons.UNPRESS)
+        cs = self.authority_frame(ci, packer, 70, active=False, physical_delay_ns=200_000_000 if reset == 'main' else 1_000_000,
+                                  button=CruiseButtons.DECEL_SET if reset == 'set' else
+                                  CruiseButtons.RES_ACCEL if reset != 'main' else CruiseButtons.UNPRESS)
+        if reset in ('set', 'main'):
+          self.assertTrue(cs.steerFaultTemporary, reset)
+          cs = self.authority_frame(ci, packer, 71, active=False)
+        resume_tick = 72
+        if reset == 'set':
+          # SET alone is not the physical disable required to reset a loss latch.
+          self.assertTrue(cs.steerFaultTemporary, reset)
+          self.assertTrue(ci.CS.steering_authority.latched, reset)
+          self.assertEqual(ci.CS.steering_authority.disable_ns, 0, reset)
+          for tick in range(72, 82):
+            self.authority_frame(ci, packer, tick, active=False, regen=True)
+          self.assertGreater(ci.CS.steering_authority.neutral_ns, ci.CS.steering_authority.disable_ns, reset)
+          cs = self.authority_frame(ci, packer, 82, active=False, button=CruiseButtons.DECEL_SET)
+          self.assertTrue(cs.steerFaultTemporary, reset)
+          cs = self.authority_frame(ci, packer, 83, active=False)
+          resume_tick = 84
+        context = (reset, cs.buttonEnable, cs.brakePressed, cs.regenBraking,
+                   ci.CS.steering_authority.disable_ns, ci.CS.steering_authority.neutral_ns)
+        self.assertFalse(cs.steerFaultTemporary, context)
+        self.assertFalse(ci.CS.steering_authority.latched, context)
+        for tick in range(resume_tick, resume_tick + 20):
+          self.authority_frame(ci, packer, tick)
+        self.assertTrue(ci.CS.steering_authority.seen_active, context)
+
+  def test_volt_steering_acquisition_and_invalid_status(self):
+    for status, malformed in ((0, False), (4, False), (1, True)):
+      with self.subTest(status=status, malformed=malformed):
+        cp = params(CAR.CHEVROLET_VOLT_ASCM, sascm=True, alpha=True, radar=True)
+        ci, packer = CarInterface(cp), CANPacker(DBC[cp.carFingerprint][Bus.pt])
+        for tick in range(30):
+          cs = self.authority_frame(ci, packer, tick, status=status, malformed=malformed)
+          self.assertFalse(cs.steerFaultTemporary, tick)
+        for tick in range(30, 41):
+          cs = self.authority_frame(ci, packer, tick, status=status, malformed=malformed)
+        self.assertTrue(cs.steerFaultTemporary)
+        self.assertTrue(ci.CS.steering_authority.latched)
+
+    cp = params(CAR.CHEVROLET_VOLT_ASCM, sascm=True, alpha=True, radar=True)
+    ci, packer = CarInterface(cp), CANPacker(DBC[cp.carFingerprint][Bus.pt])
+    for tick in range(30):
+      self.authority_frame(ci, packer, tick)
+    for tick in range(30, 70):
+      cs = self.authority_frame(ci, packer, tick, active=tick < 40, eps_missing=True)
+    self.assertTrue(cs.steerFaultTemporary)
+    for tick in range(70, 90):
+      cs = self.authority_frame(ci, packer, tick, active=False)
+    self.assertTrue(cs.steerFaultTemporary)  # Fresh EPS does not erase a missing-feed latch.
+
+    # A total receive gap cannot conceal a stale EPS episode on the first fresh return.
+    cp = params(CAR.CHEVROLET_VOLT_ASCM, sascm=True, alpha=True, radar=True)
+    ci, packer = CarInterface(cp), CANPacker(DBC[cp.carFingerprint][Bus.pt])
+    for tick in range(30):
+      self.authority_frame(ci, packer, tick)
+    cs = self.authority_frame(ci, packer, 70)
+    self.assertTrue(cs.steerFaultTemporary)
+    self.assertTrue(ci.CS.steering_authority.latched)
+
+  def test_volt_steering_healthy_zero_and_existing_faults(self):
+    for status, active, speed, driver in ((1, True, 8., 0.), (0, False, 8., 0.),
+                                         (0, True, 0., 0.), (0, True, 8., 3.),
+                                         (2, True, 8., 0.), (3, True, 8., 0.)):
+      with self.subTest(status=status, active=active, speed=speed, driver=driver):
+        cp = params(CAR.CHEVROLET_VOLT_ASCM, sascm=True, alpha=True, radar=True)
+        ci, packer = CarInterface(cp), CANPacker(DBC[cp.carFingerprint][Bus.pt])
+        for tick in range(60):
+          cs = self.authority_frame(ci, packer, tick, status=status, active=active, speed=speed, driver=driver,
+                                    torque=0. if status == 1 else 0.02)
+        self.assertFalse(ci.CS.steering_authority.latched)
+        self.assertEqual(cs.steerFaultTemporary, status == 2)
+        self.assertEqual(cs.steerFaultPermanent, status == 3)
+    # A genuine driver pause starts a new acquisition window, unlike an unexplained brief CC drop.
+    for pause in ('regen', 'brake', 'cancel', 'main', 'override', 'standstill'):
+      with self.subTest(pause=pause):
+        cp = params(CAR.CHEVROLET_VOLT_ASCM, sascm=True, alpha=True, radar=True)
+        ci, packer = CarInterface(cp), CANPacker(DBC[cp.carFingerprint][Bus.pt])
+        for tick in range(30):
+          self.authority_frame(ci, packer, tick)
+        for tick in range(30, 50):
+          cs = self.authority_frame(ci, packer, tick, status=0, active=False,
+                                    regen=pause == 'regen', brake=pause == 'brake', main=pause != 'main',
+                                    button=CruiseButtons.CANCEL if pause == 'cancel' else CruiseButtons.UNPRESS,
+                                    driver=3. if pause == 'override' else 0., speed=0. if pause == 'standstill' else 8.)
+          self.assertFalse(cs.steerFaultTemporary)
+        for tick in range(50, 70):
+          cs = self.authority_frame(ci, packer, tick, status=0 if tick < 65 else 1)
+          self.assertFalse(cs.steerFaultTemporary, tick)
+        for tick in range(70, 80):
+          cs = self.authority_frame(ci, packer, tick)
+          self.assertFalse(cs.steerFaultTemporary, tick)
+        self.assertTrue(ci.CS.steering_authority.seen_active)
+    # Ordinary temporary EPS faults retain their normal recovery/acquisition behavior.
+    cp = params(CAR.CHEVROLET_VOLT_ASCM, sascm=True, alpha=True, radar=True)
+    ci, packer = CarInterface(cp), CANPacker(DBC[cp.carFingerprint][Bus.pt])
+    for tick in range(30):
+      self.authority_frame(ci, packer, tick)
+    for tick in range(30, 50):
+      cs = self.authority_frame(ci, packer, tick, status=2, active=False)
+      self.assertTrue(cs.steerFaultTemporary)
+      self.assertFalse(ci.CS.steering_authority.latched)
+    for tick in range(50, 80):
+      cs = self.authority_frame(ci, packer, tick, status=0 if tick < 65 else 1)
+      self.assertFalse(cs.steerFaultTemporary, tick)
+    self.assertTrue(ci.CS.steering_authority.seen_active)
+    for cp in (params(CAR.CHEVROLET_VOLT_ASCM), params(CAR.CHEVROLET_VOLT),
+               params(CAR.CHEVROLET_BOLT_EUV)):
+      self.assertFalse(CarState(cp).steering_authority.enabled)
+
   def test_eight_manual_ids_have_stock_acc_default(self):
     self.assertEqual(len(ASCM_INTERCEPT_CAR), 8)
     for car in ASCM_INTERCEPT_CAR:

openpilot/cereal/custom.capnp

@@ -127,6 +127,8 @@ struct StarPilotCarState @0xf35cc4560bbf6ec2 {
   decelHardCruise @29 :Bool;  # current/releasing decel cruise button came from GM hard-press signal
   pulseAndGlide @30 :Bool;  # developer-only wheel-button pulse-and-glide mode is enabled
   gps @31 :Gps;
+  lateralAuthorityUnavailable @32 :Bool;
+  sourceCarStateMonoTime @33 :UInt64;
 
   struct Gps {
     sourceMonoTime @0 :UInt64;  # CAN observation converted to CLOCK_MONOTONIC

openpilot/selfdrive/car/card.py

@@ -440,7 +440,6 @@ class Car:
 
     # Update carState from CAN
     CS = self.CI.update(can_list)
-    self.car_gps_publisher.update(self.CI.CS, self.pm)
     self.observe_ioniq6_long_authority(can_list, CS)
     media_owner = getattr(self, 'ioniq6_media', None)
     media_observation = media_owner.update(can_list) if media_owner is not None else None
@@ -775,6 +774,13 @@ class Car:
         publish_stock_status(self.pm, self.CP, internal, session=self.slc_producer_session,
                              sequence=self.slc_cruise_event_id, car_state_stamp=int(cs_send.logMonoTime),
                              valid=bool(cs_send.valid and not CS.canTimeout))
+    companion = None
+    steering_authority = getattr(self.CI.CS, 'steering_authority', None)
+    if self.CP.brand == 'gm' and steering_authority is not None and steering_authority.enabled:
+      companion = messaging.new_message('starpilotCarState', valid=bool(cs_send.valid), logMonoTime=int(cs_send.logMonoTime))
+      companion.starpilotCarState.lateralAuthorityUnavailable = bool(steering_authority.latched)
+      companion.starpilotCarState.sourceCarStateMonoTime = int(cs_send.logMonoTime)
+    self.car_gps_publisher.update(self.CI.CS, self.pm, companion)
     self.pm.send('carState', cs_send)
 
     wheel = getattr(self, 'wheel_publisher', None)
@@ -1010,13 +1016,16 @@ class Car:
                     for ps, cfg in zip(pandas, self.CP.safetyConfigs, strict=True)))
 
   def publish_sendcan(self, frames, valid=True):
+    steering_authority = getattr(getattr(getattr(self, 'CI', None), 'CS', None), 'steering_authority', None)
+    monitored_steering = (getattr(getattr(self, 'CP', None), 'brand', None) == 'gm' and
+                          steering_authority is not None and steering_authority.enabled)
     packet = can_list_to_can_capnp(frames, msgtype='sendcan', valid=valid)
     if getattr(self, 'volt_cc_selected', False):
       # CAN producer/parser and pandad send age use BOOTTIME; Python controls/device envelopes use MONOTONIC.
       builder = messaging.log_from_bytes(packet).as_builder()
       builder.logMonoTime = int(getattr(self, 'volt_cc_now_boot_ns', 0))
       packet = builder.to_bytes()
-    elif self.volt_startup_keepalive():
+    elif self.volt_startup_keepalive() or monitored_steering:
       builder = messaging.log_from_bytes(packet).as_builder()
       builder.logMonoTime = time.clock_gettime_ns(time.CLOCK_BOOTTIME)
       packet = builder.to_bytes()
@@ -1195,10 +1204,13 @@ class Car:
         if self.ioniq6_long_lost:
           self.publish_sendcan([], valid=False)
           return
+      steering_authority = getattr(self.CI.CS, 'steering_authority', None)
+      monitored_steering = self.CP.brand == 'gm' and steering_authority is not None and steering_authority.enabled
       # send car controls over can
       now_nanos = (self.volt_cc_now_boot_ns if getattr(self, 'volt_cc_selected', False) else
                    self.can_log_mono_time if REPLAY else
-                   time.clock_gettime_ns(time.CLOCK_BOOTTIME) if self.volt_startup_keepalive() else int(time.monotonic() * 1e9))
+                   time.clock_gettime_ns(time.CLOCK_BOOTTIME) if self.volt_startup_keepalive() or monitored_steering else
+                   int(time.monotonic() * 1e9))
       self.last_actuators_output, can_sends = self.CI.apply(CC, now_nanos)
       self.publish_sendcan(can_sends, valid=CS.canValid)
 

openpilot/selfdrive/selfdrived/selfdrived.py

@@ -29,6 +29,7 @@ from openpilot.selfdrive.selfdrived.helpers import ExcessiveActuationCheck
 from openpilot.system.manager.process_health import driving_process_failures
 from openpilot.selfdrive.selfdrived.state import StateMachine
 from opendbc.car.gm.aol import native_bootstrap_supported
+from opendbc.car.gm.steering_authority import monitored_profile
 from opendbc.car.gm.values import GMFlags, is_volt_ascm_longitudinal
 from openpilot.starpilot.aol.intent import read_settings
 from openpilot.starpilot.aol.runtime import (INTENT_MAX_AGE_NS, AxisDecision, current_intent, current_native, decide_axes,
@@ -95,6 +96,9 @@ class SelfdriveD:
     self.aol_sequence = 0
     self.aol_axis_decision = AxisDecision()
     self.aol_dm_lateral_inhibit = False
+    self.aol_authority_lost = False
+    self.steering_authority_unavailable = False
+    self.steering_authority_pair = None
     self.force_stop_hold_alert = HoldAlertState()
     self.aol_car_state_log_ns = 0
     self.aol_last_intent = None
@@ -262,10 +266,44 @@ class SelfdriveD:
       self.big_model_chestnut = False
     return big_failed
 
+  def _update_steering_authority(self, CS):
+    if not monitored_profile(self.CP):
+      self.steering_authority_unavailable = False
+      return
+    now_ns = self.aol_car_state_log_ns if REPLAY and self.aol_car_state_log_ns else time.monotonic_ns()
+    service = 'starpilotCarState'
+    source_ns = self.aol_car_state_log_ns
+    source_current = bool(getattr(self, 'conditional_car_state_valid', False) and CS.canValid and not CS.canTimeout and
+                          0 < source_ns <= now_ns and now_ns - source_ns <= INTENT_MAX_AGE_NS)
+    envelope_current = bool(self.sm.seen.get(service, False) and self.sm.valid.get(service, False) and
+                            self.sm.alive.get(service, False))
+    pair = None
+    if source_current and envelope_current:
+      message_ns = int(self.sm.logMonoTime[service])
+      companion = self.sm[service]
+      if (int(companion.sourceCarStateMonoTime) == message_ns and 0 < message_ns <= now_ns and
+          now_ns - message_ns <= INTENT_MAX_AGE_NS):
+        if message_ns == source_ns:
+          pair = (source_ns, bool(companion.lateralAuthorityUnavailable))
+          self.steering_authority_pair = pair
+        elif message_ns > source_ns:
+          previous = getattr(self, 'steering_authority_pair', None)
+          if previous is not None and previous[0] == source_ns:
+            pair = previous
+    previous_axes = self.aol_axis_decision
+    expected_active = self.active or previous_axes.lateral_active or previous_axes.longitudinal_active
+    if pair is not None:
+      self.steering_authority_unavailable = pair[1]
+    elif expected_active:
+      self.steering_authority_unavailable = True
+    if getattr(self, 'steering_authority_unavailable', False):
+      self.events.add(EventName.controlsMismatch)
+
   def update_events(self, CS):
     """Compute onroadEvents from carState"""
 
     self.events.clear()
+    self._update_steering_authority(CS)
     self.tesla_stock_alerts = []
     self.nostalgia_paddle_cancel = False
     action_owner = getattr(self, 'controller_action_owner', None)
@@ -842,21 +880,37 @@ class SelfdriveD:
     self.conditional_status = result.status
     self.conditional_result = result
 
+  def _decide_aol_axes(self, CS, intent, native):
+    return decide_axes(
+      standard_lateral=self.active, standard_longitudinal=self.enabled and self.CP.openpilotLongitudinalControl,
+      intent=intent, native=native, car_state=CS, initialized=self.initialized,
+      model_ready=bool(self.sm.all_checks(['modelV2', 'extrinsicsCalibration']) and
+                       self.sm['extrinsicsCalibration'].calStatus == log.ExtrinsicsCalibration.Status.calibrated),
+      no_entry=aol_no_entry(self.events.names, CS, paddle_only_cancel=self.nostalgia_paddle_cancel,
+                           allow_lateral_cancel=bool(intent is not None and intent.allowedLatch and intent.optionalSetRelease
+                                                     and ev6_aol_qualified(self.CP, marked_only=True)),
+                           cruise_main_required=getattr(self, 'aol_cruise_main_required', True),
+                           allow_below_engage_speed=bool(is_volt_ascm_longitudinal(self.CP) and
+                                                       not self.CP.flags & GMFlags.PEDAL_LONG)),
+      immediate_disable=self.events.contains(ET.IMMEDIATE_DISABLE),
+      dm_lockout=bool(not self.sm.all_checks(['driverMonitoringState']) or
+                      self.sm['driverMonitoringState'].lockout or self.sm['driverMonitoringState'].alwaysOnLockout or
+                      self.sm['driverMonitoringState'].alertLevel == AlertLevel.three),
+      pause_brake_mps=self.aol_settings.pause_brake_mps if self.aol_settings is not None else 0.0,
+      lateral_inhibit=self.aol_dm_lateral_inhibit)
+
   def step(self):
     CS = self.data_sample()
     self.update_events(CS)
     native = None
-    lost_active_aol = False
+    lost_active_aol = bool(self.aol_replay and getattr(self, 'steering_authority_unavailable', False) and
+                           (self.aol_axis_decision.lateral_active or self.aol_axis_decision.longitudinal_active))
     if self.aol_replay or getattr(self, 'ordinary_axis_ack_required', False):
       now_ns = self.aol_car_state_log_ns if REPLAY and self.aol_car_state_log_ns else time.monotonic_ns()
       native = current_native(self.sm, self.CP, now_ns=now_ns, axis_session_id=self.aol_session_id)
       if native is None and self.initialized:
         lost_active_aol = self.aol_axis_decision.lateral_active or self.aol_axis_decision.longitudinal_active
         self.events.add(EventName.controlsMismatch)
-    if not self.CP.passive and self.initialized:
-      self.enabled, self.active = self.state_machine.update(self.events)
-    if lost_active_aol and ET.IMMEDIATE_DISABLE not in self.state_machine.current_alert_types:
-      self.state_machine.current_alert_types.append(ET.IMMEDIATE_DISABLE)
     if self.aol_replay:
       intent = current_intent(self.sm, car_state_ns=self.aol_car_state_log_ns, now_ns=now_ns,
                               previous=getattr(self, 'aol_last_intent', None))
@@ -865,23 +919,32 @@ class SelfdriveD:
         self.aol_dm_lateral_inhibit = True
       elif intent is not None and not intent.allowedLatch:
         self.aol_dm_lateral_inhibit = False
-      self.aol_axis_decision = decide_axes(
-        standard_lateral=self.active, standard_longitudinal=self.enabled and self.CP.openpilotLongitudinalControl,
-        intent=intent, native=native, car_state=CS, initialized=self.initialized,
-        model_ready=bool(self.sm.all_checks(['modelV2', 'extrinsicsCalibration']) and
-                         self.sm['extrinsicsCalibration'].calStatus == log.ExtrinsicsCalibration.Status.calibrated),
-        no_entry=aol_no_entry(self.events.names, CS, paddle_only_cancel=self.nostalgia_paddle_cancel,
-                             allow_lateral_cancel=bool(intent is not None and intent.allowedLatch and intent.optionalSetRelease
-                                                       and ev6_aol_qualified(self.CP, marked_only=True)),
-                             cruise_main_required=getattr(self, 'aol_cruise_main_required', True),
-                             allow_below_engage_speed=bool(is_volt_ascm_longitudinal(self.CP) and
-                                                         not self.CP.flags & GMFlags.PEDAL_LONG)),
-        immediate_disable=self.events.contains(ET.IMMEDIATE_DISABLE),
-        dm_lockout=bool(not self.sm.all_checks(['driverMonitoringState']) or
-                        self.sm['driverMonitoringState'].lockout or self.sm['driverMonitoringState'].alwaysOnLockout or
-                        self.sm['driverMonitoringState'].alertLevel == AlertLevel.three),
-        pause_brake_mps=self.aol_settings.pause_brake_mps if self.aol_settings is not None else 0.0,
-        lateral_inhibit=self.aol_dm_lateral_inhibit)
+      previous = self.aol_axis_decision
+      requested = self._decide_aol_axes(CS, intent, native)
+      source_current = bool(intent is not None and getattr(self, 'conditional_car_state_valid', False) and
+                            CS.canValid and not CS.canTimeout)
+      driver_override = bool(CS.brakePressed or CS.regenBraking or CS.gasPressed or
+                             self.events.contains(ET.USER_DISABLE))
+      lost_companion = bool((previous.lateral_active or previous.longitudinal_active) and
+                            not source_current and not self.events.contains(ET.USER_DISABLE))
+      lost_permission = bool(native is not None and not driver_override and (
+        previous.lateral_active and requested.desired_lateral and native.requestedLateral and
+        not native.lateralAllowed and not CS.steeringPressed and not self.events.contains(ET.OVERRIDE_LATERAL) or
+        previous.longitudinal_active and requested.desired_longitudinal and native.requestedLongitudinal and
+        not native.longitudinalAllowed and not self.events.contains(ET.OVERRIDE_LONGITUDINAL)))
+      if self.initialized and (lost_active_aol or lost_companion or lost_permission):
+        lost_active_aol = True
+        self.aol_authority_lost = True
+      elif source_current and not intent.allowedLatch:
+        self.aol_authority_lost = False
+      if getattr(self, 'aol_authority_lost', False):
+        self.events.add(EventName.controlsMismatch)
+    if not self.CP.passive and self.initialized:
+      self.enabled, self.active = self.state_machine.update(self.events)
+    if lost_active_aol and ET.IMMEDIATE_DISABLE not in self.state_machine.current_alert_types:
+      self.state_machine.current_alert_types.append(ET.IMMEDIATE_DISABLE)
+    if self.aol_replay:
+      self.aol_axis_decision = self._decide_aol_axes(CS, intent, native)
     elif getattr(self, 'ordinary_axis_ack_required', False):
       self.aol_axis_decision = decide_ordinary_axis(
         requested=bool(self.initialized and self.conditional_car_state_valid and

openpilot/starpilot/aol/tests/test_gm.py

@@ -1162,6 +1162,7 @@ class TestGmAol(unittest.TestCase):
       self.assertEqual(controls.CP.to_dict(), cp.to_dict(), msg=ctx)
       sd = SelfdriveD.__new__(SelfdriveD)
       sd.CP, sd.initialized, sd.aol_replay = cp, True, True
+      sd.enabled = sd.active = False
       sd.aol_session_id, sd.aol_axis_decision = 'bolt-cancel', AxisDecision()
       sd.aol_dm_lateral_inhibit, sd.aol_settings = False, selected.aol_settings
       sd.nostalgia_paddle_cancel = False
@@ -1322,6 +1323,7 @@ class TestGmAol(unittest.TestCase):
           sd.sm.update_msgs(now / 1e9, [intent.as_reader(), receipt.as_reader(), model.as_reader(),
                                        calibration.as_reader(), monitoring.as_reader()])
           sd.aol_car_state_log_ns = now
+          sd.conditional_car_state_valid = cs.canValid and not cs.canTimeout
           sd.events = events
           with patch('openpilot.selfdrive.selfdrived.selfdrived.REPLAY', True), \
                patch.object(sd, 'data_sample', return_value=cs), patch.object(sd, 'update_events'), \
@@ -1471,6 +1473,7 @@ class TestGmAol(unittest.TestCase):
       self.assertEqual(controls.CP.to_dict(), cp.to_dict())
       sd = SelfdriveD.__new__(SelfdriveD)
       sd.CP, sd.initialized, sd.aol_replay = cp, True, True
+      sd.enabled = sd.active = False
       sd.aol_session_id, sd.aol_axis_decision = 'volt-brake', AxisDecision()
       sd.aol_dm_lateral_inhibit, sd.aol_settings = False, selected.aol_settings
       sd.nostalgia_paddle_cancel = False
@@ -1484,6 +1487,7 @@ class TestGmAol(unittest.TestCase):
       self.assertEqual(safety.set_safety_hooks(structs.CarParams.SafetyModel.gm, 0xD114), 0)
       safety.init_tests()
       resumed = paused = 0
+      previous_sd_onroad = None
       ctx = {'identity': str(cp.carFingerprint), 'word': '0xd114', 'phase': 'prime'}
       try:
         # Warm all selected parser/native sources, with stock cruise inactive.
@@ -1491,6 +1495,8 @@ class TestGmAol(unittest.TestCase):
           stamp = 800_000_000 + warmup * 10_000_000
           _, frames = feed_car(SimpleNamespace(update=lambda _: None), packer, stamp,
                                counter=warmup % 4, active=False, speed=8., camera=True)
+          frames = [frame for frame in frames if frame[0] != 0x184]
+          frames.append(packer.make_can_msg('PSCMStatus', 0, {'LKATorqueDeliveredStatus': 1}))
           frames.append(packer.make_can_msg('ASCMActiveCruiseControlStatus', 2, {'ACCCruiseState': 0}))
           previous = ci.update([(stamp, frames)])
           for frame in frames:
@@ -1499,23 +1505,25 @@ class TestGmAol(unittest.TestCase):
         self.assertTrue(previous.canValid, msg=ctx)
         self.assertTrue(safety.safety_config_valid(), msg=ctx)
         self.assertFalse(safety.get_controls_allowed(), msg=ctx)
-        for tick in range(140):
+        for tick in range(146):
           now = 1_000_000_000 + tick * 10_000_000
           brake = any(start <= tick < start + 12 for start in (20, 60, 100))
           fault = tick == 138
-          ctx.update(phase='scenario', tick=tick, brake=brake, fault=fault)
+          main = tick not in (140, 141)
+          ctx.update(phase='scenario', tick=tick, brake=brake, fault=fault, main=main)
           _, packets = feed_car(SimpleNamespace(update=lambda _: None), packer, now,
                                 counter=tick % 4, active=False, speed=8., camera=True)
-          packets = [frame for frame in packets if frame[0] not in (0xC9, 0x1C4, 0xBE)]
-          packets += [packer.make_can_msg('ASCMActiveCruiseControlStatus', 2, {'ACCCruiseState': 0}),
-                      packer.make_can_msg('ECMEngineStatus', 0, {'CruiseMainOn': 1}),
+          packets = [frame for frame in packets if frame[0] not in (0xC9, 0x1C4, 0xBE, 0x184)]
+          packets += [packer.make_can_msg('PSCMStatus', 0, {'LKATorqueDeliveredStatus': 1}),
+                      packer.make_can_msg('ASCMActiveCruiseControlStatus', 2, {'ACCCruiseState': 0}),
+                      packer.make_can_msg('ECMEngineStatus', 0, {'CruiseMainOn': int(main)}),
                       packer.make_can_msg('AcceleratorPedal2', 0, {'CruiseState': 0}),
                       packer.make_can_msg('ECMAcceleratorPos', 0, {'BrakePedalPos': 30 if brake else 0})]
           cs = ci.update([(now, packets)])
           self.assertTrue(cs.canValid, msg=ctx)
           self.assertEqual(cs.brakePressed, brake, msg=ctx)
           self.assertFalse(cs.cruiseState.enabled, msg=ctx)
-          self.assertTrue(cs.cruiseState.available, msg=ctx)
+          self.assertEqual(cs.cruiseState.available, main, msg=ctx)
           for frame in packets:
             self.assertTrue(native('rx', frame, now // 1000), (frame, ctx))
           safety.safety_tick()
@@ -1531,27 +1539,30 @@ class TestGmAol(unittest.TestCase):
           self.assertEqual(events.contains(ET.IMMEDIATE_DISABLE), fault, msg=ctx)
           onroad = messaging.new_message('onroadEvents', 0, valid=True, logMonoTime=now)
           onroad.onroadEvents = events.to_msg()
-          selected.sm.update_msgs(now / 1e9, [onroad.as_reader()])
-          disarm = selected.aol_disarming_fault(cs, now, now)
-          self.assertEqual(disarm, fault, msg=ctx)
+          card_onroad = onroad.as_reader() if fault or previous_sd_onroad is None else previous_sd_onroad
+          selected.sm.update_msgs(now / 1e9, [card_onroad])
+          disarm = selected.aol_disarming_fault(cs, int(card_onroad.logMonoTime), now)
+          self.assertEqual(disarm, tick in (136, 138, 139), msg=ctx)
           selected.aol_card_intent.update(cs, fault_active=disarm, now_ns=now, standard_enabled=False)
-          self.assertEqual(selected.aol_card_intent.allowed_latch, tick < 138, msg=ctx)
-          self.assertEqual(selected.aol_card_intent._main_cycle_required, tick >= 138, msg=ctx)
+          expected_latch = main and (tick < 136 or tick >= 142)
+          self.assertEqual(selected.aol_card_intent.allowed_latch, expected_latch, msg=ctx)
+          self.assertEqual(selected.aol_card_intent._main_cycle_required, 136 <= tick < 140, msg=ctx)
           allowed, pause_lat, pause_long = selected.aol_card_intent.output(cs)
           intent_value = IntentState('card', tick + 1, now, now, now + 200_000_000,
                                      allowed, pause_lat, pause_long, selected.aol_qualified,
                                      selected.aol_card_intent.allowed_latch)
           intent = decode_intent(encode_intent(intent_value))
           self.assertIsNotNone(intent, msg=ctx)
-          self.assertEqual(intent.allowedLatch, tick < 138, msg=ctx)
+          self.assertEqual(intent.allowedLatch, expected_latch, msg=ctx)
           self.assertFalse(intent.pauseLateral, msg=ctx)
           self.assertFalse(intent.pauseLongitudinal, msg=ctx)
-          args = dict(standard_lateral=False, standard_longitudinal=False, intent=intent, car_state=cs,
-                      initialized=True, model_ready=True,
-                      no_entry=aol_no_entry(events.names, cs, paddle_only_cancel=False, allow_below_engage_speed=True),
-                      immediate_disable=events.contains(ET.IMMEDIATE_DISABLE), dm_lockout=False, pause_brake_mps=10.)
-          wanted = decide_axes(native=None, **args)
-          expected_lat = not brake and tick < 138
+          no_entry = aol_no_entry(events.names, cs, paddle_only_cancel=False, allow_below_engage_speed=True)
+          immediate_disable = events.contains(ET.IMMEDIATE_DISABLE)
+          wanted = decide_axes(native=None, standard_lateral=False, standard_longitudinal=False,
+                               intent=intent, car_state=cs, initialized=True, model_ready=True,
+                               no_entry=no_entry, immediate_disable=immediate_disable,
+                               dm_lockout=False, pause_brake_mps=10.)
+          expected_lat = not brake and expected_latch
           self.assertEqual(wanted.desired_lateral, expected_lat, msg=ctx)
           self.assertFalse(wanted.desired_longitudinal, msg=ctx)
           safety.aol_set_host_request(int(wanted.desired_lateral))
@@ -1573,6 +1584,7 @@ class TestGmAol(unittest.TestCase):
           sd.sm.update_msgs(now / 1e9, [intent_message.as_reader(), receipt.as_reader(), model.as_reader(),
                                        calibration.as_reader(), monitoring.as_reader()])
           sd.aol_car_state_log_ns = now
+          sd.conditional_car_state_valid = cs.canValid and not cs.canTimeout
           sd.events = events
           with patch('openpilot.selfdrive.selfdrived.selfdrived.REPLAY', True), \
                patch.object(sd, 'data_sample', return_value=cs), patch.object(sd, 'update_events'), \
@@ -1580,12 +1592,22 @@ class TestGmAol(unittest.TestCase):
                patch.object(sd, 'publish_selfdriveState'):
             sd.step()
           actual_lat = expected_lat and tick != 135
+          actual_onroad = messaging.new_message('onroadEvents', 0, valid=True, logMonoTime=now)
+          actual_onroad.onroadEvents = sd.events.to_msg()
+          previous_sd_onroad = actual_onroad.as_reader()
           self.assertFalse(sd.enabled or sd.active, msg=ctx)
           self.assertEqual(sd.aol_axis_decision.lateral_active, actual_lat, msg=ctx)
           self.assertFalse(sd.aol_axis_decision.longitudinal_active, msg=ctx)
           if tick == 135:
+            self.assertTrue(sd.aol_authority_lost, msg=ctx)
             self.assertIn(EventName.controlsMismatch, sd.events.names, msg=ctx)
             self.assertFalse(sd.aol_axis_decision.native_acknowledged, msg=ctx)
+          if tick in (136, 137):
+            self.assertFalse(sd.aol_axis_decision.lateral_active, msg=ctx)
+            self.assertFalse(selected.aol_card_intent.allowed_latch, msg=ctx)
+          if tick == 142:
+            self.assertTrue(sd.aol_axis_decision.lateral_active, msg=ctx)
+            self.assertFalse(selected.aol_card_intent._main_cycle_required, msg=ctx)
           feed(controls, now, tick, active=sd.active, enabled=sd.enabled)
           axis = messaging.new_message('aolAxisState', valid=True, logMonoTime=now)
           axis.aolAxisState.qualified = True
@@ -1618,7 +1640,10 @@ class TestGmAol(unittest.TestCase):
             probe.brakePressed = True
             for speed in (9.99, 10., 10.01):
               probe.vEgo = speed
-              self.assertEqual(decide_axes(native=receipt_state, **dict(args, car_state=probe)).desired_lateral,
+              self.assertEqual(decide_axes(native=receipt_state, standard_lateral=False, standard_longitudinal=False,
+                                           intent=intent, car_state=probe, initialized=True, model_ready=True,
+                                           no_entry=no_entry, immediate_disable=immediate_disable,
+                                           dm_lockout=False, pause_brake_mps=10.).desired_lateral,
                                speed >= 10., (speed, ctx))
           previous = cs
         self.assertEqual(resumed, 3)
@@ -1664,6 +1689,7 @@ class TestGmAol(unittest.TestCase):
       self.assertEqual(controls.CP.to_dict(), cp.to_dict())
       sd = SelfdriveD.__new__(SelfdriveD)
       sd.CP, sd.initialized, sd.aol_replay = cp, True, True
+      sd.enabled = sd.active = False
       sd.aol_session_id, sd.aol_axis_decision = 'ascm-acc-fault', AxisDecision()
       sd.aol_dm_lateral_inhibit, sd.aol_settings = False, selected.aol_settings
       sd.nostalgia_paddle_cancel = False
@@ -1707,10 +1733,9 @@ class TestGmAol(unittest.TestCase):
           packets = [packet for packet in packets if packet[0] not in (0x1C4, 0x3D1, 0x184)]
           packets += [packer.make_can_msg('AcceleratorPedal2', 0, {'CruiseState': AccState.FAULTED if fault else AccState.ACTIVE if ordinary else 0}),
                       packer.make_can_msg('ECMEngineStatus', 0, {'CruiseMainOn': int(main)}),
-                      packer.make_can_msg('PSCMStatus', 0, {'LKATorqueDeliveredStatus': 3 if critical == 'permanentEPS' and fault else 2 if tick == 20 else 0}),
+                      packer.make_can_msg('PSCMStatus', 0, {'LKATorqueDeliveredStatus': 3 if critical == 'permanentEPS' and fault else 2 if tick == 20 else 1}),
                       packer.make_can_msg('ASCMActiveCruiseControlStatus', 2, {'ACCCruiseState': 0})]
           self.assertFalse(any(address == 0x201 for address, _, _ in packets))
-          ci.update([(now - 1_000_000, packets)])
           cs = ci.update([(now, packets)])
           self.assertTrue(cs.canValid, tick)
           self.assertEqual(cs.accFaulted, fault)
@@ -1758,6 +1783,7 @@ class TestGmAol(unittest.TestCase):
           sd.sm.update_msgs(now / 1e9, [intent.as_reader(), receipt.as_reader(), model.as_reader(),
                                        calibration.as_reader(), monitoring.as_reader()])
           sd.aol_car_state_log_ns = now
+          sd.conditional_car_state_valid = cs.canValid and not cs.canTimeout
           sd.events = events
           with patch('openpilot.selfdrive.selfdrived.selfdrived.REPLAY', True), \
                patch.object(sd, 'data_sample', return_value=cs), patch.object(sd, 'update_events'), \

openpilot/starpilot/aol/tests/test_runtime.py

@@ -646,6 +646,245 @@ class CardIntentTests(unittest.TestCase):
 
 
 class IpcAxisContractTests(unittest.TestCase):
+  def _authority_callers(self, full_op):
+    self.enterContext(mock.patch('openpilot.selfdrive.selfdrived.selfdrived.REPLAY', False))
+    from opendbc.car.gm.tests.test_bolt_volt_configurations import ordinary_params
+    from opendbc.car.gm.values import CAR
+    from openpilot.selfdrive.controls.controlsd import Controls
+    from openpilot.starpilot.aol.vehicle import policy_for
+    from openpilot.starpilot.controllers.mode_actions import SwitchbackCooldown
+    from openpilot.starpilot.lateral.tests.test_lane_runtime import feed
+
+    params = Params()
+    params.put_bool('OpenpilotEnabledToggle', True, block=True)
+    params.put_bool('SafeMode', False, block=True)
+    params.put_bool('AlwaysOnLateral', True, block=True)
+    cp = ordinary_params(CAR.CHEVROLET_VOLT_ASCM, alpha=True, sascm=True, radar=True)
+    policy = policy_for(cp)
+    cp.safetyConfigs[-1].safetyParam |= policy.safety_param_addition
+    cp.alternativeExperience |= policy.alternative_experience_addition
+    params.put('CarParams', cp.to_bytes(), block=True)
+    controls = Controls()
+    sd = SelfdriveD.__new__(SelfdriveD)
+    sd.params = params
+    sd.CP = cp.as_reader()
+    services = ['deviceState', 'pandaStates', 'driverMonitoringState', 'extrinsicsCalibration', 'modelV2',
+                'carState', 'aolIntentWire', 'aolSafetyWire', 'starpilotCarState']
+    self.enterContext(mock.patch.object(sd, 'sm', messaging.SubMaster(services, ignore_avg_freq=services), create=True))
+    sd.sm.update = mock.Mock()
+    sd.events, sd.state_machine, sd.AM = Events(), StateMachine(), AlertManager()
+    sd.state_machine.state = log.SelfdriveState.OpenpilotState.enabled if full_op else log.SelfdriveState.OpenpilotState.disabled
+    sd.enabled = sd.active = full_op
+    sd.mismatch_counter = 0
+    sd.events_prev = []
+    sd.experimental_mode = sd.requested_experimental_mode = False
+    sd.initialized = sd.aol_replay = True
+    sd.aol_authority_lost = sd.aol_dm_lateral_inhibit = False
+    sd.aol_axis_decision = AxisDecision()
+    sd.aol_settings = None
+    sd.aol_car_state_log_ns = 0
+    sd.conditional_car_state_valid = False
+    sd.aol_last_intent = None
+    sd.aol_session_id, sd.aol_sequence = 'authority-session', 0
+    sd.conditional_replay = False
+    sd.nostalgia_paddle_cancel = False
+    sd.switchback_capable = False
+    sd.switchback_cooldown = SwitchbackCooldown()
+    sd.switchback_setting_ns, sd.switchback_cooldown_ns = 0, 300_000_000_000
+    sd.personality, sd.is_metric = 1, False
+    sd.CS_prev = car_state()
+    sd.car_state_sock = object()
+    sd.update_conditional_mode = mock.Mock()
+    self.enterContext(mock.patch.object(sd, 'update_events', lambda cs: (sd.events.clear(), sd._update_steering_authority(cs))))
+
+    class Publisher:
+      def __init__(self):
+        self.messages = {}
+      def send(self, service, message):
+        self.messages[service] = messaging.log_from_bytes(message.to_bytes())
+    self.enterContext(mock.patch.object(sd, 'pm', Publisher(), create=True))
+    self.enterContext(mock.patch.object(controls, 'pm', Publisher()))
+    clock = self.enterContext(mock.patch('openpilot.selfdrive.selfdrived.selfdrived.time.monotonic_ns'))
+    recv = self.enterContext(mock.patch('openpilot.selfdrive.selfdrived.selfdrived.messaging.recv_one'))
+    base = 10_000_000_000
+    original_message = messaging.new_message
+    def clocked_message(*args, **kwargs):
+      message = original_message(*args, **kwargs)
+      if 'logMonoTime' not in kwargs:
+        message.logMonoTime = clock.return_value
+      return message
+    self.enterContext(mock.patch('openpilot.selfdrive.selfdrived.selfdrived.messaging.new_message', clocked_message))
+
+    def step(tick, *, timeout=False, old_intent=False, allowed=True, pause=False, latched=True,
+             brake=False, steering=False, gas=False, user_disable=False, observed_state=None, authority_unavailable=False, companion=True,
+             long_allowed=True, pause_long=False):
+      now = base + tick * 10_000_000
+      clock.return_value = now
+      state = car_state(brake=brake) if observed_state is None else observed_state
+      state.cruiseState.available = True
+      state.steeringPressed, state.gasPressed = steering, gas
+      state_event = messaging.new_message('carState', valid=True, logMonoTime=now)
+      state_event.carState = state
+      recv.return_value = None if timeout else state_event.as_reader()
+      frames = []
+      for service in ('deviceState', 'driverMonitoringState', 'extrinsicsCalibration', 'modelV2'):
+        message = messaging.new_message(service, valid=True, logMonoTime=now)
+        if service == 'deviceState':
+          message.deviceState.started, message.deviceState.startedMonoTime = True, base - 1_000_000_000
+        elif service == 'extrinsicsCalibration':
+          message.extrinsicsCalibration.calStatus = 'calibrated'
+        frames.append(message.as_reader())
+      intent_ns = base if old_intent else now
+      intent = messaging.new_message('aolIntentWire', 0, valid=True, logMonoTime=intent_ns)
+      intent.aolIntentWire = encode_intent(IntentState('authority-card', tick + 1, intent_ns, intent_ns,
+                                                     intent_ns + 200_000_000, latched, pause, pause_long, True, latched))
+      native = messaging.new_message('aolSafetyWire', 0, valid=True, logMonoTime=now)
+      native.aolSafetyWire = encode_safety(SafetyState(1, True, now, now + 200_000_000,
+        int(car.CarParams.SafetyModel.gm), cp.safetyConfigs[-1].safetyParam,
+        allowed, full_op and long_allowed, True, full_op, 'authority-panda', sd.aol_session_id))
+      if companion:
+        authority = messaging.new_message('starpilotCarState', valid=companion != 'invalid',
+                                          logMonoTime=now - 1 if companion == 'wrong_stamp' else now)
+        authority.starpilotCarState.sourceCarStateMonoTime = now - 1 if companion == 'wrong_source' else now
+        authority.starpilotCarState.lateralAuthorityUnavailable = authority_unavailable
+        frames.append(authority.as_reader())
+      sd.sm.update_msgs(now / 1e9, frames + [intent.as_reader(), native.as_reader()])
+      if user_disable:
+        self.enterContext(mock.patch.object(sd, 'update_events', lambda cs: (
+          sd.events.clear(), sd._update_steering_authority(cs), sd.events.add(log.OnroadEvent.EventName.pedalPressed))))
+      sd.step()
+      control_frames = []
+      with mock.patch.object(controls.sm, 'update_msgs', side_effect=lambda _timestamp, messages: control_frames.extend(messages)):
+        feed(controls, now, tick * 5, active=False, enabled=False)
+      model = messaging.new_message('modelV2', valid=True, logMonoTime=now)
+      model.modelV2 = next(message.modelV2 for message in control_frames if message.which() == 'modelV2')
+      model.modelV2.action.desiredCurvature = 0.
+      control_state = messaging.new_message('carState', valid=sd.conditional_car_state_valid,
+                                            logMonoTime=sd.aol_car_state_log_ns)
+      control_state.carState = sd.CS_prev
+      replaced = ('carState', 'aolSafetyWire', 'modelV2', 'aolAxisState', 'selfdriveState')
+      controls.sm.update_msgs(now / 1e9, [message for message in control_frames if message.which() not in replaced] +
+                             [control_state.as_reader(), native.as_reader(), model.as_reader(),
+                              sd.pm.messages['aolAxisState'], sd.pm.messages['selfdriveState']])
+      command, lac_log = controls.state_control()
+      controls.publish(command, lac_log)
+      return sd.pm.messages['aolAxisState'].aolAxisState, sd.pm.messages['selfdriveState'].selfdriveState, command
+    return sd, controls, step
+
+  def _assert_withdrawal_alert(self, controls, message):
+    from openpilot.starpilot.audio.axis_alerts import AxisAlerts
+    from openpilot.starpilot.ui.runtime_snapshot import current_alert
+    now = int(message.logMonoTime)
+    alert = current_alert(controls.sm, now, after_frame=0)
+    self.assertTrue(alert.critical)
+    self.assertEqual(alert.size, 'full')
+    self.assertEqual(alert.text1, 'TAKE CONTROL IMMEDIATELY')
+    self.assertEqual(AxisAlerts().update(controls.sm, int(message.selfdriveState.alertSound.raw), now),
+                     log.SelfdriveState.AudibleAlert.warningImmediate)
+
+  def test_actual_callers_companion_loss_disables_and_requires_intent_withdrawal(self):
+    for full_op in (False, True):
+      for loss in ('car_state', 'intent', 'authority_companion', 'wrong_stamp', 'wrong_source', 'invalid'):
+        with self.subTest(full_op=full_op, loss=loss), OpenpilotPrefix():
+          sd, controls, step = self._authority_callers(full_op)
+          axis, _, command = step(0)
+          self.assertTrue(axis.lateralActive and axis.nativeAcknowledged and command.latActive)
+          self.assertAlmostEqual(command.actuators.torque, 0.)
+          retained, _, command = step(2, timeout=True)
+          self.assertTrue(retained.lateralActive and command.latActive)
+          self.assertNotIn(log.OnroadEvent.EventName.controlsMismatch, sd.events.names)
+          self.assertEqual(retained.sourceCarStateMonoTime, 10_000_000_000)
+          axis, state, command = step(7, timeout=loss == 'car_state', old_intent=loss == 'intent', companion=False if loss == 'authority_companion' else loss)
+          self.assertFalse(sd.enabled or sd.active or axis.lateralActive or axis.longitudinalActive or command.latActive or command.longActive)
+          self.assertIn(log.OnroadEvent.EventName.controlsMismatch, sd.events.names)
+          self.assertEqual(state.alertText1, 'TAKE CONTROL IMMEDIATELY')
+          self.assertEqual(state.alertSound, log.SelfdriveState.AudibleAlert.warningImmediate)
+          self.assertFalse(controls.pm.messages['carControl'].carControl.latActive)
+          self._assert_withdrawal_alert(controls, sd.pm.messages['selfdriveState'])
+          self.assertEqual(axis.sourceCarStateMonoTime, 0 if loss == 'car_state' else 10_070_000_000)
+          self.assertFalse(sd.pm.messages['aolAxisState'].valid if loss == 'car_state' else axis.lateralActive)
+          self.assertFalse(step(8)[0].lateralActive)  # Fresh input alone cannot restart the latched axis.
+          self.assertFalse(step(9, latched=False)[0].lateralActive)
+          self.assertTrue(step(10)[0].lateralActive)  # Intent owner supplies its fresh rearm after withdrawal.
+
+  def test_actual_callers_fresh_native_denial_preserves_deliberate_pauses(self):
+    for full_op in (False, True):
+      for reason in ('denial', 'pause', 'brake', 'steering', 'gas', 'user_disable', 'startup',
+                     *(('long_denial', 'long_pause') if full_op else ())):
+        with self.subTest(full_op=full_op, reason=reason), OpenpilotPrefix():
+          sd, controls, step = self._authority_callers(full_op)
+          if reason != 'startup':
+            self.assertTrue(step(0)[0].lateralActive)
+          axis, state, command = step(1, allowed=reason in ('long_denial', 'long_pause'), pause=reason == 'pause',
+                                     long_allowed=reason not in ('long_denial', 'long_pause'), pause_long=reason == 'long_pause',
+                                     brake=reason == 'brake', steering=reason == 'steering',
+                                     gas=reason == 'gas', user_disable=reason == 'user_disable')
+          self.assertEqual(bool(axis.lateralActive), reason == 'long_pause')
+          self.assertEqual(bool(command.latActive), reason == 'long_pause')
+          if reason in ('denial', 'long_denial'):
+            self.assertFalse(sd.enabled or sd.active)
+            self.assertEqual(state.alertText2, 'Controls Mismatch')
+            self.assertEqual(state.alertSound, log.SelfdriveState.AudibleAlert.warningImmediate)
+            self.assertTrue(sd.aol_authority_lost)
+            self._assert_withdrawal_alert(controls, sd.pm.messages['selfdriveState'])
+          else:
+            self.assertNotIn(log.OnroadEvent.EventName.controlsMismatch, sd.events.names)
+            self.assertFalse(sd.aol_authority_lost)
+
+  def test_actual_eps_monitor_fault_reaches_both_axis_callers(self):
+    from opendbc.can import CANPacker
+    from opendbc.car import Bus
+    from opendbc.car.gm.interface import CarInterface
+    from opendbc.car.gm.tests.test_ascm_intercept import TestAscmIntercept
+    from opendbc.car.gm.values import DBC
+    for full_op in (False, True):
+      with self.subTest(full_op=full_op), OpenpilotPrefix():
+        sd, _, step = self._authority_callers(full_op)
+        ci = CarInterface(sd.CP)
+        packer = CANPacker(DBC[sd.CP.carFingerprint][Bus.pt])
+        producer = TestAscmIntercept()
+        for tick in range(20):
+          producer.authority_frame(ci, packer, tick, torque=0.)
+        healthy = producer.authority_frame(ci, packer, 20, torque=0.)
+        self.assertTrue(healthy.canValid and not healthy.canTimeout)
+        self.assertTrue(ci.CS.steering_authority.seen_active)
+        self.assertFalse(healthy.steerFaultTemporary)
+        self.assertTrue(step(0, observed_state=healthy)[0].lateralActive)
+        for tick in range(21, 30):
+          producer.authority_frame(ci, packer, tick, torque=0.)
+        loss = producer.authority_frame(ci, packer, 30, status=0, torque=0.)
+        self.assertTrue(loss.canValid and not loss.canTimeout)
+        self.assertTrue(loss.steerFaultTemporary)
+        axis, state, command = step(1, observed_state=loss, authority_unavailable=ci.CS.steering_authority.latched)
+        self.assertFalse(axis.lateralActive or command.latActive)
+        self.assertEqual(state.alertText2, 'Controls Mismatch')
+        self.assertEqual(state.alertSound, log.SelfdriveState.AudibleAlert.warningImmediate)
+        self.assertFalse(sd.enabled or sd.active)
+        self.assertIn(log.OnroadEvent.EventName.controlsMismatch, sd.events.names)
+        for tick in range(31, 40):
+          producer.authority_frame(ci, packer, tick, status=0, torque=0.)
+        recovered_status = producer.authority_frame(ci, packer, 40, torque=0.)
+        self.assertTrue(recovered_status.canValid and not recovered_status.canTimeout)
+        self.assertTrue(recovered_status.steerFaultTemporary)
+        self.assertFalse(step(2, observed_state=recovered_status, authority_unavailable=ci.CS.steering_authority.latched)[0].lateralActive)
+        # A fresh physical status2 is ordinary temporary EPS limitation, not the monitor's latched loss.
+        sd, _, step = self._authority_callers(full_op)
+        ci = CarInterface(sd.CP)
+        for tick in range(20):
+          producer.authority_frame(ci, packer, tick, torque=0.)
+        healthy = producer.authority_frame(ci, packer, 20, torque=0.)
+        self.assertTrue(healthy.canValid and not healthy.canTimeout)
+        self.assertTrue(step(0, observed_state=healthy)[0].lateralActive)
+        for tick in range(21, 30):
+          producer.authority_frame(ci, packer, tick, torque=0.)
+        temporary = producer.authority_frame(ci, packer, 30, status=2, torque=0.)
+        self.assertTrue(temporary.canValid and not temporary.canTimeout)
+        self.assertFalse(ci.CS.steering_authority.latched)
+        axis, state, command = step(1, observed_state=temporary)
+        self.assertFalse(axis.lateralActive or command.latActive)
+        self.assertEqual(state.alertSound, log.SelfdriveState.AudibleAlert.prompt)
+        self.assertNotIn(log.OnroadEvent.EventName.controlsMismatch, sd.events.names)
+
   def test_car_state_timeout_retains_only_original_fresh_valid_sample(self):
     sd = SelfdriveD.__new__(SelfdriveD)
     sd.car_state_sock = object()
@@ -727,6 +966,7 @@ class IpcAxisContractTests(unittest.TestCase):
     sd.initialized = True
     sd.aol_replay = True
     sd.aol_car_state_log_ns = 0
+    sd.conditional_car_state_valid = True
     sd.aol_session_id = 'drive-session'
     sd.aol_axis_decision = AxisDecision()
     sd.aol_dm_lateral_inhibit = False
@@ -799,6 +1039,7 @@ class IpcAxisContractTests(unittest.TestCase):
     sd.enabled = sd.active = False
     sd.initialized = sd.aol_replay = True
     sd.aol_car_state_log_ns = 0
+    sd.conditional_car_state_valid = True
     sd.aol_session_id = 'drive-session'
     sd.aol_axis_decision = AxisDecision()
     sd.aol_dm_lateral_inhibit = False
@@ -1299,6 +1540,7 @@ class IpcAxisContractTests(unittest.TestCase):
       sd.initialized = True
       sd.aol_replay = aol_enabled
       sd.aol_car_state_log_ns = 0
+      sd.conditional_car_state_valid = True
       sd.aol_session_id = 'drive-session'
       sd.aol_axis_decision = AxisDecision()
       sd.aol_dm_lateral_inhibit = False

openpilot/starpilot/gps/publisher.py

@@ -15,25 +15,31 @@ class CarGpsPublisher:
   def __init__(self, *, mono_clock=time.monotonic_ns, boot_clock=boot_time_ns):
     self.mono_clock, self.boot_clock = mono_clock, boot_clock
     self.last_source_ns = 0
+    self.cached_gps = None
 
-  def update(self, car_state, pm):
+  def update(self, car_state, pm, message=None):
     getter = getattr(car_state, "get_car_gps", None)
     fix = getter() if getter is not None else None
-    if fix is None:
-      return
-    source_ns = int(fix["timestamp_nanos"])
-    if source_ns <= self.last_source_ns:
-      return
-    self.last_source_ns = source_ns
-    before, boot, after = self.mono_clock(), self.boot_clock(), self.mono_clock()
-    # pandad timestamps received CAN in BOOTTIME; Python Events use MONOTONIC.
-    age = boot - source_ns
-    if not 0 <= after - before <= 1_000_000 or not 0 <= age <= GPS_MAX_AGE_NS:
-      return
-    stamp = before - age
-    if stamp <= 0:
-      return
-    message = messaging.new_message("starpilotCarState", valid=bool(fix["hasFix"]), logMonoTime=after)
-    message.starpilotCarState.gps = {key: fix[key] for key in FIELDS}
-    message.starpilotCarState.gps.sourceMonoTime = stamp
+    gps, after = None, 0
+    if fix is not None:
+      source_ns = int(fix["timestamp_nanos"])
+      if source_ns > self.last_source_ns:
+        self.last_source_ns = source_ns
+        before, boot, after = self.mono_clock(), self.boot_clock(), self.mono_clock()
+        # pandad timestamps received CAN in BOOTTIME; Python Events use MONOTONIC.
+        age = boot - source_ns
+        if 0 <= after - before <= 1_000_000 and 0 <= age <= GPS_MAX_AGE_NS:
+          stamp = before - age
+          if stamp > 0:
+            gps = {key: fix[key] for key in FIELDS}
+            gps["vNED"] = list(fix["vNED"])
+            gps["sourceMonoTime"] = stamp
+            self.cached_gps = gps if fix["hasFix"] else None
+    if message is None:
+      if gps is None:
+        return
+      message = messaging.new_message("starpilotCarState", valid=bool(gps["hasFix"]), logMonoTime=after)
+    gps = gps if gps is not None else self.cached_gps
+    if gps is not None and 0 < gps["sourceMonoTime"] <= int(message.logMonoTime) <= gps["sourceMonoTime"] + GPS_MAX_AGE_NS:
+      message.starpilotCarState.gps = gps
     pm.send("starpilotCarState", message)

openpilot/starpilot/gps/tests/test_car_gps.py

@@ -1,4 +1,5 @@
 from types import SimpleNamespace
+from typing import Any, cast
 from unittest.mock import Mock, patch
 
 import pytest
@@ -6,6 +7,7 @@ import pytest
 from opendbc.can import CANPacker
 from opendbc.car.gm.interface import CarInterface
 from opendbc.car.gm.values import CAR
+from opendbc.car import structs as car
 from openpilot.cereal import log, messaging
 from openpilot.starpilot.gps.publisher import CarGpsPublisher
 from openpilot.starpilot.gps.source import GPS_SOURCES, observation, select_location
@@ -130,3 +132,115 @@ def test_old_car_state_wire_has_no_gps_fix():
   sm = Messages()
   sm.send('starpilotCarState', event)
   assert select_location(sm, 101_000_000_000) is None
+
+
+def test_fault_companion_without_gps_retains_car_state_stamp_and_validity():
+  owner, sm = CarGpsPublisher(), Messages()
+  for valid in (True, False):
+    cs_send = messaging.new_message('carState', valid=valid, logMonoTime=101_000_000_000)
+    companion = messaging.new_message('starpilotCarState', valid=bool(cs_send.valid), logMonoTime=int(cs_send.logMonoTime))
+    companion.starpilotCarState.lateralAuthorityUnavailable = True
+    companion.starpilotCarState.sourceCarStateMonoTime = int(cs_send.logMonoTime)
+    owner.update(SimpleNamespace(), sm, companion)
+    assert sm.valid['starpilotCarState'] == cs_send.valid
+    assert sm.logMonoTime['starpilotCarState'] == cs_send.logMonoTime
+    assert sm['starpilotCarState'].sourceCarStateMonoTime == cs_send.logMonoTime
+    assert sm['starpilotCarState'].lateralAuthorityUnavailable
+    assert not sm['starpilotCarState'].gps.hasFix
+    assert observation(sm, 'starpilotCarState', cs_send.logMonoTime) is None
+
+
+def test_fault_companion_cached_gps_keeps_original_source_stamp_and_expires():
+  state, sm, clock = live_can_fix(), Messages(), [101_000_000_000]
+  owner = CarGpsPublisher(mono_clock=lambda: clock[0], boot_clock=lambda: clock[0] + 50_000_000_000)
+  owner.update(state, sm)
+  source_stamp = sm['starpilotCarState'].gps.sourceMonoTime
+  for elapsed in (1_000_000_000, 2_500_000_001):
+    clock[0] = source_stamp + elapsed
+    companion = messaging.new_message('starpilotCarState', valid=True, logMonoTime=clock[0])
+    companion.starpilotCarState.sourceCarStateMonoTime = clock[0]
+    companion.starpilotCarState.lateralAuthorityUnavailable = True
+    owner.update(state, sm, companion)
+    assert sm.logMonoTime['starpilotCarState'] == clock[0]
+    assert sm['starpilotCarState'].sourceCarStateMonoTime == clock[0]
+    assert sm['starpilotCarState'].lateralAuthorityUnavailable
+    if elapsed <= 2_500_000_000:
+      assert sm['starpilotCarState'].gps.sourceMonoTime == source_stamp
+      assert location(sm, clock[0]) == (source_stamp, (-110., 40.), 0., None)
+    else:
+      assert not sm['starpilotCarState'].gps.hasFix
+      assert location(sm, clock[0]) is None
+
+
+def test_fault_companion_clock_interruption_cannot_invent_gps_fix():
+  clock, sm = iter([101_000_000_000, 101_002_000_000]), Messages()
+  owner = CarGpsPublisher(mono_clock=lambda: next(clock), boot_clock=lambda: 151_000_000_000)
+  companion = messaging.new_message('starpilotCarState', valid=True, logMonoTime=101_002_000_000)
+  companion.starpilotCarState.sourceCarStateMonoTime = companion.logMonoTime
+  companion.starpilotCarState.lateralAuthorityUnavailable = True
+  owner.update(live_can_fix(), sm, companion)
+  assert sm['starpilotCarState'].lateralAuthorityUnavailable
+  assert not sm['starpilotCarState'].gps.hasFix
+  assert location(sm, companion.logMonoTime) is None
+
+
+@pytest.mark.parametrize('brand,enabled,valid', [('gm', True, True), ('gm', True, False), ('gm', False, True), ('honda', True, True)])
+def test_actual_card_publishes_fault_companion_before_matching_car_state(brand, enabled, valid):
+  from openpilot.selfdrive.car.card import Car
+
+  card = cast(Any, Car.__new__(Car))
+  card.CP = SimpleNamespace(brand=brand)
+  card.CI = SimpleNamespace(CS=SimpleNamespace(steering_authority=SimpleNamespace(enabled=enabled, latched=True)))
+  card.pm = Messages()
+  card.sm = SimpleNamespace(frame=1, all_checks=lambda _services: True)
+  card.car_params_published = True
+  card.last_actuators_output = car.CarControl.Actuators.new_message()
+  card.slc_replay = card.curve_replay = card.aol_replay = False
+  card.can_rcv_cum_timeout_counter = 0
+  card.rk = SimpleNamespace(remaining=0.)
+  card.v_cruise_helper = SimpleNamespace(slc_cruise_change=None)
+  card.car_gps_publisher = CarGpsPublisher()
+  CS = car.CarState.new_message(canValid=valid)
+  card.state_publish(CS, None)
+  assert ('starpilotCarState' in card.pm.data) == (brand == 'gm' and enabled)
+  if brand == 'gm' and enabled:
+    assert list(card.pm.data).index('starpilotCarState') < list(card.pm.data).index('carState')
+    assert card.pm.valid['starpilotCarState'] == card.pm.valid['carState'] == valid
+    assert card.pm.logMonoTime['starpilotCarState'] == card.pm.logMonoTime['carState']
+    assert card.pm['starpilotCarState'].sourceCarStateMonoTime == card.pm.logMonoTime['carState']
+    assert card.pm['starpilotCarState'].lateralAuthorityUnavailable
+    assert not card.pm['starpilotCarState'].gps.hasFix
+
+
+@pytest.mark.parametrize('brand,enabled,keepalive', [('gm', True, False), ('gm', False, False), ('honda', True, False),
+                                                   ('gm', False, True)])
+def test_actual_card_monitored_apply_and_sendcan_use_same_boottime(brand, enabled, keepalive):
+  from openpilot.selfdrive.car.card import Car
+
+  card = cast(Any, Car.__new__(Car))
+  card.CP = SimpleNamespace(brand=brand, alternativeExperience=0)
+  card.CI = SimpleNamespace(CS=SimpleNamespace(steering_authority=SimpleNamespace(enabled=enabled)), CC=SimpleNamespace(),
+                            apply=Mock(return_value=(car.CarControl.Actuators.new_message(), [])))
+  card.pm = Mock()
+  card.sm = SimpleNamespace(seen={'carControl': True}, alive={'carControl': True}, valid={'carControl': True},
+                            logMonoTime={'carControl': 101_000_000_000}, recv_time={'carControl': 101.},
+                            all_alive=lambda _services: True)
+  card.ci_initialized = True
+  card.ioniq6_long_prearmed = False
+  card.vehicle_startup = SimpleNamespace(owner=None, check=Mock())
+  card.volt_startup_keepalive = Mock(return_value=keepalive)
+  CS, CC = car.CarState.new_message(canValid=True), car.CarControl.new_message()
+  with (patch('openpilot.selfdrive.car.card.REPLAY', False),
+        patch('openpilot.selfdrive.car.card.time.monotonic', return_value=101.),
+        patch('openpilot.selfdrive.car.card.time.monotonic_ns', return_value=101_000_000_000),
+        patch('openpilot.selfdrive.car.card.time.clock_gettime_ns', return_value=151_000_000_000)):
+    card.controls_update(CS, CC)
+  expected = 151_000_000_000 if keepalive or brand == 'gm' and enabled else 101_000_000_000
+  card.CI.apply.assert_called_once_with(CC, expected)
+  service, packet = card.pm.send.call_args.args
+  assert service == 'sendcan'
+  with log.Event.from_bytes(packet) as event:
+    assert event.logMonoTime == expected
+    assert event.valid
+    assert len(event.sendcan) == 0
+  card.vehicle_startup.check.assert_called_once_with()