230 lines
7.8 KiB
Python
230 lines
7.8 KiB
Python
"""
|
|
Copyright © IQ.Lvbs, apart of Project Teal Lvbs, All Rights Reserved, licensed under https://konn3kt.com/tos
|
|
"""
|
|
|
|
from types import SimpleNamespace
|
|
|
|
from iqpilot.cereal import custom
|
|
from iqdbc.car import structs
|
|
from iqdbc.car.hyundai.values import HyundaiFlags, HyundaiFlagsIQ
|
|
from iqpilot.sab.behavior import SteeringAssistanceBehavior
|
|
from iqpilot.selfdrive.selfdrived.iq_events import IQEvents
|
|
from iqpilot.selfdrive.selfdrived.events import Events
|
|
from iqpilot.cereal import log
|
|
|
|
|
|
ButtonType = structs.CarState.ButtonEvent.Type
|
|
EventName = log.OnroadEvent.EventName
|
|
EventNameIQ = custom.IQOnroadEvent.EventName
|
|
GuidanceState = custom.AlwaysOnLateral.AlwaysOnLateralState
|
|
|
|
|
|
class MockParams:
|
|
def __init__(self, main_cruise_allowed: bool = False, aol_enabled: bool = True,
|
|
pause_on_steering_override: bool = False):
|
|
self.main_cruise_allowed = main_cruise_allowed
|
|
self.aol_enabled = aol_enabled
|
|
self.pause_on_steering_override = pause_on_steering_override
|
|
|
|
def get_bool(self, key: str) -> bool:
|
|
return {
|
|
"AolEnabled": self.aol_enabled,
|
|
"AolMainCruiseAllowed": self.main_cruise_allowed,
|
|
"AolUnifiedEngagementMode": False,
|
|
"AolPauseOnSteeringOverride": self.pause_on_steering_override,
|
|
"JoystickDebugMode": False,
|
|
}.get(key, False)
|
|
|
|
def get(self, key: str, return_default: bool = False):
|
|
if key == "AolSteeringMode":
|
|
return 0 if return_default else b"0"
|
|
return None
|
|
|
|
def remove(self, key: str) -> None:
|
|
return None
|
|
|
|
|
|
def make_selfdrive(cp_flags: int, brand: str = "hyundai", main_cruise_allowed: bool = False,
|
|
aol_enabled: bool = True, cp_iq_flags: int = 0,
|
|
pause_on_steering_override: bool = False):
|
|
cp = SimpleNamespace(
|
|
brand=brand,
|
|
flags=cp_flags,
|
|
passive=False,
|
|
notCar=False,
|
|
safetyModel=structs.CarParams.SafetyModel.noOutput,
|
|
)
|
|
cp_iq = SimpleNamespace(flags=cp_iq_flags)
|
|
return SimpleNamespace(
|
|
CP=cp,
|
|
CP_IQ=cp_iq,
|
|
params=MockParams(main_cruise_allowed, aol_enabled, pause_on_steering_override),
|
|
state_machine=SimpleNamespace(soft_disable_timer=0, current_alert_types=[]),
|
|
events=Events(),
|
|
events_iq=IQEvents(),
|
|
CS_prev=SimpleNamespace(
|
|
gasPressed=False,
|
|
cruiseState=SimpleNamespace(available=False),
|
|
lateralAvailable=False,
|
|
),
|
|
enabled=False,
|
|
enabled_prev=False,
|
|
initialized=True,
|
|
)
|
|
|
|
|
|
def make_car_state():
|
|
return SimpleNamespace(
|
|
started=True,
|
|
standstill=False,
|
|
doorOpen=False,
|
|
seatbeltUnlatched=False,
|
|
gearShifter=structs.CarState.GearShifter.drive,
|
|
vEgo=0.0,
|
|
gasPressed=False,
|
|
brakePressed=False,
|
|
cruiseState=SimpleNamespace(available=False),
|
|
lateralAvailable=False,
|
|
buttonEvents=[structs.CarState.ButtonEvent(pressed=True, type=ButtonType.lkas)],
|
|
)
|
|
|
|
|
|
def make_vw_car_state(cruise_available: bool, cruise_fault_lateral: bool = False):
|
|
return SimpleNamespace(
|
|
started=True,
|
|
standstill=False,
|
|
doorOpen=False,
|
|
seatbeltUnlatched=False,
|
|
gearShifter=structs.CarState.GearShifter.drive,
|
|
vEgo=0.0,
|
|
gasPressed=False,
|
|
brakePressed=False,
|
|
cruiseState=SimpleNamespace(available=cruise_available),
|
|
lateralAvailable=cruise_available or cruise_fault_lateral,
|
|
cruiseFaultLateralMode=cruise_fault_lateral,
|
|
buttonEvents=[],
|
|
)
|
|
|
|
|
|
def test_hyundai_lkas_button_can_arm_guidance_before_lateral_available():
|
|
selfdrive = make_selfdrive(0, cp_iq_flags=HyundaiFlagsIQ.HAS_LFA_BUTTON)
|
|
guidance = SteeringAssistanceBehavior(selfdrive)
|
|
car_state = make_car_state()
|
|
car_state.buttonEvents = [structs.CarState.ButtonEvent(pressed=True, type=ButtonType.lfaButton)]
|
|
|
|
guidance.update_events(car_state)
|
|
|
|
assert selfdrive.events_iq.has(EventNameIQ.alcEngaged)
|
|
|
|
|
|
def test_hyundai_lkas_button_stays_inactive_without_platform_support():
|
|
selfdrive = make_selfdrive(0)
|
|
guidance = SteeringAssistanceBehavior(selfdrive)
|
|
|
|
guidance.update_events(make_car_state())
|
|
|
|
assert not selfdrive.events_iq.has(EventNameIQ.alcEngaged)
|
|
|
|
|
|
def test_main_cruise_drop_cuts_guidance_even_if_lateral_signal_stays_true():
|
|
selfdrive = make_selfdrive(0, brand="volkswagen", main_cruise_allowed=True)
|
|
selfdrive.CS_prev = make_vw_car_state(cruise_available=True)
|
|
guidance = SteeringAssistanceBehavior(selfdrive)
|
|
guidance.enabled = True
|
|
|
|
guidance.update_events(make_vw_car_state(cruise_available=False))
|
|
|
|
assert selfdrive.events_iq.has(EventNameIQ.alcDisengaged)
|
|
|
|
|
|
def test_faulted_lateral_mode_does_not_force_disable_guidance():
|
|
selfdrive = make_selfdrive(0, brand="volkswagen", main_cruise_allowed=True)
|
|
selfdrive.CS_prev = make_vw_car_state(cruise_available=True)
|
|
guidance = SteeringAssistanceBehavior(selfdrive)
|
|
guidance.enabled = True
|
|
|
|
guidance.update_events(make_vw_car_state(cruise_available=False, cruise_fault_lateral=True))
|
|
|
|
assert not selfdrive.events_iq.has(EventNameIQ.alcDisengaged)
|
|
|
|
|
|
def test_main_switch_rising_edge_arms_guidance_during_faulted_cruise():
|
|
selfdrive = make_selfdrive(0, brand="volkswagen", main_cruise_allowed=True)
|
|
selfdrive.CS_prev = make_vw_car_state(cruise_available=False, cruise_fault_lateral=False)
|
|
guidance = SteeringAssistanceBehavior(selfdrive)
|
|
|
|
guidance.update_events(make_vw_car_state(cruise_available=False, cruise_fault_lateral=True))
|
|
|
|
assert selfdrive.events_iq.has(EventNameIQ.alcEngaged)
|
|
|
|
|
|
def test_main_cruise_rising_edge_does_not_engage_when_toggle_is_off():
|
|
selfdrive = make_selfdrive(0, brand="volkswagen", main_cruise_allowed=True, aol_enabled=False)
|
|
selfdrive.CS_prev = make_vw_car_state(cruise_available=False)
|
|
guidance = SteeringAssistanceBehavior(selfdrive)
|
|
|
|
guidance.update(make_vw_car_state(cruise_available=True))
|
|
|
|
assert not selfdrive.events_iq.has(EventNameIQ.alcEngaged)
|
|
assert not guidance.active
|
|
assert not guidance.enabled
|
|
assert guidance.state_machine.state == custom.AlwaysOnLateral.AlwaysOnLateralState.disabled
|
|
|
|
|
|
def run_cycle(guidance, selfdrive, car_state, steering_pressed: bool):
|
|
selfdrive.events.clear()
|
|
selfdrive.events_iq.clear()
|
|
if steering_pressed:
|
|
selfdrive.events.add(EventName.steerOverride)
|
|
guidance.update(car_state)
|
|
selfdrive.CS_prev = car_state
|
|
|
|
|
|
def make_engaged_guidance(pause_on_steering_override: bool):
|
|
selfdrive = make_selfdrive(0, brand="volkswagen", pause_on_steering_override=pause_on_steering_override)
|
|
selfdrive.CS_prev = make_vw_car_state(cruise_available=True)
|
|
guidance = SteeringAssistanceBehavior(selfdrive)
|
|
guidance.enabled = True
|
|
guidance.state_machine.state = GuidanceState.enabled
|
|
return guidance, selfdrive
|
|
|
|
|
|
def test_steering_override_parks_guidance_when_enabled():
|
|
guidance, selfdrive = make_engaged_guidance(True)
|
|
|
|
run_cycle(guidance, selfdrive, make_vw_car_state(cruise_available=True), steering_pressed=True)
|
|
|
|
assert guidance.state_machine.state == GuidanceState.paused
|
|
assert guidance.enabled
|
|
assert not guidance.active
|
|
|
|
|
|
def test_guidance_resumes_once_steering_is_released():
|
|
guidance, selfdrive = make_engaged_guidance(True)
|
|
|
|
run_cycle(guidance, selfdrive, make_vw_car_state(cruise_available=True), steering_pressed=True)
|
|
run_cycle(guidance, selfdrive, make_vw_car_state(cruise_available=True), steering_pressed=False)
|
|
|
|
assert guidance.state_machine.state == GuidanceState.enabled
|
|
assert guidance.active
|
|
|
|
|
|
def test_steering_override_keeps_torque_when_option_is_off():
|
|
guidance, selfdrive = make_engaged_guidance(False)
|
|
|
|
run_cycle(guidance, selfdrive, make_vw_car_state(cruise_available=True), steering_pressed=True)
|
|
|
|
assert guidance.state_machine.state == GuidanceState.overriding
|
|
assert guidance.active
|
|
|
|
|
|
def test_main_cruise_rising_edge_engages_when_toggle_is_on():
|
|
selfdrive = make_selfdrive(0, brand="volkswagen", main_cruise_allowed=True, aol_enabled=True)
|
|
selfdrive.CS_prev = make_vw_car_state(cruise_available=False)
|
|
guidance = SteeringAssistanceBehavior(selfdrive)
|
|
|
|
guidance.update(make_vw_car_state(cruise_available=True))
|
|
|
|
assert selfdrive.events_iq.has(EventNameIQ.alcEngaged)
|
|
assert guidance.active
|