Damp EV6 near-center steering while preserving turn-in

openpilot/starpilot/lateral/hkg_vehicle_policy.py

@@ -156,7 +156,8 @@ class HKGVehicleTorquePolicy(HKGShapedTorquePolicy):
     if self.profile == 'kia_stinger_2022':
       return value * shaping.get_kia_stinger_2022_center_taper_scale(setpoint, speed)
     if self.profile == 'kia_ev6':
-      return value * shaping.get_kia_ev6_low_speed_center_taper_scale(setpoint, speed) * shaping.get_kia_ev6_center_output_scale(setpoint, speed)
+      return (value * shaping.get_kia_ev6_low_speed_center_taper_scale(setpoint, speed) *
+              shaping.get_kia_ev6_center_output_scale(setpoint, speed, self.future_lateral_accel))
     if self.profile == 'kia_carnival':
       return (
         value

openpilot/starpilot/lateral/hkg_vehicle_shaping.py

@@ -544,6 +544,8 @@ KIA_EV6_LOW_SPEED_CENTER_TAPER_SPEED_WIDTH = 1.4
 
 KIA_EV6_CENTER_OUTPUT_TAPER_MAX = 0.14
 
+KIA_EV6_HIGHWAY_CENTER_DAMPING = 0.08
+
 KIA_EV6_CENTER_OUTPUT_TAPER_LAT = 0.30
 
 KIA_EV6_CENTER_OUTPUT_TAPER_LAT_WIDTH = 0.08
@@ -1298,12 +1300,17 @@ def get_kia_ev6_low_speed_center_taper_scale(desired_lateral_accel: float, v_ego
   reduction = KIA_EV6_LOW_SPEED_CENTER_TAPER_MAX * speed_weight * center_weight
   return 1.0 - reduction
 
-def get_kia_ev6_center_output_scale(desired_lateral_accel: float, v_ego: float) -> float:
+def get_kia_ev6_center_output_scale(desired_lateral_accel: float, v_ego: float,
+                                  future_lateral_accel: float | None = None) -> float:
   speed_weight = _kia_ev6_sigmoid((v_ego - KIA_EV6_CENTER_OUTPUT_TAPER_SPEED) /
                                   KIA_EV6_CENTER_OUTPUT_TAPER_SPEED_WIDTH)
   center_weight = _kia_ev6_sigmoid((KIA_EV6_CENTER_OUTPUT_TAPER_LAT - abs(desired_lateral_accel)) /
                                    KIA_EV6_CENTER_OUTPUT_TAPER_LAT_WIDTH)
   reduction = _flm_vehicle_knob("hyundai_kia_ev6.center_output_taper_max", KIA_EV6_CENTER_OUTPUT_TAPER_MAX) * speed_weight * center_weight
+  future_accel = abs(desired_lateral_accel) if future_lateral_accel is None else max(abs(desired_lateral_accel), abs(future_lateral_accel))
+  future_center_weight = _kia_ev6_sigmoid((KIA_EV6_CENTER_OUTPUT_TAPER_LAT - future_accel) /
+                                         KIA_EV6_CENTER_OUTPUT_TAPER_LAT_WIDTH)
+  reduction += KIA_EV6_HIGHWAY_CENTER_DAMPING * speed_weight * future_center_weight
   return 1.0 - reduction
 
 

openpilot/starpilot/lateral/tests/test_hkg_vehicle_policy.py

@@ -136,3 +136,61 @@ def test_kona_2022_friction_does_not_change_sibling_policies(monkeypatch, vehicl
   controller = LatControlTorque(cp.as_reader(), interfaces[vehicle](cp), .01, controller_mode=ControllerMode.STARPILOT)
   threshold, scale = controller.starpilot_extension.policy.friction_context(.1, 0., 0., 120. / 3.6)
   assert threshold > 0. and scale > 0.
+
+
+@pytest.mark.parametrize('alpha', (False, True))
+@pytest.mark.parametrize('direction', (-1., 1.))
+def test_ev6_highway_center_damping_preserves_controller_and_curve_response(monkeypatch, alpha, direction):
+  from types import SimpleNamespace
+  import math
+  from openpilot.cereal import log
+  from opendbc.car.vehicle_model import VehicleModel
+  from openpilot.starpilot.lateral import hkg_vehicle_shaping as shaping
+
+  cp = interfaces[CAR.KIA_EV6].get_non_essential_params(CAR.KIA_EV6)
+  cp.openpilotLongitudinalControl, cp.pcmCruise = alpha, not alpha
+  cs = SimpleNamespace(vEgo=26., steeringAngleDeg=0., steeringPressed=False)
+  params = log.VehicleParameters.new_message(angleOffsetDeg=0., roll=0.)
+  selected = shaping.KIA_EV6_HIGHWAY_CENTER_DAMPING
+  assert selected == .08
+
+  def run(taper, turn, mode=ControllerMode.STARPILOT):
+    monkeypatch.setattr(shaping, 'KIA_EV6_HIGHWAY_CENTER_DAMPING', taper)
+    controller = LatControlTorque(cp.as_reader(), interfaces[cp.carFingerprint](cp), .01, controller_mode=mode)
+    vm = VehicleModel(cp)
+    outputs, stages = [], []
+    for tick in range(300):
+      acceleration = direction * (1.5 if turn else .12 * math.sin(2 * math.pi * .4 * tick * .01))
+      value, _, state = controller.update(True, cs, vm, params, False, acceleration / cs.vEgo**2, False, .386)
+      assert math.isfinite(value) and abs(value) <= controller.steer_max + 1e-6
+      outputs.append(value)
+      stages.append((state.p, state.i, state.d, state.f))
+    off, _, state = controller.update(False, cs, vm, params, False, 0., False, .386)
+    assert off == 0. and not state.active
+    return outputs, stages, controller.torque_params.to_dict(), (controller.pid.pos_limit, controller.pid.neg_limit, controller.steer_max)
+
+  actual, baseline = run(selected, False), run(0., False)
+  assert actual[1:] == baseline[1:]
+  assert sum(x*x for x in actual[0]) < .88 * sum(x*x for x in baseline[0])
+  assert all(a*b >= -1e-12 for a, b in zip(actual[0], baseline[0], strict=True))
+  actual_turn, baseline_turn = run(selected, True), run(0., True)
+  assert actual_turn[1:] == baseline_turn[1:]
+  assert actual_turn[0] == pytest.approx(baseline_turn[0], rel=1e-6, abs=1e-8)
+  assert run(selected, False, ControllerMode.STANDARD) == run(0., False, ControllerMode.STANDARD)
+
+
+def test_ev6_taper_keeps_low_speed_and_substantial_curve_effect_small():
+  from openpilot.starpilot.lateral import hkg_vehicle_shaping as shaping
+  for speed in (0., 3., 26., 36.):
+    for acceleration in (-2., -1., 0., 1., 2.):
+      actual = shaping.get_kia_ev6_center_output_scale(acceleration, speed)
+      speed_weight = shaping._kia_ev6_sigmoid((speed - shaping.KIA_EV6_CENTER_OUTPUT_TAPER_SPEED) /
+                                              shaping.KIA_EV6_CENTER_OUTPUT_TAPER_SPEED_WIDTH)
+      center_weight = shaping._kia_ev6_sigmoid((shaping.KIA_EV6_CENTER_OUTPUT_TAPER_LAT - abs(acceleration)) /
+                                               shaping.KIA_EV6_CENTER_OUTPUT_TAPER_LAT_WIDTH)
+      old = 1. - .14 * speed_weight * center_weight
+      assert .78 <= actual <= old <= 1.
+      if speed <= 3.:
+        assert old - actual < .002
+      if abs(acceleration) >= 1.:
+        assert old - actual < .00002

prebuilt.json

@@ -1 +1 @@
-{"protected_inventory_sha256": "5f1f7ec8d576e6d23efd49385d7a5ed51ad4aa7f4a82fd4bb06ee66c39256b34", "schema_version": 1}
+{"protected_inventory_sha256": "db0ba46e6dd835c0385acc3ad1edd799346f150f1c4b51b301152ce54ffc6cd6", "schema_version": 1}