Parametrize MIN_CAN_SPEED in car interfaces (#2684)

* Parametrize MIN_CAN_SPEED in car interfaces * fixed instance in planner.py * fix typo * var name change * changed var name to minSpeedCan for consistency with other uses of CAN in the capnproto files * added default value to get_std_params, removed unneeded instances from car interface files * Revert PEP8 autoformat corrections * update refs Co-authored-by: Adeeb Shihadeh <adeebshihadeh@gmail.com>
2020-12-04 05:09:34 -05:00 · 2020-12-04 05:09:34 -05:00 · 093456cc40
parent 61ec745ffc
commit 093456cc40
11 changed files with 26 additions and 22 deletions
--- a/selfdrive/car/honda/interface.py
+++ b/selfdrive/car/honda/interface.py
@ -16,6 +16,7 @@ A_ACC_MAX = max(_A_CRUISE_MAX_V_FOLLOWING)
 ButtonType = car.CarState.ButtonEvent.Type
 EventName = car.CarEvent.EventName

+
 def compute_gb_honda(accel, speed):
  creep_brake = 0.0
  creep_speed = 2.3
--- a/selfdrive/car/hyundai/interface.py
+++ b/selfdrive/car/hyundai/interface.py
@ -96,9 +96,9 @@ class CarInterface(CarInterfaceBase):
      ret.lateralTuning.pid.kpV, ret.lateralTuning.pid.kiV = [[0.25], [0.05]]
    elif candidate in [CAR.IONIQ, CAR.IONIQ_EV_LTD]:
      ret.lateralTuning.pid.kf = 0.00006
-      ret.mass = 1490. + STD_CARGO_KG   #weight per hyundai site https://www.hyundaiusa.com/ioniq-electric/specifications.aspx
+      ret.mass = 1490. + STD_CARGO_KG  # weight per hyundai site https://www.hyundaiusa.com/ioniq-electric/specifications.aspx
      ret.wheelbase = 2.7
-      ret.steerRatio = 13.73   #Spec
+      ret.steerRatio = 13.73  # Spec
      tire_stiffness_factor = 0.385
      ret.lateralTuning.pid.kiBP, ret.lateralTuning.pid.kpBP = [[0.], [0.]]
      ret.lateralTuning.pid.kpV, ret.lateralTuning.pid.kiV = [[0.25], [0.05]]
@ -178,7 +178,6 @@ class CarInterface(CarInterfaceBase):
      ret.lateralTuning.pid.kiBP, ret.lateralTuning.pid.kpBP = [[0.], [0.]]
      ret.lateralTuning.pid.kpV, ret.lateralTuning.pid.kiV = [[0.16], [0.01]]

-
    # these cars require a special panda safety mode due to missing counters and checksums in the messages
    if candidate in [CAR.HYUNDAI_GENESIS, CAR.IONIQ_EV_LTD, CAR.IONIQ, CAR.KONA_EV, CAR.KIA_SORENTO, CAR.SONATA_2019,
                     CAR.KIA_NIRO_EV, CAR.KIA_OPTIMA, CAR.VELOSTER, CAR.KIA_STINGER, CAR.GENESIS_G70, CAR.GENESIS_G80]:
@ -207,7 +206,7 @@ class CarInterface(CarInterfaceBase):
    ret.canValid = self.cp.can_valid and self.cp_cam.can_valid

    events = self.create_common_events(ret)
-    #TODO: addd abs(self.CS.angle_steers) > 90 to 'steerTempUnavailable' event
+    # TODO: addd abs(self.CS.angle_steers) > 90 to 'steerTempUnavailable' event

    # low speed steer alert hysteresis logic (only for cars with steer cut off above 10 m/s)
    if ret.vEgo < (self.CP.minSteerSpeed + 2.) and self.CP.minSteerSpeed > 10.:
--- a/selfdrive/car/interfaces.py
+++ b/selfdrive/car/interfaces.py
@ -53,7 +53,7 @@ class CarInterfaceBase():
  def get_std_params(candidate, fingerprint):
    ret = car.CarParams.new_message()
    ret.carFingerprint = candidate
-    ret.isPandaBlack = True # TODO: deprecate this field
+    ret.isPandaBlack = True  # TODO: deprecate this field

    # standard ALC params
    ret.steerControlType = car.CarParams.SteerControlType.torque
@ -71,6 +71,7 @@ class CarInterfaceBase():
    ret.brakeMaxV = [1.]
    ret.openpilotLongitudinalControl = False
    ret.startAccel = 0.0
+    ret.minSpeedCan = 0.3
    ret.stoppingControl = False
    ret.longitudinalTuning.deadzoneBP = [0.]
    ret.longitudinalTuning.deadzoneV = [0.]
@ -182,9 +183,9 @@ class CarStateBase:
  @staticmethod
  def parse_gear_shifter(gear: str) -> car.CarState.GearShifter:
    d: Dict[str, car.CarState.GearShifter] = {
-      'P': GearShifter.park, 'R': GearShifter.reverse, 'N': GearShifter.neutral,
-      'E': GearShifter.eco, 'T': GearShifter.manumatic, 'D': GearShifter.drive,
-      'S': GearShifter.sport, 'L': GearShifter.low, 'B': GearShifter.brake
+        'P': GearShifter.park, 'R': GearShifter.reverse, 'N': GearShifter.neutral,
+        'E': GearShifter.eco, 'T': GearShifter.manumatic, 'D': GearShifter.drive,
+        'S': GearShifter.sport, 'L': GearShifter.low, 'B': GearShifter.brake
    }
    return d.get(gear, GearShifter.unknown)

--- a/selfdrive/car/mazda/interface.py
+++ b/selfdrive/car/mazda/interface.py
@ -52,7 +52,6 @@ class CarInterface(CarInterfaceBase):
      ret.lateralTuning.pid.kpV, ret.lateralTuning.pid.kiV = [[0.19], [0.019]]
      ret.lateralTuning.pid.kf = 0.00006

-
    # No steer below disable speed
    ret.minSteerSpeed = LKAS_LIMITS.DISABLE_SPEED * CV.KPH_TO_MS

--- a/selfdrive/car/nissan/interface.py
+++ b/selfdrive/car/nissan/interface.py
@ -4,6 +4,7 @@ from selfdrive.car.nissan.values import CAR
 from selfdrive.car import STD_CARGO_KG, scale_rot_inertia, scale_tire_stiffness, gen_empty_fingerprint
 from selfdrive.car.interfaces import CarInterfaceBase

+
 class CarInterface(CarInterfaceBase):
  def __init__(self, CP, CarController, CarState):
    super().__init__(CP, CarController, CarState)
--- a/selfdrive/car/subaru/interface.py
+++ b/selfdrive/car/subaru/interface.py
@ -62,7 +62,7 @@ class CarInterface(CarInterfaceBase):
      ret.lateralTuning.pid.kpV, ret.lateralTuning.pid.kiV = [[0.01, 0.065, 0.2], [0.001, 0.015, 0.025]]

    if candidate in [CAR.FORESTER_PREGLOBAL, CAR.OUTBACK_PREGLOBAL_2018]:
-      ret.safetyParam = 1 # Outback 2018-2019 and Forester have reversed driver torque signal
+      ret.safetyParam = 1  # Outback 2018-2019 and Forester have reversed driver torque signal
      ret.mass = 1568 + STD_CARGO_KG
      ret.wheelbase = 2.67
      ret.centerToFront = ret.wheelbase * 0.5
--- a/selfdrive/car/toyota/interface.py
+++ b/selfdrive/car/toyota/interface.py
@ -8,6 +8,7 @@ from selfdrive.car.interfaces import CarInterfaceBase

 EventName = car.CarEvent.EventName

+
 class CarInterface(CarInterfaceBase):
  @staticmethod
  def compute_gb(accel, speed):
@ -195,6 +196,7 @@ class CarInterface(CarInterfaceBase):

    elif candidate in [CAR.COROLLA_TSS2, CAR.COROLLAH_TSS2]:
      stop_and_go = True
+      ret.minSpeedCan = 0.375
      ret.safetyParam = 73
      ret.wheelbase = 2.63906
      ret.steerRatio = 13.9
@ -256,7 +258,7 @@ class CarInterface(CarInterfaceBase):
    elif candidate == CAR.PRIUS_TSS2:
      stop_and_go = True
      ret.safetyParam = 73
-      ret.wheelbase = 2.70002 #from toyota online sepc.
+      ret.wheelbase = 2.70002  # from toyota online sepc.
      ret.steerRatio = 13.4   # True steerRation from older prius
      tire_stiffness_factor = 0.6371   # hand-tune
      ret.mass = 3115. * CV.LB_TO_KG + STD_CARGO_KG
--- a/selfdrive/car/volkswagen/interface.py
+++ b/selfdrive/car/volkswagen/interface.py
@ -6,6 +6,7 @@ from selfdrive.car.interfaces import CarInterfaceBase
 GEAR = car.CarState.GearShifter
 EventName = car.CarEvent.EventName

+
 class CarInterface(CarInterfaceBase):
  def __init__(self, CP, CarController, CarState):
    super().__init__(CP, CarController, CarState)
--- a/selfdrive/controls/lib/longcontrol.py
+++ b/selfdrive/controls/lib/longcontrol.py
@ -5,8 +5,7 @@ from selfdrive.controls.lib.pid import PIController
 LongCtrlState = log.ControlsState.LongControlState

 STOPPING_EGO_SPEED = 0.5
-MIN_CAN_SPEED = 0.3  # TODO: parametrize this in car interface
-STOPPING_TARGET_SPEED = MIN_CAN_SPEED + 0.01
+STOPPING_TARGET_SPEED_OFFSET = 0.01
 STARTING_TARGET_SPEED = 0.5
 BRAKE_THRESHOLD_TO_PID = 0.2

@ -18,12 +17,13 @@ RATE = 100.0


 def long_control_state_trans(active, long_control_state, v_ego, v_target, v_pid,
-                             output_gb, brake_pressed, cruise_standstill):
+                             output_gb, brake_pressed, cruise_standstill, min_speed_can):
  """Update longitudinal control state machine"""
