diff --git a/selfdrive/controls/lib/latcontrol_torque.py b/selfdrive/controls/lib/latcontrol_torque.py index 5d6573b16..a04e2505d 100644 --- a/selfdrive/controls/lib/latcontrol_torque.py +++ b/selfdrive/controls/lib/latcontrol_torque.py @@ -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)