Toyota PCM Compensation

Co-Authored-By: Irene <12470297+cydia2020@users.noreply.github.com>

common/params.cc

@@ -230,6 +230,7 @@ std::unordered_map<std::string, uint32_t> keys = {
     {"dp_lat_lane_change_assist_auto_timer", PERSISTENT},
     {"dp_lat_road_edge_detection", PERSISTENT},
     {"dp_device_disable_logging", PERSISTENT},
+    {"dp_toyota_pcm_compensation", PERSISTENT},
 };
 
 } // namespace

selfdrive/car/toyota/carcontroller.py

@@ -8,7 +8,12 @@ from openpilot.selfdrive.car.toyota.values import CAR, STATIC_DSU_MSGS, NO_STOP_
                                         UNSUPPORTED_DSU_CAR
 from opendbc.can.packer import CANPacker
 
+# dp
+from openpilot.common.params import Params
 from openpilot.dp_ext.selfdrive.car.toyota.door_lock_controller import DoorLockController
+from openpilot.dp_ext.selfdrive.car.toyota.pcm_compensation_controller import PCMCompensationController
+# for pcm compensation
+LongCtrlState = car.CarControl.Actuators.LongControlState
 
 SteerControlType = car.CarParams.SteerControlType
 VisualAlert = car.CarControl.HUDControl.VisualAlert
@@ -45,12 +50,15 @@ class CarController(CarControllerBase):
     self.accel = 0
     # dp
     self.dlc = DoorLockController()
+    self.pcc = PCMCompensationController(CP, self.params, Params().get_bool("dp_toyota_pcm_compensation"))
 
   def update(self, CC, CS, now_nanos):
     actuators = CC.actuators
     hud_control = CC.hudControl
     pcm_cancel_cmd = CC.cruiseControl.cancel
     lat_active = CC.latActive and abs(CS.out.steeringTorque) < MAX_USER_TORQUE
+    # dp - for pcm compensation
+    stopping = actuators.longControlState == LongCtrlState.stopping
 
     # *** control msgs ***
     can_sends = []
@@ -108,7 +116,10 @@ class CarController(CarControllerBase):
                                                           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)
+    if self.pcc.is_enabled():
+      pcm_accel_cmd = self.pcc.get_pcm_accel_cmd(CC.longActive, CS.out.vEgo, CS.pcm_neutral_force, actuators.accel)
+    else:
+      pcm_accel_cmd = clip(actuators.accel, self.params.ACCEL_MIN, self.params.ACCEL_MAX)
 
     # 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):
@@ -135,15 +146,19 @@ class CarController(CarControllerBase):
         else:
           self.distance_button = 0
 
+      # dp - for pcm compensation
+      # when stopping, send -2.5 raw acceleration immediately to prevent vehicle from creeping, else send actuators.accel
+      accel_raw = -2.5 if self.pcc.is_enabled() and stopping else actuators.accel
+
       # Lexus IS uses a different cancellation message
       if pcm_cancel_cmd and self.CP.carFingerprint in UNSUPPORTED_DSU_CAR:
         can_sends.append(toyotacan.create_acc_cancel_command(self.packer))
       elif self.CP.openpilotLongitudinalControl:
