Should not fault with new panda

panda

@@ -1 +1 @@
-Subproject commit 499906f3244712c1edd7f1cc8e92112d11f2b505
+Subproject commit edb3602d0aa714f4db52c1e6342558ecbee578d1

selfdrive/car/toyota/interface.py

@@ -118,7 +118,7 @@ class CarInterface(CarInterfaceBase):
       ret.steerRatio = 14.3
       tire_stiffness_factor = 0.7933
       ret.mass = 3585. * CV.LB_TO_KG + STD_CARGO_KG  # Average between ICE and Hybrid
-      set_lat_tune(ret.lateralTuning, LatTunes.PID_D)
+      set_lat_tune(ret.lateralTuning, LatTunes.PID_J)
 
       # 2019+ Rav4 TSS2 uses two different steering racks and specific tuning seems to be necessary.
       # See https://github.com/commaai/openpilot/pull/21429#issuecomment-873652891
@@ -133,7 +133,7 @@ class CarInterface(CarInterfaceBase):
       ret.steerRatio = 13.9
       tire_stiffness_factor = 0.444  # not optimized yet
       ret.mass = 3060. * CV.LB_TO_KG + STD_CARGO_KG
-      set_lat_tune(ret.lateralTuning, LatTunes.PID_D)
+      set_lat_tune(ret.lateralTuning, LatTunes.PID_J)
 
     elif candidate in (CAR.LEXUS_ES_TSS2, CAR.LEXUS_ESH_TSS2, CAR.LEXUS_ESH):
       stop_and_go = True
@@ -141,7 +141,7 @@ class CarInterface(CarInterfaceBase):
       ret.steerRatio = 16.0  # not optimized
       tire_stiffness_factor = 0.444  # not optimized yet
       ret.mass = 3677. * CV.LB_TO_KG + STD_CARGO_KG  # mean between min and max
-      set_lat_tune(ret.lateralTuning, LatTunes.PID_D)
+      set_lat_tune(ret.lateralTuning, LatTunes.PID_J)
 
     elif candidate == CAR.SIENNA:
       stop_and_go = True

selfdrive/car/toyota/tunes.py

@@ -105,9 +105,9 @@ def set_lat_tune(tune, name):
       tune.pid.kiV = [0.05]
       tune.pid.kf = 0.00004
     elif name == LatTunes.PID_J:
-      tune.pid.kpV = [0.19]
-      tune.pid.kiV = [0.02]
-      tune.pid.kf = 0.00007818594
+      tune.pid.kpV = [0.5]
+      tune.pid.kiV = [0.025]
+      tune.pid.kf = 1.0
     elif name == LatTunes.PID_L:
       tune.pid.kpV = [0.3]
       tune.pid.kiV = [0.05]

selfdrive/car/toyota/values.py

@@ -18,7 +18,7 @@ class CarControllerParams:
   ACCEL_MIN = -3.5  # m/s2
 
   STEER_MAX = 1500
-  STEER_DELTA_UP = 10       # 1.5s time to peak torque
+  STEER_DELTA_UP = 15       # 1.0s time to peak torque
   STEER_DELTA_DOWN = 25     # always lower than 45 otherwise the Rav4 faults (Prius seems ok with 50)
   STEER_ERROR_MAX = 350     # max delta between torque cmd and torque motor
 

selfdrive/controls/controlsd.py

@@ -518,7 +518,7 @@ class Controls:
                                                                              lat_plan.curvatureRates)
       actuators.steer, actuators.steeringAngleDeg, lac_log = self.LaC.update(CC.latActive, CS, self.CP, self.VM,
                                                                              params, self.last_actuators, desired_curvature,
-                                                                             desired_curvature_rate)
+                                                                             desired_curvature_rate, self.sm['liveLocationKalman'])
     else:
       lac_log = log.ControlsState.LateralDebugState.new_message()
       if self.sm.rcv_frame['testJoystick'] > 0:

selfdrive/controls/lib/latcontrol.py

@@ -13,7 +13,7 @@ class LatControl(ABC):
     self.sat_count = 0.
 
   @abstractmethod
-  def update(self, active, CS, CP, VM, params, last_actuators, desired_curvature, desired_curvature_rate):
+  def update(self, active, CS, CP, VM, params, last_actuators, desired_curvature, desired_curvature_rate, llk):
     pass
 
   def reset(self):

selfdrive/controls/lib/latcontrol_angle.py

@@ -7,7 +7,7 @@ STEER_ANGLE_SATURATION_THRESHOLD = 2.5  # Degrees
 
 
 class LatControlAngle(LatControl):
