Toyota: API is now gas/brake

opendbc/car/toyota/carcontroller.py

@@ -1,16 +1,19 @@
 import copy
+import math
 from opendbc.car import apply_meas_steer_torque_limits, apply_std_steer_angle_limits, common_fault_avoidance, make_tester_present_msg, structs
 from opendbc.car.can_definitions import CanData
 from opendbc.car.common.numpy_fast import clip
 from opendbc.car.interfaces import CarControllerBase
 from opendbc.car.toyota import toyotacan
+from openpilot.common.pid import PIDController
 from opendbc.car.toyota.values import CAR, STATIC_DSU_MSGS, NO_STOP_TIMER_CAR, TSS2_CAR, \
                                         CarControllerParams, ToyotaFlags, \
                                         UNSUPPORTED_DSU_CAR
 from opendbc.can.packer import CANPacker
 
 SteerControlType = structs.CarParams.SteerControlType
 VisualAlert = structs.CarControl.HUDControl.VisualAlert
+LongCtrlState = structs.CarControl.Actuators.LongControlState
 
 # LKA limits
 # EPS faults if you apply torque while the steering rate is above 100 deg/s for too long
@@ -36,8 +39,11 @@ def __init__(self, dbc_name, CP):
     self.last_standstill = False
     self.standstill_req = False
     self.steer_rate_counter = 0
+    self.pcm_accel_comp = 0
     self.distance_button = 0
 
+    self.pid = PIDController(k_p=1.0, k_i=0.25, k_f=0)
+
     self.packer = CANPacker(dbc_name)
     self.accel = 0
 
@@ -99,7 +105,33 @@ 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)
+    # 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
+
+    # FIXME? neutral force will only be positive under ~5 mph, which messes up stopping control considerably
+    # not sure why this isn't captured in the PCM accel net, maybe that just ignores creep force + high speed deceleration
+    # it also doesn't seem to capture slightly more braking on downhills (VSC1S07->ASLP (pitch, deg.) might have some clues)
+    offset = min(CS.pcm_neutral_force / self.CP.mass, 0.0)
+    pitch_offset = math.sin(math.radians(CS.vsc_slope_angle)) * 9.81  # downhill is negative
+    # TODO: these limits are too slow to prevent a jerk when engaging, ramp down on engage?
+    # self.pcm_accel_comp = clip(actuators.accel - CS.pcm_accel_net, self.pcm_accel_comp - 0.05, self.pcm_accel_comp + 0.05)
+    pcm_accel_comp = self.pid.update(actuators.accel - CS.pcm_calc_accel_net)
+    self.pcm_accel_comp = clip(pcm_accel_comp, self.pcm_accel_comp - 0.005, self.pcm_accel_comp + 0.005)
+    if CS.out.cruiseState.standstill or actuators.longControlState == LongCtrlState.stopping:
+      self.pcm_accel_comp = 0.0
+      self.pid.reset()
+    pcm_accel_cmd = actuators.accel + self.pcm_accel_comp  # + offset
+    # pcm_accel_cmd = actuators.accel - pitch_offset
+
+    if not CC.longActive:
+      self.pid.reset()
+      self.pcm_accel_comp = 0.0
+      pcm_accel_cmd = 0.0
+
+    pcm_accel_cmd = clip(pcm_accel_cmd, self.params.ACCEL_MIN, self.params.ACCEL_MAX_COMP)
 
     # on entering standstill, send standstill request
     if CS.out.standstill and not self.last_standstill and (self.CP.carFingerprint not in NO_STOP_TIMER_CAR):

opendbc/car/toyota/carstate.py

@@ -47,6 +47,11 @@ def __init__(self, CP):
     self.low_speed_lockout = False
     self.acc_type = 1
     self.lkas_hud = {}
+    self.pcm_accel_net = 0.0
+    self.pcm_true_accel_net = 0.0
+    self.pcm_calc_accel_net = 0.0
+    self.pcm_neutral_force = 0.0
+    self.vsc_slope_angle = 0.0
 
   def update(self, cp, cp_cam, *_) -> structs.CarState:
     ret = structs.CarState()
@@ -71,6 +76,13 @@ def update(self, cp, cp_cam, *_) -> structs.CarState:
     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"]  # this is only accurate for braking * 43
+    self.pcm_true_accel_net = cp.vl["CLUTCH"]["TRUE_ACCEL_NET"]  # this is only accurate for acceleration * 78
+    self.pcm_calc_accel_net = cp.vl["GEAR_PACKET_HYBRID"]["CAR_MOVEMENT"] / 78 - cp.vl["BRAKE"]["BRAKE_PEDAL"] / 43
+    self.pcm_neutral_force = cp.vl["PCM_CRUISE"]["NEUTRAL_FORCE"]
+    self.vsc_slope_angle = cp.vl["VSC1S07"]["ASLP"]
+
     ret.standstill = abs(ret.vEgoRaw) < 1e-3
 
     ret.steeringAngleDeg = cp.vl["STEER_ANGLE_SENSOR"]["STEER_ANGLE"] + cp.vl["STEER_ANGLE_SENSOR"]["STEER_FRACTION"]