-        can_sends.append(toyotacan.create_accel_command(self.packer, pcm_accel_cmd, pcm_cancel_cmd, self.standstill_req, lead, CS.acc_type, fcw_alert,
+        can_sends.append(toyotacan.create_accel_command(self.packer, pcm_accel_cmd, accel_raw, pcm_cancel_cmd, self.standstill_req, lead, CS.acc_type, fcw_alert,
                                                         self.distance_button))
         self.accel = pcm_accel_cmd
       else:
-        can_sends.append(toyotacan.create_accel_command(self.packer, 0, pcm_cancel_cmd, False, lead, CS.acc_type, False, self.distance_button))
+        can_sends.append(toyotacan.create_accel_command(self.packer, 0, 0, pcm_cancel_cmd, False, lead, CS.acc_type, False, self.distance_button))
 
     # *** hud ui ***
     if self.CP.carFingerprint != CAR.TOYOTA_PRIUS_V:

selfdrive/car/toyota/carstate.py

@@ -51,7 +51,10 @@ class CarState(CarStateBase):
     self.acc_type = 1
     self.lkas_hud = {}
 
+    # dp
     self.zssc = ZSSController()
+    # for pcm compensation
+    self.pcm_neutral_force = 0.
 
   def update(self, cp, cp_cam):
     ret = car.CarState.new_message()
@@ -157,6 +160,8 @@ class CarState(CarStateBase):
       ret.cruiseState.standstill = self.pcm_acc_status == 7
     ret.cruiseState.enabled = bool(cp.vl["PCM_CRUISE"]["CRUISE_ACTIVE"])
     ret.cruiseState.nonAdaptive = self.pcm_acc_status in (1, 2, 3, 4, 5, 6)
+    # dp - for pcm compensation
+    self.pcm_neutral_force = cp.vl["PCM_CRUISE"]["NEUTRAL_FORCE"]
 
     ret.genericToggle = bool(cp.vl["LIGHT_STALK"]["AUTO_HIGH_BEAM"])
     ret.espDisabled = cp.vl["ESP_CONTROL"]["TC_DISABLED"] != 0

selfdrive/car/toyota/interface.py

@@ -7,6 +7,9 @@ from openpilot.selfdrive.car import create_button_events, get_safety_config
 from openpilot.selfdrive.car.disable_ecu import disable_ecu
 from openpilot.selfdrive.car.interfaces import CarInterfaceBase
 
+# dp
+from openpilot.common.params import Params
+
 ButtonType = car.CarState.ButtonEvent.Type
 EventName = car.CarEvent.EventName
 SteerControlType = car.CarParams.SteerControlType
@@ -142,7 +145,19 @@ class CarInterface(CarInterfaceBase):
     ret.minEnableSpeed = -1. if stop_and_go else MIN_ACC_SPEED
 
     tune = ret.longitudinalTuning
-    if candidate in TSS2_CAR:
+    # dp
+    if Params().get_bool("dp_toyota_pcm_compensation"):
+      # on stock Toyota this is -2.5
+      ret.stopAccel = -2.5
+
+      tune.deadzoneBP = [0., 16., 20., 30.]
+      tune.deadzoneV = [.04, .05, .08, .15]
+      ret.stoppingDecelRate = 0.17
+      tune.kpBP = [0., 5.]
+      tune.kpV = [0.8, 1.]
+      tune.kiBP = [0., 5.]
+      tune.kiV = [0.3, 1.]
+    elif candidate in TSS2_CAR:
       tune.kpV = [0.0]
       tune.kiV = [0.5]
       ret.vEgoStopping = 0.25

selfdrive/car/toyota/toyotacan.py

@@ -33,7 +33,7 @@ def create_lta_steer_command(packer, steer_control_type, steer_angle, steer_req,
   return packer.make_can_msg("STEERING_LTA", 0, values)
 
 
-def create_accel_command(packer, accel, pcm_cancel, standstill_req, lead, acc_type, fcw_alert, distance):
+def create_accel_command(packer, accel, accel_raw, pcm_cancel, standstill_req, lead, acc_type, fcw_alert, distance):
   # TODO: find the exact canceling bit that does not create a chime
   values = {
     "ACCEL_CMD": accel,
@@ -45,6 +45,7 @@ def create_accel_command(packer, accel, pcm_cancel, standstill_req, lead, acc_ty
     "CANCEL_REQ": pcm_cancel,
     "ALLOW_LONG_PRESS": 1,
     "ACC_CUT_IN": fcw_alert,  # only shown when ACC enabled
+    "ACCEL_CMD_ALT":  accel_raw,  # raw accel command, pcm uses this to calculate a compensatory force
   }
   return packer.make_can_msg("ACC_CONTROL", 0, values)
 

system/manager/manager.py

@@ -69,6 +69,7 @@ def manager_init() -> None:
     ("dp_lat_lane_change_assist_auto_timer", "1.5"),
     ("dp_lat_road_edge_detection", "0"),
     ("dp_device_disable_logging", "0"),
+    ("dp_toyota_pcm_compensation", "0"),
   ]
   if not PC:
     default_params.append(("LastUpdateTime", datetime.datetime.now(datetime.UTC).replace(tzinfo=None).isoformat().encode('utf8')))