+  stopping_target_speed = min_speed_can + STOPPING_TARGET_SPEED_OFFSET
  stopping_condition = (v_ego < 2.0 and cruise_standstill) or \
                       (v_ego < STOPPING_EGO_SPEED and
-                        ((v_pid < STOPPING_TARGET_SPEED and v_target < STOPPING_TARGET_SPEED) or
-                        brake_pressed))
+                        ((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

@ -78,9 +78,9 @@ class LongControl():
    output_gb = self.last_output_gb
    self.long_control_state = long_control_state_trans(active, self.long_control_state, CS.vEgo,
                                                       v_target_future, self.v_pid, output_gb,
-                                                       CS.brakePressed, CS.cruiseState.standstill)
+                                                       CS.brakePressed, CS.cruiseState.standstill, CP.minSpeedCan)

-    v_ego_pid = max(CS.vEgo, MIN_CAN_SPEED)  # Without this we get jumps, CAN bus reports 0 when speed < 0.3
+    v_ego_pid = max(CS.vEgo, CP.minSpeedCan)  # Without this we get jumps, CAN bus reports 0 when speed < 0.3

    if self.long_control_state == LongCtrlState.off or CS.gasPressed:
      self.reset(v_ego_pid)
--- a/selfdrive/controls/lib/planner.py
+++ b/selfdrive/controls/lib/planner.py
@ -10,7 +10,7 @@ from common.realtime import sec_since_boot
 from selfdrive.swaglog import cloudlog
 from selfdrive.config import Conversions as CV
 from selfdrive.controls.lib.speed_smoother import speed_smoother
-from selfdrive.controls.lib.longcontrol import LongCtrlState, MIN_CAN_SPEED
+from selfdrive.controls.lib.longcontrol import LongCtrlState
 from selfdrive.controls.lib.fcw import FCWChecker
 from selfdrive.controls.lib.long_mpc import LongitudinalMpc
 from selfdrive.controls.lib.drive_helpers import V_CRUISE_MAX
@ -21,7 +21,7 @@ AWARENESS_DECEL = -0.2     # car smoothly decel at .2m/s^2 when user is distract
 # lookup tables VS speed to determine min and max accels in cruise
 # make sure these accelerations are smaller than mpc limits
 _A_CRUISE_MIN_V = [-1.0, -.8, -.67, -.5, -.30]
-_A_CRUISE_MIN_BP = [   0., 5.,  10., 20.,  40.]
+_A_CRUISE_MIN_BP = [  0.,  5.,  10., 20.,  40.]

 # need fast accel at very low speed for stop and go
 # make sure these accelerations are smaller than mpc limits
@ -144,7 +144,7 @@ class Planner():
    else:
      starting = long_control_state == LongCtrlState.starting
      a_ego = min(sm['carState'].aEgo, 0.0)
-      reset_speed = MIN_CAN_SPEED if starting else v_ego
+      reset_speed = self.CP.minSpeedCan if starting else v_ego
      reset_accel = self.CP.startAccel if starting else a_ego
      self.v_acc = reset_speed
      self.a_acc = reset_accel
--- a/selfdrive/test/process_replay/ref_commit
+++ b/selfdrive/test/process_replay/ref_commit
@ -1 +1 @@
-8cc45567f52bb3bff7d354b8c387b2dc51c90731
+f60a1c8e24558c1470f9a240a12037579d62c2c1