@@ -188,12 +200,16 @@ def get_can_parser(CP):
       ("BODY_CONTROL_STATE_2", 2),
       ("ESP_CONTROL", 3),
       ("EPS_STATUS", 25),
+      ("GEAR_PACKET_HYBRID", 60),
+      ("BRAKE", 80),
       ("BRAKE_MODULE", 40),
       ("WHEEL_SPEEDS", 80),
       ("STEER_ANGLE_SENSOR", 80),
       ("PCM_CRUISE", 33),
       ("PCM_CRUISE_SM", 1),
+      ("VSC1S07", 20),
       ("STEER_TORQUE_SENSOR", 50),
+      ("CLUTCH", 16),
     ]
 
     if CP.carFingerprint != CAR.TOYOTA_MIRAI:

opendbc/car/toyota/interface.py

@@ -123,11 +123,11 @@ def _get_params(ret: structs.CarParams, candidate, fingerprint, car_fw, experime
 
     tune = ret.longitudinalTuning
     if candidate in TSS2_CAR:
-      tune.kpV = [0.0]
-      tune.kiV = [0.5]
+      tune.kiBP = [5., 35.]
+      tune.kiV = [0.5, 0.5]
       ret.vEgoStopping = 0.25
       ret.vEgoStarting = 0.25
-      ret.stoppingDecelRate = 0.3  # reach stopping target smoothly
+      ret.stoppingDecelRate = 0.6  # reach stopping target smoothly
     else:
       tune.kiBP = [0., 5., 35.]
       tune.kiV = [3.6, 2.4, 1.5]

opendbc/car/toyota/values.py

@@ -15,7 +15,9 @@
 
 
 class CarControllerParams:
-  ACCEL_MAX = 1.5  # m/s2, lower than allowed 2.0 m/s2 for tuning reasons
+  # limit compensated accel to 1.5 m/s2
+  ACCEL_MAX = 2.0  # m/s2
+  ACCEL_MAX_COMP = 2.0  # m/s2
   ACCEL_MIN = -3.5  # m/s2
 
   STEER_STEP = 1

opendbc/dbc/generator/toyota/_toyota_2017.dbc

@@ -211,6 +211,7 @@ BO_ 865 CLUTCH: 8 XXX
  SG_ ACC_FAULTED : 32|1@0+ (1,0) [0|1] "" XXX
  SG_ GAS_PEDAL_ALT : 23|8@0+ (0.005,0) [0|1] "" XXX
  SG_ CLUTCH_RELEASED : 38|1@0+ (1,0) [0|1] "" XXX
+ SG_ TRUE_ACCEL_NET : 48|16@1+ (0.0002,-6.5536) [0|4095] "" XXX
 
 BO_ 869 DSU_CRUISE : 7 DSU
  SG_ RES_BTN : 3|1@0+ (1,0) [0|0] "" XXX

opendbc/dbc/toyota_new_mc_pt_generated.dbc

@@ -215,6 +215,7 @@ BO_ 865 CLUTCH: 8 XXX
  SG_ ACC_FAULTED : 32|1@0+ (1,0) [0|1] "" XXX
  SG_ GAS_PEDAL_ALT : 23|8@0+ (0.005,0) [0|1] "" XXX
  SG_ CLUTCH_RELEASED : 38|1@0+ (1,0) [0|1] "" XXX
+ SG_ TRUE_ACCEL_NET : 48|16@1+ (0.0002,-6.5536) [0|4095] "" XXX
 
 BO_ 869 DSU_CRUISE : 7 DSU
  SG_ RES_BTN : 3|1@0+ (1,0) [0|0] "" XXX

opendbc/dbc/toyota_nodsu_pt_generated.dbc

@@ -215,6 +215,7 @@ BO_ 865 CLUTCH: 8 XXX
  SG_ ACC_FAULTED : 32|1@0+ (1,0) [0|1] "" XXX
  SG_ GAS_PEDAL_ALT : 23|8@0+ (0.005,0) [0|1] "" XXX
  SG_ CLUTCH_RELEASED : 38|1@0+ (1,0) [0|1] "" XXX
+ SG_ TRUE_ACCEL_NET : 48|16@1+ (0.0002,-6.5536) [0|4095] "" XXX
 
 BO_ 869 DSU_CRUISE : 7 DSU
  SG_ RES_BTN : 3|1@0+ (1,0) [0|0] "" XXX

opendbc/dbc/toyota_tnga_k_pt_generated.dbc

@@ -215,6 +215,7 @@ BO_ 865 CLUTCH: 8 XXX
  SG_ ACC_FAULTED : 32|1@0+ (1,0) [0|1] "" XXX
  SG_ GAS_PEDAL_ALT : 23|8@0+ (0.005,0) [0|1] "" XXX
  SG_ CLUTCH_RELEASED : 38|1@0+ (1,0) [0|1] "" XXX
+ SG_ TRUE_ACCEL_NET : 48|16@1+ (0.0002,-6.5536) [0|4095] "" XXX
 
 BO_ 869 DSU_CRUISE : 7 DSU
  SG_ RES_BTN : 3|1@0+ (1,0) [0|0] "" XXX