Toyota long maneuvers v1

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

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

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