diff --git a/openpilot/starpilot/lateral/ioniq6_policy.py b/openpilot/starpilot/lateral/ioniq6_policy.py index f5c3c86e44..a4364f7c41 100644 --- a/openpilot/starpilot/lateral/ioniq6_policy.py +++ b/openpilot/starpilot/lateral/ioniq6_policy.py @@ -681,6 +681,7 @@ def get_ioniq_6_highway_transition_output_taper_scale(desired_lateral_accel: flo def get_ioniq_6_low_speed_angle_assist_torque(desired_angle_deg: float, actual_angle_deg: float, current_output_torque: float, v_ego: float) -> float: angle_error = desired_angle_deg - actual_angle_deg + turn_in_output = None if desired_angle_deg * angle_error > 0.0: speed_weight = _ioniq_6_sigmoid((IONIQ_6_LOW_SPEED_ANGLE_ASSIST_SPEED - max(v_ego, 0.0)) / IONIQ_6_LOW_SPEED_ANGLE_ASSIST_SPEED_WIDTH) @@ -701,15 +702,16 @@ def get_ioniq_6_low_speed_angle_assist_torque(desired_angle_deg: float, actual_a -angle_error, ) if abs(assist_torque) < 1e-4: - return current_output_torque - - if current_output_torque * assist_torque >= 0.0: + turn_in_output = current_output_torque + elif current_output_torque * assist_torque >= 0.0: add_scale = float(np.interp(abs(current_output_torque), IONIQ_6_LOW_SPEED_ANGLE_ASSIST_ADD_BP, IONIQ_6_LOW_SPEED_ANGLE_ASSIST_ADD_V)) - return clip_scalar(current_output_torque + (assist_torque * add_scale), -1.0, 1.0) - - return clip_scalar(current_output_torque + assist_torque, -1.0, 1.0) + turn_in_output = clip_scalar(current_output_torque + (assist_torque * add_scale), -1.0, 1.0) + else: + turn_in_output = clip_scalar(current_output_torque + assist_torque, -1.0, 1.0) + if abs(desired_angle_deg) >= IONIQ_6_LOW_SPEED_ANGLE_ASSIST_DESIRED_ANGLE_WIDTH: + return turn_in_output speed_weight = _ioniq_6_sigmoid((IONIQ_6_LOW_SPEED_UNWIND_ASSIST_SPEED - max(v_ego, 0.0)) / IONIQ_6_LOW_SPEED_UNWIND_ASSIST_SPEED_WIDTH) @@ -719,13 +721,15 @@ def get_ioniq_6_low_speed_angle_assist_torque(desired_angle_deg: float, actual_a actual_angle_weight = _ioniq_6_sigmoid((abs(actual_angle_deg) - IONIQ_6_LOW_SPEED_UNWIND_ASSIST_ACTUAL_ANGLE) / IONIQ_6_LOW_SPEED_UNWIND_ASSIST_ACTUAL_ANGLE_WIDTH) assist_torque = math.copysign(IONIQ_6_LOW_SPEED_UNWIND_ASSIST_MAX_TORQUE * speed_weight * error_weight * actual_angle_weight, -angle_error) - if abs(assist_torque) < 1e-4: - return current_output_torque - - if current_output_torque * assist_torque >= 0.0: - assist_torque *= IONIQ_6_LOW_SPEED_UNWIND_ASSIST_BLEND - - return clip_scalar(current_output_torque + assist_torque, -1.0, 1.0) + opposition = max(-current_output_torque * math.copysign(1.0, assist_torque), 0.0) + assist_torque *= float(np.interp(opposition, [0.0, IONIQ_6_LOW_SPEED_ANGLE_ASSIST_ADD_BP[1]], + [IONIQ_6_LOW_SPEED_UNWIND_ASSIST_BLEND, 1.0])) + unwind_output = (current_output_torque if abs(assist_torque) < 1e-4 else + clip_scalar(current_output_torque + assist_torque, -1.0, 1.0)) + if turn_in_output is None: + return unwind_output + turn_in_weight = abs(desired_angle_deg) / IONIQ_6_LOW_SPEED_ANGLE_ASSIST_DESIRED_ANGLE_WIDTH + return unwind_output + (turn_in_output - unwind_output) * turn_in_weight # Buffer curvature and scale at the current speed to avoid pull-away unwind diff --git a/openpilot/starpilot/lateral/tests/test_ioniq6_center_assist.py b/openpilot/starpilot/lateral/tests/test_ioniq6_center_assist.py index 8a1439d345..3d01d70e70 100644 --- a/openpilot/starpilot/lateral/tests/test_ioniq6_center_assist.py +++ b/openpilot/starpilot/lateral/tests/test_ioniq6_center_assist.py @@ -42,3 +42,24 @@ def test_center_correction_retains_output_saturation_and_symmetry(): negative = assist(-actual - error, -actual, -torque, speed) assert -1. <= positive <= 1. assert positive == -negative + + +@pytest.mark.parametrize('speed', [0.5, 1., 3., 5.]) +@pytest.mark.parametrize('actual', [-30., -15., 15., 30.]) +@pytest.mark.parametrize('torque', [-.3, 0., .3]) +def test_straight_request_transition_with_turned_wheel_is_continuous(speed, actual, torque): + epsilon = 1e-7 + below = assist(-epsilon, actual, torque, speed) + center = assist(0., actual, torque, speed) + above = assist(epsilon, actual, torque, speed) + assert max(abs(below - center), abs(above - center)) < 1e-6, 'TURN_ASSIST_STRAIGHT_BRANCH_STEP' + + +@pytest.mark.parametrize('speed', [0.5, 1., 3., 5.]) +@pytest.mark.parametrize('actual', [-30., -15., 15., 30.]) +def test_unwind_pid_zero_transition_is_continuous(speed, actual): + epsilon = 1e-7 + below = assist(0., actual, -epsilon, speed) + center = assist(0., actual, 0., speed) + above = assist(0., actual, epsilon, speed) + assert max(abs(below - center), abs(above - center)) < 1e-6, 'TURN_ASSIST_OUTPUT_BRANCH_STEP' diff --git a/prebuilt.json b/prebuilt.json index 22793fa5dc..3c286dcff4 100644 --- a/prebuilt.json +++ b/prebuilt.json @@ -1 +1 @@ -{"protected_inventory_sha256": "1bea5be8bc244297fa6a9149095b12ea138be51cccdbafe05b61d6b947cf8c3c", "schema_version": 1} +{"protected_inventory_sha256": "6b126101d486d9cc4817a3020c17945a3df6052252949df9da8bc2bda4c3a9d8", "schema_version": 1}