Fix SDGM Volt Alpha lateral startup and withdrawal

openpilot/selfdrive/selfdrived/selfdrived.py

@@ -283,7 +283,9 @@ class SelfdriveD:
     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)
-    pair = companion.current(self.sm, source_ns=source_ns, now_ns=now_ns, drain=False) if drained else None
+    from openpilot.starpilot.car.gm.aol import sdgm_alpha_lateral_owner
+    pair = companion.current(self.sm, source_ns=source_ns, now_ns=now_ns, drain=False,
+                             allow_raw_projection=sdgm_alpha_lateral_owner(self.CP)) if drained else None
     if not source_current:
       pair = None
     previous_axes = self.aol_axis_decision

openpilot/starpilot/aol/tests/test_gm_held_native.py

@@ -847,3 +847,93 @@ def test_rebuckled_belt_retention_requires_prior_paired_lat_only_proof(fault):
       assert decision.lateral_active and not decision.desired_longitudinal and not decision.longitudinal_active
     else:
       assert reset and feedback.failed and not card.aol_card_intent.allowed_latch
+
+
+def _sdgm_pedal_state(card, stamp, tick, gesture=None, manual=False):
+  from opendbc.car.gm.tests.test_volt_camera_control import feed_camera
+
+  packer = CANPacker(DBC[card.CP.carFingerprint][Bus.pt])
+  empty = SimpleNamespace(update=lambda _: None)
+  _, frames = feed_camera(empty, packer, stamp, counter=tick % 4, speed=16.0, brake=gesture == 'brake', regen=gesture == 'paddle', active=True, eps_status=1)
+  # Same recorded forward manual-mode report during Mary paddle withdrawal.
+  frames = [f for f in frames if f[0] not in (0x1F5, 0xBE, 0xC9)]
+  frames += [
+    packer.make_can_msg('ECMAcceleratorPos', 0, {'BrakePedalPos': 8 if gesture == 'brake' else 0}),
+    packer.make_can_msg('ECMEngineStatus', 0, {'CruiseMainOn': 1, 'BrakePressed': int(gesture == 'brake')}),
+  ]
+  frames.append(packer.make_can_msg('ECMPRDNL2', 0, {'PRNDL2': 4, 'ManualMode': int(manual)}))
+  if tick == 0:
+    card.CI.update([(stamp - 1_000_000, frames)])
+  cs = card.CI.update([(stamp, frames)])
+  assert cs.canValid and not cs.canTimeout
+  assert cs.regenBraking == (gesture == 'paddle')
+  assert cs.brakePressed == (gesture == 'brake')
+  assert str(cs.gearShifter) == ('manumatic' if manual else 'drive')
+  return cs
+
+
+@pytest.mark.parametrize('c9', (False, True))
+@pytest.mark.parametrize('gesture', ('brake', 'paddle'))
+@pytest.mark.parametrize('event_first', (False, True))
+@pytest.mark.parametrize('health_allowed', (False, True))
+def test_sdgm_alpha_pedal_does_not_erase_armed_lateral(c9, gesture, event_first, health_allowed):
+  from opendbc.car.gm.tests.test_volt_sdgm_control import sdgm_params
+  from openpilot.starpilot.aol.tests.test_gm import TestGmAol as GmFixture
+  from openpilot.starpilot.aol.wire import IntentState
+
+  with OpenpilotPrefix():
+    saved = Params()
+    saved.put_bool('AlwaysOnLateral', True, block=True)
+    card = GmFixture.card(sdgm_params(alpha=True, brake_c9=c9), saved)
+    try:
+      cp = card.CP
+      feedback = TransportPauseFeedback(cp)
+      cs = _sdgm_pedal_state(card, NOW, 0)
+      card.aol_card_intent.update(cs, fault_active=False, now_ns=NOW)
+      assert card.aol_card_intent.allowed_latch
+      sm, prior = peers(cp, card.slc_producer_session, NOW)
+      assert not feedback.observe(sm, cp, cs, NOW, latched=True, intent_producer_session=card.slc_producer_session)
+      now = NOW + 10_000_000
+      cs = _sdgm_pedal_state(card, now, 1, gesture, True)
+      native = replace(
+        prior,
+        observedMonoTime=now,
+        validUntilMonoTime=now + 200_000_000,
+        longitudinalAllowed=False,
+        pandaInventory=(replace(prior.pandaInventory[0], controlsAllowed=health_allowed),),
+      )
+      sm, _ = peers(cp, card.slc_producer_session, now, controls=health_allowed, native=native, sequence=2, long_active=False)
+      a = sm.messages['aolAxisState'].as_builder()
+      a.aolAxisState.faultEventMonoTime = NOW if event_first else now
+      # Event first: the last full desired request may precede the ordinary disengagement result.
+      a.aolAxisState.desiredLongitudinal = event_first
+      e = sm.messages['onroadEvents'].as_builder()
+      e.onroadEvents = [{'name': 'pedalPressed', 'userDisable': True, 'noEntry': True}]
+      sm = Peers([sm.messages['aolSafetyWire'], sm.messages['pandaStates'], a.as_reader(), e.as_reader()])
+      fault = feedback.observe(sm, cp, cs, now, latched=True, intent_producer_session=card.slc_producer_session)
+      card.aol_card_intent.update(cs, fault_active=fault, now_ns=now)
+      assert not fault and card.aol_card_intent.allowed_latch, (gesture, event_first, health_allowed, feedback.__dict__)
+      intent = IntentState(card.slc_producer_session, 2, now, now, now + 200_000_000, True, False, False, True, True)
+      d = decide_axes(
+        standard_lateral=False,
+        standard_longitudinal=False,
+        intent=intent,
+        native=native,
+        car_state=cs,
+        initialized=True,
+        model_ready=True,
+        no_entry=False,
+        immediate_disable=False,
+        dm_lockout=False,
+        pause_brake_mps=0.0,
+      )
+      assert d.lateral_active and not d.longitudinal_active
+      # Fresh independent LAT native acknowledgment after release; LONG stays disabled.
+      later = now + 10_000_000
+      cs = _sdgm_pedal_state(card, later, 2)
+      native = replace(native, observedMonoTime=later, validUntilMonoTime=later + 200_000_000, requestedLongitudinal=False)
+      sm, _ = peers(cp, card.slc_producer_session, later, controls=False, native=native, sequence=3, long_active=False)
+      assert not feedback.observe(sm, cp, cs, later, latched=True, intent_producer_session=card.slc_producer_session)
+      assert card.aol_card_intent.allowed_latch and not native.longitudinalAllowed
+    finally:
+      card.vehicle_startup.close()

