Preserve stopped MQB brake-held engagement until brake release

opendbc_repo/opendbc/safety/modes/volkswagen_stock_aol.h

@@ -141,7 +141,7 @@ static void vw_aol_optional_rx(const CANPacket_t *msg) {
 }
 
 static void vw_aol_rx(const CANPacket_t *msg) {
-  if (vw_aol_longitudinal && brake_pressed) {
+  if (vw_aol_longitudinal && brake_pressed && (!brake_pressed_prev || vehicle_moving)) {
     vw_aol_long_armed = false;
   }
   if (vw_aol_enabled) {
@@ -192,7 +192,7 @@ static void vw_aol_rx(const CANPacket_t *msg) {
           vw_aol_clear();
         } else if (buttons == 0U) {
           if (((vw_aol_buttons & 3U) != 0U) && controls_allowed && acc_main_on &&
-              vw_aol_gear_drive && vw_aol_eps_ready && !brake_pressed) {
+              vw_aol_gear_drive && vw_aol_eps_ready && (!brake_pressed || (!vw_aol_manual && !vehicle_moving))) {
             vw_aol_token = true;
             vw_aol_long_armed = true;
           }

opendbc_repo/opendbc/safety/tests/test_volkswagen_brake_pre_enable.py

@@ -0,0 +1,137 @@
+"""MQB stop-and-go accepts a fresh physical engagement while the brake inhibits output."""
+import unittest
+
+import pytest
+
+from opendbc.car import structs
+from opendbc.safety.tests import test_volkswagen_stock_aol as stock
+from opendbc.safety.tests.libsafety import libsafety_py
+
+
+class BrakeOwner(stock.TestVolkswagenStockAol):
+  direct = False
+  def packet(self, name, fields, bus=0):
+    if self.direct and name == 'Gateway_73':
+      name, fields = 'Motor_EV_01', {'MO_Waehlpos': 5 if fields['GE_Fahrstufe'] == 8 else 3}
+    return super().packet(name, fields, bus)
+
+
+def prepare(word):
+  debug = libsafety_py.libsafety.set_safety_hooks(structs.CarParams.SafetyModel.allOutput, 0) == 0
+  owner = BrakeOwner()
+  owner.direct = word == 41
+  owner.configure(word)
+  owner.manual = word == 25
+  for _ in range(12):
+    owner.feed(moving=False, brake=True)
+  return owner, debug
+
+
+def request(owner, button):
+  assert owner.safety.safety_rx_hook(owner.packet('GRA_ACC_01', {button: 1}))
+  owner.feed(moving=False, brake=True)
+  owner.safety.aol_set_host_request(3)
+
+
+def assert_acc_inhibited(owner):
+  assert owner.safety.aol_get_permission_mask() & 2 == 0
+  assert not owner.safety.safety_tx_hook(owner.packet('ACC_06', {'ACC_Sollbeschleunigung_02': 0.5}))
+
+
+@pytest.mark.parametrize('word', (9, 41))
+@pytest.mark.parametrize('button', ('GRA_Tip_Setzen', 'GRA_Tip_Wiederaufnahme'))
+def test_brake_held_request_only_grants_long_after_physical_release(word, button):
+  owner, debug = prepare(word)
+  try:
+    request(owner, button)
+    for _ in range(12):
+      owner.feed(moving=False, brake=True)
+      owner.safety.aol_set_host_request(3)
+      assert_acc_inhibited(owner)
+      assert owner.safety.aol_get_permission_mask() == 0
+      if debug:
+        assert owner.safety.get_controls_allowed()
+    owner.feed(moving=False)
+    owner.safety.aol_set_host_request(3)
+    assert owner.safety.aol_get_permission_mask() == (2 if debug else 0), 'VW_BRAKE_PRE_ENABLE_REQUEST_LOST'
+    assert bool(owner.safety.safety_tx_hook(owner.packet('ACC_06', {'ACC_Sollbeschleunigung_02': 0.5}))) == debug
+    owner.feed()
+    owner.safety.aol_set_host_request(3)
+    assert owner.safety.aol_get_permission_mask() == (3 if debug else 0)
+    owner.feed(brake=True)
+    owner.feed()
+    owner.safety.aol_set_host_request(3)
+    assert owner.safety.aol_get_permission_mask() & 2 == 0
+  finally:
+    owner.tearDown()
+
+
+@pytest.mark.parametrize('word', (9, 25, 41))
+def test_brake_release_without_explicit_request_never_arms_long(word):
+  owner, _ = prepare(word)
+  try:
+    owner.feed(moving=False)
+    owner.safety.aol_set_host_request(3)
+    assert owner.safety.aol_get_permission_mask() & 2 == 0
+    owner.feed()
+    owner.safety.aol_set_host_request(3)
+    assert owner.safety.aol_get_permission_mask() & 2 == 0
+  finally:
+    owner.tearDown()
+
+
+@pytest.mark.parametrize('word', (9, 41))
+@pytest.mark.parametrize('fault', ('cancel', 'main', 'reverse', 'permanent', 'moving', 'heartbeat', 'relay', 'expiry', 'crc'))
+def test_brake_held_request_cannot_survive_native_disengagement(word, fault):
+  owner, _ = prepare(word)
+  try:
+    request(owner, 'GRA_Tip_Wiederaufnahme')
+    if fault in ('cancel', 'main', 'reverse', 'permanent', 'moving'):
+      kwargs = {'moving': False, 'brake': True}
+      kwargs.update({'cancel': {'cancel': True}, 'main': {'main': False}, 'reverse': {'gear': False},
+                     'permanent': {'permanent': True}, 'moving': {'moving': True}}[fault])
+      owner.feed(**kwargs)
+    elif fault == 'heartbeat':
+      owner.safety.set_aol_test_heartbeat(False)
+    elif fault == 'relay':
+      owner.safety.set_relay_malfunction(True)
+    elif fault == 'expiry':
+      owner.time += 300_001
+      owner.safety.set_timer(owner.time)
+    else:
+      bad = owner.packet('TSK_06', {'TSK_Status': 2})
+      bad[0].data[0] ^= 0xFF
+      assert not owner.safety.safety_rx_hook(bad)
+    assert_acc_inhibited(owner)
+    owner.safety.set_aol_test_heartbeat(True)
+    owner.safety.set_relay_malfunction(False)
+    for _ in range(12):
+      owner.feed(moving=False, brake=True)
+    owner.feed(moving=False)
+    owner.safety.aol_set_host_request(3)
+    assert owner.safety.aol_get_permission_mask() & 2 == 0
+  finally:
+    owner.tearDown()
+
+
+def test_manual_brake_held_request_does_not_add_stop_and_go_support():
+  owner, _ = prepare(25)
+  try:
+    request(owner, 'GRA_Tip_Wiederaufnahme')
+    owner.feed(moving=False)
+    owner.safety.aol_set_host_request(3)
+    assert owner.safety.aol_get_permission_mask() & 2 == 0
+  finally:
+    owner.tearDown()
+
+
+class TestVolkswagenBrakePreEnable(unittest.TestCase):
+  def test_native_brake_pre_enable(self):
+    for word in (9, 41):
+      for button in ('GRA_Tip_Setzen', 'GRA_Tip_Wiederaufnahme'):
+        test_brake_held_request_only_grants_long_after_physical_release(word, button)
+      for fault in ('cancel', 'main', 'reverse', 'permanent', 'moving', 'heartbeat', 'relay', 'expiry', 'crc'):
+        test_brake_held_request_cannot_survive_native_disengagement(word, fault)
+    for word in (9, 25, 41):
+      test_brake_release_without_explicit_request_never_arms_long(word)
+    test_manual_brake_held_request_does_not_add_stop_and_go_support()

openpilot/starpilot/aol/tests/test_mqb_actual_callers.py

@@ -30,7 +30,8 @@ from openpilot.starpilot.aol.tests.stock_lateral_test_helpers import authority_c
 @pytest.mark.parametrize('offset', (0,))
 @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):
+                                                               stop_and_resume=False, candidate=CAR.VOLKSWAGEN_GOLF_MK7,
+                                                               brake_held_engagement=False):
   with ExitStack() as cleanup:
     cleanup.enter_context(OpenpilotPrefix())
     cleanup.enter_context(mock.patch.dict('os.environ', {'SIMULATION': '0', 'AOL_REPLAY_RUNTIME': '1'}))
@@ -43,7 +44,7 @@ def test_mqb_full_engagement_brake_reverse_drive_actual_callers(gearbox, alpha,
     if gearbox != 'manual':
       fp[offset][0xAD if gearbox == 'automatic' else 0x187] = 8
     fp[offset + 1][0xFD] = 8
-    if stop_and_resume:
+    if stop_and_resume or brake_held_engagement:
       fp[offset + 2][0x126] = 8
       fp[offset][0x6B8] = 8
     cp = CarInterface.get_params(candidate, fp, [], alpha, False, False)
@@ -162,7 +163,7 @@ 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
+      brake = 65 <= tick < 75 or (brake_held_engagement and tick < 29)
       reverse = 85 <= tick < 95
       ordinary = engage_on_set and 21 <= tick < 65
       overrides = {
@@ -175,7 +176,7 @@ 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'),
-                                      0 if stop_and_resume and 45 <= tick < 56 else 72),
+                                      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)},
         'ACC_02': {'ACC_Gesetzte_Zeitluecke': 1},
@@ -217,6 +218,12 @@ 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 brake_held_engagement and 22 <= tick < 29:
+        assert state.standstill and state.brakePressed
+        assert selfdrive.state == log.SelfdriveState.OpenpilotState.preEnabled
+        assert not command.longActive and not (permission & 2)
+      if brake_held_engagement and 38 <= tick < 45:
+        assert command.longActive and axis.longitudinalActive and permission & 2, "VW_BRAKE_RELEASE_NO_LONG"
       if tick >= 112:
         assert command.driverMonitoringEscalation
       if stop_and_resume and 45 <= tick < 56:
@@ -273,3 +280,10 @@ def test_mqb_full_engagement_brake_reverse_drive_actual_callers(gearbox, alpha,
 def test_mqb_stop_pauses_only_lateral_and_recovers_without_mismatch(gearbox, alpha):
   test_mqb_full_engagement_brake_reverse_drive_actual_callers(
     gearbox, alpha, 0, False, True, stop_and_resume=True, candidate=CAR.VOLKSWAGEN_GOLF_MK7 if gearbox == "direct" else CAR.SEAT_ATECA_MK1)
+
+
+@pytest.mark.parametrize('gearbox', ('automatic', 'direct'))
+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)