Preserve Subaru intent through stop and source handoffs

openpilot/starpilot/aol/source_availability.py

@@ -47,12 +47,14 @@ def stock_lateral_role(self, CS, native, intent=None, *, allow_independent_cance
     settings = getattr(self, 'aol_settings', None)
     previous = getattr(self, 'aol_axis_decision', None)
     from opendbc.car.gm.aol import qualified_gm
+    from opendbc.car.subaru.stock_aol import qualified as subaru_qualified
 
     gas_role = bool((self.CP.brand == 'hyundai' and len(self.CP.safetyConfigs) == 1 and
       stock_canfd_qualified(self.CP, marked_only=True) or self.CP.brand == 'gm' and qualified_gm(self.CP) and self.CP.pcmCruise and
-      not self.CP.openpilotLongitudinalControl) and settings is not None and settings.enabled and
+      not self.CP.openpilotLongitudinalControl or
+      self.CP.brand == 'subaru' and subaru_qualified(self.CP, marked_only=True)) and settings is not None and settings.enabled and
       CS.gasPressed and not CS.brakePressed and not CS.regenBraking and
-      previous is not None and (previous.lateral_active or self.CP.brand == 'gm' and
+      previous is not None and (previous.lateral_active or self.CP.brand in ('gm', 'subaru') and
        getattr(self, 'aol_fault_reason', 'none') == 'availabilityPause' and previous.desired_lateral) and
       previous.native_acknowledged and not previous.longitudinal_active and
       (CS.gearShifter == car.CarState.GearShifter.drive or self.CP.brand == 'gm' and

openpilot/starpilot/aol/transport_pause.py

@@ -123,8 +123,10 @@ def stock_lateral_off(cp, cs, native, *, allow_independent_cancel=False, allow_i
   from openpilot.starpilot.car.toyota.aol import qualified as toyota_qualified
 
   from opendbc.car.gm.aol import qualified_gm
+  from opendbc.car.subaru.stock_aol import qualified as subaru_qualified
 
-  gm_stock_gas = bool(cp.brand == 'gm' and qualified_gm(cp) and cp.pcmCruise and not cp.openpilotLongitudinalControl)
+  stock_gas = bool(cp.brand == 'gm' and qualified_gm(cp) and cp.pcmCruise and not cp.openpilotLongitudinalControl or
+                   cp.brand == 'subaru' and subaru_qualified(cp, marked_only=True))
   independent_angle = angle_qualified(cp, marked_only=True)
   independent_brake = independent_angle or (allow_independent_brake and (
     toyota_qualified(cp, marked_only=True) and not cs.cruiseState.enabled or
@@ -135,7 +137,7 @@ def stock_lateral_off(cp, cs, native, *, allow_independent_cancel=False, allow_i
     not cs.accFaulted and not cs.doorOpen and not cs.seatbeltUnlatched and
     (not cs.brakePressed or independent_brake and allow_independent_brake) and
     not cs.regenBraking and (not cs.gasPressed or independent_angle or
-      allow_independent_gas and (gm_stock_gas or toyota_qualified(cp, marked_only=True) or
+      allow_independent_gas and (stock_gas or toyota_qualified(cp, marked_only=True) or
         cp.brand == 'hyundai' and stock_canfd_qualified(cp, marked_only=True))) and
     (not physical_cancel(cs, cp) or independent_angle and allow_independent_cancel) and
     native is not None and native.requestedLateral and native.lateralAllowed and not native.requestedLongitudinal)
@@ -874,14 +876,16 @@ class TransportPauseFeedback:
       not cs.regenBraking and self.lateral_only_baseline and
       not native.requestedLongitudinal and not native.longitudinalAllowed)
     from opendbc.car.gm.aol import qualified_gm
-    gm_stock_gas = bool(cp.brand == 'gm' and qualified_gm(cp) and cp.pcmCruise and not cp.openpilotLongitudinalControl and
+    from opendbc.car.subaru.stock_aol import qualified as subaru_qualified
+    stock_gas = bool((cp.brand == 'gm' and qualified_gm(cp) and cp.pcmCruise and not cp.openpilotLongitudinalControl or
+      cp.brand == 'subaru' and subaru_qualified(cp, marked_only=True)) and
       cs.gasPressed and not cs.brakePressed and not cs.regenBraking and self.lateral_only_baseline and
       not native.requestedLongitudinal and not native.longitudinalAllowed)
     stock_user = stock_lateral_user_event(cp)
     stock_lateral = bool(reason == 'availabilityPause' and axis.sourceCarStateMonoTime == 0 and
       not axis.lateralActive and not axis.longitudinalActive and not axis.desiredLongitudinal and
       stock_lateral_off(cp, cs, native, allow_independent_brake=toyota_pedal,
-                        allow_independent_gas=gm_stock_gas or toyota_pedal and cs.gasPressed) and previous is not None and
+                        allow_independent_gas=stock_gas or toyota_pedal and cs.gasPressed) and previous is not None and
       previous[0] == self.session and previous[7] == self.session and
       previous[14] == intent_producer_session and previous[15] == self.availability_epoch and
       (previous[11] and not previous[12] and previous[13] and not previous[10] and
@@ -1184,7 +1188,7 @@ class TransportPauseFeedback:
       return False
     native = _configured_transport_native(sm, cp, self.session, now_ns, allow_restricted_lateral=True)
     return bool(native is not None and native.pandaInventory and native.requestedLateral and
-      not native.lateralAllowed and
+      (not native.lateralAllowed or subaru_stop and not axis.desiredLateral and not axis.lateralActive) and
       (not native.requestedLongitudinal and not combined_stop or
        combined_stop and native.requestedLongitudinal and native.longitudinalAllowed) and
       native.observedMonoTime >= self.lateral_native_ns and

openpilot/starpilot/car/subaru/tests/selfdrive_step.py

@@ -4,9 +4,11 @@ from unittest.mock import patch
 from openpilot.cereal import messaging
 
 
-def selfdrive_step(sd, published, native, previous_controls, cp, safety, now, *, calibration):
+def selfdrive_step(sd, published, native, previous_controls, cp, safety, now, *, calibration, source_timeout=False):
   peers = []
   for service in sd.sm.data:
+    if source_timeout and service in ('carState', 'carOutput', 'aolIntentWire'):
+      continue
     if service in ('alertDebug', 'lateralManeuverPlan', 'laneChangeAssistWire', 'starpilotLongitudinalPlan'):
       continue
     if service == 'aolIntentWire':
@@ -60,7 +62,7 @@ def selfdrive_step(sd, published, native, previous_controls, cp, safety, now, *,
     patch('openpilot.selfdrive.selfdrived.selfdrived.time.monotonic', return_value=now / 1e9),
     patch('openpilot.selfdrive.selfdrived.selfdrived.time.clock_gettime_ns', return_value=now),
     patch('openpilot.selfdrive.selfdrived.selfdrived.messaging.recv_one_or_none', return_value=None),
-    patch('openpilot.selfdrive.selfdrived.selfdrived.messaging.recv_one', return_value=published['carState']),
+    patch('openpilot.selfdrive.selfdrived.selfdrived.messaging.recv_one', return_value=None if source_timeout else published['carState']),
     patch.object(sd.sm, 'update', side_effect=lambda _: sd.sm.update_msgs(now / 1e9, peers)),
     patch.object(sd.pm, 'send', side_effect=capture),
   ):

openpilot/starpilot/car/subaru/tests/test_stock_aol_callers.py

@@ -24,7 +24,8 @@ from openpilot.starpilot.car.subaru.tests.native_fixture import subaru_native_pa
 @pytest.mark.parametrize('candidate', [CAR.SUBARU_ASCENT_2023, CAR.SUBARU_LEGACY_2025, CAR.SUBARU_CROSSTREK_2025, CAR.SUBARU_FORESTER, CAR.SUBARU_OUTBACK])
 def test_actual_full_engage_brake_reverse_temporary_recovery_mainoff(candidate, subaru_native_pair, tmp_path, aol=True, manual=False, stop_resume=False,
                                                                       hard_stop_native_first=False, main_only=False, preferences=None, firmware=None,
-                                                                      deadline_gap=False, fixed_angle_handoff=False):
+                                                                      deadline_gap=False, fixed_angle_handoff=False, host_first_stop=False,
+                                                                      source_outage=False):
   fw = firmware_records(candidate, FW_VERSIONS, 'subaru') if firmware is None else firmware
   rows = []
   with actual_factory(candidate, gen_empty_fingerprint(), fw, '0' * 17, manual=manual, aol=aol, preferences=preferences) as (card, params, matches):
@@ -64,6 +65,8 @@ def test_actual_full_engage_brake_reverse_temporary_recovery_mainoff(candidate,
       ticks = (list(range(125)) + list(range(125, 135)) * 30 + list(range(135, 460))
                if hard_stop_native_first else range(460))
       gap_tick = 75 if main_only else 210
+      last_card_published = {}
+      pending_host_can = []
       for elapsed, tick in enumerate(ticks):
         now = base + elapsed * 10_000_000 + (22_504_479 if deadline_gap and elapsed >= gap_tick else 0)
         cruise = not main_only and (30 <= tick < 110 or tick >= 410)
@@ -71,9 +74,11 @@ def test_actual_full_engage_brake_reverse_temporary_recovery_mainoff(candidate,
         reverse = 150 <= tick < 185
         temporary = 225 <= tick < 260
         main = not 310 <= tick < 350 and (tick >= 30 or not main_only)
-        stopped = stop_resume and (125 <= tick < 135 if hard_stop_native_first else 185 <= tick < 195)
-        first_resume = 145 <= tick < 150 if hard_stop_native_first else 125 <= tick < 145
+        stopped = stop_resume and (125 <= tick < 135 if hard_stop_native_first or host_first_stop else 185 <= tick < 195)
+        first_resume = 145 <= tick < 150 if hard_stop_native_first or host_first_stop else 125 <= tick < 145
+        source_missing = bool(source_outage and 75 <= tick < 81)
         values = {
+          'Throttle': {'Throttle_Pedal': 100 if source_outage and 60 <= tick < 90 else 0},
           'Steering_Torque': {'Steer_Warning': int(temporary),
                              'Steer_Torque_Sensor': (250 if 70 <= tick < 76 else 143 if 76 <= tick < 90 else 0) if fixed_angle_handoff else 0},
           'Steering_2': {'Steering_Angle': (5.65 if tick < 78 else 5.61) if fixed_angle_handoff else 0},
@@ -85,11 +90,13 @@ def test_actual_full_engage_brake_reverse_temporary_recovery_mainoff(candidate,
           'Transmission': {'Gear': 3 if reverse else 121},
         }
         incoming = stream.frames(values, tick=tick)
+        if source_outage:
+          pending_host_can.append((now, incoming))
         safety.set_timer((now // 1000) & 0xFFFFFFFF)
         for addr, data, bus in incoming:
           safety.safety_rx_hook(libsafety_py.make_CANPacket(addr, bus, data))
         safety.safety_tick()
-        hard_stop_receipt = hard_stop_native_first and elapsed == 125
+        hard_stop_receipt = (hard_stop_native_first or host_first_stop) and elapsed == 125
         if hard_stop_receipt:
           # Native zero wheels and its USB poll precede the host's stopped CarState.
           receipt, hard_order = pandad.step(driven, published, now)
@@ -110,7 +117,7 @@ def test_actual_full_engage_brake_reverse_temporary_recovery_mainoff(candidate,
         if 'carControl' in latest_controls:
           peers.append(latest_controls['carControl'])
         peers.extend(driven_new[s] for s in ('onroadEvents', 'selfdriveState', 'aolAxisState') if s in driven_new and s in card.sm.data)
-        if receipt is not None and (tick % 10 == 1 or hard_stop_receipt):
+        if receipt is not None and (tick % 10 == 1 or hard_stop_receipt and not host_first_stop or host_first_stop and elapsed == 127):
           peers.append(receipt.as_reader())
         card.sm.update_msgs((now + 1000) / 1e9, peers)
         published = {}
@@ -123,25 +130,30 @@ def test_actual_full_engage_brake_reverse_temporary_recovery_mainoff(candidate,
           wire = msg if isinstance(msg, bytes) else msg.to_bytes()
           _published[service] = messaging.log_from_bytes(wire)
 
-        with (
-          patch.object(card.sm, 'update'),
-          patch('openpilot.selfdrive.car.card.messaging.drain_sock_raw', return_value=[b'can']),
-          patch('openpilot.selfdrive.car.card.can_capnp_to_list', return_value=[(now, incoming)]),
-          patch('openpilot.selfdrive.car.card.REPLAY', False),
-          patch('openpilot.selfdrive.car.card.time.monotonic_ns', return_value=now + 2000),
-          patch('openpilot.selfdrive.car.card.time.monotonic', return_value=(now + 2000) / 1e9),
-          patch('openpilot.selfdrive.car.card.time.clock_gettime_ns', return_value=now + 2000),
-          patch('openpilot.selfdrive.car.card.messaging.new_message', side_effect=stamped),
-          patch.object(card.pm, 'send', side_effect=capture),
-        ):
-          card.step()
+        if source_missing:
+          published = {key: value for key, value in last_card_published.items() if key != 'sendcan'}
+        else:
+          with (
+            patch.object(card.sm, 'update'),
+            patch('openpilot.selfdrive.car.card.messaging.drain_sock_raw', return_value=[b'can']),
+            patch('openpilot.selfdrive.car.card.can_capnp_to_list', return_value=list(pending_host_can) if source_outage else [(now, incoming)]),
+            patch('openpilot.selfdrive.car.card.REPLAY', False),
+            patch('openpilot.selfdrive.car.card.time.monotonic_ns', return_value=now + 2000),
+            patch('openpilot.selfdrive.car.card.time.monotonic', return_value=(now + 2000) / 1e9),
+            patch('openpilot.selfdrive.car.card.time.clock_gettime_ns', return_value=now + 2000),
+            patch('openpilot.selfdrive.car.card.messaging.new_message', side_effect=stamped),
+            patch.object(card.pm, 'send', side_effect=capture),
+          ):
+            card.step()
+          last_card_published = dict(published)
+          pending_host_can.clear()
         if tick % 10 == 0:
           published['pandaStates'] = panda.as_reader()
         cs = published['carState'].carState
         assert cs.brakePressed == brake and cs.gearShifter == (structs.CarState.GearShifter.reverse if reverse else structs.CarState.GearShifter.drive)
         if tick >= 10:
           assert cs.canValid and safety.safety_config_valid()
-        if hard_stop_receipt:
+        if hard_stop_receipt and not host_first_stop:
           _assert_hard_stop_fault_guards(card, cs, now + 2000)
         # Deliver each genuine10Hz native receipt once; do not refresh receive time/frequency from a cached snapshot.
         new_native = receipt if receipt is not None and (tick % 10 == 1 or hard_stop_receipt) else None
@@ -151,7 +163,8 @@ def test_actual_full_engage_brake_reverse_temporary_recovery_mainoff(candidate,
           with patch('openpilot.selfdrive.controls.controlsd.time.monotonic_ns', return_value=now + 2000):
             expired, _ = controls.state_control()
           assert not expired.latActive and not expired.longActive
-        driven_new = selfdrive_step(sd, published, new_native, previous_controls, cp, safety, now, calibration='calibrated')
+        driven_new = selfdrive_step(sd, published, new_native, previous_controls, cp, safety, now, calibration='calibrated',
+                                   source_timeout=source_missing)
         if deadline_gap and elapsed == gap_tick:
           current = driven_new['aolAxisState'].aolAxisState
           assert current.lateralActive and not sd.aol_authority_lost, 'SUBARU_FRESH_SUCCESSOR_LOST_LATERAL'
@@ -202,7 +215,8 @@ def test_actual_full_engage_brake_reverse_temporary_recovery_mainoff(candidate,
             request = bool(decoder.vl[steering_name]['LKAS_Request'])
             output = decoder.vl[steering_name]['LKAS_Output']
             decoded_steering.append({'request': request, 'output': output, 'nativeAccepted': accepted})
-            if not stopped and (65 <= tick < 90 or aol and (first_resume or 205 <= tick < 220 or 280 <= tick < 300)):
+            if not stopped and (65 <= tick < 90 and not (source_outage and 75 <= tick < 84) or
+                                aol and (first_resume or 205 <= tick < 220 or 280 <= tick < 300)):
               if fixed_angle_handoff and 72 <= tick < 78:
                 assert not request and accepted, (tick, decoded_steering)
               else:
@@ -241,11 +255,15 @@ def test_actual_full_engage_brake_reverse_temporary_recovery_mainoff(candidate,
         }
         rows.append(row)
         if stopped:
-          assert not row['lat'] and row['cardLatch'] and row['permission'] == 0 and row['fault'] == 'none', row
+          assert not row['lat'] and row['cardLatch'] and row['permission'] == 0 and row['fault'] == 'none', ('SUBARU_STOP_LOST_ARMED_INTENT', row)
+        if source_outage and 78 <= tick < 81:
+          assert not row['lat'] and not row['long'] and row['fault'] == 'availabilityPause', ('SUBARU_SOURCE_GAP_BECAME_CRITICAL', row)
+        if source_outage and 84 <= tick < 90:
+          assert row['lat'] and row['cardLatch'] and row['permission'] == 1 and not row['controls'], row
         assert not command.longActive, row
         if 10 <= tick < 30:
           assert not command.latActive and not bool(intent and intent.allowedLatch) and row['permission'] == 0, row
-        if 65 <= tick < 90:
+        if 65 <= tick < 90 and not (source_outage and 75 <= tick < 84):
           assert command.latActive and bool(intent and intent.allowedLatch) == aol and row['permission'] == int(aol), row
         if not stopped and (first_resume or 205 <= tick < 220 or 280 <= tick < 300):
           assert command.latActive == aol and bool(intent and intent.allowedLatch) == aol and row['permission'] == int(aol) and not row['controls'], row
@@ -388,3 +406,15 @@ def test_actual_subaru_expired_output_keeps_user_disable(event, subaru_native_pa
 def test_actual_fixed_angle_driver_handoff_keeps_native_rate_limit(candidate, subaru_native_pair, tmp_path, preferences=None):
   test_actual_full_engage_brake_reverse_temporary_recovery_mainoff(
     candidate, subaru_native_pair, tmp_path, preferences=preferences, fixed_angle_handoff=True)
+
+
+@pytest.mark.parametrize('candidate', [CAR.SUBARU_IMPREZA_2020, CAR.SUBARU_OUTBACK_2023, CAR.SUBARU_CROSSTREK_2025])
+def test_actual_host_first_stop_retains_subaru_intent(candidate, subaru_native_pair, tmp_path):
+  test_actual_full_engage_brake_reverse_temporary_recovery_mainoff(
+    candidate, subaru_native_pair, tmp_path, main_only=True, stop_resume=True, host_first_stop=True)
+
+
+@pytest.mark.parametrize('candidate', [CAR.SUBARU_IMPREZA_2020, CAR.SUBARU_OUTBACK_2023, CAR.SUBARU_CROSSTREK_2025])
+def test_actual_held_gas_source_outage_withdraws_and_recovers_subaru(candidate, subaru_native_pair, tmp_path):
+  test_actual_full_engage_brake_reverse_temporary_recovery_mainoff(
+    candidate, subaru_native_pair, tmp_path, main_only=True, source_outage=True)

prebuilt.json

@@ -1 +1 @@
-{"protected_inventory_sha256": "ea25a21a8aae0d7bb29b4ad9a4ac129a1a32c44066adc9211c1ef44ba4973748", "schema_version": 1}
+{"protected_inventory_sha256": "4a6d1ae614b9a07311e0e5f7565ea677ca19303cfc64d7557523499b56279f52", "schema_version": 1}