activate derivative action

Author: felsager

common/pid.py

@@ -51,8 +51,8 @@ def update(self, error, error_rate=0.0, speed=0.0, feedforward=0., freeze_integr
     self.d = error_rate * self.k_d
 
     if not freeze_integrator:
-      if error_expected is not None:
-        error = error_expected
+      #if error_expected is not None:
+      #  error = error_expected
       i = self.i + error * self.k_i * self.i_rate
 
       # Don't allow windup if already clipping

selfdrive/controls/lib/latcontrol_torque.py

@@ -6,6 +6,7 @@
 from opendbc.car.lateral import FRICTION_THRESHOLD, get_friction
 from openpilot.common.constants import ACCELERATION_DUE_TO_GRAVITY
 from openpilot.selfdrive.controls.lib.latcontrol import DT_CTRL, LatControl
+from openpilot.common.filter_simple import FirstOrderFilter
 from openpilot.common.pid import PIDController
 
 # At higher speeds (25+mph) we can assume:
@@ -36,6 +37,7 @@ def __init__(self, CP, CI):
     self.update_limits()
     self.steering_angle_deadzone_deg = self.torque_params.steeringAngleDeadzoneDeg
     self.requested_lateral_accel_buffer = deque([0.] * LATACCEL_REQUEST_BUFFER_SIZE, maxlen=LATACCEL_REQUEST_BUFFER_SIZE)
+    self.error_pre = 0.0
 
   def update_live_torque_params(self, latAccelFactor, latAccelOffset, friction):
     self.torque_params.latAccelFactor = latAccelFactor
@@ -68,11 +70,13 @@ def update(self, active, CS, VM, params, steer_limited_by_safety, desired_curvat
       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 = lag_compensated_desired_lateral_accel + low_speed_factor * desired_curvature
-      setpoint_expected = current_expected_lateral_accel + low_speed_factor * current_expected_curvature
+      #setpoint_expected = 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
-      error_expected = float(setpoint_expected - measurement)
-
+      #error_expected = float(setpoint_expected - measurement)
+      error = float(setpoint - measurement)
+      error_rate = (error - self.error_pre) / DT_CTRL
+      self.error_pre = error
       # do error correction in lateral acceleration space, convert at end to handle non-linear torque responses correctly
       pid_log.error = float(setpoint - measurement)
       ff = gravity_adjusted_lateral_accel
@@ -82,10 +86,11 @@ def update(self, active, CS, VM, params, steer_limited_by_safety, desired_curvat
 
       freeze_integrator = steer_limited_by_safety or CS.steeringPressed or CS.vEgo < 5
       output_lataccel = self.pid.update(pid_log.error,
+                                        error_rate,
                                         feedforward=ff,
                                         speed=CS.vEgo,
-                                        freeze_integrator=freeze_integrator,
-                                        error_expected=error_expected)
+                                        freeze_integrator=freeze_integrator,)
+                                        #error_expected=error_expected)
       output_torque = self.torque_from_lateral_accel(output_lataccel, self.torque_params)
 
       pid_log.active = True