🔧 Tweak Toyota Long Tuning for My Tastes

The Rav4 2019 TSS2 has ~1.4s lag between when accel commands are sent
to the car and when they are acted upon when the car is starting from
a dead stop.  Stock feels fine with this limitation, so OP can too.

Most of these changes are designed to fix any sluggishness caused by
the Toyota Tune.  Additional changes need to be made to the Long MPC
in order to solve the sluggish start completely.

Changes:

Decrease vEgoStopping and vEgoStarting to 0.2.  Car is perfectly
capable of controlling at these speeds.  Lower value allows for state
machine to exit stopping phase just a bit faster.

Double the ActuatorDelay lag to .3 so we look into the future just a bit
more, seems a bit more inline with realworld experience.

Halve stoppingDecelRate to match the OEM stop_accel of -.4m/s2.  Makes
the final jerk in stopping a bit smoother

Increase the PID max accel to 1.8 m/s2 tappering back to 1.5 m/s2 at 5m/s.

Increase planner max accel to 1.4 m/s2 from 1.2 m/s2 starting from a stop
tapering back to 1.2 m/s2 at 10m/s.

Decreased planner max accel at 55mph, seemed a little uncomfortable to me.

Decrease A_Change Timescale in Long_MPC to match more closely what toyota
lag is.

Ignore some messages and fields in process_replay test in order to
keep it working.

selfdrive/car/toyota/carcontroller.py

@@ -6,6 +6,7 @@
                                            create_fcw_command, create_lta_steer_command
 from selfdrive.car.toyota.values import CAR, STATIC_DSU_MSGS, NO_STOP_TIMER_CAR, TSS2_CAR, \
                                         MIN_ACC_SPEED, PEDAL_TRANSITION, CarControllerParams
+from selfdrive.car.toyota.interface import CarInterface
 from opendbc.can.packer import CANPacker
 
 VisualAlert = car.CarControl.HUDControl.VisualAlert
@@ -20,6 +21,7 @@ def __init__(self, dbc_name, CP, VM):
     self.last_standstill = False
     self.standstill_req = False
     self.steer_rate_limited = False
+    self.CP = CP
 
     self.packer = CANPacker(dbc_name)
     self.gas = 0
@@ -46,7 +48,8 @@ def update(self, CC, CS):
       interceptor_gas_cmd = clip(pedal_command, 0., MAX_INTERCEPTOR_GAS)
     else:
       interceptor_gas_cmd = 0.
-    pcm_accel_cmd = clip(actuators.accel, CarControllerParams.ACCEL_MIN, CarControllerParams.ACCEL_MAX)
+    pid_accel_limits = CarInterface.get_pid_accel_limits(self.CP, CS.out.vEgo, None)  # Need to get cruise speed from somewhere
+    pcm_accel_cmd = clip(actuators.accel, pid_accel_limits[0], pid_accel_limits[1])
 
     # steer torque
     new_steer = int(round(actuators.steer * CarControllerParams.STEER_MAX))

selfdrive/car/toyota/interface.py

@@ -1,6 +1,8 @@
 #!/usr/bin/env python3
 from cereal import car
 from common.conversions import Conversions as CV
+from panda import Panda
+from common.numpy_fast import interp
 from selfdrive.car.toyota.tunes import LatTunes, LongTunes, set_long_tune, set_lat_tune
 from selfdrive.car.toyota.values import Ecu, CAR, ToyotaFlags, TSS2_CAR, NO_DSU_CAR, MIN_ACC_SPEED, EPS_SCALE, EV_HYBRID_CAR, CarControllerParams
 from selfdrive.car import STD_CARGO_KG, scale_rot_inertia, scale_tire_stiffness, gen_empty_fingerprint, get_safety_config
@@ -12,7 +14,13 @@
 class CarInterface(CarInterfaceBase):
   @staticmethod
   def get_pid_accel_limits(CP, current_speed, cruise_speed):
