NewLateral2

cereal/car.capnp

@@ -532,6 +532,7 @@ struct CarParams {
     useSteeringAngle @0 :Bool;
     kp @1 :Float32;
     ki @2 :Float32;
+    kd @8 :Float32;
     friction @3 :Float32;
     kf @4 :Float32;
     steeringAngleDeadzoneDeg @5 :Float32;

selfdrive/car/interfaces.py

@@ -305,6 +305,7 @@ class CarInterfaceBase(ABC):
     tune.torque.kf = 1.0
     tune.torque.kp = 1.0
     tune.torque.ki = 0.3
+    tune.torque.kd = 0.0
     tune.torque.friction = params['FRICTION']
     tune.torque.latAccelFactor = params['LAT_ACCEL_FACTOR']
     tune.torque.latAccelOffset = 0.0

selfdrive/controls/lib/latcontrol.py

@@ -7,9 +7,8 @@ MIN_LATERAL_CONTROL_SPEED = 0.3  # m/s
 class LatControl(ABC):
   def __init__(self, CP, CI, dt):
     self.dt = dt
-    self.sat_count_rate = 1.0 * self.dt
     self.sat_limit = CP.steerLimitTimer
-    self.sat_count = 0.
+    self.sat_time = 0.
     self.sat_check_min_speed = 10.
 
     # we define the steer torque scale as [-1.0...1.0]
@@ -20,13 +19,13 @@ class LatControl(ABC):
     pass
 
   def reset(self):
-    self.sat_count = 0.
+    self.sat_time = 0.
 
   def _check_saturation(self, saturated, CS, steer_limited_by_safety, curvature_limited):
     # Saturated only if control output is not being limited by car torque/angle rate limits
     if (saturated or curvature_limited) and CS.vEgo > self.sat_check_min_speed and not steer_limited_by_safety and not CS.steeringPressed:
-      self.sat_count += self.sat_count_rate
+      self.sat_time += self.dt
     else:
-      self.sat_count -= self.sat_count_rate
-    self.sat_count = np.clip(self.sat_count, 0.0, self.sat_limit)
-    return self.sat_count > (self.sat_limit - 1e-3)
+      self.sat_time -= self.dt
+    self.sat_time = np.clip(self.sat_time, 0.0, self.sat_limit)
+    return self.sat_time > (self.sat_limit - 1e-3)

selfdrive/controls/lib/latcontrol_torque.py

@@ -3,6 +3,7 @@ import numpy as np
 from collections import deque
 
 from cereal import log
+from openpilot.common.filter_simple import FirstOrderFilter
 from openpilot.selfdrive.controls.lib.drive_helpers import MIN_SPEED, get_friction
 from openpilot.selfdrive.car.interfaces import FRICTION_THRESHOLD
 from openpilot.selfdrive.controls.lib.latcontrol import LatControl
@@ -22,6 +23,8 @@ from openpilot.frogpilot.controls.lib.neural_network_feedforward import LOW_SPEE
 # friction in the steering wheel that needs to be overcome to
 # move it at all, this is compensated for too.
 
+MAX_LAT_JERK_UP = 2.5            # m/s^3
+
 LOW_SPEED_X = [0, 10, 20, 30]
 LOW_SPEED_Y = [15, 13, 10, 5]
 
@@ -38,6 +41,8 @@ class LatControlTorque(LatControl):
     self.steering_angle_deadzone_deg = self.torque_params.steeringAngleDeadzoneDeg
     self.LATACCEL_REQUEST_BUFFER_NUM_FRAMES = int(1 / self.dt)
     self.requested_lateral_accel_buffer = deque([0.] * self.LATACCEL_REQUEST_BUFFER_NUM_FRAMES , maxlen=self.LATACCEL_REQUEST_BUFFER_NUM_FRAMES)
+    self.previous_measurement = 0.0
+    self.measurement_rate_filter = FirstOrderFilter(0.0, 1 / (2 * np.pi * (MAX_LAT_JERK_UP - 0.5)), self.dt)
 
     # FrogPilot variables
     self.nnff = NeuralNetworkFeedforward(CP, self)
@@ -60,9 +65,10 @@ class LatControlTorque(LatControl):
       output_torque = 0.0
       pid_log.active = False
     else:
-      actual_curvature = -VM.calc_curvature(math.radians(CS.steeringAngleDeg - params.angleOffsetDeg), CS.vEgo, params.roll)
+      measured_curvature = -VM.calc_curvature(math.radians(CS.steeringAngleDeg - params.angleOffsetDeg), CS.vEgo, params.roll)
       roll_compensation = params.roll * ACCELERATION_DUE_TO_GRAVITY
       curvature_deadzone = abs(VM.calc_curvature(math.radians(self.steering_angle_deadzone_deg), CS.vEgo, 0.0))
+      lateral_accel_deadzone = curvature_deadzone * CS.vEgo ** 2
 
       delay_frames = int(np.clip(lat_delay / self.dt, 1, self.LATACCEL_REQUEST_BUFFER_NUM_FRAMES))
       expected_lateral_accel = self.requested_lateral_accel_buffer[-delay_frames]
@@ -71,18 +77,19 @@ class LatControlTorque(LatControl):
       self.requested_lateral_accel_buffer.append(future_desired_lateral_accel)
       gravity_adjusted_future_lateral_accel = future_desired_lateral_accel - roll_compensation
       desired_lateral_jerk = (future_desired_lateral_accel - expected_lateral_accel) / lat_delay
-      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_NN if frogpilot_toggles.nnff else LOW_SPEED_Y)**2 / (np.clip(CS.vEgo, MIN_SPEED, np.inf) ** 2)
+      measurement = measured_curvature * CS.vEgo ** 2
+      measurement_rate = self.measurement_rate_filter.update((measurement - self.previous_measurement) / self.dt)
+      self.previous_measurement = measurement
+
+      low_speed_factor = (np.interp(CS.vEgo, LOW_SPEED_X, LOW_SPEED_Y_NN if frogpilot_toggles.nnff else LOW_SPEED_Y) / max(CS.vEgo, MIN_SPEED)) ** 2
       setpoint = lat_delay * desired_lateral_jerk + expected_lateral_accel
