diff --git a/openpilot/selfdrive/selfdrived/selfdrived.py b/openpilot/selfdrive/selfdrived/selfdrived.py index 693689c9e8..1f217aedb1 100644 --- a/openpilot/selfdrive/selfdrived/selfdrived.py +++ b/openpilot/selfdrive/selfdrived/selfdrived.py @@ -1613,6 +1613,21 @@ class SelfdriveD: sample.vEgo < (self.aol_settings.pause_brake_mps if self.aol_settings is not None else 0.0) for sample, _ in getattr(self, 'car_state_samples', ()))) + def cold_gm_park_readiness(self, CS, intent, now_ns): + from openpilot.starpilot.car.gm.aol import cold_park_readiness + from openpilot.starpilot.aol.transport_pause import _healthy_transport_native + from openpilot.starpilot.aol.runtime import INTENT_MAX_AGE_NS + if (self.CP.brand != 'gm' or not self.initialized or self.enabled or self.active or getattr(self, 'aol_ever_active', False) or + self.aol_authority_lost or not getattr(self, 'conditional_car_state_valid', False) or + intent is None or not intent.settingsQualified or not intent.lateralArmed or intent.allowedLatch or + intent.pauseLateral or intent.pauseLongitudinal or + intent.carStateLogMonoTime != self.aol_car_state_log_ns or + not 0 < intent.carStateLogMonoTime <= intent.observedMonoTime <= now_ns <= intent.validUntilMonoTime or + now_ns - intent.carStateLogMonoTime > INTENT_MAX_AGE_NS): + return False + native = _healthy_transport_native(self.sm, self.CP, CS, self.aol_session_id, now_ns) + return cold_park_readiness(self.CP, CS, native, self.events.to_msg()) + def cold_gm_main_readiness(self, CS, intent, now_ns): from openpilot.starpilot.car.gm.aol import cold_main_readiness from openpilot.starpilot.aol.transport_pause import _healthy_transport_native @@ -1897,7 +1912,9 @@ class SelfdriveD: gear_pause=reverse_pause)) for event in self.events.to_msg()) cold_main_wait = self.cold_gm_main_readiness(CS, intent, now_ns) - self.aol_fault_reason = ('availabilityPause' if ordinary_availability or cold_main_wait else + cold_park_wait = self.cold_gm_park_readiness(CS, intent, now_ns) + self.aol_fault_reason = ('none' if cold_park_wait else + 'availabilityPause' if ordinary_availability or cold_main_wait else 'critical' if self.aol_authority_lost or self.events.contains(ET.IMMEDIATE_DISABLE) or aol_hard_soft_fault and not availability else 'availabilityPause' if availability else 'transportPause' if pause else 'none') @@ -1938,6 +1955,8 @@ class SelfdriveD: self.aol_fault_reason = 'none' if cold_main_wait: self.aol_axis_decision = AxisDecision() + if cold_park_wait: + self.aol_axis_decision = AxisDecision(native_acknowledged=True) if self.aol_ordinary_source_pause: self.aol_axis_decision = self.ordinary_source_withdrawal() elif availability: diff --git a/openpilot/starpilot/aol/tests/test_gm_parked_startup.py b/openpilot/starpilot/aol/tests/test_gm_parked_startup.py new file mode 100644 index 0000000000..96a3fee3df --- /dev/null +++ b/openpilot/starpilot/aol/tests/test_gm_parked_startup.py @@ -0,0 +1,270 @@ +"""Cold GM MAIN through actual parser/Card/SelfdriveD/Controls/native RX and TX. + +Model/readiness and receive transport are bench fixtures; ECU I/O is skipped. +Neither ordinary controls_allowed nor an active steering command is seeded. +""" + +import pytest +from contextlib import ExitStack +from unittest.mock import patch +from opendbc.car.gm.values import DBC +from opendbc.car import Bus, structs +from opendbc.can import CANPacker +from openpilot.starpilot.aol.tests.test_gm_startup_main import parsed_packets, cases +from opendbc.safety.tests.libsafety import libsafety_py as native +from openpilot.cereal import messaging +from openpilot.common.prefix import OpenpilotPrefix +from openpilot.starpilot.aol.tests import test_gm as gm, test_ioniq_availability as fixture +from openpilot.starpilot.aol.intent import read_settings +from openpilot.starpilot.aol.wire import SafetyState, PandaSlot, encode_safety +from openpilot.starpilot.lateral.tests.test_lane_runtime import feed + + +@pytest.mark.parametrize('index', range(8)) +@pytest.mark.parametrize('main_action', [0, 9]) +def test_cold_park_main_waits_without_losing_intent(index, main_action): + cp = list(cases())[index] + case = fixture.IoniqAvailabilityTests(methodName='runTest') + with OpenpilotPrefix(), ExitStack() as cleanup: + cleanup.callback(case.doCleanups) + sd, controls, step = case._authority_callers(False, cp=cp, genuine_events=True) + sd.params.put('MainCruiseButtonControl', main_action, block=True) + sd.aol_settings = read_settings(sd.params) + # The initial real Controls result is disabled because no authority wire exists. + # This boot-neutral fixture establishes no Panda grant or active output. + feed(controls, 10_000_000_000, 0, active=False, enabled=False) + boot, boot_log = controls.state_control() + assert not boot.latActive and not boot.longActive + controls.publish(boot, boot_log) + card = gm.TestGmAol.card(cp, sd.params) + cleanup.callback(card.vehicle_startup.close) + card.ci_initialized = True + card.car_params_published = True + card.sm.frame = 1 + assert card.CP.to_dict() == sd.CP.to_dict() == controls.CP.to_dict() + original_pm = card.pm + captured = {} + intent_queue = [] + companion_queue = [] + original_optional = messaging.recv_one_or_none + + def deliver(sock): + if sd.steering_companion is not None and sock is sd.steering_companion.sock: + return companion_queue.pop(0) if companion_queue else None + if sd.aol_intent_companion is not None and sock is sd.aol_intent_companion.sock: + return intent_queue.pop(0) if intent_queue else None + return original_optional(sock) + + cleanup.enter_context(patch('openpilot.selfdrive.selfdrived.selfdrived.messaging.recv_one_or_none', side_effect=deliver)) + + class Tee: + def send(self, service, message): + original_pm.send(service, message) + event = messaging.log_from_bytes(message) if isinstance(message, bytes) else message.as_reader() + captured[service] = event.as_builder() + if service == 'aolIntentWire': + intent_queue.append(event) + if service == 'starpilotCarState': + companion_queue.append(event) + + card.pm = Tee() + sd_receive = sd.sm.update_msgs + last_sd_control_stamp = 0 + + def receive_sd(stamp, rows): + nonlocal last_sd_control_stamp + # Deliver the original last published Controls envelope without renewing its source. + published = controls.pm.messages.get('carControl') + rows = [r for r in rows if r.which() != 'carControl'] + if published is not None and published.logMonoTime != last_sd_control_stamp: + rows.append(published) + last_sd_control_stamp = int(published.logMonoTime) + return sd_receive(stamp, rows) + + cleanup.enter_context(patch.object(sd.sm, 'update_msgs', side_effect=receive_sd)) + safety = native.libsafety + safety.init_tests() + safety.set_alternative_experience(cp.alternativeExperience) + assert safety.set_safety_hooks(4, cp.safetyConfigs[0].safetyParam) == 0 + card_receive = card.sm.update_msgs + # Establish actual neutral producer cadence before Card publishes carOutput. + card_receive = card.sm.update_msgs + card_receive(10.0, [controls.pm.messages['carControl']]) + boot_rows = [] + with patch.object(controls.sm, 'update_msgs', side_effect=lambda stamp, rows: boot_rows.extend(rows)): + feed(controls, 10_010_000_000, 1, active=False, enabled=False) + controls.sm.update_msgs(10.01, boot_rows) + clocked = messaging.new_message + with patch( + 'openpilot.selfdrive.controls.controlsd.messaging.new_message', + side_effect=lambda *args, **kwargs: clocked(*args, **dict(kwargs, logMonoTime=10_010_000_000)), + ): + neutral, neutral_log = controls.state_control() + assert not neutral.latActive and not neutral.longActive + controls.publish(neutral, neutral_log) + card_receive(10.01, [controls.pm.messages['carControl']]) + last_control_stamp = int(controls.pm.messages['carControl'].logMonoTime) + delivered = {} + + def wire_batch(now): + mask = safety.aol_get_permission_mask() + request = safety.aol_get_request_mask() + invalid = not safety.safety_config_valid() + relay = safety.get_relay_malfunction() + p = messaging.new_message('pandaStates', 1, valid=True, logMonoTime=now) + slot = p.pandaStates[0] + slot.safetyModel = 4 + slot.safetyParam = cp.safetyConfigs[0].safetyParam + slot.alternativeExperience = cp.alternativeExperience + slot.controlsAllowed = safety.get_controls_allowed() + slot.safetyRxChecksInvalid = invalid + slot.faults = ['relayMalfunction'] if relay else [] + w = messaging.new_message('aolSafetyWire', 0, valid=True, logMonoTime=now) + w.aolSafetyWire = encode_safety( + SafetyState( + 1, + True, + now, + now + 200_000_000, + 4, + cp.safetyConfigs[0].safetyParam, + bool(mask & 1), + bool(mask & 2), + bool(request & 1), + bool(request & 2), + 'authority-panda', + sd.aol_session_id, + now, + ( + PandaSlot( + 0, + 'authority-panda', + 4, + cp.safetyConfigs[0].safetyParam, + cp.alternativeExperience, + safety.get_controls_allowed(), + invalid, + False, + 1 if relay else 0, + ), + ), + ) + ) + return p.as_reader(), w.as_reader() + + def receive_card(now): + nonlocal last_control_stamp + rows = list(wire_batch(now)) + rows += [sd.pm.messages[k] for k in ('aolAxisState', 'selfdriveState', 'onroadEvents') if k in sd.pm.messages] + published = controls.pm.messages['carControl'] + if published.logMonoTime != last_control_stamp: + rows.append(published) + last_control_stamp = int(published.logMonoTime) + unique = [] + for event in rows: + name = event.which() + key = (int(event.logMonoTime), event.as_builder().to_bytes()) + if delivered.get(name) != key: + unique.append(event) + delivered[name] = key + card_receive(now / 1e9, unique) + + packer = CANPacker(DBC[cp.carFingerprint][Bus.pt]) + + def physical(tick, stamp, driving): + packets = parsed_packets(cp, packer, stamp, tick, max(4.0, cp.minSteerSpeed + 2.0, cp.minEnableSpeed + 2.0) if driving else 0.0, True) + packets = [f for f in packets if f[0] not in (0x1F5, 0x12A, 0x184, 0x17D)] + if cp.pcmCruise and cp.networkLocation == structs.CarParams.NetworkLocation.fwdCamera: + address = packer.make_can_msg('ASCMActiveCruiseControlStatus', 2, {})[0] + packets = [f for f in packets if not (f[0] == address and f[2] == 2)] + # Factory camera reports adaptive standby, not non-adaptive cruise mode. + packets.append(packer.make_can_msg('ASCMActiveCruiseControlStatus', 2, {'ACCCruiseState': 2})) + packets += [ + packer.make_can_msg('ESPStatus', 0, {'TractionControlOn': 1}), + packer.make_can_msg('ECMPRDNL2', 0, {'PRNDL2': 4 if driving else 1}), + packer.make_can_msg('BCMDoorBeltStatus', 0, {'LeftSeatBelt': int(driving)}), + packer.make_can_msg('PSCMStatus', 0, {'LKATorqueDeliveredStatus': int(driving)}), + ] + return packets + + for warm in range(100): + stamp = 9_000_000_000 + warm * 10_000_000 + safety.set_timer(stamp // 1000) + safety.set_aol_test_heartbeat(True) + safety.aol_set_host_request(0) + packets = physical(warm, stamp, False) + for a, data, bus in packets: + safety.safety_fwd_hook(bus, a) + assert safety.safety_rx_hook(native.make_CANPacket(a, bus, data)) + card.CI.update([(stamp, packets)]) + safety.safety_tick() + assert safety.safety_config_valid() and safety.aol_get_permission_mask() == 0 + tx = 0 + active = 0 + for tick in range(1, 381): + now = 10_020_000_000 + (tick - 1) * 10_000_000 + driving = tick > 300 + safety.set_timer(now // 1000) + safety.set_aol_test_heartbeat(True) + packets = physical(tick + 100, now, driving) + for a, data, bus in packets: + safety.safety_fwd_hook(bus, a) + assert safety.safety_rx_hook(native.make_CANPacket(a, bus, data)) + cs = card.CI.update([(now, packets)]) + safety.safety_tick() + assert cs.canValid and not cs.canTimeout + card_receive(now / 1e9, []) + fault = card.aol_disarming_fault(cs, int(card.sm.logMonoTime['onroadEvents']), now) + card.aol_card_intent.update(cs, fault_active=fault, now_ns=now) + card.observe_aol_calibration(cs, now, False) + allowed = card.aol_card_intent.output(cs)[0] + safety.aol_set_host_request(int(allowed)) + mask = safety.aol_get_permission_mask() + message = messaging.new_message + with patch( + 'openpilot.selfdrive.car.card.messaging.new_message', + side_effect=lambda *args, _clock=message, _now=now, **kwargs: _clock(*args, **dict(kwargs, logMonoTime=_now)), + ): + card.state_publish(cs, None) + axis, _, cc = step( + tick, + state_stamp=now, + offset_ns=now - 10_000_000_000 - tick * 10_000_000, + car_state_message=captured['carState'], + intent_message=captured['aolIntentWire'], + car_output_message=captured['carOutput'], + car_output_valid=captured['carOutput'].valid, + allowed=bool(mask & 1), + long_allowed=bool(mask & 2), + native_request_lat=allowed, + native_request_long=False, + panda_controls_allowed=safety.get_controls_allowed(), + authority_message=captured.get('starpilotCarState'), + ) + receive_card(now) + assert not cc.enabled and not cc.longActive and not safety.get_controls_allowed() + if not driving: + assert str(cs.gearShifter) == 'park' and cs.seatbeltUnlatched and cs.standstill + assert not axis.lateralActive and not cc.latActive and safety.aol_get_request_mask() == 0 and mask == 0 + assert str(axis.faultReason) != 'critical', (tick, axis.to_dict(), sd.events.names) + assert not card.aol_disarming_fault(cs, int(card.sm.logMonoTime['onroadEvents']), now) + assert card.aol_card_intent.allowed_latch and not card.aol_card_intent._main_cycle_required + command = controls.pm.messages['carControl'] + captured.pop('sendcan', None) + with ( + patch('openpilot.selfdrive.car.card.time.monotonic_ns', return_value=now + 2), + patch('openpilot.selfdrive.car.card.time.monotonic', return_value=(now + 2) / 1e9), + patch('openpilot.selfdrive.car.card.time.clock_gettime_ns', return_value=now + 2), + ): + card.controls_update(cs, command.carControl) + emitted = captured['sendcan'] + assert emitted.valid + for frame in emitted.sendcan: + assert safety.safety_tx_hook(native.make_CANPacket(frame.address, frame.src, frame.dat)) + tx += 1 + if frame.address == 0x180: + if not driving: + assert not frame.dat[0] & 8 + else: + active += bool(frame.dat[0] & 8) + assert active > 0 and tx > 0 and card.aol_card_intent.allowed_latch diff --git a/openpilot/starpilot/aol/tests/test_gm_parked_startup_guards.py b/openpilot/starpilot/aol/tests/test_gm_parked_startup_guards.py new file mode 100644 index 0000000000..0e9b5f48ae --- /dev/null +++ b/openpilot/starpilot/aol/tests/test_gm_parked_startup_guards.py @@ -0,0 +1,197 @@ +"""Parked intent exemptions require fresh source, idle native axes and known events.""" + +from dataclasses import replace + +import pytest +from opendbc.car.structs import car +from opendbc.car.gm.tests.test_volt_sdgm_control import sdgm_params +from openpilot.cereal import messaging +from openpilot.selfdrive.selfdrived.selfdrived import SelfdriveD +from openpilot.selfdrive.selfdrived.events import Events, EventName +from openpilot.starpilot.aol.transport_pause import TransportPauseFeedback +from openpilot.starpilot.aol.vehicle import policy_for +from openpilot.starpilot.aol.wire import IntentState, PandaSlot, SafetyState, encode_safety + +NOW = 10_000_000_000 + + +def setup(): + cp = sdgm_params(alpha=False) + policy = policy_for(cp) + cp.safetyConfigs[-1].safetyParam |= policy.safety_param_addition + cp.alternativeExperience |= policy.alternative_experience_addition + cs = car.CarState(canValid=True, gearShifter='park', standstill=True, vEgo=0.0, seatbeltUnlatched=True) + cs.cruiseState.available = True + intent = IntentState('producer', 1, NOW, NOW, NOW + 200_000_000, False, False, False, True, True) + sm = messaging.SubMaster(['aolSafetyWire', 'pandaStates', 'aolAxisState', 'onroadEvents']) + events = Events() + events.add(EventName.wrongGear) + events.add(EventName.seatbeltNotLatched) + slot = PandaSlot(0, 'panda', 4, cp.safetyConfigs[0].safetyParam, cp.alternativeExperience) + native = SafetyState(1, True, NOW, NOW + 200_000_000, 4, cp.safetyConfigs[0].safetyParam, False, False, False, False, 'panda', 'session', NOW, (slot,)) + + def publish(native=native): + wire = messaging.new_message('aolSafetyWire', 0, valid=True, logMonoTime=NOW) + wire.aolSafetyWire = encode_safety(native) + panda = messaging.new_message('pandaStates', 1, valid=True, logMonoTime=NOW) + p = panda.pandaStates[0] + p.safetyModel, p.safetyParam, p.alternativeExperience = 4, cp.safetyConfigs[0].safetyParam, cp.alternativeExperience + event = messaging.new_message('onroadEvents', 0, valid=True, logMonoTime=NOW) + event.onroadEvents = events.to_msg() + sm.update_msgs(NOW / 1e9 + (sm.frame + 1) * 0.000001, [wire.as_reader(), panda.as_reader(), event.as_reader()]) + + publish() + sd = SelfdriveD.__new__(SelfdriveD) + sd.CP, sd.sm, sd.events = cp, sm, events + sd.enabled = sd.active = sd.aol_ever_active = sd.aol_authority_lost = False + sd.initialized = sd.conditional_car_state_valid = True + sd.aol_car_state_log_ns, sd.aol_session_id = NOW, 'session' + return sd, cs, intent, native, publish + + +@pytest.mark.parametrize( + 'field,value', + [ + ('settingsQualified', False), + ('lateralArmed', False), + ('allowedLatch', True), + ('pauseLateral', True), + ('pauseLongitudinal', True), + ('carStateLogMonoTime', NOW - 1), + ('observedMonoTime', NOW + 1), + ('validUntilMonoTime', NOW - 1), + ], +) +def test_source_intent_must_match_current_park(field, value): + sd, cs, intent, _, _ = setup() + assert sd.cold_gm_park_readiness(cs, intent, NOW) + assert not sd.cold_gm_park_readiness(cs, replace(intent, **{field: value}), NOW) + + +@pytest.mark.parametrize( + 'field,value', + [ + ('initialized', False), + ('enabled', True), + ('active', True), + ('aol_ever_active', True), + ('aol_authority_lost', True), + ('conditional_car_state_valid', False), + ], +) +def test_only_cold_inactive_source_can_wait(field, value): + sd, cs, intent, _, _ = setup() + assert sd.cold_gm_park_readiness(cs, intent, NOW) + setattr(sd, field, value) + assert not sd.cold_gm_park_readiness(cs, intent, NOW) + + +@pytest.mark.parametrize( + 'field,value', + [ + ('gearShifter', 'unknown'), + ('gearShifter', 'drive'), + ('gearShifter', 'reverse'), + ('standstill', False), + ('vEgo', 0.31), + ('vEgo', float('nan')), + ('canValid', False), + ('canTimeout', True), + ('steerFaultTemporary', True), + ('steerFaultPermanent', True), + ('accFaulted', True), + ('doorOpen', True), + ('gasPressed', True), + ('steeringDisengage', True), + ], +) +def test_vehicle_faults_cannot_be_classified_as_park_readiness(field, value): + sd, cs, intent, _, _ = setup() + assert sd.cold_gm_park_readiness(cs, intent, NOW) + setattr(cs, field, value) + assert not sd.cold_gm_park_readiness(cs, intent, NOW) + + +@pytest.mark.parametrize('field', ['requestedLateral', 'requestedLongitudinal', 'lateralAllowed', 'longitudinalAllowed']) +def test_idle_ack_never_accepts_a_native_axis(field): + sd, cs, intent, native, publish = setup() + publish(replace(native, **{field: True})) + assert not sd.cold_gm_park_readiness(cs, intent, NOW) + + +@pytest.mark.parametrize( + 'event', + [ + EventName.commIssue, + EventName.commIssueAvgFreq, + EventName.locationdTemporaryError, + EventName.controlsMismatch, + EventName.processNotRunning, + EventName.selfdrivedLagging, + ], +) +def test_only_known_startup_host_events_can_wait(event): + sd, cs, intent, _, _ = setup() + sd.events.add(event) + assert sd.cold_gm_park_readiness(cs, intent, NOW) == (event in (EventName.commIssue, EventName.commIssueAvgFreq, EventName.locationdTemporaryError)) + + +@pytest.mark.parametrize('denial', [None, 'producer', 'epoch', 'source', 'expired', 'events', 'native', 'failed', 'active']) +def test_card_requires_same_identity_fresh_idle_proof(denial): + sd, cs, _, native, publish = setup() + sm = sd.sm + feedback = TransportPauseFeedback(sd.CP) + + def axis(sequence=1): + event = messaging.new_message('aolAxisState', valid=True, logMonoTime=NOW) + a = event.aolAxisState + a.qualified = a.nativeAcknowledged = True + a.sessionId = a.faultSessionId = 'session' + a.intentProducerSessionId = 'producer' + a.sequence, a.observedMonoTime, a.validUntilMonoTime = sequence, NOW, NOW + 30_000_000 + a.sourceCarStateMonoTime = a.faultEventMonoTime = NOW + a.faultReason = 'none' + return event + + a = axis() + sm.update_msgs(NOW / 1e9 + (sm.frame + 1) * 0.000001, [a.as_reader()]) + assert not feedback.observe(sm, sd.CP, cs, NOW, latched=True, intent_producer_session='producer') + assert feedback.temporary_host_qualified + a = axis(2) + if denial == 'producer': + a.aolAxisState.intentProducerSessionId = 'other' + if denial == 'epoch': + a.aolAxisState.intentIntegrityEpoch = 1 + if denial == 'source': + a.aolAxisState.sourceCarStateMonoTime = NOW - 30_000_001 + if denial == 'expired': + a.aolAxisState.validUntilMonoTime = NOW - 1 + if denial == 'native': + publish(replace(native, requestedLateral=True, lateralAllowed=True)) + if denial == 'failed': + feedback.failed = True + if denial == 'active': + feedback.ever_lateral_active = True + sm.update_msgs(NOW / 1e9 + (sm.frame + 1) * 0.000001, [a.as_reader()]) + if denial == 'events': + sm.valid['onroadEvents'] = False + feedback.observe(sm, sd.CP, cs, NOW, latched=True, intent_producer_session='producer') + assert feedback.temporary_host_qualified == (denial is None) + + +@pytest.mark.parametrize( + 'name,soft,expected', + [ + ('wrongGear', True, True), + ('wrongGear', False, False), + ('seatbeltNotLatched', True, False), + ('buttonCancel', False, False), + ], +) +def test_only_known_park_gear_user_disable_can_keep_idle_intent(name, soft, expected): + from openpilot.starpilot.car.gm.aol import cold_park_readiness + + sd, cs, _, native, _ = setup() + event = messaging.new_message('onroadEvents', 1).onroadEvents[0] + event.name, event.userDisable, event.softDisable, event.noEntry = name, True, soft, True + assert cold_park_readiness(sd.CP, cs, native, [event]) == expected diff --git a/openpilot/starpilot/aol/transport_pause.py b/openpilot/starpilot/aol/transport_pause.py index b66975dc82..e1766fc2e0 100644 --- a/openpilot/starpilot/aol/transport_pause.py +++ b/openpilot/starpilot/aol/transport_pause.py @@ -263,6 +263,7 @@ class TransportPauseFeedback: self.ever_lateral_active = False self.lag_withdrawal_ns = 0 self.cold_main_wait_ns = 0 + self.cold_park_waiting = False self.lateral_only_baseline = False self.lateral_source_ns = 0 self.lateral_inventory = None @@ -962,6 +963,7 @@ class TransportPauseFeedback: axis = sm['aolAxisState'] if not latched or cp_identity(cp) != self.cp or self.session and str(axis.sessionId) != self.session: self.temporary_eps_event_ns = 0 + self.cold_park_waiting = False stamp = int(sm.logMonoTime['aolAxisState']) reason = str(axis.faultReason) signature = (str(axis.sessionId), int(axis.sequence), stamp, int(axis.observedMonoTime), @@ -976,10 +978,28 @@ class TransportPauseFeedback: cp_identity(cp) == self.cp) continuity = bool(not self.session or axis.sessionId == self.session and (axis.sequence > self.sequence or axis.sequence == self.sequence and signature == self.signature)) - if (self.cold_main_wait_ns and current and continuity and self.signature is not None and + if ((self.cold_main_wait_ns or self.cold_park_waiting) and current and continuity and self.signature is not None and signature[14:16] != self.signature[14:16]): self.failed = True return True + if (cp.brand == 'gm' and current and continuity and latched and not self.failed and + not self.ever_lateral_active and not self.pending_event and reason == 'none' and + axis.nativeAcknowledged and not any(signature[9:13]) and + intent_producer_session and signature[14] == intent_producer_session and + (self.signature is None or signature[14:16] == self.signature[14:16]) and + 0 < axis.sourceCarStateMonoTime <= stamp and now_ns - axis.sourceCarStateMonoTime <= AXIS_MAX_AGE_NS): + from openpilot.starpilot.car.gm.aol import cold_park_readiness + native = _healthy_transport_native(sm, cp, cs, str(axis.sessionId), now_ns) + event_ns = int(sm.logMonoTime['onroadEvents']) + if (axis.faultEventMonoTime == event_ns and sm.seen['onroadEvents'] and + sm.valid['onroadEvents'] and sm.alive['onroadEvents'] and + 0 < event_ns <= stamp and now_ns - event_ns <= 1_500_000_000 and + cold_park_readiness(cp, cs, native, sm['onroadEvents'])): + self.cold_park_waiting = True + self.session = str(axis.sessionId) + self.sequence, self.signature = int(axis.sequence), signature + self.temporary_host_qualified = True + return False if cp.brand == 'ford' and latched: from openpilot.starpilot.car.ford.command_pause import retain_command_pause_intent if retain_command_pause_intent(self, sm, cp, cs, axis, now_ns, latched=latched, producer=intent_producer_session): @@ -1138,6 +1158,7 @@ class TransportPauseFeedback: return False current = bool(current and (transport_healthy or cancel_native is not None)) if current and baseline and not latched: + self.cold_park_waiting = False self.temporary_eps_event_ns = 0 self.session = '' self.pending_event = 0 @@ -1235,6 +1256,7 @@ class TransportPauseFeedback: self.ever_lateral_active = True self.lag_withdrawal_ns = 0 self.cold_main_wait_ns = 0 + self.cold_park_waiting = False self.temporary_eps_event_ns = 0 self.lateral_native_ns = int(native.observedMonoTime) self.lateral_inventory = (native.pandaSerial, tuple(p.hardwareSerial for p in native.pandaInventory)) diff --git a/openpilot/starpilot/car/gm/aol.py b/openpilot/starpilot/car/gm/aol.py index 39fea230c7..13a39dec6d 100644 --- a/openpilot/starpilot/car/gm/aol.py +++ b/openpilot/starpilot/car/gm/aol.py @@ -129,6 +129,30 @@ def create_intent(cp, settings): return GmAolCardIntent(settings, cp) if qualified_gm(cp) else None +def cold_park_readiness(cp, cs, native, events): + """Retain cold MAIN intent while parked; acknowledge only idle native axes.""" + from opendbc.car.structs import car + from openpilot.cereal import log + names = (log.OnroadEvent.EventName.wrongGear, log.OnroadEvent.EventName.seatbeltNotLatched, + log.OnroadEvent.EventName.commIssue, log.OnroadEvent.EventName.commIssueAvgFreq, + log.OnroadEvent.EventName.locationdTemporaryError) + return bool(qualified_gm(cp) and aol_enabled(cp) and + cs.gearShifter == car.CarState.GearShifter.park and cs.standstill and + math.isfinite(cs.vEgo) and abs(cs.vEgo) <= .3 and + cs.canValid and not cs.canTimeout and cs.cruiseState.available and not cs.cruiseState.enabled and + not cs.steerFaultTemporary and not cs.steerFaultPermanent and not cs.accFaulted and + not cs.doorOpen and not cs.gasPressed and not cs.steeringDisengage and + native is not None and native.pandaInventory and + not native.requestedLateral and not native.lateralAllowed and + not native.requestedLongitudinal and not native.longitudinalAllowed and + not any(event.immediateDisable or event.softDisable and event.name not in names or + event.userDisable and not ( + event.name == log.OnroadEvent.EventName.wrongGear and event.noEntry and event.softDisable or + event.name == log.OnroadEvent.EventName.pedalPressed and (cs.brakePressed or cs.regenBraking) or + event.name == log.OnroadEvent.EventName.pcmDisable and not cs.cruiseState.enabled) + for event in events)) + + def cold_main_readiness(cp, cs, native, events, *, recovering=False): """A MAIN request may wait for host startup without acquiring either axis.""" from openpilot.cereal import log diff --git a/prebuilt.json b/prebuilt.json index 3fc091160a..892bf196a1 100644 --- a/prebuilt.json +++ b/prebuilt.json @@ -1 +1 @@ -{"protected_inventory_sha256": "d72c9d55978b75baeffe7829ccbbba98ce816f4242fde163e5b97de3d1db4c32", "schema_version": 1} +{"protected_inventory_sha256": "019fb2d9b4d111e8747993e41674e537a5dbcfec69a9bddaf089526d110267a3", "schema_version": 1}