-    return CarControllerParams.ACCEL_MIN, CarControllerParams.ACCEL_MAX
+    if CP.carFingerprint in TSS2_CAR:
+      # Allow for higher accel from PID controller at low speeds
+      return CarControllerParams.ACCEL_MIN, interp(current_speed,
+                                                   CarControllerParams.ACCEL_MAX_TSS2_BP,
+                                                   CarControllerParams.ACCEL_MAX_TSS2_VALS)
+    else:
+      return CarControllerParams.ACCEL_MIN, CarControllerParams.ACCEL_MAX
 
   @staticmethod
   def get_params(candidate, fingerprint=gen_empty_fingerprint(), car_fw=[], disable_radar=False):  # pylint: disable=dangerous-default-value
@@ -256,7 +264,12 @@ def get_params(candidate, fingerprint=gen_empty_fingerprint(), car_fw=[], disabl
       set_long_tune(ret.longitudinalTuning, LongTunes.PEDAL)
     elif candidate in TSS2_CAR:
       set_long_tune(ret.longitudinalTuning, LongTunes.TSS2)
-      ret.stoppingDecelRate = 0.3  # reach stopping target smoothly
+      # Improved longitudinal tune settings from sshane
+      ret.vEgoStopping = 0.2  # car is near 0.1 to 0.2 when car starts requesting stopping accel
+      ret.vEgoStarting = 0.2  # needs to be > or == vEgoStopping
+      ret.stoppingDecelRate = 0.4  # reach stopping target smoothly - seems to take 0.5 seconds to go from 0 to -0.4
+      ret.longitudinalActuatorDelayLowerBound = 0.3
+      ret.longitudinalActuatorDelayUpperBound = 0.3
     else:
       set_long_tune(ret.longitudinalTuning, LongTunes.TSS)
 

selfdrive/car/toyota/values.py

@@ -16,6 +16,9 @@
 class CarControllerParams:
   ACCEL_MAX = 1.5  # m/s2, lower than allowed 2.0 m/s2 for tuning reasons
   ACCEL_MIN = -3.5  # m/s2
+  # KRKeegan increase allowed PID accel for sluggish start
+  ACCEL_MAX_TSS2_VALS = [1.8, ACCEL_MAX]
+  ACCEL_MAX_TSS2_BP =   [0.0, 6.0]
 
   STEER_MAX = 1500
   STEER_DELTA_UP = 15       # 1.5s time to peak torque

selfdrive/controls/lib/longitudinal_mpc_lib/long_mpc.py

@@ -253,7 +253,9 @@ def set_weights_for_lead_policy(self, prev_accel_constraint=True):
                                    A_EGO_COST, a_change_cost * cost_mulitpliers[0],
                                    J_EGO_COST * cost_mulitpliers[1]]))
     for i in range(N):
-      W[4,4] = a_change_cost * cost_mulitpliers[0] * np.interp(T_IDXS[i], [0.0, 1.0, 2.0], [1.0, 1.0, 0.0])
+      # reduce the cost on (a-a_prev) later in the horizon.
+      # KRKeegan, decreased timescale to .5s since Toyota lag is set to .3s
+      W[4,4] = a_change_cost * cost_mulitpliers[0] * np.interp(T_IDXS[i], [0.0, 0.5, 2.0], [1.0, 1.0, 0.0])
       self.solver.cost_set(i, 'W', W)
     # Setting the slice without the copy make the array not contiguous,
     # causing issues with the C interface.

selfdrive/controls/lib/longitudinal_planner.py

@@ -17,8 +17,8 @@
 LON_MPC_STEP = 0.2  # first step is 0.2s
 AWARENESS_DECEL = -0.2  # car smoothly decel at .2m/s^2 when user is distracted
 A_CRUISE_MIN = -1.2
-A_CRUISE_MAX_VALS = [1.2, 1.2, 0.8, 0.6]
-A_CRUISE_MAX_BP = [0., 15., 25., 40.]
+A_CRUISE_MAX_VALS = [1.4, 1.2, 0.7, 0.6]  # Sets the limits of the planner accel, PID may exceed
+A_CRUISE_MAX_BP = [0., 10., 25., 40.]
 
 # Lookup table for turns
 _A_TOTAL_MAX_V = [1.7, 3.2]