"""Synthetic parsed CAN and native axis/controller projection, not vehicle replay.""" from dataclasses import replace from types import SimpleNamespace import unittest from unittest.mock import patch from opendbc.can import CANPacker, CANParser from opendbc.car import Bus, structs from opendbc.car.hyundai.interface import CarInterface from opendbc.car.hyundai.values import CAR, DBC, HyundaiFlags from opendbc.car.vehicle_model import VehicleModel from opendbc.safety.tests.common import make_msg from opendbc.safety.tests.libsafety import libsafety_py from opendbc.safety.tests.test_hyundai import checksum from openpilot.starpilot.aol.tests.test_forte_intent import params, settings from openpilot.starpilot.car.hyundai.aol import create_intent, policy_for from openpilot.starpilot.aol.runtime import current_native, decide_axes from openpilot.starpilot.aol.wire import SAFETY_SERVICE, SafetyState, encode_safety from openpilot.cereal import log from openpilot.selfdrive.controls.lib.latcontrol_torque import LatControlTorque from openpilot.starpilot.lateral.controller_selection import ControllerMode class Samples(dict): def __init__(self, native, stamp): super().__init__({SAFETY_SERVICE: encode_safety(native)}) self.valid = self.alive = self.seen = {SAFETY_SERVICE: True} self.logMonoTime = {SAFETY_SERVICE: stamp} class ForteStream: def __init__(self, *, main_action=0, source=0x391, aol=True, car=CAR.KIA_FORTE_2019_NON_SCC, controller_mode=None): self.source = source self.cp = params(car, source=source) policy = policy_for(self.cp) if aol: self.cp.safetyConfigs[0].safetyParam |= policy.safety_param_addition self.cp.alternativeExperience |= policy.alternative_experience_addition self.ci = CarInterface(self.cp) self.ci.update([]) self.packer = CANPacker(DBC[self.cp.carFingerprint][Bus.pt]) self.output_parser = CANParser(DBC[self.cp.carFingerprint][Bus.pt], [('LKAS11', 100)], 0) self.lateral_controller = (LatControlTorque(self.cp.as_reader(), self.ci, .01, controller_mode=controller_mode) if controller_mode is not None else None) self.vehicle_model = VehicleModel(self.cp) self.vehicle_parameters = log.VehicleParameters.new_message(angleOffsetDeg=0., roll=0.) self.desired_curvature = 0. self.last_torque = 0. self.last_normalized_torque = 0. self.intent = create_intent(self.cp, settings(main=main_action)) self.intent.settings = replace(self.intent.settings, enabled=aol) self.safety = libsafety_py.libsafety # Harness preloads the qualified candidate. self.now = 1_000_000_000 self.count = 0 self.reset() def reset(self): self.safety.init_tests() self.safety.set_alternative_experience(self.cp.alternativeExperience) assert self.safety.set_safety_hooks(int(self.cp.safetyConfigs[0].safetyModel.raw), self.cp.safetyConfigs[0].safetyParam) == 0 self.safety.set_timer(self.now // 1000) self.safety.set_aol_test_heartbeat(True) # Synthetic transport envelope only. def tick(self, *, main=False, cruise=False, bcm=False, clu=False, gear='D', eps=False, fatal=False, rejection=0, cruise_button=0): self.now += 10_000_000 n = self.count self.count += 1 gears = self.ci.CS.shifter_values gear_code = next(code for code, name in gears.items() if name == gear) values: dict[str, dict[str, float]] = { 'EMS16': {'CRUISE_LAMP_M': int(main), 'CRUISE_LAMP_S': int(cruise), 'AliveCounter': n % 4}, 'WHL_SPD11': {'WHL_SPD_FL': 72, 'WHL_SPD_FR': 72, 'WHL_SPD_RL': 72, 'WHL_SPD_RR': 72, 'WHL_SPD_AliveCounter_LSB': n % 4, 'WHL_SPD_AliveCounter_MSB': (n // 4) % 4}, 'TCS13': {'AliveCounterTCS': n % 8}, 'MDPS12': {'CF_Mdps_ToiUnavail': int(eps), 'CR_Mdps_StrColTq': 0., 'CR_Mdps_OutTq': 0.}, 'SAS11': {'SAS_Angle': 0., 'SAS_Speed': 0.}, 'CLU11': {'CF_Clu_AliveCnt1': n % 16, 'CF_Clu_CruiseSwState': cruise_button}, 'LVR12': {'CF_Lvr_Gear': gear_code}, 'BCM_PO_11': {'LDA_BTN': int(bcm)}, 'CLU13': {'CF_Clu_LdwsLkasSW': int(clu)}, 'CGW1': {'CF_Gway_DrvSeatBeltSw': 1}, } if self.source == 0x50c: del values['BCM_PO_11'] frames = [] for name in ('EMS12', 'TCS11', 'TCS15', 'CLU15', 'ESP12', 'CGW2', *values): frame = self.packer.make_can_msg(name, 0, values.get(name, {})) if name in ('EMS16', 'WHL_SPD11', 'TCS13'): frame = checksum(frame) frames.append(frame) frames.append(self.packer.make_can_msg('LKAS11', 2, {'CF_Lkas_MsgCount': n % 16})) if not self.cp.flags & HyundaiFlags.NON_SCC_NO_FCA: frames.append(self.packer.make_can_msg('FCA11', 2, {})) self.safety.set_timer(self.now // 1000) for address, data, bus in frames: self.safety.safety_rx_hook(make_msg(bus, address, len(data), data)) self.safety.safety_tick() with patch('opendbc.car.hyundai.non_scc_aol.time.CLOCK_BOOTTIME', 7, create=True), \ patch('opendbc.car.hyundai.non_scc_aol.time.clock_gettime_ns', return_value=self.now): self.cs = self.ci.update([(self.now, frames)]) self.intent.update(self.cs, now_ns=self.now, fault_active=fatal, native_rejection_ns=rejection) return self.axis() def axis(self, receipt=None): intent = SimpleNamespace(allowedLatch=self.intent.allowed_latch, pauseLateral=self.intent.pause_lateral, pauseLongitudinal=self.intent.pause_longitudinal) kwargs: dict = {'standard_lateral': False, 'standard_longitudinal': False, 'intent': intent, 'car_state': self.cs, 'initialized': True, 'model_ready': True, 'no_entry': False, 'immediate_disable': False, 'dm_lockout': False, 'pause_brake_mps': 0.} desired = decide_axes(native=None, **kwargs) self.safety.aol_set_host_request(int(desired.desired_lateral)) permission = self.safety.aol_get_permission_mask() native = SafetyState(1, True, self.now, self.now + 30_000_000, int(self.cp.safetyConfigs[0].safetyModel.raw), self.cp.safetyConfigs[0].safetyParam, bool(permission & 1), False, bool(self.safety.aol_get_request_mask() & 1), False, 'synthetic-panda', 'session') if receipt is not None: native = receipt(native) acknowledged = current_native(Samples(native, self.now), self.cp, now_ns=self.now, axis_session_id='session') decision = decide_axes(native=acknowledged, **kwargs) command = structs.CarControl() command.latActive = decision.lateral_active if self.lateral_controller is not None: torque, _, _ = self.lateral_controller.update( decision.lateral_active, self.cs, self.vehicle_model, self.vehicle_parameters, False, self.desired_curvature, False, self.cp.steerActuatorDelay) command.actuators.torque = float(torque) else: command.actuators.torque = 0.01 if decision.lateral_active else 0. self.last_normalized_torque = float(command.actuators.torque) _, packets = self.ci.apply(command.as_reader(), self.now) lkas = [packet for packet in packets if packet[0] == 0x340] assert lkas, 'Actual controller must emit LKAS' self.output_parser.update((self.now, lkas)) self.last_torque = self.output_parser.vl['LKAS11']['CR_Lkas_StrToqReq'] for address, data, bus in lkas: assert self.safety.safety_tx_hook(make_msg(bus, address, len(data), data)), 'Actual LKAS rejected' return decision class TestForteIntegration(unittest.TestCase): def tearDown(self): libsafety_py.libsafety.set_alternative_experience(0) def warm(self, stream, **kwargs): for _ in range(12): result = stream.tick(**kwargs) self.assertTrue(stream.cs.canValid) return result def test_both_forte_controllers_reach_native_with_original_torque_limit(self): for car in (CAR.KIA_FORTE_2019_NON_SCC, CAR.KIA_FORTE_2021_NON_SCC): for source in (0, 0x391): for mode in (ControllerMode.STANDARD, ControllerMode.STARPILOT): for direction in (-1., 1.): with self.subTest(car=car, source=source, mode=mode, direction=direction): case = f'{car}, source={source:#x}, controller={mode}, direction={direction}' stream = ForteStream(car=car, source=source, main_action=9, controller_mode=mode) self.assertEqual(stream.cp.safetyConfigs[0].safetyParam, 0x1c00 if source else 0x1400) self.assertEqual(stream.cp.alternativeExperience, 32) self.assertEqual(stream.lateral_controller.controller_mode, mode) self.assertEqual(bool(stream.lateral_controller.starpilot_extension), mode == ControllerMode.STARPILOT) self.assertTrue(self.warm(stream, main=True).lateral_active, case) self.assertGreater(stream.cs.vEgo, 19., case) self.assertEqual(stream.cs.steeringAngleDeg, 0., case) self.assertEqual(stream.cs.steeringTorque, 0., case) self.assertFalse(stream.cs.steeringPressed, case) stream.desired_curvature = direction * .01 for _ in range(180): self.assertTrue(stream.tick(main=True).lateral_active, case) self.assertLessEqual(abs(stream.last_torque), 255, case) context = (f'{case}; vEgo={stream.cs.vEgo}, angle={stream.cs.steeringAngleDeg}, ' + f'driverTorque={stream.cs.steeringTorque}, normalized={stream.last_normalized_torque}, ' + f'sentTorque={stream.last_torque}, factor={stream.lateral_controller.torque_params.latAccelFactor}') self.assertGreater(abs(stream.last_torque), 200, context) self.assertLess(stream.last_torque * direction, 0., context) if mode == ControllerMode.STANDARD: self.assertEqual(stream.last_torque, -direction * 255, context) self.assertFalse(stream.tick(main=False).lateral_active, case) self.assertEqual(stream.last_torque, 0, case) def test_native_reset_requires_neutral_and_new_physical_gesture(self): s = ForteStream() self.warm(s) self.assertTrue(s.tick(bcm=True).lateral_active) s.reset() self.assertFalse(self.warm(s, bcm=True).lateral_active) s.tick(bcm=True, rejection=s.now + 10_000_000) self.assertFalse(s.tick(bcm=True).lateral_active) s.tick() self.assertTrue(s.tick(bcm=True).lateral_active) def test_main_boot_both_button_orders_and_combined_sources(self): s = ForteStream(main_action=9) self.assertTrue(self.warm(s, main=True).lateral_active) self.assertFalse(s.tick(main=True, cruise=True, bcm=True).lateral_active) s.tick(main=True, cruise=True) self.assertTrue(s.tick(main=True, cruise=True, clu=True).lateral_active) t = ForteStream() self.warm(t) self.assertTrue(t.tick(bcm=True).lateral_active) self.assertTrue(t.tick(bcm=True, clu=False).lateral_active) self.assertTrue(t.tick(bcm=True, clu=False).lateral_active) t.tick() self.assertFalse(t.tick(clu=True).lateral_active) def test_wrong_session_stale_receipt_and_output_pause(self): s = ForteStream(main_action=9) self.assertTrue(self.warm(s, main=True).lateral_active) self.assertFalse(s.axis(lambda n: replace(n, axisSessionId='old')).lateral_active) self.assertFalse(s.axis(lambda n: replace(n, validUntilMonoTime=s.now - 1)).lateral_active) self.assertFalse(s.tick(main=True, eps=True).lateral_active) self.assertTrue(s.intent.allowed_latch) self.assertFalse(s.tick(main=True, gear='R').lateral_active) self.assertTrue(s.tick(main=True).lateral_active) self.assertFalse(s.tick(main=True, fatal=True).lateral_active) self.assertFalse(s.tick(main=True).lateral_active) s.tick(main=False) self.assertTrue(s.tick(main=True).lateral_active) def test_50c_only_ordinary_profile_does_not_require_absent_bcm(self): s = ForteStream(source=0x50c, aol=False) self.assertEqual(s.cp.safetyConfigs[0].safetyParam, 0x1000) self.assertEqual(s.cp.alternativeExperience, 0) self.assertFalse(self.warm(s, main=True).lateral_active) s.tick(main=True, cruise_button=2) # Actual SET interaction precedes cruise rising. self.assertFalse(s.tick(main=True, cruise=True).lateral_active) self.assertTrue(s.cs.cruiseState.available) self.assertTrue(s.cs.cruiseState.enabled) self.assertTrue(s.safety.get_controls_allowed()) self.assertEqual(s.safety.aol_get_permission_mask(), 0) def test_invalid_main_off_cannot_rearm_and_old_denial_preserves_new_edge(self): s = ForteStream(main_action=9) self.warm(s, main=True) s.tick(main=True, fatal=True) # A malformed/unhealthy state envelope is not confirmed main-OFF evidence. # Parsed CAN remains the source; the health failure is explicitly injected. s.cs.canValid = False s.cs.cruiseState.available = False s.intent.update(s.cs, now_ns=s.now + 1, fault_active=False) self.assertFalse(s.tick(main=True).lateral_active) s.tick(main=False) self.assertTrue(s.tick(main=True).lateral_active) t = ForteStream() self.warm(t) old_denial = t.now self.assertTrue(t.tick(bcm=True).lateral_active) self.assertTrue(t.tick(bcm=True, rejection=old_denial).lateral_active) def test_unselected_clu_source_grants_only_after_its_own_fresh_neutral(self): s = ForteStream() # BCM arrives first and remains the required native-health alternative. # CLU's independent neutral and press must still reach the port owner. self.warm(s) self.assertFalse(s.safety.get_controls_allowed()) self.assertTrue(s.tick(clu=True).lateral_active) self.assertTrue(s.intent.allowed_latch) self.assertEqual(s.safety.aol_get_permission_mask(), 1) self.assertFalse(s.safety.get_controls_allowed()) self.assertTrue(s.tick(clu=True, bcm=False).lateral_active) s.tick() self.assertFalse(s.tick(clu=True).lateral_active)