-      measurement = actual_lateral_accel
       error = setpoint - measurement
       error_lsf = error + low_speed_factor * error
 
       if self.nnff_loaded and frogpilot_toggles.nnff or frogpilot_toggles.nnff_lite:
         pid_log, ff = self.nnff.compute_nnff(
-          CS, VM, actual_lateral_accel, error, future_desired_lateral_accel, gravity_adjusted_future_lateral_accel,
+          CS, VM, measurement, error, future_desired_lateral_accel, gravity_adjusted_future_lateral_accel,
           llk, measurement, model_data, params, pid_log, setpoint, frogpilot_toggles
         )
 
@@ -102,9 +109,10 @@ class LatControlTorque(LatControl):
 
         freeze_integrator = steer_limited_by_safety or CS.steeringPressed or CS.vEgo < 5
         output_lataccel = self.pid.update(pid_log.error,
-                                        feedforward=ff,
-                                        speed=CS.vEgo,
-                                        freeze_integrator=freeze_integrator)
+                                         -measurement_rate,
+                                          feedforward=ff,
+                                          speed=CS.vEgo,
+                                          freeze_integrator=freeze_integrator)
         output_torque = self.torque_from_lateral_accel(output_lataccel, self.torque_params)
 
       pid_log.active = True
@@ -113,8 +121,8 @@ class LatControlTorque(LatControl):
       pid_log.d = float(self.pid.d)
       pid_log.f = float(self.pid.f)
       pid_log.output = float(-output_torque)  # TODO: log lat accel?
-      pid_log.actualLateralAccel = float(actual_lateral_accel)
-      pid_log.desiredLateralAccel = float(expected_lateral_accel)
+      pid_log.actualLateralAccel = float(measurement)
+      pid_log.desiredLateralAccel = float(setpoint)
       pid_log.saturated = bool(self._check_saturation(self.steer_max - abs(output_torque) < 1e-3, CS, steer_limited_by_safety, curvature_limited))
 
     # TODO left is positive in this convention

selfdrive/controls/lib/pid.py

@@ -26,7 +26,7 @@ class PIDController:
     self.neg_p_limit = neg_p_limit
 
     self.i_unwind_rate = 0.3 / rate
-    self.i_rate = 1.0 / rate
+    self.i_dt = 1.0 / rate
     self.speed = 0.0
 
     self.reset()
@@ -61,19 +61,19 @@ class PIDController:
   def update(self, error, error_rate=0.0, speed=0.0, override=False, feedforward=0., freeze_integrator=False):
     self.speed = speed
 
-    self.p = float(error) * self.k_p
+    self.p = self.k_p * float(error)
     if self.pos_p_limit is not None and self.p > self.pos_p_limit:
       self.p = self.pos_p_limit
     elif self.neg_p_limit is not None and self.p < self.neg_p_limit:
       self.p = self.neg_p_limit
-    self.f = feedforward * self.k_f
-    self.d = error_rate * self.k_d
+    self.d = self.k_d * error_rate
+    self.f = self.k_f * feedforward
 
     if override:
       self.i -= self.i_unwind_rate * float(np.sign(self.i))
     else:
       if not freeze_integrator:
-        self.i = self.i + error * self.k_i * self.i_rate
+        self.i = self.i + self.k_i * self.i_dt * error
 
         # Clip i to prevent exceeding control limits
         control_no_i = self.p + self.d + self.f