openpilot/starpilot/aol/tests/test_gm_main_response.py

@@ -21,17 +21,20 @@ from openpilot.starpilot.aol.wire import PandaSlot, SafetyState, IntentState
 NOW = 10_000_000_000
 
 
+@pytest.mark.parametrize('sdgm_acknowledged', (False, True))
 @pytest.mark.parametrize('variation', [
   'delayed_idle', 'new_source', 'future_source', 'expired', 'producer', 'epoch', 'session',
   'invalid', 'native_denied', 'native_config', 'critical', 'cancel', 'settings_off',
 ])
-def test_actual_card_main_rearm_waits_only_for_older_idle_response(variation):
+def test_actual_card_main_rearm_waits_only_for_older_idle_response(variation, sdgm_acknowledged):
   with OpenpilotPrefix(), patch.dict(os.environ, {'SIMULATION': '1', 'AOL_REPLAY_RUNTIME': '0'}):
     saved = Params()
     saved.put_bool('AlwaysOnLateral', True, block=True)
-    cp = ascm_params(CAR.CHEVROLET_VOLT_ASCM, accelerator=False, radar=False, alpha=False)
+    from opendbc.car.gm.tests.test_volt_sdgm_control import sdgm_params
+    cp = sdgm_params(alpha=True) if sdgm_acknowledged else ascm_params(
+      CAR.CHEVROLET_VOLT_ASCM, accelerator=False, radar=False, alpha=False)
     card = gm_fixtures.TestGmAol.card(cp, saved)
