mawei/StarPilot
1
0
Fork 0
mirror of https://github.com/firestar5683/StarPilot.git synced 2026-08-21 00:03:45 +08:00
Code Issues Packages Projects Releases Wiki Activity

Angler Wish

Browse Source
This commit is contained in:
firestar5683
2026-06-19 13:48:30 -05:00
parent 496d36f805
commit 0e5d40fb1b
2 changed files with 52 additions and 0 deletions
Download Patch File Download Diff File Expand all files Collapse all files
selfdrive/controls/lib/latcontrol_torque.py
+37
View File
@@ -547,12 +547,20 @@ IONIQ_6_CRAWL_TURN_IN_FF_SPEED = 4.5
IONIQ_6_CRAWL_TURN_IN_FF_SPEED_WIDTH = 0.8
IONIQ_6_CRAWL_TURN_IN_FF_LAT = 0.10
IONIQ_6_CRAWL_TURN_IN_FF_LAT_WIDTH = 0.05
IONIQ_6_LOW_SPEED_ANGLE_ASSIST_MAX_TORQUE = 0.34
IONIQ_6_LOW_SPEED_ANGLE_ASSIST_SPEED = 3.6
IONIQ_6_LOW_SPEED_ANGLE_ASSIST_SPEED_WIDTH = 0.45
IONIQ_6_LOW_SPEED_ANGLE_ASSIST_ERROR = 4.0
IONIQ_6_LOW_SPEED_ANGLE_ASSIST_ERROR_WIDTH = 1.8
IONIQ_6_LOW_SPEED_ANGLE_ASSIST_DESIRED_ANGLE = 10.0
IONIQ_6_LOW_SPEED_ANGLE_ASSIST_DESIRED_ANGLE_WIDTH = 4.0
IONIQ_6_HIGH_SPEED_RIGHT_TURN_IN_FF_BOOST = 0.10
IONIQ_6_HIGH_SPEED_RIGHT_TURN_IN_FF_SPEED = 18.0
IONIQ_6_HIGH_SPEED_RIGHT_TURN_IN_FF_SPEED_WIDTH = 2.5
IONIQ_6_HIGH_SPEED_RIGHT_TURN_IN_FF_LAT_START = 0.06
IONIQ_6_HIGH_SPEED_RIGHT_TURN_IN_FF_LAT_END = 0.22
IONIQ_6_HIGH_SPEED_RIGHT_TURN_IN_FF_LAT_WIDTH = 0.035
IONIQ_6_LOW_SPEED_PID_RESET_SPEED = 0.1 * CV.MPH_TO_MS
IONIQ_6_HEAVY_DIRECTIONAL_TAPER_LAT_START = 0.82
IONIQ_6_HEAVY_DIRECTIONAL_TAPER_LAT_WIDTH = 0.12
IONIQ_6_HEAVY_DIRECTIONAL_TAPER_BASE_LEFT = 0.10
@@ -1848,6 +1856,28 @@ def get_ioniq_6_output_taper_scale(desired_lateral_accel: float, desired_lateral
return center_scale * directional_scale
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
if desired_angle_deg * angle_error <= 0.0:
return current_output_torque
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)
error_weight = _ioniq_6_sigmoid((abs(angle_error) - IONIQ_6_LOW_SPEED_ANGLE_ASSIST_ERROR) /
IONIQ_6_LOW_SPEED_ANGLE_ASSIST_ERROR_WIDTH)
desired_angle_weight = _ioniq_6_sigmoid((abs(desired_angle_deg) - IONIQ_6_LOW_SPEED_ANGLE_ASSIST_DESIRED_ANGLE) /
IONIQ_6_LOW_SPEED_ANGLE_ASSIST_DESIRED_ANGLE_WIDTH)
assist_torque = math.copysign(IONIQ_6_LOW_SPEED_ANGLE_ASSIST_MAX_TORQUE * speed_weight * error_weight * desired_angle_weight, angle_error)
if abs(assist_torque) < 1e-4:
return current_output_torque
if current_output_torque * assist_torque >= 0.0:
return float(np.clip(math.copysign(max(abs(current_output_torque), abs(assist_torque)), assist_torque), -1.0, 1.0))
return float(np.clip(current_output_torque + assist_torque, -1.0, 1.0))
def kia_ev6_lateral_testing_ground_active() -> bool:
return testing_ground.use(KIA_EV6_LATERAL_TESTING_GROUND_ID, KIA_EV6_LATERAL_TESTING_GROUND_VARIANT)
@@ -2041,6 +2071,8 @@ class LatControlTorque(LatControl):
self.is_civic_bosch_modified = CP.carFingerprint == HONDA_CAR.HONDA_CIVIC_BOSCH and bool(CP.flags & HondaFlags.EPS_MODIFIED)
self.is_volt_cc = CP.carFingerprint == GM_CAR.CHEVROLET_VOLT_CC
self.is_silverado = CP.carFingerprint == GM_CAR.CHEVROLET_SILVERADO
if self.is_ioniq_6:
self.low_speed_reset_threshold = min(self.low_speed_reset_threshold, IONIQ_6_LOW_SPEED_PID_RESET_SPEED)
self.use_bolt_ff_scaling = self.is_bolt_2022_2023 or self.is_bolt_2018_2021 or self.is_bolt_2017
self.use_bolt_ki_multiplier = self.use_bolt_ff_scaling
self.torque_ff_scale_pos = 1.0
@@ -2274,6 +2306,11 @@ class LatControlTorque(LatControl):
output_torque *= get_bolt_2017_torque_scale(setpoint, desired_lateral_jerk, CS.vEgo)
elif bolt_2018_2021_tuned_path_active:
output_torque *= get_bolt_2018_2021_dynamic_torque_scale(setpoint, desired_lateral_jerk, CS.vEgo)
elif ioniq_6_active:
desired_angle_no_offset = math.degrees(VM.get_steer_from_curvature(-desired_curvature, CS.vEgo, params.roll))
actual_angle_no_offset = CS.steeringAngleDeg - params.angleOffsetDeg
output_torque = get_ioniq_6_low_speed_angle_assist_torque(desired_angle_no_offset, actual_angle_no_offset,
output_torque, CS.vEgo)
elif volt_standard_test_active:
output_torque *= volt_standard_center_taper
elif kia_niro_phev_2022_active:
selfdrive/controls/tests/test_latcontrol.py
+15
View File
@@ -60,6 +60,7 @@ from openpilot.selfdrive.controls.lib.latcontrol_torque import (
get_ioniq_6_ff_scale,
get_ioniq_6_friction_scale,
get_ioniq_6_friction_threshold,
get_ioniq_6_low_speed_angle_assist_torque,
get_kia_forte_center_taper_scale,
get_kia_forte_ff_scale,
get_kia_ev6_center_taper_scale,
@@ -475,6 +476,19 @@ class TestLatControl:
assert get_ioniq_6_ff_scale(0.30, 0.60, 6.0) > get_ioniq_6_ff_scale(0.30, 0.60, 12.0)
assert get_ioniq_6_ff_scale(0.30, -0.60, 3.0) < get_ioniq_6_ff_scale(0.30, 0.60, 3.0)
def test_ioniq_6_low_speed_angle_assist_curve(self):
base = 0.05
boosted = get_ioniq_6_low_speed_angle_assist_torque(22.0, 0.0, base, 1.0)
faded = get_ioniq_6_low_speed_angle_assist_torque(22.0, 0.0, base, 4.5)
unwind = get_ioniq_6_low_speed_angle_assist_torque(22.0, 30.0, base, 1.0)
opposing = get_ioniq_6_low_speed_angle_assist_torque(-22.0, 0.0, 0.08, 1.0)
assert boosted > base
assert faded < boosted
assert abs(faded - base) < 0.04
assert unwind == pytest.approx(base)
assert opposing < 0.0
def test_ioniq_6_directional_taper_curve(self):
assert get_ioniq_6_directional_taper_scale(0.0, 0.0) == 1.0
assert get_ioniq_6_directional_taper_scale(-0.5, 0.0) < get_ioniq_6_directional_taper_scale(0.5, 0.0) < 1.0
@@ -658,6 +672,7 @@ class TestLatControl:
assert lac_log.active
assert controller.torque_params.latAccelFactor == pytest.approx(3.0 * 1.22)
assert controller.low_speed_reset_threshold == pytest.approx(0.1 * 0.44704)
def test_elantra_non_scc_default_update_path(self):
controller, VM, CS, params, starpilot_toggles = self._build_torque_controller(HYUNDAI.HYUNDAI_ELANTRA_HEV_2022_NON_SCC)