use current expected curvature for low speed factor

Author: felsager

selfdrive/controls/lib/latcontrol_torque.py

@@ -61,12 +61,13 @@ def update(self, active, CS, VM, params, steer_limited_by_safety: bool, desired_
       lag_compensated_desired_lateral_accel = desired_curvature * CS.vEgo ** 2
       self.requested_lateral_accel_buffer.appendleft(lag_compensated_desired_lateral_accel)
       current_expected_lateral_accel = self.requested_lateral_accel_buffer[delay_frames]
+      current_expected_curvature = current_expected_lateral_accel / (CS.vEgo ** 2)
       actual_lateral_accel = actual_curvature * CS.vEgo ** 2
       lateral_accel_deadzone = curvature_deadzone * CS.vEgo ** 2
 
       low_speed_factor = np.interp(CS.vEgo, LOW_SPEED_X, LOW_SPEED_Y)**2
       # pid error calculated as difference between expected and measured lateral acceleration
-      setpoint = current_expected_lateral_accel + low_speed_factor * desired_curvature
+      setpoint = current_expected_lateral_accel + low_speed_factor * current_expected_curvature
       measurement = actual_lateral_accel + low_speed_factor * actual_curvature
       gravity_adjusted_lateral_accel = lag_compensated_desired_lateral_accel - roll_compensation