diff --git a/common/params.cc b/common/params.cc index 4d5360903..070e4e4df 100644 --- a/common/params.cc +++ b/common/params.cc @@ -230,6 +230,7 @@ std::unordered_map 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 diff --git a/selfdrive/car/toyota/carcontroller.py b/selfdrive/car/toyota/carcontroller.py index eefc05167..f45d1b775 100644 --- a/selfdrive/car/toyota/carcontroller.py +++ b/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: diff --git a/selfdrive/car/toyota/carstate.py b/selfdrive/car/toyota/carstate.py index 26a245b2e..c0be916b4 100644 --- a/selfdrive/car/toyota/carstate.py +++ b/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 diff --git a/selfdrive/car/toyota/interface.py b/selfdrive/car/toyota/interface.py index 98f63597e..8fe247638 100644 --- a/selfdrive/car/toyota/interface.py +++ b/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 diff --git a/selfdrive/car/toyota/toyotacan.py b/selfdrive/car/toyota/toyotacan.py index 1cc99b41b..058e8ffb0 100644 --- a/selfdrive/car/toyota/toyotacan.py +++ b/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) diff --git a/system/manager/manager.py b/system/manager/manager.py index c873591f5..6904677da 100755 --- a/system/manager/manager.py +++ b/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')))