diff --git a/openpilot/starpilot/galaxy/tests/test_settings.py b/openpilot/starpilot/galaxy/tests/test_settings.py index a8d815da40..05db377303 100644 --- a/openpilot/starpilot/galaxy/tests/test_settings.py +++ b/openpilot/starpilot/galaxy/tests/test_settings.py @@ -1121,7 +1121,7 @@ class SettingsGatewayTest(unittest.TestCase): from opendbc.car.mazda.values import CAR from openpilot.starpilot.lateral.controller_selection import policy_for cp = interfaces[CAR.MAZDA_CX5_2022].get_non_essential_params(CAR.MAZDA_CX5_2022) - self.assertIsNone(policy_for(cp)) + self.assertEqual(policy_for(cp), 'generic') self.context.value = AuthorityContext(True, cp, cp.as_reader().as_builder().to_bytes()) page = self.page('torque') index = next(i for i, row in enumerate(page['rows']) if row['label'] == 'Turn Assist') diff --git a/openpilot/starpilot/lateral/controller_selection.py b/openpilot/starpilot/lateral/controller_selection.py index c2e35dbb36..3457534207 100644 --- a/openpilot/starpilot/lateral/controller_selection.py +++ b/openpilot/starpilot/lateral/controller_selection.py @@ -5,9 +5,11 @@ from __future__ import annotations from dataclasses import dataclass from enum import StrEnum import json +import math from pathlib import Path from opendbc.car import structs +from opendbc.car.values import PLATFORMS DOCUMENT_KEY = 'LateralControllerSelection' @@ -62,6 +64,25 @@ KNOWN_POLICIES.update(dict.fromkeys(( 'HYUNDAI_KONA_EV_2022', ), 'hyundai')) +KNOWN_POLICIES.update({str(identity): platform.__class__.__module__.split('.')[-2] + for identity, platform in PLATFORMS.items()}) + + +def generic_torque_supported(cp) -> bool: + try: + platform = PLATFORMS.get(str(cp.carFingerprint)) + if (platform is None or platform.__class__.__module__.split('.')[-2] != str(cp.brand) or + cp.notCar or cp.passive or cp.dashcamOnly or + cp.steerControlType != structs.CarParams.SteerControlType.torque or cp.lateralTuning.which() != 'torque'): + return False + tune = cp.lateralTuning.torque + return (math.isfinite(tune.latAccelFactor) and tune.latAccelFactor > 0 and + math.isfinite(tune.latAccelOffset) and math.isfinite(tune.friction) and tune.friction >= 0 and + math.isfinite(tune.steeringAngleDeadzoneDeg) and tune.steeringAngleDeadzoneDeg >= 0) + except (AttributeError, TypeError, ValueError, OverflowError): + return False + + class ControllerMode(StrEnum): STANDARD = 'standard' STARPILOT = 'starpilot' @@ -75,7 +96,7 @@ class ControllerSelection: def policy_for(CP) -> str | None: - """Return only an already-implemented, exact torque policy identity.""" + """Prefer exact vehicle policies, otherwise use the shared torque law.""" if CP.notCar: return None from openpilot.starpilot.lateral.hkg_vehicle_policy import policy_for as hkg_vehicle_policy @@ -132,7 +153,7 @@ def policy_for(CP) -> str | None: CP.steerControlType == structs.CarParams.SteerControlType.torque and CP.lateralTuning.which() == 'torque' and not CP.dashcamOnly and not CP.passive): return 'ioniq6' - return None + return "generic" if generic_torque_supported(CP) else None def turn_assist_supported(CP) -> bool: @@ -151,7 +172,7 @@ def default_selection(CP) -> ControllerSelection: policy = policy_for(CP) from openpilot.starpilot.lateral.hkg_vehicle_policy import PROFILES forte_non_scc = policy == 'kia_forte' and str(CP.carFingerprint) in ('KIA_FORTE_2019_NON_SCC', 'KIA_FORTE_2021_NON_SCC') - mode = ControllerMode.STARPILOT if policy and (policy not in PROFILES or forte_non_scc) else ControllerMode.STANDARD + mode = ControllerMode.STARPILOT if policy and policy != 'generic' and (policy not in PROFILES or forte_non_scc) else ControllerMode.STANDARD return ControllerSelection(mode, policy, 'default') @@ -239,9 +260,12 @@ def learning_allowed(params, CP, *, selection: ControllerSelection | None = None preference = read_preference(params, CP) if not preference.valid or not preference.enabled: return False - if policy_for(CP) is None: + policy = policy_for(CP) + if policy is None: return True selected = read_selection(params, CP) if selection is None else selection + if policy == 'generic': + return selected.source != 'invalid' from openpilot.starpilot.lateral.gm_geometry_runtime import read_geometry geometry = read_geometry(params, CP) if geometry.force_off: diff --git a/openpilot/starpilot/lateral/generic_torque_policy.py b/openpilot/starpilot/lateral/generic_torque_policy.py new file mode 100644 index 0000000000..19b0a2625f --- /dev/null +++ b/openpilot/starpilot/lateral/generic_torque_policy.py @@ -0,0 +1,19 @@ +"""Shared StarPilot torque shaping using the vehicle's own calibration and conversion.""" + +from openpilot.common.scalar_clip import clip_scalar +from openpilot.starpilot.lateral.controller_selection import generic_torque_supported +from openpilot.starpilot.lateral.hkg_torque_policy import HKGShapedTorquePolicy +from openpilot.starpilot.lateral.torque_shaping import get_standard_friction_threshold + + +class GenericTorquePolicy(HKGShapedTorquePolicy): + supported_cp = staticmethod(generic_torque_supported) + + def feedforward(self, value, setpoint, jerk, speed): + return value + + def friction(self, setpoint, jerk, speed): + return get_standard_friction_threshold(speed), 1.0 + + def output(self, value, setpoint, speed): + return clip_scalar(value, -self.parent.steer_max, self.parent.steer_max) diff --git a/openpilot/starpilot/lateral/tests/test_controller_selection.py b/openpilot/starpilot/lateral/tests/test_controller_selection.py index 90d50d2b98..94a4c2229d 100644 --- a/openpilot/starpilot/lateral/tests/test_controller_selection.py +++ b/openpilot/starpilot/lateral/tests/test_controller_selection.py @@ -95,10 +95,11 @@ class TestControllerSelection(unittest.TestCase): wrong = ioniq.as_reader().as_builder() wrong.notCar = True self.assertIsNone(policy_for(wrong)) - with self.assertRaises(ValueError): - unsupported = cp_for(TOYOTA.TOYOTA_RAV4_TSS2_2022) - LatControlTorque(unsupported.as_reader(), interfaces[TOYOTA.TOYOTA_RAV4_TSS2_2022](unsupported), DT_CTRL, - controller_mode=ControllerMode.STARPILOT) + generic = cp_for(TOYOTA.TOYOTA_RAV4_TSS2_2022) + controller = LatControlTorque(generic.as_reader(), interfaces[TOYOTA.TOYOTA_RAV4_TSS2_2022](generic), DT_CTRL, + controller_mode=ControllerMode.STARPILOT) + self.assertEqual(controller.controller_policy, 'generic') + self.assertIsNotNone(selected_policy(controller)) def test_standard_skips_policy_constructor_and_ioniq_factor_multiplier(self): for vehicle in (HYUNDAI.HYUNDAI_IONIQ_6, HYUNDAI.GENESIS_G70_2020, TOYOTA.TOYOTA_COROLLA_TSS2): diff --git a/openpilot/starpilot/lateral/tests/test_genesis_gv70_policy.py b/openpilot/starpilot/lateral/tests/test_genesis_gv70_policy.py index 9cc594d6f2..2915559c61 100644 --- a/openpilot/starpilot/lateral/tests/test_genesis_gv70_policy.py +++ b/openpilot/starpilot/lateral/tests/test_genesis_gv70_policy.py @@ -76,10 +76,10 @@ def test_exact_admission_and_saved_standard_keeps_upstream_controller(): changed = cp.as_reader().as_builder() changed.lateralTuning.init('pid') assert not policy.supported_cp(changed) - for vehicle in (CAR.GENESIS_GV70_1ST_GEN, CAR.GENESIS_GV70_ELECTRIFIED_2ND_GEN): + for vehicle, expected in ((CAR.GENESIS_GV70_1ST_GEN, 'generic'), (CAR.GENESIS_GV70_ELECTRIFIED_2ND_GEN, None)): sibling = interfaces[vehicle].get_non_essential_params(vehicle) assert not policy.supported_cp(sibling) - assert policy_for(sibling) is None + assert policy_for(sibling) == expected def test_stabilization_only_changes_feedforward_feedback_remains_immediate(): diff --git a/openpilot/starpilot/lateral/tests/test_global_controller_choice.py b/openpilot/starpilot/lateral/tests/test_global_controller_choice.py new file mode 100644 index 0000000000..264973d2b8 --- /dev/null +++ b/openpilot/starpilot/lateral/tests/test_global_controller_choice.py @@ -0,0 +1,149 @@ +import json +import math +from types import SimpleNamespace + +import pytest + +from opendbc.car import DT_CTRL, structs +from opendbc.car.car_helpers import interfaces +from opendbc.car.hyundai.values import CAR +from opendbc.car.vehicle_model import VehicleModel +from openpilot.common.params import Params +from openpilot.selfdrive.controls.lib.latcontrol_torque import LatControlTorque +from openpilot.starpilot.lateral.controller_selection import ( + DOCUMENT_KEY, ControllerMode, default_selection, learning_allowed, + parse_document, policy_for, read_selection, replace_mode, +) +from openpilot.starpilot.lateral.torque_extension import selected_policy +from openpilot.starpilot.ui.feature_settings_owner import FeatureSettingsOwner +from openpilot.starpilot.ui.feature_settings_state import row_change + + +def cp_for(identity): + return interfaces[identity].get_non_essential_params(identity) + + +def owner_for(params, cp): + return FeatureSettingsOwner(params, lambda group: True, vehicle_fingerprint=lambda: str(cp.carFingerprint), + vehicle_params=lambda: cp) + + +def test_niro_2023_choice_is_visible_and_saves(tmp_path): + params = Params(str(tmp_path)) + cp = cp_for(CAR.KIA_NIRO_EV_2ND_GEN) + assert cp.steerControlType == structs.CarParams.SteerControlType.torque + assert cp.lateralTuning.which() == 'torque' + owner = owner_for(params, cp) + snapshot = owner.snapshot('torque', parked=True, system_long=False, lateral_context=False, metric=False) + row = next((row for row in snapshot.rows if row.key == DOCUMENT_KEY), None) + assert row is not None, '2023 Niro EV must expose the controller selector' + assert row.choices == ('Stock Controller', 'StarPilot Controller') + assert row.value == 'Stock Controller' + request = row_change(row) + assert request is not None and request.value == 'StarPilot Controller' + assert owner.apply(request) + assert read_selection(params, cp).mode == ControllerMode.STARPILOT + assert not owner.apply(request), 'stale expected bytes must remain rejected' + ci = interfaces[CAR.KIA_NIRO_EV_2ND_GEN](cp) + controller = LatControlTorque(cp.as_reader(), ci, DT_CTRL, controller_mode=read_selection(params, cp).mode) + assert selected_policy(controller) is not None + assert controller.controller_policy == 'generic' + assert controller.torque_params.to_dict() == cp.lateralTuning.torque.to_dict() + + +def test_every_registered_torque_tune_can_save_both_choices(): + from openpilot.starpilot.lateral.controller_selection import generic_torque_supported + checked = [] + for identity in interfaces: + cp = cp_for(identity) + if not generic_torque_supported(cp): + continue + assert policy_for(cp) is not None, identity + for mode in ControllerMode: + raw = replace_mode(None, cp, mode) + assert parse_document(raw)['vehicles'][str(identity)]['brand'] == str(cp.brand) + from openpilot.starpilot.lateral.controller_selection import selection_from_bytes + selected = selection_from_bytes(cp, raw) + assert selected.mode == mode and selected.source == 'saved', identity + checked.append(str(identity)) + assert str(CAR.KIA_NIRO_EV_2ND_GEN) in checked + assert len(checked) > 100 + print('Registered torque controller selection coverage:', len(checked)) + + +def test_saved_vehicle_choices_remain_independent(): + from opendbc.car.honda.values import CAR as HONDA + niro = cp_for(CAR.KIA_NIRO_EV_2ND_GEN) + honda = cp_for(HONDA.HONDA_FIT_4G) + raw = replace_mode(None, niro, ControllerMode.STARPILOT) + raw = replace_mode(raw, honda, ControllerMode.STANDARD) + from openpilot.starpilot.lateral.controller_selection import selection_from_bytes + assert selection_from_bytes(niro, raw).mode == ControllerMode.STARPILOT + assert selection_from_bytes(honda, raw).mode == ControllerMode.STANDARD + + +@pytest.mark.parametrize('failure', ['brand', 'unknown', 'angle', 'pid', 'passive', 'dashcam', 'notCar', 'nan', 'factor', 'friction']) +def test_generic_refuses_invalid_controller_configuration(failure): + from openpilot.starpilot.lateral.controller_selection import generic_torque_supported + cp = cp_for(CAR.KIA_NIRO_EV_2ND_GEN) + if failure == 'brand': + cp.brand = 'toyota' + elif failure == 'unknown': + cp.carFingerprint = 'UNREGISTERED' + elif failure == 'angle': + cp.steerControlType = structs.CarParams.SteerControlType.angle + elif failure == 'pid': + cp.lateralTuning.init('pid') + elif failure in ('passive', 'dashcam', 'notCar'): + setattr(cp, 'dashcamOnly' if failure == 'dashcam' else failure, True) + elif failure == 'nan': + cp.lateralTuning.torque.latAccelFactor = float('nan') + elif failure == 'factor': + cp.lateralTuning.torque.latAccelFactor = 0 + elif failure == 'friction': + cp.lateralTuning.torque.friction = -1 + assert not generic_torque_supported(cp) + assert policy_for(cp) is None + + +def test_generic_defaults_and_existing_learning_are_preserved(tmp_path): + params = Params(str(tmp_path)) + cp = cp_for(CAR.KIA_NIRO_EV_2ND_GEN) + assert default_selection(cp).mode == ControllerMode.STANDARD + assert learning_allowed(params, cp) + params.put_bool('ForceAutoTuneOff', True, block=True) + assert not learning_allowed(params, cp) + params.put(DOCUMENT_KEY, json.loads(replace_mode(None, cp, ControllerMode.STARPILOT)), block=True) + assert not learning_allowed(params, cp) + + +def test_shared_controller_uses_real_interfaces_and_keeps_output_limits(): + checked = set() + for identity in interfaces: + cp = cp_for(identity) + if policy_for(cp) != 'generic' or str(cp.brand) in checked: + continue + checked.add(str(cp.brand)) + ci = interfaces[identity](cp) + standard = LatControlTorque(cp.as_reader(), ci, DT_CTRL, controller_mode=ControllerMode.STANDARD) + custom = LatControlTorque(cp.as_reader(), ci, DT_CTRL, controller_mode=ControllerMode.STARPILOT) + assert standard.starpilot_extension is None + assert custom.starpilot_extension is not None + assert custom.torque_params.to_dict() == standard.torque_params.to_dict() + for acceleration in (-1., -.2, 0., .2, 1.): + assert (custom.torque_from_lateral_accel(acceleration, custom.torque_params) == + standard.torque_from_lateral_accel(acceleration, standard.torque_params)) + assert custom.pid.pos_limit == standard.pid.pos_limit + assert custom.pid.neg_limit == standard.pid.neg_limit + state = structs.CarState.new_message() + vm = VehicleModel(cp) + params = SimpleNamespace(angleOffsetDeg=0., roll=0.01) + for active, speed, curvature, pressed, limited in ((False, 0., 0., False, False), (True, 12., .001, False, False), + (True, 25., -.001, False, True), (True, 15., .002, True, False), (False, 15., .002, False, False)): + state.vEgo, state.steeringPressed = speed, pressed + output, _, message = custom.update(active, state, vm, params, limited, curvature, False, .2) + assert math.isfinite(output) and abs(output) <= custom.steer_max + assert message.active == active + if not active: + assert output == 0 + assert 'hyundai' in checked and len(checked) >= 3 diff --git a/openpilot/starpilot/lateral/tests/test_global_torque_editor.py b/openpilot/starpilot/lateral/tests/test_global_torque_editor.py index 6c909a9028..235660a649 100644 --- a/openpilot/starpilot/lateral/tests/test_global_torque_editor.py +++ b/openpilot/starpilot/lateral/tests/test_global_torque_editor.py @@ -92,7 +92,7 @@ def test_generic_partial_manual_tune_honors_scoped_learning_preference(enabled): from openpilot.starpilot.lateral.tests.test_torque_family import learner from openpilot.starpilot.lateral.torque_runtime import TorqueHost, read_settings cp = interfaces['HONDA_FIT_4G'].get_non_essential_params('HONDA_FIT_4G') - assert cp.lateralTuning.which() == 'torque' and policy_for(cp) is None + assert cp.lateralTuning.which() == 'torque' and policy_for(cp) == 'generic' with OpenpilotPrefix(): params = Params() params.put(KEY, replace_preference(None, cp, enabled), block=True) diff --git a/openpilot/starpilot/lateral/torque_extension.py b/openpilot/starpilot/lateral/torque_extension.py index 3737a09d66..b0501cc71d 100644 --- a/openpilot/starpilot/lateral/torque_extension.py +++ b/openpilot/starpilot/lateral/torque_extension.py @@ -31,6 +31,9 @@ def create_extension(parent, cp, mode, selected, *, turn_assist=False): raise ValueError('HKG torque selection does not match CarParams') policy = HKGVehicleTorquePolicy(parent, cp) return TorqueExtension(policy, parameter_factor=policy.FACTOR_MULT) + if selected == 'generic': + from openpilot.starpilot.lateral.generic_torque_policy import GenericTorquePolicy + return TorqueExtension(GenericTorquePolicy(parent, cp)) if selected == 'ioniq6': from openpilot.starpilot.lateral.ioniq6_policy import Ioniq6TorquePolicy policy = Ioniq6TorquePolicy(parent, cp, turn_assist=turn_assist) diff --git a/openpilot/starpilot/lateral/torque_runtime.py b/openpilot/starpilot/lateral/torque_runtime.py index ee65471907..3872d26148 100644 --- a/openpilot/starpilot/lateral/torque_runtime.py +++ b/openpilot/starpilot/lateral/torque_runtime.py @@ -187,7 +187,7 @@ class TorqueHost: # existing live manual preference for other optional replay-host vehicles. if allow_learning is None: from openpilot.starpilot.lateral.controller_selection import policy_for - if policy_for(CP) is None: + if policy_for(CP) in (None, 'generic'): from openpilot.starpilot.lateral.learning_preferences import read_preference preference = read_preference(params, CP) allow_learning = preference.valid and preference.enabled diff --git a/openpilot/starpilot/ui/tests/test_controller_feature.py b/openpilot/starpilot/ui/tests/test_controller_feature.py index d966d199c5..368545b4fd 100644 --- a/openpilot/starpilot/ui/tests/test_controller_feature.py +++ b/openpilot/starpilot/ui/tests/test_controller_feature.py @@ -71,10 +71,11 @@ class TestControllerFeature(unittest.TestCase): self.cp.notCar = True self.assertIsNone(self.row()) - def test_registered_stock_torque_uses_scoped_learning_without_controller_choice(self): + def test_registered_stock_torque_has_both_controllers_and_scoped_learning(self): self.cp = interfaces[HONDA.HONDA_FIT_4G].get_non_essential_params(HONDA.HONDA_FIT_4G) self.assertEqual(self.cp.lateralTuning.which(), 'torque') - self.assertIsNone(self.row()) + self.assertEqual(self.require_row().choices, ('Stock Controller', 'StarPilot Controller')) + self.assertEqual(self.require_row().value, 'Stock Controller') learning = self.require_row('ForceAutoTuneOff') self.assertEqual(learning.label, 'Use StarPilot Auto Learn') self.assertEqual(learning.value, 'On') diff --git a/openpilot/starpilot/ui/tests/test_feature_settings_owner.py b/openpilot/starpilot/ui/tests/test_feature_settings_owner.py index 79f29882f9..abbd869144 100644 --- a/openpilot/starpilot/ui/tests/test_feature_settings_owner.py +++ b/openpilot/starpilot/ui/tests/test_feature_settings_owner.py @@ -365,7 +365,7 @@ class FeatureSettingsOwnerTests(unittest.TestCase): from openpilot.starpilot.lateral.controller_selection import policy_for from openpilot.starpilot.vehicle_preferences import VehicleStartupPreferences cp = interfaces[CAR.MAZDA_CX5_2022].get_non_essential_params(CAR.MAZDA_CX5_2022) - self.assertIsNone(policy_for(cp)) + self.assertEqual(policy_for(cp), 'generic') self.owner.vehicle_params = lambda: cp self.owner.controller.vehicle_params = self.owner.vehicle_params self.fingerprint = cp.carFingerprint diff --git a/prebuilt.json b/prebuilt.json index 362036b5de..15586d69e4 100644 --- a/prebuilt.json +++ b/prebuilt.json @@ -1 +1 @@ -{"protected_inventory_sha256": "342afaf60ffcf7ce4a22e944e4aa384bfb13c5c08688db613d9a6795bab44822", "schema_version": 1} +{"protected_inventory_sha256": "350f0562c24df598ab6d98691eb08c22e1b5f6d92813a8cc2daa6438def63df2", "schema_version": 1}