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

THIS DESERVES A BUMP

Browse Source
This commit is contained in:
firestar5683
2026-06-11 23:04:34 -05:00
parent 5da99bc977
commit 21b1bfd600
1 changed files with 45 additions and 8 deletions
Download Patch File Download Diff File Expand all files Collapse all files
selfdrive/controls/lib/latcontrol_torque.py
+45 -8
View File
@@ -40,13 +40,20 @@ LP_FILTER_CUTOFF_HZ = 1.2
JERK_LOOKAHEAD_SECONDS = 0.19
JERK_GAIN = 0.22
LAT_ACCEL_REQUEST_BUFFER_SECONDS = 1.0
VERSION = 2
VERSION = 3
DEBUG_TORQUE_TUNE = False
FF_SCALE_BLEND_LAT_ACCEL = 0.05
DEADZONE_BOOST_LAT_ACCEL = 0.15
UNWIND_D_DES_THRESHOLD = -1.0
UNWIND_LAT_ACCEL_NEAR_ZERO = 0.3
MIN_LATERAL_CONTROL_SPEED = 0.3
LOW_SPEED_ANGLE_ASSIST_START_SPEED = 0.25
LOW_SPEED_ANGLE_ASSIST_FULL_SPEED = 0.6
LOW_SPEED_ANGLE_ASSIST_FADE_START_SPEED = 2.5
LOW_SPEED_ANGLE_ASSIST_END_SPEED = 5.0
LOW_SPEED_ANGLE_ASSIST_ERROR_DEADZONE_DEG = 3.0
LOW_SPEED_ANGLE_ASSIST_ERROR_RISE_DEG = 10.0
LOW_SPEED_ANGLE_ASSIST_MAX_TORQUE = 0.55
CIVIC_BOSCH_MODIFIED_B_FIXED_FRICTION_THRESHOLD = 0.30
CIVIC_BOSCH_MODIFIED_B_LAT_ACCEL_FACTOR_MULT = 1.20
CIVIC_BOSCH_MODIFIED_A_VARIANT_LAT_ACCEL_FACTOR_MULT = 1.00
@@ -478,8 +485,8 @@ IONIQ_6_TRANSITION_SPEED = 10.0
IONIQ_6_PHASE_SCALE = 0.10
IONIQ_6_TURN_IN_BOOST_LEFT = 1.64
IONIQ_6_TURN_IN_BOOST_RIGHT = 1.88
IONIQ_6_UNWIND_TAPER_LEFT = 3.18
IONIQ_6_UNWIND_TAPER_RIGHT = 6.55
IONIQ_6_UNWIND_TAPER_LEFT = 3.95
IONIQ_6_UNWIND_TAPER_RIGHT = 8.25
IONIQ_6_FRICTION_MULT = 0.928
IONIQ_6_FRICTION_LAT_RISE = 0.20
IONIQ_6_FRICTION_JERK_RISE = 0.24
@@ -496,7 +503,7 @@ IONIQ_6_CENTER_TAPER_LAT = 0.24
IONIQ_6_CENTER_TAPER_LAT_WIDTH = 0.025
IONIQ_6_CENTER_TAPER_SPEED = 18.0
IONIQ_6_CENTER_TAPER_SPEED_WIDTH = 2.5
IONIQ_6_HIGHWAY_CENTER_TAPER_MAX = 0.034
IONIQ_6_HIGHWAY_CENTER_TAPER_MAX = 0.039
IONIQ_6_HIGHWAY_CENTER_TAPER_LAT = 0.09
IONIQ_6_HIGHWAY_CENTER_TAPER_LAT_WIDTH = 0.03
IONIQ_6_HIGHWAY_CENTER_TAPER_SPEED = 24.5
@@ -512,8 +519,8 @@ IONIQ_6_DIRECTIONAL_TAPER_LAT_END = 0.90
IONIQ_6_DIRECTIONAL_TAPER_LAT_WIDTH = 0.06
IONIQ_6_DIRECTIONAL_TAPER_BASE_LEFT = 0.13
IONIQ_6_DIRECTIONAL_TAPER_BASE_RIGHT = 0.45
IONIQ_6_DIRECTIONAL_TAPER_UNWIND_LEFT = 1.82
IONIQ_6_DIRECTIONAL_TAPER_UNWIND_RIGHT = 3.28
IONIQ_6_DIRECTIONAL_TAPER_UNWIND_LEFT = 2.20
IONIQ_6_DIRECTIONAL_TAPER_UNWIND_RIGHT = 4.05
IONIQ_6_DIRECTIONAL_TAPER_FLOOR_LEFT = 0.48
IONIQ_6_DIRECTIONAL_TAPER_FLOOR_RIGHT = 0.52
IONIQ_6_DIRECTIONAL_TAPER_UNWIND_FLOOR_LEFT = 0.10
@@ -535,8 +542,8 @@ 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
IONIQ_6_HEAVY_DIRECTIONAL_TAPER_BASE_RIGHT = 0.17
IONIQ_6_HEAVY_DIRECTIONAL_TAPER_UNWIND_LEFT = 0.62
IONIQ_6_HEAVY_DIRECTIONAL_TAPER_UNWIND_RIGHT = 0.94
IONIQ_6_HEAVY_DIRECTIONAL_TAPER_UNWIND_LEFT = 0.74
IONIQ_6_HEAVY_DIRECTIONAL_TAPER_UNWIND_RIGHT = 1.14
IONIQ_6_OUTPUT_TAPER_SPEED = 8.5
IONIQ_6_OUTPUT_TAPER_SPEED_WIDTH = 2.5
IONIQ_6_OUTPUT_CENTER_TAPER_BLEND = 0.90
@@ -642,6 +649,21 @@ def get_friction_threshold(v_ego: float) -> float:
return float(np.interp(v_ego, [1 * CV.MPH_TO_MS, 20 * CV.MPH_TO_MS, 75 * CV.MPH_TO_MS], [0.16, 0.19, 0.27]))
def get_low_speed_angle_assist_torque(v_ego: float, steering_angle_error_deg: float) -> float:
speed_factor = np.interp(v_ego,
[LOW_SPEED_ANGLE_ASSIST_START_SPEED,
LOW_SPEED_ANGLE_ASSIST_FULL_SPEED,
LOW_SPEED_ANGLE_ASSIST_FADE_START_SPEED,
LOW_SPEED_ANGLE_ASSIST_END_SPEED],
[0.0, 1.0, 1.0, 0.0])
error_mag = max(abs(steering_angle_error_deg) - LOW_SPEED_ANGLE_ASSIST_ERROR_DEADZONE_DEG, 0.0)
if error_mag <= 0.0 or speed_factor <= 0.0:
return 0.0
error_factor = 1.0 - math.exp(-error_mag / LOW_SPEED_ANGLE_ASSIST_ERROR_RISE_DEG)
return float(LOW_SPEED_ANGLE_ASSIST_MAX_TORQUE * speed_factor * error_factor)
def get_trailer_lateral_assist_factor(trailer_load_kg: float, v_ego: float, desired_lateral_accel: float) -> float:
load_factor = np.clip(trailer_load_kg / TRAILER_LOAD_FULL_ASSIST_KG, 0.0, 1.0)
speed_factor = np.interp(v_ego, [TRAILER_LATERAL_MIN_SPEED, TRAILER_LATERAL_FULL_SPEED], [0.0, 1.0])
@@ -2081,6 +2103,8 @@ class LatControlTorque(LatControl):
raw_lateral_jerk = (future_desired_lateral_accel - expected_lateral_accel) / max(lat_delay, self.dt)
raw_lateral_jerk = np.clip(raw_lateral_jerk, -MAX_LAT_JERK_UP, MAX_LAT_JERK_UP)
desired_lateral_jerk = np.clip(self.jerk_filter.update(raw_lateral_jerk), -MAX_LAT_JERK_UP, MAX_LAT_JERK_UP)
desired_steering_angle_deg = math.degrees(VM.get_steer_from_curvature(-desired_curvature, CS.vEgo, params.roll)) + params.angleOffsetDeg
steering_angle_error_deg = desired_steering_angle_deg - CS.steeringAngleDeg
gravity_adjusted_future_lateral_accel = future_desired_lateral_accel - roll_compensation
setpoint = expected_lateral_accel + desired_lateral_jerk * lat_delay
desired_lateral_accel_rate = (setpoint - self.prev_desired_lateral_accel) / self.dt
@@ -2225,6 +2249,19 @@ class LatControlTorque(LatControl):
output_torque *= volt_standard_center_taper
elif self.is_civic_bosch_modified and civic_bosch_modified_a_lateral_testing_ground_active():
output_torque *= civic_bosch_modified_a_center_taper
# At crawl speed, desired lateral acceleration collapses with v^2. Use steering-angle
# error to enforce a small torque floor so the wheel starts moving before speed builds.
if not CS.steeringPressed:
low_speed_angle_assist_torque = get_low_speed_angle_assist_torque(CS.vEgo, steering_angle_error_deg)
if low_speed_angle_assist_torque > 0.0:
assist_torque = math.copysign(low_speed_angle_assist_torque, steering_angle_error_deg)
if output_torque * assist_torque < 0.0:
output_torque = assist_torque
else:
output_torque = math.copysign(max(abs(output_torque), low_speed_angle_assist_torque), assist_torque)
output_torque = float(np.clip(output_torque, -self.steer_max, self.steer_max))
pid_log.active = True
pid_log.p = float(self.pid.p)
pid_log.i = float(self.pid.i)