-    assert card.CP.safetyConfigs[0].safetyParam == 0x605
+    assert card.CP.safetyConfigs[0].safetyParam == (0x5007 if sdgm_acknowledged else 0x605)
     assert card.CP.alternativeExperience == 32
     packer = CANPacker(DBC[cp.carFingerprint][Bus.pt])
     card.ci_initialized = True
@@ -91,7 +94,8 @@ def test_actual_card_main_rearm_waits_only_for_older_idle_response(variation):
       packets = parsed_packets(cp, packer, now, tick, 8., main)
       if mutate and variation == 'cancel':
         packets = [p for p in packets if p[0] != 0x1E1]
-        packets.append(packer.make_can_msg('ASCMSteeringButton', 0, {'ACCButtons': CruiseButtons.CANCEL}))
+        from opendbc.car.gm.bolt_cc import button_bytes
+        packets.append((0x1E1, button_bytes(CruiseButtons.CANCEL, tick % 4), 0))
       batch = messaging.log_from_bytes(module.can_list_to_can_capnp(packets)).as_builder()
       batch.logMonoTime = now
       with patch.object(module, 'REPLAY', False), \
@@ -102,6 +106,8 @@ def test_actual_card_main_rearm_waits_only_for_older_idle_response(variation):
         card.step()
       assert card.CS_prev.canValid and not card.CS_prev.canTimeout
       assert card.CS_prev.cruiseState.available == main
+      if mutate and variation == 'cancel':
+        assert any(str(event.type) == 'cancel' and event.pressed for event in card.CS_prev.buttonEvents)
       assert not card.last_actuators_output.torqueOutputCan
       return now
 
@@ -116,13 +122,17 @@ def test_actual_card_main_rearm_waits_only_for_older_idle_response(variation):
       assert card.aol_card_intent.allowed_latch
       edge = card.aol_card_intent.main_arm_ns
       healthy = card.aol_transport_feedback.healthy_feedback_ns
-      cycle(5, ack=False, source=off_source, mutate=True)
-      if variation != 'delayed_idle':
+      cycle(5, ack=sdgm_acknowledged, source=off_source, mutate=True)
+      if variation not in ('delayed_idle', 'new_source', 'cancel'):
         assert not card.aol_card_intent.allowed_latch
         return
       assert card.aol_card_intent.allowed_latch
       assert card.aol_card_intent.main_arm_ns == edge
-      assert card.aol_transport_feedback.healthy_feedback_ns == healthy
+      if variation == 'delayed_idle':
+        assert card.aol_transport_feedback.healthy_feedback_ns == healthy
+      else:
+        # A fresh negotiated OFF receipt may retain intent; it grants neither axis.
+        assert card.aol_transport_feedback.healthy_feedback_ns >= healthy
       # Intent retention cannot grant either axis against the unchanged idle native status.
       cs = card.CS_prev
       intent = IntentState(card.slc_producer_session, 1, NOW, NOW, NOW + 30_000_000,

openpilot/starpilot/aol/tests/test_gm_steering_receive_order.py

@@ -16,7 +16,7 @@ from openpilot.starpilot.car.gm.steering_companion import SteeringCompanion
 @pytest.mark.parametrize('c9', [False, True])
 @pytest.mark.parametrize('full_op', [False, True])
 @pytest.mark.parametrize('condition', ['healthy', 'sm_fault', 'sm_invalid', 'sm_wrong_source',
-                                     'sm_expired', 'sm_future', 'missing_exact', 'raw_fault', 'raw_invalid'])
+                                     'sm_expired', 'sm_expired_fault', 'sm_expired_invalid', 'sm_future', 'missing_exact', 'raw_fault', 'raw_invalid'])
 def test_exact_card_raw_companion_survives_older_cached_projection(c9, full_op, condition):
   from openpilot.selfdrive.car.card import Car
   from openpilot.starpilot.aol.tests.test_gm_source_deadline import bind_steering_receipts
