Parameterize longitudinal control using carParams (#22200)

* use CP.startAccel

* missing values

* order

* update ref

* cereal

selfdrive/car/interfaces.py

@@ -73,9 +73,12 @@ class CarInterfaceBase():
     ret.steerRatioRear = 0.  # no rear steering, at least on the listed cars aboveA
     ret.openpilotLongitudinalControl = False
     ret.minSpeedCan = 0.3
-    ret.stoppingDecelRate = 0.8 # brake_travel/s while trying to stop
+    ret.startAccel = -0.8
+    ret.stopAccel = -2.0
     ret.startingAccelRate = 3.2 # brake_travel/s while releasing on restart
+    ret.stoppingDecelRate = 0.8 # brake_travel/s while trying to stop
     ret.vEgoStopping = 0.5
+    ret.vEgoStarting = 0.5
     ret.stoppingControl = True
     ret.longitudinalTuning.deadzoneBP = [0.]
     ret.longitudinalTuning.deadzoneV = [0.]

selfdrive/controls/controlsd.py

@@ -16,7 +16,7 @@ from selfdrive.car.car_helpers import get_car, get_startup_event, get_one_can
 from selfdrive.controls.lib.lane_planner import CAMERA_OFFSET
 from selfdrive.controls.lib.drive_helpers import update_v_cruise, initialize_v_cruise
 from selfdrive.controls.lib.drive_helpers import get_lag_adjusted_curvature
-from selfdrive.controls.lib.longcontrol import LongControl, STARTING_TARGET_SPEED
+from selfdrive.controls.lib.longcontrol import LongControl
 from selfdrive.controls.lib.latcontrol_pid import LatControlPID
 from selfdrive.controls.lib.latcontrol_indi import LatControlINDI
 from selfdrive.controls.lib.latcontrol_lqr import LatControlLQR
@@ -318,7 +318,7 @@ class Controls:
       v_future = speeds[-1]
     else:
       v_future = 100.0
-    if CS.brakePressed and v_future >= STARTING_TARGET_SPEED \
+    if CS.brakePressed and v_future >= self.CP.vEgoStarting \
       and self.CP.openpilotLongitudinalControl and CS.vEgo < 0.3:
       self.events.add(EventName.noTarget)
 

selfdrive/controls/lib/longcontrol.py

@@ -1,5 +1,6 @@
 from cereal import car
 from common.numpy_fast import clip, interp
+from common.realtime import DT_CTRL
 from selfdrive.controls.lib.pid import PIController
 from selfdrive.controls.lib.drive_helpers import CONTROL_N
 from selfdrive.modeld.constants import T_IDXS
@@ -7,12 +8,6 @@ from selfdrive.modeld.constants import T_IDXS
 LongCtrlState = car.CarControl.Actuators.LongControlState
 
 STOPPING_TARGET_SPEED_OFFSET = 0.01
-STARTING_TARGET_SPEED = 0.5
-DECEL_THRESHOLD_TO_PID = 0.8
-
-DECEL_STOPPING_TARGET = 2.0  # apply at least this amount of brake to maintain the vehicle stationary
-
-RATE = 100.0
 
 # As per ISO 15622:2018 for all speeds
 ACCEL_MIN_ISO = -3.5 # m/s^2
@@ -28,7 +23,7 @@ def long_control_state_trans(CP, active, long_control_state, v_ego, v_target, v_
                         ((v_pid < stopping_target_speed and v_target < stopping_target_speed) or
                          brake_pressed))
 
-  starting_condition = v_target > STARTING_TARGET_SPEED and not cruise_standstill
+  starting_condition = v_target > CP.vEgoStarting and not cruise_standstill
 
   if not active:
     long_control_state = LongCtrlState.off
@@ -49,7 +44,7 @@ def long_control_state_trans(CP, active, long_control_state, v_ego, v_target, v_
     elif long_control_state == LongCtrlState.starting:
       if stopping_condition:
         long_control_state = LongCtrlState.stopping
-      elif output_accel >= -DECEL_THRESHOLD_TO_PID:
+      elif output_accel >= CP.startAccel:
         long_control_state = LongCtrlState.pid
 
   return long_control_state
@@ -60,7 +55,7 @@ class LongControl():
     self.long_control_state = LongCtrlState.off  # initialized to off
     self.pid = PIController((CP.longitudinalTuning.kpBP, CP.longitudinalTuning.kpV),
                             (CP.longitudinalTuning.kiBP, CP.longitudinalTuning.kiV),
-                            rate=RATE,
+                            rate=1/DT_CTRL,
                             sat_limit=0.8)
     self.v_pid = 0.0
     self.last_output_accel = 0.0
@@ -119,16 +114,16 @@ class LongControl():
     # Intention is to stop, switch to a different brake control until we stop
     elif self.long_control_state == LongCtrlState.stopping:
       # Keep applying brakes until the car is stopped
-      if not CS.standstill or output_accel > -DECEL_STOPPING_TARGET:
-        output_accel -= CP.stoppingDecelRate / RATE
+      if not CS.standstill or output_accel > CP.stopAccel:
+        output_accel -= CP.stoppingDecelRate * DT_CTRL
       output_accel = clip(output_accel, accel_limits[0], accel_limits[1])
 
       self.reset(CS.vEgo)
 
     # Intention is to move again, release brake fast before handing control to PID
     elif self.long_control_state == LongCtrlState.starting:
-      if output_accel < -DECEL_THRESHOLD_TO_PID:
-        output_accel += CP.startingAccelRate / RATE
+      if output_accel < CP.startAccel:
+        output_accel += CP.startingAccelRate * DT_CTRL
       self.reset(CS.vEgo)
 
     self.last_output_accel = output_accel

selfdrive/test/process_replay/ref_commit

@@ -1 +1 @@
-65c39ad66072b5106399605761c48e6cad0ee3a5
+0ab64a741a496f0bc7351d0e95364e8a192a87e9