-  def update(self, active, CS, CP, VM, params, last_actuators, desired_curvature, desired_curvature_rate):
+  def update(self, active, CS, CP, VM, params, last_actuators, desired_curvature, desired_curvature_rate, llk):
     angle_log = log.ControlsState.LateralAngleState.new_message()
 
     if CS.vEgo < MIN_STEER_SPEED or not active:

selfdrive/controls/lib/latcontrol_indi.py

@@ -65,7 +65,7 @@ class LatControlINDI(LatControl):
     self.steer_filter.x = 0.
     self.speed = 0.
 
-  def update(self, active, CS, CP, VM, params, last_actuators, desired_curvature, desired_curvature_rate):
+  def update(self, active, CS, CP, VM, params, last_actuators, desired_curvature, desired_curvature_rate, llk):
     self.speed = CS.vEgo
     # Update Kalman filter
     y = np.array([[math.radians(CS.steeringAngleDeg)], [math.radians(CS.steeringRateDeg)]])

selfdrive/controls/lib/latcontrol_lqr.py

@@ -31,7 +31,7 @@ class LatControlLQR(LatControl):
     super().reset()
     self.i_lqr = 0.0
 
-  def update(self, active, CS, CP, VM, params, last_actuators, desired_curvature, desired_curvature_rate):
+  def update(self, active, CS, CP, VM, params, last_actuators, desired_curvature, desired_curvature_rate, llk):
     lqr_log = log.ControlsState.LateralLQRState.new_message()
 
     steers_max = get_steer_max(CP, CS.vEgo)

selfdrive/controls/lib/latcontrol_pid.py

@@ -1,5 +1,3 @@
-import math
-
 from selfdrive.controls.lib.pid import PIController
 from selfdrive.controls.lib.drive_helpers import get_steer_max
 from selfdrive.controls.lib.latcontrol import LatControl, MIN_STEER_SPEED
@@ -18,16 +16,16 @@ class LatControlPID(LatControl):
     super().reset()
     self.pid.reset()
 
-  def update(self, active, CS, CP, VM, params, last_actuators, desired_curvature, desired_curvature_rate):
+  def update(self, active, CS, CP, VM, params, last_actuators, desired_curvature, desired_curvature_rate, llk):
     pid_log = log.ControlsState.LateralPIDState.new_message()
-    pid_log.steeringAngleDeg = float(CS.steeringAngleDeg)
-    pid_log.steeringRateDeg = float(CS.steeringRateDeg)
+    #pid_log.steeringAngleDeg = float(CS.steeringAngleDeg)
+    #pid_log.steeringRateDeg = float(CS.steeringRateDeg)
 
-    angle_steers_des_no_offset = math.degrees(VM.get_steer_from_curvature(-desired_curvature, CS.vEgo, params.roll))
-    angle_steers_des = angle_steers_des_no_offset + params.angleOffsetDeg
+    #angle_steers_des_no_offset = math.degrees(VM.get_steer_from_curvature(-desired_curvature, CS.vEgo, params.roll))
+    #angle_steers_des = angle_steers_des_no_offset + params.angleOffsetDeg
 
-    pid_log.steeringAngleDesiredDeg = angle_steers_des
-    pid_log.angleError = angle_steers_des - CS.steeringAngleDeg
+    #pid_log.steeringAngleDesiredDeg = angle_steers_des
+    #pid_log.angleError = angle_steers_des - CS.steeringAngleDeg
     if CS.vEgo < MIN_STEER_SPEED or not active:
       output_steer = 0.0
       pid_log.active = False
@@ -37,12 +35,15 @@ class LatControlPID(LatControl):
       self.pid.pos_limit = steers_max
       self.pid.neg_limit = -steers_max
 
+      lateral_accel_desired = desired_curvature * CS.vEgo**2
+      lateral_accel_actual = llk.angularVelocityCalibrated.value[2] * CS.vEgo
+
       # offset does not contribute to resistive torque
-      steer_feedforward = self.get_steer_feedforward(angle_steers_des_no_offset, CS.vEgo)
+      steer_feedforward = -(lateral_accel_desired - 9.81 * llk.orientationNED.value[0])/2.0
 
       deadzone = 0.0
 
-      output_steer = self.pid.update(angle_steers_des, CS.steeringAngleDeg, override=CS.steeringPressed,
+      output_steer = self.pid.update(-lateral_accel_desired, -lateral_accel_actual, override=CS.steeringPressed,
                                      feedforward=steer_feedforward, speed=CS.vEgo, deadzone=deadzone)
       pid_log.active = True
       pid_log.p = self.pid.p
@@ -51,4 +52,4 @@ class LatControlPID(LatControl):
       pid_log.output = output_steer
       pid_log.saturated = self._check_saturation(steers_max - abs(output_steer) < 1e-3, CS)
 
-    return output_steer, angle_steers_des, pid_log
+    return output_steer, 0.0, pid_log