diff --git a/openpilot/starpilot/lateral/hkg_vehicle_policy.py b/openpilot/starpilot/lateral/hkg_vehicle_policy.py index 4a0a76e3c5..568f5d6edf 100644 --- a/openpilot/starpilot/lateral/hkg_vehicle_policy.py +++ b/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 diff --git a/openpilot/starpilot/lateral/hkg_vehicle_shaping.py b/openpilot/starpilot/lateral/hkg_vehicle_shaping.py index 97567cb2ba..935f97ee34 100644 --- a/openpilot/starpilot/lateral/hkg_vehicle_shaping.py +++ b/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 diff --git a/openpilot/starpilot/lateral/tests/test_hkg_vehicle_policy.py b/openpilot/starpilot/lateral/tests/test_hkg_vehicle_policy.py index 0b7fc32b04..6e376679fd 100644 --- a/openpilot/starpilot/lateral/tests/test_hkg_vehicle_policy.py +++ b/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 diff --git a/prebuilt.json b/prebuilt.json index 569d6ceb98..0525f4aec0 100644 --- a/prebuilt.json +++ b/prebuilt.json @@ -1 +1 @@ -{"protected_inventory_sha256": "5f1f7ec8d576e6d23efd49385d7a5ed51ad4aa7f4a82fd4bb06ee66c39256b34", "schema_version": 1} +{"protected_inventory_sha256": "db0ba46e6dd835c0385acc3ad1edd799346f150f1c4b51b301152ce54ffc6cd6", "schema_version": 1}