diff --git a/cereal/car.capnp b/cereal/car.capnp index 19cc1b5..40e86a5 100644 --- a/cereal/car.capnp +++ b/cereal/car.capnp @@ -383,6 +383,7 @@ struct CarControl { leftLaneVisible @7: Bool; rightLaneDepart @8: Bool; leftLaneDepart @9: Bool; + leadVelocity @10: Float32; enum VisualAlert { # these are the choices from the Honda diff --git a/common/params.cc b/common/params.cc index b97a0de..24f6f00 100644 --- a/common/params.cc +++ b/common/params.cc @@ -242,6 +242,7 @@ std::unordered_map keys = { {"CustomSignals", PERSISTENT}, {"CustomSounds", PERSISTENT}, {"CustomTheme", PERSISTENT}, + {"CydiaTune", PERSISTENT}, {"DecelerationProfile", PERSISTENT}, {"DisengageVolume", PERSISTENT}, {"DriveStats", PERSISTENT}, diff --git a/opendbc/toyota_new_mc_pt_generated.dbc b/opendbc/toyota_new_mc_pt_generated.dbc index 04910dc..3573fa6 100644 --- a/opendbc/toyota_new_mc_pt_generated.dbc +++ b/opendbc/toyota_new_mc_pt_generated.dbc @@ -202,6 +202,7 @@ BO_ 835 ACC_CONTROL: 8 DSU SG_ ACC_TYPE : 23|2@0+ (1,0) [0|3] "" HCU SG_ CANCEL_REQ : 24|1@0+ (1,0) [0|1] "" HCU SG_ ACC_CUT_IN : 25|1@0+ (1,0) [0|1] "" XXX + SG_ LEAD_VEHICLE_STOPPED : 29|1@0+ (1,0) [0|0] "" DSU SG_ PERMIT_BRAKING : 30|1@0+ (1,0) [0|1] "" HCU SG_ RELEASE_STANDSTILL : 31|1@0+ (1,0) [0|1] "" HCU SG_ ITS_CONNECT_LEAD : 39|8@0+ (1,0) [0|1] "" Vector__XXX @@ -450,6 +451,7 @@ CM_ SG_ 835 ACC_MALFUNCTION "display ACC fault on dash if set to 1"; CM_ SG_ 835 ACC_CUT_IN "Display blinking yellow lead if set to 1"; CM_ SG_ 835 DISTANCE "Cycle through ACC following distance from long, mid, short when set to 1"; CM_ SG_ 835 ITS_CONNECT_LEAD "Displayed when lead car is capable of ITS Connect"; +CM_ SG_ 835 LEAD_VEHICLE_STOPPED "LVSTP in leaked DBC, set to 1 when lead is stopped"; CM_ SG_ 835 ALLOW_LONG_PRESS "Enable Toyota's factory set speed increment behaviour, available on both metrics cars and imperial unit cars"; CM_ SG_ 835 PERMIT_BRAKING "Original ACC has this going high when a car in front is detected. In openpilot and before the PERMIT_BRAKING name, this was 'SET_ME_1' and is hardcoded to be high. Unsure if only informational or has an effect though existing usage in openpilot is to always set it to 1. Originally 'PMTBRKG' in the leaked toyota_2017_ref_pt.dbc file and name expansion speculated to be PerMiT BRaKinG."; CM_ SG_ 835 ACCEL_CMD_ALT "Copy of main ACCEL_CMD, but across 8 bits instead of 16 bits like ACCEL_CMD. Unsure if only informational or has an effect. Likely informational as existing openpilot sets this to 0 and no loss of functionality observed. Originally 'AT_RAW' in leaked toyota_2017_ref_pt.dbc file."; diff --git a/opendbc/toyota_nodsu_pt_generated.dbc b/opendbc/toyota_nodsu_pt_generated.dbc index 7538b46..e2d109d 100644 --- a/opendbc/toyota_nodsu_pt_generated.dbc +++ b/opendbc/toyota_nodsu_pt_generated.dbc @@ -202,6 +202,7 @@ BO_ 835 ACC_CONTROL: 8 DSU SG_ ACC_TYPE : 23|2@0+ (1,0) [0|3] "" HCU SG_ CANCEL_REQ : 24|1@0+ (1,0) [0|1] "" HCU SG_ ACC_CUT_IN : 25|1@0+ (1,0) [0|1] "" XXX + SG_ LEAD_VEHICLE_STOPPED : 29|1@0+ (1,0) [0|0] "" DSU SG_ PERMIT_BRAKING : 30|1@0+ (1,0) [0|1] "" HCU SG_ RELEASE_STANDSTILL : 31|1@0+ (1,0) [0|1] "" HCU SG_ ITS_CONNECT_LEAD : 39|8@0+ (1,0) [0|1] "" Vector__XXX @@ -450,6 +451,7 @@ CM_ SG_ 835 ACC_MALFUNCTION "display ACC fault on dash if set to 1"; CM_ SG_ 835 ACC_CUT_IN "Display blinking yellow lead if set to 1"; CM_ SG_ 835 DISTANCE "Cycle through ACC following distance from long, mid, short when set to 1"; CM_ SG_ 835 ITS_CONNECT_LEAD "Displayed when lead car is capable of ITS Connect"; +CM_ SG_ 835 LEAD_VEHICLE_STOPPED "LVSTP in leaked DBC, set to 1 when lead is stopped"; CM_ SG_ 835 ALLOW_LONG_PRESS "Enable Toyota's factory set speed increment behaviour, available on both metrics cars and imperial unit cars"; CM_ SG_ 835 PERMIT_BRAKING "Original ACC has this going high when a car in front is detected. In openpilot and before the PERMIT_BRAKING name, this was 'SET_ME_1' and is hardcoded to be high. Unsure if only informational or has an effect though existing usage in openpilot is to always set it to 1. Originally 'PMTBRKG' in the leaked toyota_2017_ref_pt.dbc file and name expansion speculated to be PerMiT BRaKinG."; CM_ SG_ 835 ACCEL_CMD_ALT "Copy of main ACCEL_CMD, but across 8 bits instead of 16 bits like ACCEL_CMD. Unsure if only informational or has an effect. Likely informational as existing openpilot sets this to 0 and no loss of functionality observed. Originally 'AT_RAW' in leaked toyota_2017_ref_pt.dbc file."; diff --git a/opendbc/toyota_tnga_k_pt_generated.dbc b/opendbc/toyota_tnga_k_pt_generated.dbc index f54f7df..f07442c 100644 --- a/opendbc/toyota_tnga_k_pt_generated.dbc +++ b/opendbc/toyota_tnga_k_pt_generated.dbc @@ -202,6 +202,7 @@ BO_ 835 ACC_CONTROL: 8 DSU SG_ ACC_TYPE : 23|2@0+ (1,0) [0|3] "" HCU SG_ CANCEL_REQ : 24|1@0+ (1,0) [0|1] "" HCU SG_ ACC_CUT_IN : 25|1@0+ (1,0) [0|1] "" XXX + SG_ LEAD_VEHICLE_STOPPED : 29|1@0+ (1,0) [0|0] "" DSU SG_ PERMIT_BRAKING : 30|1@0+ (1,0) [0|1] "" HCU SG_ RELEASE_STANDSTILL : 31|1@0+ (1,0) [0|1] "" HCU SG_ ITS_CONNECT_LEAD : 39|8@0+ (1,0) [0|1] "" Vector__XXX @@ -450,6 +451,7 @@ CM_ SG_ 835 ACC_MALFUNCTION "display ACC fault on dash if set to 1"; CM_ SG_ 835 ACC_CUT_IN "Display blinking yellow lead if set to 1"; CM_ SG_ 835 DISTANCE "Cycle through ACC following distance from long, mid, short when set to 1"; CM_ SG_ 835 ITS_CONNECT_LEAD "Displayed when lead car is capable of ITS Connect"; +CM_ SG_ 835 LEAD_VEHICLE_STOPPED "LVSTP in leaked DBC, set to 1 when lead is stopped"; CM_ SG_ 835 ALLOW_LONG_PRESS "Enable Toyota's factory set speed increment behaviour, available on both metrics cars and imperial unit cars"; CM_ SG_ 835 PERMIT_BRAKING "Original ACC has this going high when a car in front is detected. In openpilot and before the PERMIT_BRAKING name, this was 'SET_ME_1' and is hardcoded to be high. Unsure if only informational or has an effect though existing usage in openpilot is to always set it to 1. Originally 'PMTBRKG' in the leaked toyota_2017_ref_pt.dbc file and name expansion speculated to be PerMiT BRaKinG."; CM_ SG_ 835 ACCEL_CMD_ALT "Copy of main ACCEL_CMD, but across 8 bits instead of 16 bits like ACCEL_CMD. Unsure if only informational or has an effect. Likely informational as existing openpilot sets this to 0 and no loss of functionality observed. Originally 'AT_RAW' in leaked toyota_2017_ref_pt.dbc file."; diff --git a/selfdrive/car/toyota/carcontroller.py b/selfdrive/car/toyota/carcontroller.py index 7df3e00..fe8cd4b 100644 --- a/selfdrive/car/toyota/carcontroller.py +++ b/selfdrive/car/toyota/carcontroller.py @@ -11,6 +11,7 @@ from opendbc.can.packer import CANPacker SteerControlType = car.CarParams.SteerControlType VisualAlert = car.CarControl.HUDControl.VisualAlert +LongCtrlState = car.CarControl.Actuators.LongControlState # LKA limits # EPS faults if you apply torque while the steering rate is above 100 deg/s for too long @@ -25,6 +26,11 @@ MAX_USER_TORQUE = 500 MAX_LTA_ANGLE = 94.9461 # deg MAX_LTA_DRIVER_TORQUE_ALLOWANCE = 150 # slightly above steering pressed allows some resistance when changing lanes +# 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: def __init__(self, dbc_name, CP, VM): @@ -37,6 +43,7 @@ class CarController: self.last_standstill = False self.standstill_req = False self.steer_rate_counter = 0 + self.prohibit_neg_calculation = True self.packer = CANPacker(dbc_name) self.gas = 0 @@ -45,11 +52,14 @@ class CarController: # FrogPilot variables params = Params() + self.cydia_tune = params.get_bool("CydiaTune") + def update(self, CC, CS, now_nanos, frogpilot_variables): 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 + stopping = actuators.longControlState == LongCtrlState.stopping # *** control msgs *** can_sends = [] @@ -118,10 +128,31 @@ class CarController: interceptor_gas_cmd = clip(pedal_command, 0., MAX_INTERCEPTOR_GAS) else: interceptor_gas_cmd = 0. - if frogpilot_variables.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 + # NO_STOP_TIMER_CAR will creep if compensation is applied when stopping or stopped, don't compensate when stopped or stopping + should_compensate = True + if (self.CP.carFingerprint in NO_STOP_TIMER_CAR and actuators.accel < 1e-3 or stopping) or CS.out.vEgo < 1e-3: + should_compensate = 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 should_compensate 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_variables.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. # TODO: probably can delete this. CS.pcm_acc_status uses a different signal # than CS.cruiseState.enabled. confirm they're not meaningfully different @@ -144,16 +175,18 @@ class CarController: # 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 # 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, frogpilot_variables)) 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, + can_sends.append(toyotacan.create_accel_command(self.packer, 0, 0, pcm_cancel_cmd, False, lead, CS.acc_type, False, frogpilot_variables)) if self.frame % 2 == 0 and self.CP.enableGasInterceptor and self.CP.openpilotLongitudinalControl: diff --git a/selfdrive/car/toyota/carstate.py b/selfdrive/car/toyota/carstate.py index 18c8067..7182ea4 100644 --- a/selfdrive/car/toyota/carstate.py +++ b/selfdrive/car/toyota/carstate.py @@ -151,6 +151,7 @@ class CarState(CarStateBase): ret.cruiseState.standstill = self.pcm_acc_status == 7 ret.cruiseState.enabled = bool(cp.vl["PCM_CRUISE"]["CRUISE_ACTIVE"]) ret.cruiseState.nonAdaptive = cp.vl["PCM_CRUISE"]["CRUISE_STATE"] in (1, 2, 3, 4, 5, 6) + 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 6d3a614..2aaf377 100644 --- a/selfdrive/car/toyota/interface.py +++ b/selfdrive/car/toyota/interface.py @@ -267,7 +267,21 @@ class CarInterface(CarInterfaceBase): tune = ret.longitudinalTuning tune.deadzoneBP = [0., 9.] tune.deadzoneV = [.0, .15] - if candidate in TSS2_CAR or ret.enableGasInterceptor: + if params.get_bool("CydiaTune"): + # on stock Toyota this is -2.5 + ret.stopAccel = -2.5 + tune.deadzoneBP = [0., 16., 20., 30.] + tune.deadzoneV = [0., .03, .06, .15] + ret.stoppingDecelRate = 0.17 # This is okay for TSS-P + if candidate in TSS2_CAR: + ret.vEgoStopping = 0.25 + ret.vEgoStarting = 0.25 + ret.stoppingDecelRate = 0.009 # reach stopping target smoothly + tune.kpBP = [0., 5.] + tune.kpV = [0.8, 1.] + tune.kiBP = [0., 5.] + tune.kiV = [0.3, 1.] + elif candidate in TSS2_CAR or ret.enableGasInterceptor: tune.kpBP = [0., 5., 20.] tune.kpV = [1.3, 1.0, 0.7] tune.kiBP = [0., 5., 12., 20., 27.] diff --git a/selfdrive/car/toyota/toyotacan.py b/selfdrive/car/toyota/toyotacan.py index f8d9db5..ab25797 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, frogpilot_variables): +def create_accel_command(packer, accel, accel_raw, pcm_cancel, standstill_req, lead, acc_type, fcw_alert, frogpilot_variables): # 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": 0, "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": 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/selfdrive/frogpilot/ui/vehicle_settings.cc b/selfdrive/frogpilot/ui/vehicle_settings.cc index ed27626..48b111d 100644 --- a/selfdrive/frogpilot/ui/vehicle_settings.cc +++ b/selfdrive/frogpilot/ui/vehicle_settings.cc @@ -11,6 +11,7 @@ FrogPilotVehiclesPanel::FrogPilotVehiclesPanel(SettingsWindow *parent) : FrogPil if (param == "LongitudinalTune") { std::vector> tuneOptions{ {"StockTune", tr("Stock")}, + {"CydiaTune", tr("Cydia's")}, }; toggle = new FrogPilotButtonsParamControl(param, title, desc, icon, tuneOptions);