Preserve MQB lateral intent through crawl and brake joins

openpilot/selfdrive/selfdrived/selfdrived.py

@@ -1785,7 +1785,8 @@ class SelfdriveD:
     from openpilot.starpilot.car.mazda.aol import physical_cancel_lateral_allowed as mazda_cancel_lateral_allowed
     from openpilot.starpilot.car.ford.aol import physical_cancel_lateral_allowed as ford_cancel_lateral_allowed
     from openpilot.starpilot.car.subaru.aol import sng_standstill_lateral_inhibit
-    from openpilot.starpilot.car.volkswagen.aol import lateral_output_inhibited as volkswagen_lateral_output_inhibited
+    from openpilot.starpilot.car.volkswagen.aol import (lateral_output_inhibited as volkswagen_lateral_output_inhibited,
+                                                         below_engage_lateral_allowed as volkswagen_below_engage_lateral_allowed)
     from openpilot.starpilot.aol.dm import lateral_event_context
     from openpilot.starpilot.car.toyota.legacy_lateral import below_engage_lateral_allowed as toyota_below_engage_lateral_allowed
     retained_dm_lateral, lateral_events = lateral_event_context(self, CS, intent, native, now_ns, ordinary_lag)
@@ -1824,7 +1825,8 @@ class SelfdriveD:
                                                        not self.CP.flags & GMFlags.PEDAL_LONG) or
                              toyota_below_engage_lateral_allowed(self.CP, CS, intent, native,
                                source_ns=getattr(self, 'aol_car_state_log_ns', 0), now_ns=now_ns,
-                               axis_session_id=getattr(self, 'aol_session_id', ''))),
+                               axis_session_id=getattr(self, 'aol_session_id', '')) or
+                             volkswagen_below_engage_lateral_allowed(self.CP, CS, intent)),
       immediate_disable=self.events.contains(ET.IMMEDIATE_DISABLE),
       dm_lockout=bool(not self.sm.all_checks(['driverMonitoringState']) or
                       not retained_dm_lateral and (self.sm['driverMonitoringState'].lockout or
@@ -1880,7 +1882,9 @@ class SelfdriveD:
     ordinary_ford = bool(self.CP.brand == 'ford' and self.CP.openpilotLongitudinalControl and self.CP.safetyConfigs and
       ford_native_accepts_cp(self.CP, int(car.CarParams.SafetyModel.ford), self.CP.safetyConfigs[-1].safetyParam))
     # Driver steering does not invalidate a still-current native LAT grant.
-    steering_override_allowed = ordinary_ford or self.CP.brand == 'hyundai'
+    from opendbc.car.volkswagen.long_aol import qualified as volkswagen_long_qualified
+    steering_override_allowed = (ordinary_ford or self.CP.brand == 'hyundai' or
+                                 volkswagen_long_qualified(self.CP, marked_only=True))
     proof = getattr(self, 'aol_published_input_proof', None)
     prior = getattr(self, 'aol_published_input_native', None)
     anchor = getattr(self, 'aol_ordinary_source_anchor', None)

openpilot/starpilot/aol/tests/test_meb_actual_callers.py

@@ -114,7 +114,7 @@ def test_meb_full_engagement_brake_reverse_drive_actual_callers(identity, stock,
     # Explicit ambient producers added since the original shared caller fixture.
     # Keep production SubMaster health/frequency checks; never ignore a required service.
     from openpilot.cereal.services import SERVICE_LIST
-    ambient_services = ('alertDebug', 'lateralManeuverPlan', 'laneChangeAssistWire',
+    ambient_services = ('laneChangeAssistWire',
                         'starpilotLongitudinalPlan', 'slcState', 'gpsLocation')
     original_sd_update = sd.sm.update_msgs
     ambient_due = {}

openpilot/starpilot/aol/tests/test_mqb_actual_callers.py

@@ -31,7 +31,7 @@ from openpilot.starpilot.aol.tests.stock_lateral_test_helpers import authority_c
 @pytest.mark.parametrize('fixed', (False,))
 def test_mqb_full_engagement_brake_reverse_drive_actual_callers(gearbox, alpha, offset, fixed, engage_on_set,
                                                                stop_and_resume=False, candidate=CAR.VOLKSWAGEN_GOLF_MK7,
-                                                               brake_held_engagement=False):
+                                                               brake_held_engagement=False, creep_start=False, early_brake=False):
   with ExitStack() as cleanup:
     cleanup.enter_context(OpenpilotPrefix())
     cleanup.enter_context(mock.patch.dict('os.environ', {'SIMULATION': '0', 'AOL_REPLAY_RUNTIME': '1'}))
@@ -116,7 +116,7 @@ def test_mqb_full_engagement_brake_reverse_drive_actual_callers(gearbox, alpha,
     # Explicit ambient producers added since the original shared caller fixture.
     # Keep production SubMaster health/frequency checks; never ignore a required service.
     from openpilot.cereal.services import SERVICE_LIST
-    ambient_services = ('alertDebug', 'lateralManeuverPlan', 'laneChangeAssistWire',
+    ambient_services = ('laneChangeAssistWire',
                         'starpilotLongitudinalPlan', 'slcState', 'gpsLocation')
     original_sd_update = sd.sm.update_msgs
     ambient_due = {}
@@ -156,6 +156,8 @@ def test_mqb_full_engagement_brake_reverse_drive_actual_callers(gearbox, alpha,
     for tick in range(120):
       now = 10_000_000_000 + tick * 10_000_000
       native.set_timer(1_000_000 + tick * 10_000)
+      if creep_start and tick >= 12:
+        native.set_aol_test_heartbeat(bool(card.aol_card_intent.allowed_latch or request))
       if quiet is not None:
         quiet.set_timer(1_000_000 + tick * 10_000)
         quiet.safety_tick()
@@ -163,12 +165,12 @@ def test_mqb_full_engagement_brake_reverse_drive_actual_callers(gearbox, alpha,
         assert not quiet.get_controls_allowed()
         assert native.get_current_safety_mode() != quiet.get_current_safety_mode()
 
-      brake = 65 <= tick < 75 or (brake_held_engagement and tick < 29)
+      brake = (60 if early_brake else 65) <= tick < 75 or (brake_held_engagement and tick < 29)
       reverse = 85 <= tick < 95
       ordinary = engage_on_set and 21 <= tick < 65
       overrides = {
         'TSK_06': {'TSK_Status': 3 if ordinary and not alpha else 2},
-        'LH_EPS_03': {'EPS_HCA_Status': 3, 'EPS_Lenkmoment': 0},
+        'LH_EPS_03': {'EPS_HCA_Status': 3, 'EPS_Lenkmoment': 100 if early_brake and 52 <= tick <= 61 else 0},
         'Motor_20': {'MO_Fahrpedalrohwert_01': 0},
         'Motor_14': {'MO_Fahrer_bremst': int(brake), 'MO_Kuppl_schalter': 0},
         'ESP_05': {'ESP_Fahrer_bremst': int(brake)},
@@ -176,9 +178,10 @@ def test_mqb_full_engagement_brake_reverse_drive_actual_callers(gearbox, alpha,
         'Gateway_72': {'BCM1_Rueckfahrlicht_Schalter': int(reverse)},
         'Motor_EV_01': {'MO_Waehlpos': 3 if reverse else 5},
         'ESP_19': dict.fromkeys(('ESP_VL_Radgeschw_02', 'ESP_VR_Radgeschw_02', 'ESP_HL_Radgeschw_02', 'ESP_HR_Radgeschw_02'),
+                                      (72 if tick < 12 else 0 if tick < 20 else 1 if tick < 24 else 2 if tick < 46 else 72) if creep_start else
                                       0 if (stop_and_resume and 45 <= tick < 56) or (brake_held_engagement and tick < 30) else 72),
         'Airbag_02': {'AB_Gurtschloss_FA': 0 if 105 <= tick < 111 else 3},
-        'GRA_ACC_01': {'GRA_Hauptschalter': 1, 'GRA_Tip_Setzen': int(engage_on_set and tick == 20)},
+        'GRA_ACC_01': {'GRA_Hauptschalter': 1, 'GRA_Tip_Setzen': int(engage_on_set and (tick in (20, 50) if creep_start else tick == 20))},
         'ACC_02': {'ACC_Gesetzte_Zeitluecke': 1},
       }
       incoming = []
@@ -196,6 +199,9 @@ def test_mqb_full_engagement_brake_reverse_drive_actual_callers(gearbox, alpha,
       native.safety_tick()
       native.aol_set_host_request(request)
       permission = native.aol_get_permission_mask()
+      if early_brake and tick == 60:
+        incoming = [(address, data, bus) for address, data, bus in incoming if address not in (0x106, 0x3BE)]
+        assert not native.get_controls_allowed() and not permission & 2
       original_message = messaging.new_message
       def clocked_card_message(*args, original_message=original_message, now=now, **kwargs):
         message = original_message(*args, **kwargs)
@@ -218,6 +224,11 @@ def test_mqb_full_engagement_brake_reverse_drive_actual_callers(gearbox, alpha,
         allowed=bool(permission & 1), long_allowed=bool(permission & 2),
         native_request_lat=bool(request & 1), native_request_long=bool(request & 2),
         panda_controls_allowed=native.get_controls_allowed())
+      if early_brake and tick == 60:
+        assert not state.brakePressed and state.steeringPressed
+        assert log.OnroadEvent.EventName.controlsMismatch not in sd.events.names, 'VW_EARLY_BRAKE_STEERING_MISMATCH'
+        assert not command.longActive
+        assert card.aol_card_intent.allowed_latch
       if brake_held_engagement and 22 <= tick < 29:
         assert state.standstill and state.brakePressed
         assert selfdrive.state == log.SelfdriveState.OpenpilotState.preEnabled
@@ -262,6 +273,9 @@ def test_mqb_full_engagement_brake_reverse_drive_actual_callers(gearbox, alpha,
       emitted.extend(sent)
       lat_commands.append(command.latActive)
       long_commands.append(command.longActive)
+    if creep_start:
+      assert any(lat_commands[35:45]), 'AOL must request and obtain native steering below the manual long-enable speed'
+      assert not any(long_commands[:46]), 'Creep AOL must not enable longitudinal below its independent minimum'
     assert any(lat_commands[35:60])
     assert any(lat_commands[76:84])  # braking released long, not qualified lateral intent
     assert not any(lat_commands[86:95])
@@ -270,7 +284,7 @@ def test_mqb_full_engagement_brake_reverse_drive_actual_callers(gearbox, alpha,
     assert not any(long_commands[66:])  # no automatic longitudinal restoration
     assert emitted
     if alpha and engage_on_set:
-      assert any(long_commands[35:60])
+      assert any(long_commands[50:65] if creep_start else long_commands[35:60])
     else:
       assert not any(long_commands)
 
@@ -287,3 +301,50 @@ def test_mqb_brake_held_engagement_waits_for_release_actual_callers(gearbox):
   test_mqb_full_engagement_brake_reverse_drive_actual_callers(
     gearbox, True, 0, False, True, brake_held_engagement=True,
     candidate=CAR.SEAT_ATECA_MK1 if gearbox == 'automatic' else CAR.VOLKSWAGEN_GOLF_MK7)
+
+
+@pytest.mark.parametrize('offset,fixed', [(0, False), (0, True), (4, False), (4, True)])
+def test_mqb_manual_alpha_stopped_creep_preserves_native_arm_and_fresh_speed_engagement(offset, fixed):
+  test_mqb_full_engagement_brake_reverse_drive_actual_callers(
+    'manual', True, offset, fixed, True, creep_start=True)
+
+
+@pytest.mark.parametrize('veto', ('none', 'can', 'timeout', 'temporary', 'permanent', 'main', 'reverse', 'latch', 'pause', 'automatic', 'stock'))
+def test_manual_cruise_minimum_exception_retains_physical_and_intent_vetoes(veto):
+  from types import SimpleNamespace
+  from openpilot.starpilot.car.volkswagen.aol import below_engage_lateral_allowed
+  from openpilot.starpilot.aol.vehicle import policy_for
+  fp = {bus: {} for bus in range(8)}
+  fp[1][0xFD] = 8
+  cp = CarInterface.get_params(CAR.VOLKSWAGEN_GOLF_MK7, fp, [], True, False, False)
+  policy = policy_for(cp)
+  cp.safetyConfigs[-1].safetyParam |= policy.safety_param_addition
+  cp.alternativeExperience |= policy.alternative_experience_addition
+  state = structs.CarState(canValid=True, gearShifter=structs.CarState.GearShifter.drive)
+  state.cruiseState.available = True
+  intent = SimpleNamespace(allowedLatch=True, pauseLateral=False)
+  if veto in ('can', 'timeout', 'temporary', 'permanent'):
+    setattr(state, {'can': 'canValid', 'timeout': 'canTimeout', 'temporary': 'steerFaultTemporary',
+                    'permanent': 'steerFaultPermanent'}[veto], veto != 'can')
+  elif veto == 'main':
+    state.cruiseState.available = False
+  elif veto == 'reverse':
+    state.gearShifter = structs.CarState.GearShifter.reverse
+  elif veto == 'latch':
+    intent.allowedLatch = False
+  elif veto == 'pause':
+    intent.pauseLateral = True
+  elif veto == 'automatic':
+    cp.transmissionType = structs.CarParams.TransmissionType.automatic
+    cp.safetyConfigs[-1].safetyParam = 9
+  elif veto == 'stock':
+    cp.openpilotLongitudinalControl, cp.pcmCruise = False, True
+    cp.safetyConfigs[-1].safetyParam = 24
+  assert below_engage_lateral_allowed(cp, state, intent) is (veto == 'none')
+
+
+@pytest.mark.parametrize('gearbox', ('automatic', 'manual', 'direct'))
+def test_native_brake_precedes_parsed_pedal_during_driver_steering(gearbox):
+  test_mqb_full_engagement_brake_reverse_drive_actual_callers(
+    gearbox, True, 0, False, True, early_brake=True,
+    candidate=CAR.SEAT_ATECA_MK1 if gearbox == 'automatic' else CAR.VOLKSWAGEN_GOLF_MK7)

openpilot/starpilot/car/volkswagen/aol.py

@@ -20,10 +20,21 @@ def lateral_output_inhibited(cp, state):
 def temporary_restriction(cp, state):
   gears = structs.CarState.GearShifter
   return bool(state is not None and qualified(cp, marked_only=True) and state.canValid and not state.canTimeout and
-              not state.steerFaultPermanent and (state.steerFaultTemporary or (state.standstill and not cp.steerAtStandstill) or
+              not state.steerFaultPermanent and (state.steerFaultTemporary or lateral_output_inhibited(cp, state) or
               state.gearShifter not in (gears.drive, gears.sport, gears.eco, gears.manumatic)))
 
 
+def below_engage_lateral_allowed(cp, state, intent):
+  """A manual cruise minimum does not disarm already armed lateral intent."""
+  return bool(long_qualified(cp, marked_only=True) and
+    cp.transmissionType == structs.CarParams.TransmissionType.manual and
+    state is not None and state.canValid and not state.canTimeout and
+    not state.steerFaultPermanent and not state.steerFaultTemporary and state.cruiseState.available and
+    state.gearShifter in (structs.CarState.GearShifter.drive, structs.CarState.GearShifter.sport,
+                         structs.CarState.GearShifter.eco, structs.CarState.GearShifter.manumatic) and
+    intent is not None and intent.allowedLatch and not intent.pauseLateral)
+
+
 def policy_for(cp):
   if not qualified(cp):
     return AolVehiclePolicy()

prebuilt.json

@@ -1 +1 @@
-{"protected_inventory_sha256": "9ab4514235e9f198d01de8aa1290a066f6c13833c31f5b7576f3ebb2a9c6df58", "schema_version": 1}
+{"protected_inventory_sha256": "e75cecb62177b15d482163f596d34f5e0da46d48f67bdf86a6943811f6aa162d", "schema_version": 1}