diff --git a/selfdrive/car/toyota/carcontroller.py b/selfdrive/car/toyota/carcontroller.py index e21998b75..32fb2387e 100644 --- a/selfdrive/car/toyota/carcontroller.py +++ b/selfdrive/car/toyota/carcontroller.py @@ -9,6 +9,7 @@ from openpilot.selfdrive.car.toyota.values import CAR, STATIC_DSU_MSGS, NO_STOP_ UNSUPPORTED_DSU_CAR, STOP_AND_GO_CAR from opendbc.can.packer import CANPacker +LongCtrlState = car.CarControl.Actuators.LongControlState SteerControlType = car.CarParams.SteerControlType VisualAlert = car.CarControl.HUDControl.VisualAlert @@ -31,6 +32,12 @@ UNLOCK_CMD = b"\x40\x05\x30\x11\x00\x40\x00\x00" PARK = car.CarState.GearShifter.park +# PCM compensatory force calculation threshold +# a variation in accel command is more pronounced at higher speeds, let compensatory forces ramp to zero before +# applying when speed is high +COMPENSATORY_CALCULATION_THRESHOLD_V = [-0.3, -0.25, 0.] # m/s^2 +COMPENSATORY_CALCULATION_THRESHOLD_BP = [0., 11., 23.] # m/s + class CarController(CarControllerBase): def __init__(self, dbc_name, CP, VM): self.CP = CP @@ -40,6 +47,7 @@ class CarController(CarControllerBase): self.last_angle = 0 self.alert_active = False self.last_standstill = False + self.prohibit_neg_calculation = True self.standstill_req = False self.steer_rate_counter = 0 self.distance_button = 0 @@ -51,6 +59,8 @@ class CarController(CarControllerBase): # FrogPilot variables params = Params() + self.cydia_tune = params.get_bool("CydiaTune") + self.doors_locked = False self.doors_unlocked = True @@ -59,6 +69,7 @@ class CarController(CarControllerBase): hud_control = CC.hudControl pcm_cancel_cmd = CC.cruiseControl.cancel lat_active = CC.latActive and abs(CS.out.steeringTorque) < MAX_USER_TORQUE + stopping = actuators.longControlState == LongCtrlState.stopping # *** control msgs *** can_sends = [] @@ -129,10 +140,30 @@ class CarController(CarControllerBase): interceptor_gas_cmd = 0.12 if CS.out.standstill else 0. else: interceptor_gas_cmd = 0. - if frogpilot_toggles.sport_plus: - pcm_accel_cmd = clip(actuators.accel, self.params.ACCEL_MIN, self.params.ACCEL_MAX_PLUS) + + # prohibit negative compensatory calculations when first activating long after accelerator depression or engagement + if not CC.longActive: + self.prohibit_neg_calculation = True + + comp_thresh = interp(CS.out.vEgo, COMPENSATORY_CALCULATION_THRESHOLD_BP, COMPENSATORY_CALCULATION_THRESHOLD_V) + # don't reset until a reasonable compensatory value is reached + if CS.pcm_neutral_force > comp_thresh * self.CP.mass: + self.prohibit_neg_calculation = False + + # limit minimum to only positive until first positive is reached after engagement, don't calculate when long isn't active + if CC.longActive and not self.prohibit_neg_calculation and self.cydia_tune: + accel_offset = CS.pcm_neutral_force / self.CP.mass else: - pcm_accel_cmd = clip(actuators.accel, self.params.ACCEL_MIN, self.params.ACCEL_MAX) + accel_offset = 0. + + # only calculate pcm_accel_cmd when long is active to prevent disengagement from accelerator depression + if CC.longActive: + if frogpilot_toggles.sport_plus: + pcm_accel_cmd = clip(actuators.accel + accel_offset, self.params.ACCEL_MIN, self.params.ACCEL_MAX_PLUS) + else: + pcm_accel_cmd = clip(actuators.accel + accel_offset, self.params.ACCEL_MIN, self.params.ACCEL_MAX) + else: + pcm_accel_cmd = 0. # 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 or self.CP.enableGasInterceptor): @@ -150,6 +181,8 @@ class CarController(CarControllerBase): # we can spam can to cancel the system even if we are using lat only control if (self.frame % 3 == 0 and self.CP.openpilotLongitudinalControl) or pcm_cancel_cmd: lead = hud_control.leadVisible or CS.out.vEgo < 12. # at low speed we always assume the lead is present so ACC can be engaged + # when stopping, send -2.5 raw acceleration immediately to prevent vehicle from creeping, else send actuators.accel + accel_raw = -2.5 if stopping and self.cydia_tune else actuators.accel # Press distance button until we are at the correct bar length. Only change while enabled to avoid skipping startup popup if self.frame % 6 == 0 and self.CP.openpilotLongitudinalControl: @@ -163,11 +196,11 @@ class CarController(CarControllerBase): 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, actuators.accel, pcm_cancel_cmd, self.standstill_req, lead, CS.acc_type, fcw_alert, self.distance_button, frogpilot_toggles)) 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, frogpilot_toggles)) + can_sends.append(toyotacan.create_accel_command(self.packer, 0, 0, pcm_cancel_cmd, False, lead, CS.acc_type, False, self.distance_button, frogpilot_toggles)) if self.frame % 2 == 0 and self.CP.enableGasInterceptor and self.CP.openpilotLongitudinalControl: # send exactly zero if gas cmd is zero. Interceptor will send the max between read value and gas cmd. diff --git a/selfdrive/car/toyota/carstate.py b/selfdrive/car/toyota/carstate.py index 3102db8b5..8477600be 100644 --- a/selfdrive/car/toyota/carstate.py +++ b/selfdrive/car/toyota/carstate.py @@ -221,6 +221,8 @@ class CarState(CarStateBase): message_keys = ["LDA_ON_MESSAGE", "SET_ME_X02"] self.lkas_enabled = any(self.lkas_hud.get(key) == 1 for key in message_keys) + self.pcm_neutral_force = cp.vl["PCM_CRUISE"]["NEUTRAL_FORCE"] + # ZSS Support - Credit goes to the DragonPilot team! if self.CP.flags & ToyotaFlags.ZSS and self.zss_threshold_count < ZSS_THRESHOLD_COUNT: zorro_steer = cp.vl["SECONDARY_STEER_ANGLE"]["ZORRO_STEER"] diff --git a/selfdrive/car/toyota/interface.py b/selfdrive/car/toyota/interface.py index 41b838215..de3d315f0 100644 --- a/selfdrive/car/toyota/interface.py +++ b/selfdrive/car/toyota/interface.py @@ -136,7 +136,12 @@ class CarInterface(CarInterfaceBase): ret.minEnableSpeed = -1. if (candidate in STOP_AND_GO_CAR or ret.enableGasInterceptor) else MIN_ACC_SPEED tune = ret.longitudinalTuning - if candidate in TSS2_CAR or ret.enableGasInterceptor: + if params.get_bool("CydiaTune"): + tune.kpV = [0.0] + tune.kiV = [0.5] + ret.stopAccel = -2.5 # on stock Toyota this is -2.5 + ret.stoppingDecelRate = 0.17 + elif candidate in TSS2_CAR or ret.enableGasInterceptor: 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 7d9c12a2f..f38401ef8 100644 --- a/selfdrive/car/toyota/toyotacan.py +++ b/selfdrive/car/toyota/toyotacan.py @@ -33,10 +33,10 @@ 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, frogpilot_toggles): +def create_accel_command(packer, accel, accel_raw, pcm_cancel, standstill_req, lead, acc_type, fcw_alert, distance, frogpilot_toggles): # TODO: find the exact canceling bit that does not create a chime values = { - "ACCEL_CMD": accel, + "ACCEL_CMD": accel, # compensated accel command "ACC_TYPE": acc_type, "DISTANCE": distance, "MINI_CAR": lead, @@ -45,6 +45,7 @@ def create_accel_command(packer, accel, pcm_cancel, standstill_req, lead, acc_ty "CANCEL_REQ": pcm_cancel, "ALLOW_LONG_PRESS": 2 if frogpilot_toggles.reverse_cruise_increase else 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)