@@ -48,7 +48,7 @@ def test_exact_card_raw_companion_survives_older_cached_projection(c9, full_op,
     card.v_cruise_helper = VCruiseHelper(card.CP)
     source = 10_010_000_000
     now = source + 8_000_000
-    older = source - (40_000_000 if condition == 'sm_expired' else 10_000_000)
+    older = source - (40_000_000 if condition.startswith('sm_expired') else 10_000_000)
     original_message = messaging.new_message
 
     def publish(stamp, *, fault=False, valid=True, enabled=True):
@@ -60,7 +60,7 @@ def test_exact_card_raw_companion_survives_older_cached_projection(c9, full_op,
                  original_message(*a, **dict(kw, logMonoTime=stamp))):
         card.state_publish(cs, None)
 
-    publish(older, fault=condition == 'sm_fault', valid=condition != 'sm_invalid')
+    publish(older, fault=condition in ('sm_fault','sm_expired_fault'), valid=condition not in ('sm_invalid','sm_expired_invalid'))
     assert actual_recv(sd.car_state_sock).logMonoTime == older
     cached = actual_recv(latest).as_builder()
     assert cached.logMonoTime == older
@@ -94,7 +94,7 @@ def test_exact_card_raw_companion_survives_older_cached_projection(c9, full_op,
          patch('openpilot.starpilot.car.gm.steering_companion.messaging.recv_one_or_none', side_effect=receive_raw), \
          patch('openpilot.selfdrive.selfdrived.selfdrived.time.monotonic_ns', return_value=now):
       axis, _, cc = step(1)
-    healthy = condition == 'healthy'
+    healthy = condition in ('healthy', 'sm_expired')
     assert axis.sourceCarStateMonoTime == source
     assert bool(axis.lateralActive) == healthy and bool(cc.latActive) == healthy
     assert bool(cc.longActive) == (healthy and full_op)
@@ -103,3 +103,28 @@ def test_exact_card_raw_companion_survives_older_cached_projection(c9, full_op,
       assert sd.sm.logMonoTime['starpilotCarState'] == older
       assert sd.steering_companion.samples[source][0] == source
       assert axis.validUntilMonoTime == axis.observedMonoTime + 30_000_000
+
+
+def test_sdgm_alpha_owner_excludes_pedal_and_stock():
+  from opendbc.car.gm.values import GMFlags, apply_gm_auto_hold, camera_acc_pedal_profile, is_volt_sdgm_profile
+  from openpilot.starpilot.car.gm.aol import sdgm_alpha_lateral_owner
+  for c9 in (False, True):
+    for hold in (False, True):
+      cp = sdgm_params(alpha=True, brake_c9=c9)
+      cp.alternativeExperience = 32
+      apply_gm_auto_hold(cp, hold)
+      assert sdgm_alpha_lateral_owner(cp)
+      cp.alternativeExperience = 0
+      assert not sdgm_alpha_lateral_owner(cp)
+    for alpha, sascm in ((False, True), (True, False)):
+      cp = sdgm_params(alpha=alpha, sascm=sascm, brake_c9=c9)
+      cp.alternativeExperience = 32
+      assert not sdgm_alpha_lateral_owner(cp)
+  # A separately supported SDGM interceptor has its own withdrawal protocol.
+  cp = sdgm_params(alpha=True)
+  cp.alternativeExperience = 32
+  cp.flags |= int(GMFlags.PEDAL_LONG)
+  cp.safetyConfigs[0].safetyParam = 0xE500
+  assert camera_acc_pedal_profile(cp) is not None
+  assert is_volt_sdgm_profile(cp, longitudinal=True)
+  assert not sdgm_alpha_lateral_owner(cp)

openpilot/starpilot/aol/transport_pause.py

@@ -317,6 +317,8 @@ class TransportPauseFeedback:
     from opendbc.car import structs
     ev9 = ev9_qualified(cp, marked_only=True)
     early_long = gm or cp.openpilotLongitudinalControl and qualified_ioniq6(cp) or ev9
+    from openpilot.starpilot.car.gm.aol import sdgm_alpha_pedal_withdrawal
+    sdgm_pedal = sdgm_alpha_pedal_withdrawal(cp, cs)
     event_ns = int(sm.logMonoTime['onroadEvents'])
     main_off_recovery = bool(0 < event_ns <= self.ccnc_main_off_source_ns <= now_ns and
       now_ns - self.ccnc_main_off_source_ns <= AXIS_MAX_AGE_NS)
@@ -340,7 +342,8 @@ class TransportPauseFeedback:
         not 0 < int(sm.logMonoTime['onroadEvents']) <= now_ns or
         now_ns - int(sm.logMonoTime['onroadEvents']) > 1_500_000_000 or
         any(event.immediateDisable or event.softDisable or
-            event.userDisable and not (ev9_main_off and event.name == log.OnroadEvent.EventName.wrongCarMode)
+            event.userDisable and not (ev9_main_off and event.name == log.OnroadEvent.EventName.wrongCarMode or
+              sdgm_pedal and event.name == log.OnroadEvent.EventName.pedalPressed)
             for event in sm['onroadEvents'])):
       return None
     native = independent_lateral_health(sm, cp, cs, self.session, now_ns)
@@ -411,7 +414,9 @@ class TransportPauseFeedback:
       not axis.desiredLongitudinal and not axis.longitudinalActive and
       all(event.name in (stock_user, log.OnroadEvent.EventName.pedalPressed) and
           not event.softDisable and not event.immediateDisable for event in users))
-    pedal = bool(cs.brakePressed and users and all(event.name == log.OnroadEvent.EventName.pedalPressed and
+    from openpilot.starpilot.car.gm.aol import sdgm_alpha_pedal_withdrawal
+    pedal = bool((cs.brakePressed or sdgm_alpha_pedal_withdrawal(cp, cs)) and users and all(
+      event.name == log.OnroadEvent.EventName.pedalPressed and
       not event.softDisable and not event.immediateDisable for event in users) and not faults)
     if axis.longitudinalActive and not location_fault or not (temporary or eps_recovery or pedal):
       return False
@@ -1103,6 +1108,11 @@ 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))
+      from openpilot.starpilot.car.gm.aol import sdgm_alpha_lateral_owner
+      if (sdgm_alpha_lateral_owner(cp) and latched and current and continuity and self.signature is not None and
+          signature[14:16] != self.signature[14:16]):
+        self.failed = True
+        return True
       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

openpilot/starpilot/car/gm/aol.py

@@ -43,6 +43,26 @@ def f1_conventional_lateral_owner(cp) -> bool:
               int(cp.safetyConfigs[0].safetyParam) == VOLT_CC_F1_WORD)
 
 
