Toyota long PCM compensation

selfdrive/car/toyota/carcontroller.py

@@ -1,11 +1,11 @@
 from cereal import car
-from openpilot.common.numpy_fast import clip
+from openpilot.common.numpy_fast import clip, interp
 from openpilot.selfdrive.car import apply_meas_steer_torque_limits, apply_std_steer_angle_limits, common_fault_avoidance, make_can_msg
 from openpilot.selfdrive.car.interfaces import CarControllerBase
 from openpilot.selfdrive.car.toyota import toyotacan
 from openpilot.selfdrive.car.toyota.values import CAR, STATIC_DSU_MSGS, NO_STOP_TIMER_CAR, TSS2_CAR, \
-                                        CarControllerParams, ToyotaFlags, \
-                                        UNSUPPORTED_DSU_CAR
+  CarControllerParams, ToyotaFlags, \
+  UNSUPPORTED_DSU_CAR
 from opendbc.can.packer import CANPacker
 
 SteerControlType = car.CarParams.SteerControlType
@@ -25,6 +25,17 @@
 MAX_LTA_DRIVER_TORQUE_ALLOWANCE = 150  # slightly above steering pressed allows some resistance when changing lanes
 
 
+def compute_gb_toyota(accel, speed):
+  # from honda nidec
+  creep_brake = 0.0
+  creep_speed = 2.3
+  creep_brake_value = 0.15
+  if speed < creep_speed:
+    creep_brake = (creep_speed - speed) / creep_speed * creep_brake_value
+  gb = accel - creep_brake
+  return gb
+
+
 class CarController(CarControllerBase):
   def __init__(self, dbc_name, CP, VM):
     self.CP = CP
@@ -36,6 +47,7 @@ def __init__(self, dbc_name, CP, VM):
     self.last_standstill = False
     self.standstill_req = False
     self.steer_rate_counter = 0
+    self.pcm_accel_comp = 0
     self.distance_button = 0
 
     self.packer = CANPacker(dbc_name)
@@ -100,7 +112,31 @@ def update(self, CC, CS, now_nanos):
                                                           lta_active, self.frame // 2, torque_wind_down))
 
     # *** gas and brake ***
-    pcm_accel_cmd = clip(actuators.accel, self.params.ACCEL_MIN, self.params.ACCEL_MAX)
+
+    gas_accel = compute_gb_toyota(actuators.accel, CS.out.vEgo)
+
+    # wind brake from air resistance decel at high speed
+    wind_brake = interp(CS.out.vEgo, [0.0, 2.3, 35.0], [0.001, 0.002, 0.15])
+
+    gas_accel += wind_brake
+
+    # we will throw out PCM's compensations, but that may be a good thing. for example:
+    # we lose things like pitch compensation, gas to maintain speed, brake to compensate for creeping, etc.
+    # but also remove undesirable "snap to standstill" behavior when not requesting enough accel at low speeds,
+    # lag to start moving, lag to start braking, etc.
+    # PI should compensate for lack of the desirable behaviors, but might be worse than the PCM doing them
+    self.pcm_accel_comp = clip(gas_accel - CS.pcm_accel_net, self.pcm_accel_comp - 0.03, self.pcm_accel_comp + 0.03)
+    pcm_accel_cmd = gas_accel + self.pcm_accel_comp
+
+    # add back gas to maintain speed
+    # accel_offset = CS.pcm_neutral_force / self.CP.mass
+    # fact = interp(CS.out.vEgo, [3, 6], [0.0, 1.0])  # not at low speed
+    # pcm_accel_cmd -= min(accel_offset / 2, 0) * fact  # only add, reduce effect
+
+    if not CC.longActive:
+      pcm_accel_cmd = 0.0
+
+    pcm_accel_cmd = clip(pcm_accel_cmd, self.params.ACCEL_MIN, self.params.ACCEL_MAX)
 
     # TODO: probably can delete this. CS.pcm_acc_status uses a different signal
     # than CS.cruiseState.enabled. confirm they're not meaningfully different
@@ -149,7 +185,7 @@ def update(self, CC, CS, now_nanos):
       # - there is something to stop displaying
       send_ui = False
       if ((fcw_alert or steer_alert) and not self.alert_active) or \
-         (not (fcw_alert or steer_alert) and self.alert_active):
+        (not (fcw_alert or steer_alert) and self.alert_active):
         send_ui = True
         self.alert_active = not self.alert_active
       elif pcm_cancel_cmd:

selfdrive/car/toyota/carstate.py

@@ -48,6 +48,8 @@ def __init__(self, CP):
     self.low_speed_lockout = False
     self.acc_type = 1
     self.lkas_hud = {}
+    self.pcm_accel_net = 0.0
+    self.pcm_neutral_force = 0.0
 
   def update(self, cp, cp_cam):
     ret = car.CarState.new_message()
@@ -72,6 +74,9 @@ def update(self, cp, cp_cam):
     ret.vEgo, ret.aEgo = self.update_speed_kf(ret.vEgoRaw)
     ret.vEgoCluster = ret.vEgo * 1.015  # minimum of all the cars
 
+    # thought to be the gas/brake as issued by the pcm (0=coasting)
+    self.pcm_accel_net = cp.vl["PCM_CRUISE"]["ACCEL_NET"]
+    self.pcm_neutral_force = cp.vl["PCM_CRUISE"]["NEUTRAL_FORCE"]
     ret.standstill = abs(ret.vEgoRaw) < 1e-3
 
     ret.steeringAngleDeg = cp.vl["STEER_ANGLE_SENSOR"]["STEER_ANGLE"] + cp.vl["STEER_ANGLE_SENSOR"]["STEER_FRACTION"]

selfdrive/car/toyota/interface.py

@@ -145,14 +145,42 @@ def _get_params(ret, candidate, fingerprint, car_fw, experimental_long, docs):
     tune.deadzoneBP = [0., 9.]
     tune.deadzoneV = [.0, .15]
     if candidate in TSS2_CAR:
-      tune.kpBP = [0., 5., 20.]
-      tune.kpV = [1.3, 1.0, 0.7]
-      tune.kiBP = [0., 5., 12., 20., 27.]
-      tune.kiV = [.35, .23, .20, .17, .1]
+      tune.kpBP = [5., 35.]
+      tune.kpV = [0.5, 0.5]
+      tune.kiBP = [0.]
+      tune.kiV = [.25]
       if candidate in TSS2_CAR:
-        ret.vEgoStopping = 0.25
-        ret.vEgoStarting = 0.25
+        ret.vEgoStopping = 0.1
+        ret.vEgoStarting = 0.1
         ret.stoppingDecelRate = 0.3  # reach stopping target smoothly
+
+        # some notes:
+        # longcontrol has smart accel calculation that takes into account the difference between:
+        # - ideal accel given current planned speed and future planned speed from actuator delay
+        # - the current planned accel
+        # and outputs an acceleration to move your current planned accel towards the ideal accel from the speeds.
+        # this likely accounts for slightly incorrect actuator delay, but i'm guessing it can't account for sudden changes in accel and will undershoot
+
+        # try playing around with this:
+        """
+        t = 0.15  # + 0.2
+        ideal_accel = (23.2273 - 23.7665) / t  # future - current plan
+        a_target_now = -1.16447  # current plan
+        print(f'{ideal_accel=}')
+        comp_accel = 2 * ideal_accel - a_target_now
+        print(f'{comp_accel=}')  # this generally matched the accel at plan index 6/7 (0.35s/0.48s)
+
+        Out: ideal_accel=-1.5337244444444473  # current plan undershooting this, so longcontrol decreases feedforward accel:
+        Out: comp_accel=-1.9029788888888948
+        """
+
+        # by getting the delay more dialed in, a_target_now should move towards ideal_accel, causing its effect to lessen and lag to go down
+
+        # TODO: is the reason current planned accel doesn't match future planned speeds because the planner is constrained to start near the ego?
+
+        # TODO: we add 0.2s delay to lat, why not long?
+        ret.longitudinalActuatorDelayLowerBound = 0.15 + 0.15
+        ret.longitudinalActuatorDelayUpperBound = 0.15 + 0.25
     else:
       tune.kpBP = [0., 5., 35.]
       tune.kiBP = [0., 35.]

selfdrive/car/toyota/values.py

@@ -16,7 +16,7 @@
 
 
 class CarControllerParams:
-  ACCEL_MAX = 1.5  # m/s2, lower than allowed 2.0 m/s2 for tuning reasons
+  ACCEL_MAX = 2.0  # m/s2, lower than allowed 2.0 m/s2 for tuning reasons
   ACCEL_MIN = -3.5  # m/s2
 
   STEER_STEP = 1