diff --git a/selfdrive/car/toyota/carcontroller.py b/selfdrive/car/toyota/carcontroller.py index f004ae20b2e294..c37237ceff9d20 100644 --- a/selfdrive/car/toyota/carcontroller.py +++ b/selfdrive/car/toyota/carcontroller.py @@ -55,18 +55,20 @@ def update(self, CC, CS, now_nanos): if self.gac_pressed_frame is not None: # button is held #gac_pressed_duration = (self.frame - self.gac_pressed_frame) * DT_CTRL - manual_accel = -1. + manual_accel = None # gas and brake a = actuators.accel if manual_accel is None else manual_accel actuators.accel = a # need this??? to show up in logs?? + MPH_TO_MS = .447 + mtm = MPH_TO_MS if self.CP.enableGasInterceptor and CC.longActive: MAX_INTERCEPTOR_GAS = 0.5 # RAV4 has very sensitive gas pedal if self.CP.carFingerprint in (CAR.RAV4, CAR.RAV4H, CAR.HIGHLANDER, CAR.HIGHLANDERH): PEDAL_SCALE = interp(CS.out.vEgo, [0.0, MIN_ACC_SPEED, MIN_ACC_SPEED + PEDAL_TRANSITION], [0.15, 0.3, 0.0]) elif self.CP.carFingerprint in (CAR.COROLLA,): - PEDAL_SCALE = interp(CS.out.vEgo, [0.0, MIN_ACC_SPEED, MIN_ACC_SPEED + PEDAL_TRANSITION], [0.3, 0.4, 0.0]) + PEDAL_SCALE = interp(CS.out.vEgo, [0.0, 4.*mtm, 8.*mtm, MIN_ACC_SPEED, MIN_ACC_SPEED + PEDAL_TRANSITION], [.12, .19, 0.3, 0.4, 0.0]) else: PEDAL_SCALE = interp(CS.out.vEgo, [0.0, MIN_ACC_SPEED, MIN_ACC_SPEED + PEDAL_TRANSITION], [0.4, 0.5, 0.0]) # offset for creep and windbrake diff --git a/selfdrive/car/toyota/interface.py b/selfdrive/car/toyota/interface.py index 8b3fd048d996e9..b07795c41d039d 100644 --- a/selfdrive/car/toyota/interface.py +++ b/selfdrive/car/toyota/interface.py @@ -224,6 +224,8 @@ def _get_params(ret, candidate, fingerprint, car_fw, experimental_long): tune.kpV = [1.3, 1.0, 0.7] tune.kiBP = [0., 5., 12., 20., 27.] tune.kiV = [.35, .23, .20, .17, .1] + ret.stoppingDecelRate = 0.4 # reach stopping target smoothly + ret.stopAccel = -.7 if candidate in TSS2_CAR: ret.vEgoStopping = 0.25 ret.vEgoStarting = 0.25 diff --git a/selfdrive/controls/controlsd.py b/selfdrive/controls/controlsd.py index bc541548cde89b..d0ece1638a6492 100755 --- a/selfdrive/controls/controlsd.py +++ b/selfdrive/controls/controlsd.py @@ -3,6 +3,7 @@ import math from typing import SupportsFloat +from selfdrive.controls.lib.drive_helpers import CONTROL_N from cereal import car, log from common.numpy_fast import clip from common.realtime import sec_since_boot, config_realtime_process, Priority, Ratekeeper, DT_CTRL @@ -60,6 +61,8 @@ class Controls: def __init__(self, sm=None, pm=None, can_sock=None, CI=None): + self.maneuvering = False + self.maneuver_begin_frame = None config_realtime_process(4, Priority.CTRL_HIGH) # Ensure the current branch is cached, otherwise the first iteration of controlsd lags @@ -571,6 +574,22 @@ def state_control(self, CS): lat_plan = self.sm['lateralPlan'] long_plan = self.sm['longitudinalPlan'] + for b in CS.buttonEvents: + if b.type == car.CarState.ButtonEvent.Type.gapAdjustCruise: + if b.pressed is True: + self.LoC.reset(v_pid=CS.vEgo) + self.maneuvering = True + self.maneuver_begin_frame = self.sm.frame + elif b.pressed is False: + self.LoC.reset(v_pid=CS.vEgo) + self.maneuvering = False + self.maneuver_begin_frame = None + + if self.maneuvering: + long_plan.speeds = [5. * CV.MPH_TO_MS] * CONTROL_N + long_plan.accels = [-1.] * CONTROL_N + long_plan.jerks = [0.] * CONTROL_N + CC = car.CarControl.new_message() CC.enabled = self.enabled