Remove Slow Logic atm

selfdrive/controls/lib/latcontrol_torque.py

@@ -40,20 +40,13 @@ LP_FILTER_CUTOFF_HZ = 1.2
 JERK_LOOKAHEAD_SECONDS = 0.19
 JERK_GAIN = 0.22
 LAT_ACCEL_REQUEST_BUFFER_SECONDS = 1.0
-VERSION = 3
+VERSION = 2
 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.5
-LOW_SPEED_ANGLE_ASSIST_FADE_START_SPEED = 2.2
-LOW_SPEED_ANGLE_ASSIST_END_SPEED = 3.2
-LOW_SPEED_ANGLE_ASSIST_ERROR_DEADZONE_DEG = 4.5
-LOW_SPEED_ANGLE_ASSIST_ERROR_RISE_DEG = 12.0
-LOW_SPEED_ANGLE_ASSIST_MAX_TORQUE = 0.36
 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
@@ -485,8 +478,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.95
-IONIQ_6_UNWIND_TAPER_RIGHT = 8.25
+IONIQ_6_UNWIND_TAPER_LEFT = 3.18
+IONIQ_6_UNWIND_TAPER_RIGHT = 6.55
 IONIQ_6_FRICTION_MULT = 0.928
 IONIQ_6_FRICTION_LAT_RISE = 0.20
 IONIQ_6_FRICTION_JERK_RISE = 0.24
@@ -503,7 +496,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.039
+IONIQ_6_HIGHWAY_CENTER_TAPER_MAX = 0.034
 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
@@ -519,8 +512,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 = 2.20
-IONIQ_6_DIRECTIONAL_TAPER_UNWIND_RIGHT = 4.05
+IONIQ_6_DIRECTIONAL_TAPER_UNWIND_LEFT = 1.82
+IONIQ_6_DIRECTIONAL_TAPER_UNWIND_RIGHT = 3.28
 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
@@ -542,8 +535,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.74
-IONIQ_6_HEAVY_DIRECTIONAL_TAPER_UNWIND_RIGHT = 1.14
+IONIQ_6_HEAVY_DIRECTIONAL_TAPER_UNWIND_LEFT = 0.62
+IONIQ_6_HEAVY_DIRECTIONAL_TAPER_UNWIND_RIGHT = 0.94
 IONIQ_6_OUTPUT_TAPER_SPEED = 8.5
 IONIQ_6_OUTPUT_TAPER_SPEED_WIDTH = 2.5
 IONIQ_6_OUTPUT_CENTER_TAPER_BLEND = 0.90
@@ -649,21 +642,6 @@ 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])
@@ -2103,8 +2081,6 @@ 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
@@ -2249,19 +2225,6 @@ 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)