From 5afc2a3c622d5ea6a069e4fffd1767f50cb13b58 Mon Sep 17 00:00:00 2001 From: rav4kumar Date: Thu, 12 Sep 2024 17:29:30 -0700 Subject: [PATCH] revert "Toyota: API is now gas/brake #1189" --- selfdrive/car/toyota/carcontroller.py | 48 +-------------------------- selfdrive/car/toyota/carstate.py | 9 ----- selfdrive/car/toyota/interface.py | 4 +-- 3 files changed, 3 insertions(+), 58 deletions(-) diff --git a/selfdrive/car/toyota/carcontroller.py b/selfdrive/car/toyota/carcontroller.py index d5ea3a6d3e..2274590160 100644 --- a/selfdrive/car/toyota/carcontroller.py +++ b/selfdrive/car/toyota/carcontroller.py @@ -1,6 +1,4 @@ from cereal import car -import math -from openpilot.common.params import Params from common.conversions import Conversions as CV from openpilot.common.numpy_fast import clip, interp from openpilot.selfdrive.car import apply_meas_steer_torque_limits, apply_std_steer_angle_limits, common_fault_avoidance, make_can_msg, make_tester_present_msg, \ @@ -15,7 +13,6 @@ from opendbc.can.packer import CANPacker GearShifter = car.CarState.GearShifter 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 @@ -47,7 +44,6 @@ class CarController(CarControllerBase): self.last_standstill = False self.standstill_req = False self.steer_rate_counter = 0 - self.pcm_accel_comp = 0 self.distance_button = 0 self.packer = CANPacker(dbc_name) @@ -144,45 +140,6 @@ class CarController(CarControllerBase): lta_active, self.frame // 2, torque_wind_down)) # *** gas and brake *** - - # 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) - sp_tss2_long_tune = Params().get_bool("ToyotaTSS2Long") - - # When sp_tss2_long_tune is True and CC.longActive - if sp_tss2_long_tune and CC.longActive: - self.pcm_accel_comp = 0.0 - pcm_accel_cmd = 0.0 - - # When sp_tss2_long_tune is False and CC.longActive - elif not sp_tss2_long_tune and CC.longActive: - 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) - - if CS.out.cruiseState.standstill or actuators.longControlState == LongCtrlState.stopping: - self.pcm_accel_comp = 0.0 - - # You can uncomment these lines as needed - pcm_accel_cmd = actuators.accel + self.pcm_accel_comp # + offset - # pcm_accel_cmd = actuators.accel - pitch_offset - - # In case CC.longActive is False - else: - 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) - - if self.CP.enableGasInterceptorDEPRECATED and CC.longActive: MAX_INTERCEPTOR_GAS = 0.5 # RAV4 has very sensitive gas pedal @@ -198,10 +155,7 @@ class CarController(CarControllerBase): interceptor_gas_cmd = clip(pedal_command, 0., MAX_INTERCEPTOR_GAS) else: interceptor_gas_cmd = 0. - if not sp_tss2_long_tune: - pcm_accel_cmd = clip(pcm_accel_cmd, self.params.ACCEL_MIN, self.params.ACCEL_MAX) - else: - pcm_accel_cmd = clip(actuators.accel, self.params.ACCEL_MIN, self.params.ACCEL_MAX) + pcm_accel_cmd = clip(actuators.accel, self.params.ACCEL_MIN, self.params.ACCEL_MAX) # TODO: probably can delete this. CS.pcm_acc_status uses a different signal # than CS.cruiseState.enabled. confirm they're not meaningfully different diff --git a/selfdrive/car/toyota/carstate.py b/selfdrive/car/toyota/carstate.py index 6d7cc72527..65d095c9ee 100644 --- a/selfdrive/car/toyota/carstate.py +++ b/selfdrive/car/toyota/carstate.py @@ -56,9 +56,6 @@ class CarState(CarStateBase): self.low_speed_lockout = False self.acc_type = 1 self.lkas_hud = {} - self.pcm_accel_net = 0.0 - self.pcm_neutral_force = 0.0 - self.vsc_slope_angle = 0.0 self.lkas_enabled = None self.prev_lkas_enabled = None @@ -120,11 +117,6 @@ class CarState(CarStateBase): 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"] - self.pcm_neutral_force = cp.vl["PCM_CRUISE"]["NEUTRAL_FORCE"] - self.vsc_slope_angle = cp.vl["VSC1"]["SLOPE_ANGLE"] - ret.standstill = abs(ret.vEgoRaw) < 1e-3 if self.CP.carFingerprint != CAR.TOYOTA_PRIUS_V: @@ -410,7 +402,6 @@ class CarState(CarStateBase): ("STEER_ANGLE_SENSOR", 80), ("PCM_CRUISE", 33), ("PCM_CRUISE_SM", 1), - ("VSC1", 20), ("STEER_TORQUE_SENSOR", 50), ] diff --git a/selfdrive/car/toyota/interface.py b/selfdrive/car/toyota/interface.py index 6cd345a2ae..8667e23adf 100644 --- a/selfdrive/car/toyota/interface.py +++ b/selfdrive/car/toyota/interface.py @@ -184,8 +184,8 @@ class CarInterface(CarInterfaceBase): #tune.kiV = [.35, .3168, .1965, .1965, .184, .101, .06] custom_tss2_longitudinal_tuning() else: - tune.kpBP = [0., 5., 20.] - tune.kpV = [2.3, 1.0, 0.7] + tune.kpV = [0.0] + tune.kiV = [0.5] if candidate in TSS2_CAR: default_tss2_longitudinal_tuning() else: