"""Native Corolla torque source selection, saved choices, and controller gates.""" import math import os from pathlib import Path from types import SimpleNamespace import unittest from unittest import mock from openpilot.cereal import messaging from openpilot.common.params import ParamKeyFlag, Params from openpilot.common.prefix import OpenpilotPrefix from openpilot.selfdrive.controls.controlsd import Controls from openpilot.selfdrive.locationd.torqued import TorqueEstimator from openpilot.starpilot.lateral.tests.test_lane_runtime import feed, publish_corolla_params from openpilot.starpilot.lateral.controller_selection import DOCUMENT_KEY as CONTROLLER_KEY, ControllerMode, replace_mode from openpilot.starpilot.lateral.torque_runtime import TorqueHost, development_enabled, read_settings from openpilot.starpilot.lateral.torque_tuning import TorqueSource from opendbc.car.car_helpers import interfaces from opendbc.car.toyota.values import CAR def corolla(): return interfaces[CAR.TOYOTA_COROLLA_TSS2].get_non_essential_params(CAR.TOYOTA_COROLLA_TSS2) class LearnedFrame: def __init__(self, now_ns, *, factor=2.0, offset=0.1, friction=0.1, version=1, valid=True): self.logMonoTime = {'lateralTorqueParameters': now_ns} self.recv_time = {'lateralTorqueParameters': now_ns / 1e9} self.checked = True self.state = SimpleNamespace(useParams=True, valid=valid, version=version, latAccelFactorFiltered=factor, latAccelOffsetFiltered=offset, frictionCoefficientFiltered=friction) def all_checks(self, _names): return self.checked def __getitem__(self, _name): return self.state class TorqueRuntimeTests(unittest.TestCase): def test_saved_partial_precedence_and_corrupt_raw_bytes(self): with OpenpilotPrefix(): params, cp = Params(), corolla() host = TorqueHost(params, cp) base = host.vehicle self.assertIsNone(read_settings(params, base).user_friction) params.put_bool('AdvancedLateralTune', True, block=True) params.put('SteerLatAccel', base.lat_accel_factor * 1.2, block=True) params.put('SteerFriction', min(1.0, base.friction * 1.2), block=True) settings = read_settings(params, base) self.assertTrue(settings.valid) host.settings = settings learned = LearnedFrame(1_000_000_000, factor=base.lat_accel_factor * 1.1, offset=0.1, friction=base.friction * 1.1) selected = host._select(learned, 1_000_000_000) self.assertEqual(selected.source, TorqueSource.USER) self.assertEqual(selected.lat_accel_factor, settings.user_factor) self.assertEqual(selected.lat_accel_offset, base.lat_accel_offset) self.assertEqual(selected.friction, settings.user_friction) Path(params.get_param_path('SteerLatAccel')).write_bytes(b'nan') self.assertFalse(read_settings(params, base).valid) self.assertTrue(params.get_bool('AdvancedLateralTune')) host.settings = read_settings(params, base) self.assertEqual(host._select(learned, 1_000_000_000), base) self.assertEqual(host.sample(learned, now_ns=1_000_000_000, lat_active=True), base) def test_frozen_auto_synced_stock_and_individual_overrides(self): with OpenpilotPrefix(): params, cp = Params(), corolla() host = TorqueHost(params, cp) base = host.vehicle now = 1_000_000_000 learned = LearnedFrame(now, factor=base.lat_accel_factor * 1.1, offset=0.1, friction=base.friction * 1.1) params.put_bool('AdvancedLateralTune', True, block=True) params.put('SteerLatAccel', base.lat_accel_factor, block=True) params.put('SteerFriction', base.friction, block=True) host.settings = read_settings(params, base) self.assertIsNone(host.settings.user_factor) self.assertIsNone(host.settings.user_friction) self.assertEqual(host._select(learned, now).source, TorqueSource.LEARNED) params.put('SteerLatAccel', base.lat_accel_factor * 1.2, block=True) host.settings = read_settings(params, base) factor_only = host._select(learned, now) self.assertEqual(factor_only.source, TorqueSource.USER) self.assertAlmostEqual(factor_only.lat_accel_factor, base.lat_accel_factor * 1.2) self.assertEqual(factor_only.lat_accel_offset, base.lat_accel_offset) self.assertAlmostEqual(factor_only.friction, base.friction * 1.1) params.put('SteerLatAccel', base.lat_accel_factor, block=True) params.put('SteerFriction', base.friction * 1.2, block=True) host.settings = read_settings(params, base) friction_only = host._select(learned, now) self.assertEqual(friction_only.source, TorqueSource.USER) self.assertAlmostEqual(friction_only.lat_accel_factor, base.lat_accel_factor * 1.1) self.assertAlmostEqual(friction_only.lat_accel_offset, 0.1) self.assertAlmostEqual(friction_only.friction, base.friction * 1.2) # A frozen stock-sync file distinguishes an old stock save from a user edit # after a later CarParams retune changes the current stock value. old_stock = base.lat_accel_factor * 0.9 params.put('SteerLatAccel', old_stock, block=True) params.put('SteerLatAccelStock', old_stock, block=True) params.clear_all(ParamKeyFlag.CLEAR_ON_MANAGER_START) self.assertAlmostEqual(params.get('SteerLatAccelStock'), old_stock) host.settings = read_settings(params, base) self.assertIsNone(host.settings.user_factor) self.assertAlmostEqual(host._select(learned, now).lat_accel_factor, learned.state.latAccelFactorFiltered) # Old untouched stock may be outside today's safe custom range: it is a # stock-tracking marker, not a custom override to reject or apply. older_stock = base.lat_accel_factor * 2.0 params.put('SteerLatAccel', older_stock, block=True) params.put('SteerLatAccelStock', older_stock, block=True) host.settings = read_settings(params, base) self.assertTrue(host.settings.valid) self.assertIsNone(host.settings.user_factor) self.assertAlmostEqual(host._select(learned, now).lat_accel_factor, learned.state.latAccelFactorFiltered) params.put('SteerFriction', base.friction, block=True) params.put_bool('ForceAutoTuneOff', True, block=True) host.settings = read_settings(params, base) self.assertEqual(host._select(learned, now), base) def test_malformed_saved_bool_and_numeric_settings_fail_closed(self): with OpenpilotPrefix(): params, cp = Params(), corolla() host = TorqueHost(params, cp) params.put_bool('AdvancedLateralTune', True, block=True) for key, bad in (('ForceAutoTuneOff', b'2'), ('SteerLatAccel', b'nan'), ('SteerFriction', b'inf'), ('SteerLatAccelStock', b'bad'), ('SteerFrictionStock', b'bad')): with self.subTest(key=key): path = Path(params.get_param_path(key)) original = path.read_bytes() if path.exists() else None path.write_bytes(bad) self.assertFalse(read_settings(params, host.vehicle).valid) if original is None: path.unlink() else: path.write_bytes(original) def test_learned_source_freshness_version_and_override(self): with OpenpilotPrefix(): params, cp = Params(), corolla() host = TorqueHost(params, cp) base = host.vehicle frame = LearnedFrame(1_000_000_000, factor=base.lat_accel_factor * 1.1, offset=0.1, friction=base.friction * 1.1) self.assertEqual(host._select(frame, 1_000_000_000).source, TorqueSource.LEARNED) frame.state.version = 99 self.assertEqual(host._select(frame, 1_000_000_000), base) frame.state.version = 1 frame.logMonoTime['lateralTorqueParameters'] = 1 self.assertEqual(host._select(frame, 1_000_000_000), base) frame.logMonoTime['lateralTorqueParameters'] = 1_000_000_000 frame.checked = False self.assertEqual(host._select(frame, 1_000_000_000), base) params.put_bool('AdvancedLateralTune', True, block=True) params.put_bool('ForceAutoTuneOff', True, block=True) host.settings = read_settings(params, base) frame.checked = True self.assertEqual(host._select(frame, 1_000_000_000), base) def test_exact_car_development_policy(self): cp = corolla() with mock.patch.dict(os.environ, {'TORQUE_REPLAY_RUNTIME': '1'}): self.assertTrue(development_enabled(cp)) cp.carFingerprint = 'TOYOTA_RAV4_TSS2_2023' self.assertTrue(development_enabled(cp)) for field in ('passive', 'dashcamOnly', 'notCar'): invalid = cp.as_reader().as_builder() setattr(invalid, field, True) with self.subTest(field=field): self.assertFalse(development_enabled(invalid)) invalid = cp.as_reader().as_builder() invalid.carFingerprint = 'UNREGISTERED_TORQUE_VEHICLE' self.assertFalse(development_enabled(invalid)) invalid = cp.as_reader().as_builder() invalid.lateralTuning.torque.latAccelFactor = math.nan self.assertFalse(development_enabled(invalid)) with mock.patch.dict(os.environ, {'TORQUE_REPLAY_RUNTIME': '0'}): self.assertFalse(development_enabled(corolla())) def test_opt_in_learner_skips_old_cache_without_deleting_it(self): cp = corolla() with OpenpilotPrefix(), mock.patch('openpilot.selfdrive.locationd.torqued.get_cache', return_value=None) as get_cache: Params().put('SteerFriction', cp.lateralTuning.torque.friction * 1.2, block=True) with mock.patch.dict(os.environ, {'TORQUE_REPLAY_RUNTIME': '1'}): fresh = TorqueEstimator(cp.as_reader()) self.assertEqual(get_cache.call_count, 0) self.assertEqual(fresh.filtered_params['latAccelFactor'].x, cp.lateralTuning.torque.latAccelFactor) cp.carFingerprint = 'TOYOTA_RAV4_TSS2_2023' with mock.patch.dict(os.environ, {'TORQUE_REPLAY_RUNTIME': '1'}): TorqueEstimator(cp.as_reader()) self.assertEqual(get_cache.call_count, 0) self.assertAlmostEqual(float(Params().get('SteerFriction')), cp.lateralTuning.torque.friction * 1.2) Params().remove('SteerFriction') with mock.patch.dict(os.environ, {'TORQUE_REPLAY_RUNTIME': '1'}): TorqueEstimator(cp.as_reader()) self.assertEqual(get_cache.call_count, 2) def test_native_controls_default_equivalence_and_selected_torque(self): with OpenpilotPrefix(), mock.patch.dict(os.environ, {'REPLAY': '1', 'AOL_REPLAY_RUNTIME': '0', 'LANE_CENTERING_REPLAY_RUNTIME': '0'}), \ mock.patch('openpilot.selfdrive.controls.controlsd.messaging.PubMaster'): params = Params() publish_corolla_params(params) with mock.patch.dict(os.environ, {'TORQUE_REPLAY_RUNTIME': '0'}): baseline = Controls() with mock.patch.dict(os.environ, {'TORQUE_REPLAY_RUNTIME': '1'}): selected = Controls() self.assertIsNone(baseline.torque_host) self.assertIsNotNone(selected.torque_host) for tick in range(110): now = 1_000_000_000 + tick * 10_000_000 for controls in (baseline, selected): feed(controls, now, tick) a, _ = baseline.state_control() b, _ = selected.state_control() self.assertEqual(a.to_dict(), b.to_dict()) params.put_bool('AdvancedLateralTune', True, block=True) native_factor = selected.torque_host.vehicle.lat_accel_factor params.put('SteerLatAccel', native_factor * 1.3, block=True) torque_differences = [] for tick in range(110, 230): now = 1_000_000_000 + tick * 10_000_000 for controls in (baseline, selected): feed(controls, now, tick) reference, _ = baseline.state_control() command, _ = selected.state_control() torque_differences.append(abs(command.actuators.torque - reference.actuators.torque)) self.assertTrue(command.latActive) self.assertGreater(selected.torque_host.applied.lat_accel_factor, native_factor) self.assertLessEqual(selected.torque_host.applied.lat_accel_factor, native_factor * 1.3) self.assertGreater(selected.LaC.pid.pos_limit, baseline.LaC.pid.pos_limit) self.assertGreater(max(torque_differences), 1e-6) feed(selected, now + 10_000_000, 230, active=False, enabled=False) off, _ = selected.state_control() self.assertFalse(off.latActive) self.assertEqual(off.actuators.torque, 0.0) self.assertEqual(selected.torque_host.applied, selected.torque_host.vehicle) self.assertEqual(selected.LaC.pid.i, 0.0) feed(selected, now + 20_000_000, 231, can_valid=False) invalid, _ = selected.state_control() self.assertFalse(invalid.latActive) self.assertEqual(invalid.actuators.torque, 0.0) feed(selected, now + 30_000_000, 232) resumed, _ = selected.state_control() self.assertTrue(resumed.latActive) self.assertLess(abs(resumed.actuators.torque), selected.LaC.steer_max) feed(selected, now + 40_000_000, 233, can_timeout=True) timeout, _ = selected.state_control() self.assertFalse(timeout.latActive) self.assertEqual(timeout.actuators.torque, 0.0) feed(selected, now + 50_000_000, 234, override=True) prior_integral = selected.LaC.pid.i override, _ = selected.state_control() self.assertTrue(override.latActive) self.assertTrue(math.isfinite(override.actuators.torque)) self.assertEqual(selected.LaC.pid.i, prior_integral) def test_actual_controls_learned_to_user_to_stale_source(self): with OpenpilotPrefix(), mock.patch.dict(os.environ, {'REPLAY': '1', 'TORQUE_REPLAY_RUNTIME': '1', 'AOL_REPLAY_RUNTIME': '0', 'LANE_CENTERING_REPLAY_RUNTIME': '0'}), \ mock.patch('openpilot.selfdrive.controls.controlsd.messaging.PubMaster'): params = Params() cp = publish_corolla_params(params) Path(params.get_param_path(CONTROLLER_KEY)).write_bytes(replace_mode(None, cp, ControllerMode.STANDARD)) controls = Controls() base = controls.torque_host.vehicle target = base.lat_accel_factor * 1.1 for tick in range(120): now = 1_000_000_000 + tick * 10_000_000 feed(controls, now, tick) if tick % 25 == 0: event = messaging.new_message('lateralTorqueParameters', valid=True, logMonoTime=now) state = event.lateralTorqueParameters state.useParams = state.valid = True state.version = 1 state.latAccelFactorFiltered = target state.latAccelOffsetFiltered = 0.1 state.frictionCoefficientFiltered = base.friction controls.sm.update_msgs(now / 1e9, [event.as_reader()]) controls.state_control() self.assertEqual(controls.torque_host.selected.source, TorqueSource.LEARNED) self.assertGreater(controls.torque_host.applied.lat_accel_factor, base.lat_accel_factor) params.put_bool('AdvancedLateralTune', True, block=True) params.put('SteerLatAccel', base.lat_accel_factor * 1.2, block=True) for tick in range(120, 230): now = 1_000_000_000 + tick * 10_000_000 feed(controls, now, tick) controls.state_control() self.assertEqual(controls.torque_host.selected.source, TorqueSource.USER) self.assertEqual(controls.torque_host.selected.lat_accel_offset, base.lat_accel_offset) self.assertGreater(controls.torque_host.applied.lat_accel_factor, target) params.put_bool('AdvancedLateralTune', False, block=True) params.put('SteerLatAccel', base.lat_accel_factor, block=True) for tick in range(230, 340): now = 1_000_000_000 + tick * 10_000_000 feed(controls, now, tick) controls.state_control() self.assertEqual(controls.torque_host.selected, base) self.assertAlmostEqual(controls.torque_host.applied.lat_accel_factor, base.lat_accel_factor) if __name__ == '__main__': unittest.main()