diff --git a/openpilot/starpilot/aol/source_availability.py b/openpilot/starpilot/aol/source_availability.py index 51253ec2ac..7722461661 100644 --- a/openpilot/starpilot/aol/source_availability.py +++ b/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 diff --git a/openpilot/starpilot/aol/transport_pause.py b/openpilot/starpilot/aol/transport_pause.py index ecbe9f226b..a2c247cdae 100644 --- a/openpilot/starpilot/aol/transport_pause.py +++ b/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 diff --git a/openpilot/starpilot/car/subaru/tests/selfdrive_step.py b/openpilot/starpilot/car/subaru/tests/selfdrive_step.py index c31ebecabf..dc3190d9b3 100644 --- a/openpilot/starpilot/car/subaru/tests/selfdrive_step.py +++ b/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), ): diff --git a/openpilot/starpilot/car/subaru/tests/test_stock_aol_callers.py b/openpilot/starpilot/car/subaru/tests/test_stock_aol_callers.py index 48d4cd9469..7171ac25e3 100644 --- a/openpilot/starpilot/car/subaru/tests/test_stock_aol_callers.py +++ b/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) diff --git a/prebuilt.json b/prebuilt.json index 1261a78ef4..b164ee6947 100644 --- a/prebuilt.json +++ b/prebuilt.json @@ -1 +1 @@ -{"protected_inventory_sha256": "ea25a21a8aae0d7bb29b4ad9a4ac129a1a32c44066adc9211c1ef44ba4973748", "schema_version": 1} +{"protected_inventory_sha256": "4a6d1ae614b9a07311e0e5f7565ea677ca19303cfc64d7557523499b56279f52", "schema_version": 1}