diff --git a/selfdrive/car/interfaces.py b/selfdrive/car/interfaces.py index 4647a04244..982ba40b17 100644 --- a/selfdrive/car/interfaces.py +++ b/selfdrive/car/interfaces.py @@ -23,7 +23,7 @@ TorqueFromLateralAccelCallbackType = Callable[[float, car.CarParams.LateralTorqu MAX_CTRL_SPEED = (V_CRUISE_MAX + 4) * CV.KPH_TO_MS ACCEL_MAX = 2.0 ACCEL_MIN = -3.5 -FRICTION_THRESHOLD = 0.2 +FRICTION_THRESHOLD = 0.3 TORQUE_PARAMS_PATH = os.path.join(BASEDIR, 'selfdrive/car/torque_data/params.yaml') TORQUE_OVERRIDE_PATH = os.path.join(BASEDIR, 'selfdrive/car/torque_data/override.yaml') diff --git a/selfdrive/controls/lib/latcontrol_torque.py b/selfdrive/controls/lib/latcontrol_torque.py index 51676086ba..d10d39d945 100644 --- a/selfdrive/controls/lib/latcontrol_torque.py +++ b/selfdrive/controls/lib/latcontrol_torque.py @@ -17,7 +17,8 @@ from selfdrive.controls.lib.vehicle_model import ACCELERATION_DUE_TO_GRAVITY # friction in the steering wheel that needs to be overcome to # move it at all, this is compensated for too. -LOW_SPEED_FACTOR = 200 +LOW_SPEED_X = [0, 10, 20, 30] +LOW_SPEED_Y = [15, 13, 10, 5] class LatControlTorque(LatControl): @@ -57,8 +58,9 @@ class LatControlTorque(LatControl): actual_lateral_accel = actual_curvature * CS.vEgo ** 2 lateral_accel_deadzone = curvature_deadzone * CS.vEgo ** 2 - setpoint = desired_lateral_accel + LOW_SPEED_FACTOR * desired_curvature - measurement = actual_lateral_accel + LOW_SPEED_FACTOR * actual_curvature + low_speed_factor = interp(CS.vEgo, LOW_SPEED_X, LOW_SPEED_Y)**2 + setpoint = desired_lateral_accel + low_speed_factor * desired_curvature + measurement = actual_lateral_accel + low_speed_factor * actual_curvature error = setpoint - measurement gravity_adjusted_lateral_accel = desired_lateral_accel - params.roll * ACCELERATION_DUE_TO_GRAVITY pid_log.error = self.torque_from_lateral_accel(error, self.torque_params, error, diff --git a/selfdrive/test/process_replay/ref_commit b/selfdrive/test/process_replay/ref_commit index 8bc13f37d4..0608a6d14b 100644 --- a/selfdrive/test/process_replay/ref_commit +++ b/selfdrive/test/process_replay/ref_commit @@ -1 +1 @@ -24a8d02b148b7f6d20f641d56a7bed71c244b6e3 \ No newline at end of file +2991a54ea491633f93fae76da4f5f1d265ce311a \ No newline at end of file