+def sdgm_alpha_lateral_owner(cp) -> bool:
+  from opendbc.car.gm.values import is_volt_sdgm_accepted_envelope
+  return bool(qualified_gm(cp) and aol_enabled(cp) and is_volt_sdgm_accepted_envelope(cp))
+
+
+def sdgm_alpha_pedal_withdrawal(cp, cs) -> bool:
+  """Physical LONG withdrawal for the 2019 SASCM Alpha configuration."""
+  from opendbc.car.gm.values import is_volt_sdgm_accepted_envelope
+  from opendbc.car.structs import car
+  return bool(qualified_gm(cp) and aol_enabled(cp) and
+              is_volt_sdgm_accepted_envelope(cp) and
+              cs.canValid and not cs.canTimeout and cs.cruiseState.available and
+              not cs.steerFaultTemporary and not cs.steerFaultPermanent and not cs.accFaulted and
+              not cs.doorOpen and not cs.steeringDisengage and
+              cs.gearShifter in (car.CarState.GearShifter.drive, car.CarState.GearShifter.low,
+                                car.CarState.GearShifter.sport, car.CarState.GearShifter.brake,
+                                car.CarState.GearShifter.manumatic) and
+              (cs.brakePressed or cs.regenBraking))
+
+
 def stock_ascm_independent_lateral(cp) -> bool:
   """Stock cruise's engagement speed does not cancel independent lateral intent."""
   return bool(qualified_gm(cp) and cp.carFingerprint == CAR.CHEVROLET_VOLT_ASCM and
@@ -206,7 +226,7 @@ def prior_main_off_intent_current(feedback, sm, cp, cs, axis, now_ns, *, latched
       cs.gearShifter not in (car.CarState.GearShifter.drive, car.CarState.GearShifter.low,
                             car.CarState.GearShifter.sport, car.CarState.GearShifter.brake) or
       any(event.type == car.CarState.ButtonEvent.Type.cancel for event in cs.buttonEvents) or
-      str(axis.faultReason) != 'none' or axis.nativeAcknowledged or
+      str(axis.faultReason) != 'none' or axis.nativeAcknowledged and not sdgm_alpha_lateral_owner(cp) or
       axis.desiredLateral or axis.desiredLongitudinal or axis.lateralActive or axis.longitudinalActive or
       not 0 < axis.sourceCarStateMonoTime <= axis.observedMonoTime or axis.sourceCarStateMonoTime >= main_arm_ns or
       now_ns - axis.sourceCarStateMonoTime > AXIS_MAX_AGE_NS or

openpilot/starpilot/car/gm/steering_companion.py

@@ -37,7 +37,7 @@ class SteeringCompanion:
         return False
     return True
 
-  def current(self, sm, *, source_ns, now_ns, drain=True):
+  def current(self, sm, *, source_ns, now_ns, drain=True, allow_raw_projection=False):
     if drain and not self.drain():
       return None
     if self.sock is None and sm.seen.get(SERVICE, False):
@@ -52,8 +52,13 @@ class SteeringCompanion:
     # conflating cache was polled. Its own validity, age and fault veto remain.
     sm_stamp = int(sm.logMonoTime[SERVICE])
     sm_companion = sm[SERVICE]
-    if not (int(sm_companion.sourceCarStateMonoTime) == sm_stamp and
-            0 < sm_stamp <= now_ns and now_ns - sm_stamp <= INTENT_MAX_AGE_NS):
+    if not (int(sm_companion.sourceCarStateMonoTime) == sm_stamp and 0 < sm_stamp <= now_ns):
+      return None
+    # Exact fresh raw input can arrive after SubMaster's nonblocking poll. The
+    # older healthy projection is not a second authority source or input lease.
+    # Keep its validity, identity, future-clock and explicit fault vetoes.
+    if (now_ns - sm_stamp > INTENT_MAX_AGE_NS and not
+        (allow_raw_projection and self.sock is not None and sm_stamp < source_ns)):
       return None
     latest = self.latest
     pair = self.samples.get(source_ns)

prebuilt.json

@@ -1 +1 @@
-{"protected_inventory_sha256": "4bf70400a2b8ada30260b641016774122379af2a90b9d82eb61ad0382c1b9e2f", "schema_version": 1}
+{"protected_inventory_sha256": "92a2dd68eada077cf82f7df916d4a7cd5d8fa882abd8834c1a9ace20d7daabe2", "schema_version": 1}

tools/ci/run_vehicle_tests.py

@@ -37,6 +37,9 @@ AOL_LIFECYCLE_TARGETS = (
   "openpilot/starpilot/aol/tests/test_ev9_main_withdrawal_guards.py",
   "openpilot/starpilot/aol/tests/test_native_inventory_control_cadence.py",
   "openpilot/starpilot/aol/tests/test_gm.py",
+  "openpilot/starpilot/aol/tests/test_gm_steering_receive_order.py",
+  "openpilot/starpilot/aol/tests/test_gm_main_response.py",
+  "openpilot/starpilot/aol/tests/test_gm_held_native.py",
   "openpilot/starpilot/aol/tests/test_gm_startup_main.py",
   "openpilot/starpilot/aol/tests/test_gm_pedal_order.py",
   "openpilot/starpilot/aol/tests/test_gm_cancel_remap.py",