import tempfile import unittest from types import SimpleNamespace from unittest.mock import patch from openpilot.starpilot.controller_extensions import ManualTurnInputs, configure_controller from openpilot.starpilot.tests import test_vehicle_preferences as preferences_tests from opendbc.car.ford.values import CAR class TestControllerExtensions(unittest.TestCase): def test_real_saved_preference_default_and_disabled(self): from openpilot.common.params import Params with tempfile.TemporaryDirectory() as directory: params = Params(directory) self.assertTrue(params.get('FordHumanTurnDetection', return_default=True)) params.put_bool('FordHumanTurnDetection', False, block=True) self.assertFalse(params.get('FordHumanTurnDetection', return_default=True)) def test_actual_model_owner_and_cached_preference(self): import openpilot.cereal.messaging as messaging from openpilot.common.params import Params message = messaging.new_message('modelV2') message.modelV2.meta.laneChangeState = 'laneChangeStarting' sm = SimpleNamespace(alive={'modelV2': True}, valid={'modelV2': True}, update=lambda timeout: None) class ModelReader: alive = sm.alive valid = sm.valid def update(self, timeout): sm.update(timeout) def __getitem__(self, key): return message.modelV2 with patch.object(messaging, 'SubMaster', return_value=ModelReader()), patch.object(Params, 'get', return_value=True) as read: inputs = ManualTurnInputs(Params()) self.assertEqual(inputs.update(), (True, True, True)) for _ in range(99): inputs.update() self.assertEqual(read.call_count, 8) inputs.update() self.assertEqual(read.call_count, 12) sm.alive['modelV2'] = False self.assertEqual(inputs.update(), (True, True, False)) def test_model_preview_freshness_delay_and_invalid_output(self): import openpilot.cereal.messaging as messaging from openpilot.selfdrive.modeld.constants import ModelConstants now = 10_000_000_000 model = messaging.new_message('modelV2') model.modelV2.orientationRate.z = [20. * (value / 100.) for value in ModelConstants.T_IDXS] delay = messaging.new_message('lateralDelay') delay.lateralDelay.lateralDelay = .3 class Reader: alive = {'modelV2': True, 'lateralDelay': True} valid = dict(alive) logMonoTime = {'modelV2': now, 'lateralDelay': now} def __getitem__(self, key): return model.modelV2 if key == 'modelV2' else delay.lateralDelay inputs = ManualTurnInputs.__new__(ManualTurnInputs) vars(inputs)['sm'] = Reader() with patch('openpilot.starpilot.controller_extensions.time.monotonic_ns', return_value=now): curvature = inputs.preview_curvature(20.) assert curvature is not None self.assertAlmostEqual(curvature, .003) inputs.sm.logMonoTime['lateralDelay'] = now - 1_000_000_001 curvature = inputs.preview_curvature(20.) assert curvature is not None self.assertAlmostEqual(curvature, .002) for stamp in (0, now + 1, now - 100_000_001): inputs.sm.logMonoTime['modelV2'] = stamp self.assertIsNone(inputs.preview_curvature(20.)) inputs.sm.logMonoTime['modelV2'] = now self.assertIsNone(inputs.preview_curvature(0.)) model.modelV2.orientationRate.z = [float('nan')] * 33 self.assertIsNone(inputs.preview_curvature(20.)) def test_assist_permission_tracks_panda_health_without_affecting_preview(self): import openpilot.cereal.messaging as messaging from openpilot.common.params import Params class Reader(dict): def __init__(self): super().__init__(pandaStates=[SimpleNamespace(controlsAllowed=True)]) self.alive = self.valid = self.freq_ok = True def all_checks(self, services): self.asserted_services = services return self.alive and self.valid and self.freq_ok reader = Reader() with patch.object(messaging, 'SubMaster', return_value=reader) as factory, patch.object(Params, 'get', return_value=True): inputs = ManualTurnInputs(Params()) self.assertNotIn('pandaStates', factory.call_args.args[0]) self.assertEqual(inputs.assist_permission(), ((), False)) inputs = ManualTurnInputs(Params(), track_assist_permission=True) self.assertIn('pandaStates', factory.call_args.args[0]) self.assertEqual(inputs.assist_permission(), (reader['pandaStates'], True)) for health in ('alive', 'valid', 'freq_ok'): setattr(reader, health, False) self.assertEqual(inputs.assist_permission(), (reader['pandaStates'], False)) setattr(reader, health, True) self.assertEqual(reader.asserted_services, ['pandaStates']) def test_real_card_finalizes_then_binds_only_admitted_controller(self): harness = preferences_tests.TestVehicleStartupPreferences(methodName='test_only_exact_saved_opt_in_is_loaded') harness.setUp() self.addCleanup(harness.doCleanups) with patch('openpilot.starpilot.controller_extensions.messaging.SubMaster', return_value=SimpleNamespace()): host, _, published = harness.start(CAR.FORD_MUSTANG_MACH_E_MK1) self.assertFalse(published.passive) self.assertIsInstance(host.CI.CC.manual_turn_inputs, ManualTurnInputs) self.assertIs(host.CI.CC.manual_turn_inputs.params, harness.params) self.assertTrue(host.CI.CC.manual_turn_inputs.track_assist_permission) host, _, published = harness.start(CAR.FORD_MUSTANG_MACH_E_MK1, enabled=False) self.assertTrue(published.passive) self.assertIsNone(host.CI.CC.manual_turn_inputs) def test_standalone_controller_has_no_host_owner(self): from opendbc.car.ford.tests.test_three_ports import params from opendbc.car.ford.carcontroller import CarController from opendbc.car.ford.values import DBC from opendbc.car.ford.carstate import CarState from opendbc.car import structs cp = params(CAR.FORD_MUSTANG_MACH_E_MK1) controller = CarController(DBC[cp.carFingerprint], cp) state = CarState(cp) state.update(state.get_can_parsers(cp)) state.out = structs.CarState().as_reader() controller.update(structs.CarControl().as_reader(), state, 0) self.assertIsNone(controller.manual_turn_inputs) cp.passive = True configure_controller(SimpleNamespace(CP=cp, CC=controller), SimpleNamespace()) self.assertIsNone(controller.manual_turn_inputs) class TestAngleSnapshotParity(unittest.TestCase): def test_angle_delay_reaches_pinned_bp7_without_changing_curvature_snapshot(self): import openpilot.cereal.messaging as messaging from openpilot.selfdrive.modeld.constants import ModelConstants from opendbc.bluepilot_lateral.core.angle import AngleController, Inputs from opendbc.bluepilot_lateral.tests import reference from dataclasses import replace now = 10_000_000_000 model = messaging.new_message('modelV2') model.modelV2.orientationRate.z = [5. * (.001 + .004 * value) for value in ModelConstants.T_IDXS] delay = messaging.new_message('lateralDelay') class Reader: alive = {'modelV2': True, 'lateralDelay': True} valid = dict(alive) logMonoTime = {'modelV2': now, 'lateralDelay': now} def __getitem__(self, key): return model.modelV2 if key == 'modelV2' else delay.lateralDelay provider = ManualTurnInputs.__new__(ManualTurnInputs) vars(provider)['sm'] = Reader() provider.enabled = True with patch('openpilot.starpilot.controller_extensions.time.monotonic_ns', return_value=now): for value in (.05, .10, .125, .15, .30): delay.lateralDelay.lateralDelay = value snapshot = provider.lateral_snapshot(5., angle=True) self.assertIsNotNone(snapshot) self.assertAlmostEqual(snapshot[2], min(.15, max(.10, value))) self.assertAlmostEqual(provider.lateral_snapshot(5.)[2], min(.4, max(.2, value))) adapted, upstream = AngleController(low_gain=1., high_gain=1.05), reference.Angle(CAR.FORD_MUSTANG_MACH_E_MK1) for _ in range(20): common = Inputs(active=True, speed=5., measured_curvature=.001, desired_curvature=.001, yaw_predictions=tuple(snapshot[0].orientationRate.z), time_indices=tuple(snapshot[1]), lateral_delay=snapshot[2]) command = adapted.step(common) expected = upstream.step(replace(common, lateral_delay=delay.lateralDelay.lateralDelay)) self.assertTrue(command.active) self.assertAlmostEqual(command.path_angle, expected.path_angle, places=12) provider.sm.logMonoTime['lateralDelay'] = now - 1_000_000_001 self.assertAlmostEqual(provider.lateral_snapshot(5., angle=True)[2], .15) self.assertAlmostEqual(provider.lateral_snapshot(5.)[2], .2) for stamp in (0, now + 1, now - 100_000_001): provider.sm.logMonoTime['modelV2'] = stamp self.assertIsNone(provider.lateral_snapshot(5., angle=True)) self.assertIsNone(provider.lateral_snapshot(5.))