Offer both controllers on every torque-tuned vehicle

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')

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:

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)

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):

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():

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

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)

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)

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

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')

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

prebuilt.json

@@ -1 +1 @@
-{"protected_inventory_sha256": "342afaf60ffcf7ce4a22e944e4aa384bfb13c5c08688db613d9a6795bab44822", "schema_version": 1}
+{"protected_inventory_sha256": "350f0562c24df598ab6d98691eb08c22e1b5f6d92813a8cc2daa6438def63df2", "schema_version": 1}