diff --git a/openpilot/selfdrive/selfdrived/selfdrived.py b/openpilot/selfdrive/selfdrived/selfdrived.py old mode 100755 new mode 100644 index a9955c1039..51c3a36298 --- a/openpilot/selfdrive/selfdrived/selfdrived.py +++ b/openpilot/selfdrive/selfdrived/selfdrived.py @@ -1782,6 +1782,7 @@ 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.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) @@ -1828,7 +1829,8 @@ class SelfdriveD: self.sm['driverMonitoringState'].alertLevel == AlertLevel.three)), pause_brake_mps=self.aol_settings.pause_brake_mps if self.aol_settings is not None else 0.0, gear_pause=reverse_pause, - lateral_inhibit=self.aol_dm_lateral_inhibit or sng_standstill_lateral_inhibit(self.CP, CS) or any( + lateral_inhibit=self.aol_dm_lateral_inhibit or sng_standstill_lateral_inhibit(self.CP, CS) or + volkswagen_lateral_output_inhibited(self.CP, CS) or any( sample.steerFaultTemporary or sample.brakePressed and not sample.standstill and 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', ()))) diff --git a/openpilot/starpilot/aol/tests/test_mqb_actual_callers.py b/openpilot/starpilot/aol/tests/test_mqb_actual_callers.py index fb4a522e35..99d3126fba 100644 --- a/openpilot/starpilot/aol/tests/test_mqb_actual_callers.py +++ b/openpilot/starpilot/aol/tests/test_mqb_actual_callers.py @@ -17,7 +17,7 @@ from opendbc.car.volkswagen.interface import CarInterface from opendbc.car.volkswagen.values import CAR from opendbc.safety.tests.common import make_msg from opendbc.safety.tests.libsafety import libsafety_py -from openpilot.cereal import messaging +from openpilot.cereal import log, messaging from openpilot.common.params import Params from openpilot.common.prefix import OpenpilotPrefix from openpilot.selfdrive.car import card as card_module @@ -29,7 +29,8 @@ from openpilot.starpilot.aol.tests.stock_lateral_test_helpers import authority_c @pytest.mark.parametrize('engage_on_set', (False, True)) @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): +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): with ExitStack() as cleanup: cleanup.enter_context(OpenpilotPrefix()) cleanup.enter_context(mock.patch.dict('os.environ', {'SIMULATION': '0', 'AOL_REPLAY_RUNTIME': '1'})) @@ -42,7 +43,10 @@ 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 - cp = CarInterface.get_params(CAR.VOLKSWAGEN_GOLF_MK7, fp, [], alpha, False, False) + if stop_and_resume: + fp[offset + 2][0x126] = 8 + fp[offset][0x6B8] = 8 + cp = CarInterface.get_params(candidate, fp, [], alpha, False, False) cp.fingerprintSource = structs.CarParams.FingerprintSource.fixed if fixed else structs.CarParams.FingerprintSource.fw # Use the standard source-built loader in fresh pytest processes; private runners may preload it. native = libsafety_py.libsafety @@ -170,7 +174,8 @@ def test_mqb_full_engagement_brake_reverse_drive_actual_callers(gearbox, alpha, 'Gateway_73': {'GE_Fahrstufe': 6 if reverse else 8}, '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), + '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), '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}, @@ -214,6 +219,16 @@ def test_mqb_full_engagement_brake_reverse_drive_actual_callers(gearbox, alpha, panda_controls_allowed=native.get_controls_allowed()) if tick >= 112: assert command.driverMonitoringEscalation + if stop_and_resume and 45 <= tick < 56: + assert state.standstill and not command.latActive and not axis.desiredLateral, "VW_STOP_LATERAL_REQUEST_NOT_PAUSED" + assert log.OnroadEvent.EventName.controlsMismatch not in sd.events.names, "VW_STOP_CONTROLS_MISMATCH" + assert card.aol_card_intent.allowed_latch + if alpha and not cp.autoResumeSng: + assert cp.minEnableSpeed > 0 and not command.longActive + if alpha and engage_on_set and cp.autoResumeSng: + assert axis.desiredLongitudinal and axis.longitudinalActive and command.longActive, ("VW_STOP_LONG_DROPPED", tick, gearbox, alpha, permission, request, axis.to_dict(), list(sd.events.names)) + if stop_and_resume and 60 <= tick < 65: + assert command.latActive and card.aol_card_intent.allowed_latch request = int(axis.desiredLateral) | (int(axis.desiredLongitudinal) << 1) # Feed the exact serialized Controls publication into the actual Card caller. frames = [controls.pm.messages['carControl'], sd.pm.messages['aolAxisState'], @@ -250,3 +265,10 @@ def test_mqb_full_engagement_brake_reverse_drive_actual_callers(gearbox, alpha, assert any(long_commands[35:60]) else: assert not any(long_commands) + + +@pytest.mark.parametrize('gearbox,alpha', [('automatic', False), ('automatic', True), ('manual', False), + ('direct', False), ('direct', True)]) +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) diff --git a/openpilot/starpilot/aol/tests/test_vw_steering_speed_request.py b/openpilot/starpilot/aol/tests/test_vw_steering_speed_request.py new file mode 100644 index 0000000000..3f3f89f683 --- /dev/null +++ b/openpilot/starpilot/aol/tests/test_vw_steering_speed_request.py @@ -0,0 +1,43 @@ +import pytest +from opendbc.car import structs +from opendbc.car.volkswagen.interface import CarInterface +from opendbc.car.volkswagen.values import CAR +from openpilot.starpilot.car.volkswagen.aol import lateral_output_inhibited, policy_for + + +def params(alpha): + fp = {bus: {} for bus in range(8)} + fp[0] = {0xAD: 8, 0x6B8: 8} + fp[1] = {0xFD: 8} + fp[2] = {0x126: 8} + cp = CarInterface.get_params(CAR.SEAT_ATECA_MK1, fp, [], alpha, False, False) + policy = policy_for(cp) + cp.alternativeExperience |= policy.alternative_experience_addition + cp.safetyConfigs[-1].safetyParam |= policy.safety_param_addition + assert cp.flags == 5 and cp.alternativeExperience == 32 + assert cp.safetyConfigs[-1].safetyParam == 8 + int(alpha) + return cp + + +@pytest.mark.parametrize('alpha', [False, True]) +@pytest.mark.parametrize('relative_speed,paused', [(-.01, True), (0., True), (.01, False)]) +def test_request_uses_the_existing_actuation_speed_boundary(alpha, relative_speed, paused): + cp = params(alpha) + cs = structs.CarState(vEgo=cp.minSteerSpeed + relative_speed) + assert lateral_output_inhibited(cp, cs) is paused + + +@pytest.mark.parametrize('alpha', [False, True]) +def test_physical_standstill_inhibits_steering_even_while_speed_filter_settles(alpha): + cp = params(alpha) + cs = structs.CarState(standstill=True, vEgo=2.) + assert lateral_output_inhibited(cp, cs) + + +def test_unadmitted_configuration_is_not_reinterpreted(): + cp = params(True) + cp.safetyConfigs[0].safetyParam = 1 + assert not lateral_output_inhibited(cp, structs.CarState(standstill=True)) + cp.brand = 'hyundai' + assert not lateral_output_inhibited(cp, structs.CarState(standstill=True)) + assert not lateral_output_inhibited(None, structs.CarState(standstill=True)) diff --git a/openpilot/starpilot/car/volkswagen/aol.py b/openpilot/starpilot/car/volkswagen/aol.py index d9a5709219..7d58045a11 100644 --- a/openpilot/starpilot/car/volkswagen/aol.py +++ b/openpilot/starpilot/car/volkswagen/aol.py @@ -10,6 +10,13 @@ def qualified(cp, *, marked_only=False): return stock_qualified(cp, marked_only=marked_only) or long_qualified(cp, marked_only=marked_only) or meb_qualified(cp, marked_only=marked_only) + +def lateral_output_inhibited(cp, state): + from openpilot.starpilot.aol.runtime import ordinary_lateral_requested + return bool(qualified(cp, marked_only=True) and + not ordinary_lateral_requested(True, state, cp)) + + 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