Add longitudinal support for Subaru Crosstrek and Impreza

.gitmodules

@@ -1,6 +1,7 @@
 [submodule "panda"]
   path = panda
-  url = ../../commaai/panda.git
+  url = ../../martinl/panda.git
+  branch = feature-subaru-long
 [submodule "opendbc"]
   path = opendbc
   url = ../../commaai/opendbc.git

panda

@@ -1 +1 @@
-Subproject commit 1923b1418933e464ff7460a3e0a05d63aad0d53b
+Subproject commit 0575d7a33d2ec2d8f7c6084c9f2eda264f55ea0f

selfdrive/car/subaru/carcontroller.py

@@ -1,8 +1,24 @@
+from common.numpy_fast import clip
 from opendbc.can.packer import CANPacker
 from selfdrive.car import apply_driver_steer_torque_limits
 from selfdrive.car.subaru import subarucan
 from selfdrive.car.subaru.values import DBC, GLOBAL_GEN2, PREGLOBAL_CARS, CarControllerParams
 
+ACCEL_HYST_GAP = 10  # don't change accel command for small oscilalitons within this value
+
+def accel_hysteresis(accel, accel_steady):
+
+  # for small accel oscillations within ACCEL_HYST_GAP, don't change the accel command
+  if accel > accel_steady + ACCEL_HYST_GAP:
+    accel_steady = accel - ACCEL_HYST_GAP
+  elif accel < accel_steady - ACCEL_HYST_GAP:
+    accel_steady = accel + ACCEL_HYST_GAP
+  accel = accel_steady
+
+  return accel, accel_steady
+
+def compute_gb(accel):
+  return clip(accel/4.0, 0.0, 1.0), clip(-accel/4.0, 0.0, 1.0)
 
 class CarController:
   def __init__(self, dbc_name, CP, VM):
@@ -10,10 +26,20 @@ class CarController:
     self.apply_steer_last = 0
     self.frame = 0
 
-    self.es_lkas_cnt = -1
+    self.es_lkas_state_cnt = -1
     self.es_distance_cnt = -1
     self.es_dashstatus_cnt = -1
+    self.cruise_control_cnt = -1
+    self.brake_status_cnt = -1
+    self.es_status_cnt = -1
+    self.es_brake_cnt = -1
     self.cruise_button_prev = 0
+    self.steer_rate_limited = False
+    self.cruise_rpm_last = 0
+    self.cruise_throttle_last = 0
+    self.rpm_steady = 0
+    self.throttle_steady = 0
+
     self.last_cancel_frame = 0
 
     self.p = CarControllerParams(CP)
@@ -46,6 +72,40 @@ class CarController:
 
       self.apply_steer_last = apply_steer
 
+    ### LONG ###
+
+    cruise_rpm = 0
+    cruise_throttle = 0
+
+    brake_cmd = False
+    brake_value = 0
+
+    if self.CP.openpilotLongitudinalControl:
+
+      gas, brake = compute_gb(actuators.accel)
+
+      if CC.longActive and brake > 0:
+        brake_value = clip(int(brake * CarControllerParams.BRAKE_SCALE), CarControllerParams.BRAKE_MIN, CarControllerParams.BRAKE_MAX)
+        brake_cmd = True
+
+      # AEB passthrough
+      if CC.enabled and CS.aeb:
+        brake_cmd = False
+
+      if CC.longActive and gas > 0:
+        # limit min and max values
+        cruise_throttle = clip(int(CarControllerParams.THROTTLE_BASE + (gas * CarControllerParams.THROTTLE_SCALE)), CarControllerParams.THROTTLE_MIN, CarControllerParams.THROTTLE_MAX)
+        cruise_rpm = clip(int(CarControllerParams.RPM_BASE + (gas * CarControllerParams.RPM_SCALE)), CarControllerParams.RPM_MIN, CarControllerParams.RPM_MAX)
+        # hysteresis
+        cruise_throttle, self.throttle_steady = accel_hysteresis(cruise_throttle, self.throttle_steady)
+        cruise_rpm, self.rpm_steady = accel_hysteresis(cruise_rpm, self.rpm_steady)
+
+        # slow down the signals change
+        cruise_throttle = clip(cruise_throttle, self.cruise_throttle_last - CarControllerParams.THROTTLE_DELTA, self.cruise_throttle_last + CarControllerParams.THROTTLE_DELTA)
+        cruise_rpm = clip(cruise_rpm, self.cruise_rpm_last - CarControllerParams.RPM_DELTA, self.cruise_rpm_last + CarControllerParams.RPM_DELTA)
+
+        self.cruise_throttle_last = cruise_throttle
+        self.cruise_rpm_last = cruise_rpm
 
     # *** alerts and pcm cancel ***
 
@@ -70,20 +130,42 @@ class CarController:
         self.es_distance_cnt = CS.es_distance_msg["COUNTER"]
 
     else:
-      if pcm_cancel_cmd and (self.frame - self.last_cancel_frame) > 0.2:
-        bus = 1 if self.CP.carFingerprint in GLOBAL_GEN2 else 0
-        can_sends.append(subarucan.create_es_distance(self.packer, CS.es_distance_msg, bus, pcm_cancel_cmd))
-        self.last_cancel_frame = self.frame
-
       if self.es_dashstatus_cnt != CS.es_dashstatus_msg["COUNTER"]:
-        can_sends.append(subarucan.create_es_dashstatus(self.packer, CS.es_dashstatus_msg))
+        can_sends.append(subarucan.create_es_dashstatus(self.packer, CS.es_dashstatus_msg, CC.enabled, CC.longActive, hud_control.leadVisible))
         self.es_dashstatus_cnt = CS.es_dashstatus_msg["COUNTER"]
 
-      if self.es_lkas_cnt != CS.es_lkas_msg["COUNTER"]:
-        can_sends.append(subarucan.create_es_lkas(self.packer, CS.es_lkas_msg, CC.enabled, hud_control.visualAlert,
+      if self.es_lkas_state_cnt != CS.es_lkas_state_msg["COUNTER"]:
+        can_sends.append(subarucan.create_es_lkas_state(self.packer, CS.es_lkas_state_msg, CC.enabled, hud_control.visualAlert,
                                                   hud_control.leftLaneVisible, hud_control.rightLaneVisible,
                                                   hud_control.leftLaneDepart, hud_control.rightLaneDepart))
-        self.es_lkas_cnt = CS.es_lkas_msg["COUNTER"]
+        self.es_lkas_state_cnt = CS.es_lkas_state_msg["COUNTER"]
+
+      if self.CP.openpilotLongitudinalControl:
+        if self.es_status_cnt != CS.es_status_msg["COUNTER"]:
+          can_sends.append(subarucan.create_es_status(self.packer, CS.es_status_msg, CC.longActive, cruise_rpm))
+          self.es_status_cnt = CS.es_status_msg["COUNTER"]
+
+        if self.es_brake_cnt != CS.es_brake_msg["COUNTER"]:
+          can_sends.append(subarucan.create_es_brake(self.packer, CS.es_brake_msg, CC.enabled, brake_cmd, brake_value))
+          self.es_brake_cnt = CS.es_brake_msg["COUNTER"]
+
+        if self.cruise_control_cnt != CS.cruise_control_msg["COUNTER"]:
+          can_sends.append(subarucan.create_cruise_control(self.packer, CS.cruise_control_msg))
+          self.cruise_control_cnt = CS.cruise_control_msg["COUNTER"]
+
+        if self.brake_status_cnt != CS.brake_status_msg["COUNTER"]:
+          can_sends.append(subarucan.create_brake_status(self.packer, CS.brake_status_msg, CS.aeb))
+          self.brake_status_cnt = CS.brake_status_msg["COUNTER"]
+
+        if self.es_distance_cnt != CS.es_distance_msg["COUNTER"]:
+          can_sends.append(subarucan.create_es_distance(self.packer, CS.es_distance_msg, 0, pcm_cancel_cmd, CC.longActive, brake_cmd, brake_value, cruise_throttle))
+          self.es_distance_cnt = CS.es_distance_msg["COUNTER"]
+
+      else:
+        if pcm_cancel_cmd and (self.frame - self.last_cancel_frame) > 0.2:
+          bus = 1 if self.CP.carFingerprint in GLOBAL_GEN2 else 0
+          can_sends.append(subarucan.create_es_distance(self.packer, CS.es_distance_msg, bus, pcm_cancel_cmd, CC.longActive, brake_cmd, brake_value, cruise_throttle))
+          self.last_cancel_frame = self.frame
 
     new_actuators = actuators.copy()
     new_actuators.steer = self.apply_steer_last / self.p.STEER_MAX

selfdrive/car/subaru/carstate.py

@@ -69,6 +69,7 @@ class CarState(CarStateBase):
                         cp.vl["BodyInfo"]["DOOR_OPEN_FL"]])
     ret.steerFaultPermanent = cp.vl["Steering_Torque"]["Steer_Error_1"] == 1
 
+
     if self.car_fingerprint in PREGLOBAL_CARS:
       self.cruise_button = cp_cam.vl["ES_Distance"]["Cruise_Button"]
       self.ready = not cp_cam.vl["ES_DashStatus"]["Not_Ready_Startup"]
@@ -77,8 +78,17 @@ class CarState(CarStateBase):
       ret.cruiseState.nonAdaptive = cp_cam.vl["ES_DashStatus"]["Conventional_Cruise"] == 1
       ret.cruiseState.standstill = cp_cam.vl["ES_DashStatus"]["Cruise_State"] == 3
       ret.stockFcw = cp_cam.vl["ES_LKAS_State"]["LKAS_Alert"] == 2
+      ret.stockAeb = (cp_cam.vl["ES_LKAS_State"]["LKAS_Alert"] == 5) or \
+                     (cp_cam.vl["ES_LKAS_State"]["LKAS_Alert_Msg"] == 6) or ret.stockFcw
       self.es_lkas_msg = copy.copy(cp_cam.vl["ES_LKAS_State"])
-
+      self.es_lkas_state_msg = copy.copy(cp_cam.vl["ES_LKAS_State"])
+      cp_es_brake = cp_body if self.car_fingerprint in GLOBAL_GEN2 else cp_cam
+      self.aeb = cp_es_brake.vl["ES_Brake"]["Cruise_Brake_Active"]
+      self.es_brake_msg = copy.copy(cp_es_brake.vl["ES_Brake"])
+      cp_es_status = cp_body if self.car_fingerprint in GLOBAL_GEN2 else cp_cam
+      self.es_status_msg = copy.copy(cp_es_status.vl["ES_Status"])
+      self.cruise_control_msg = copy.copy(cp_cruise.vl["CruiseControl"])
+      self.brake_status_msg = copy.copy(cp_brakes.vl["Brake_Status"])
     cp_es_distance = cp_body if self.car_fingerprint in GLOBAL_GEN2 else cp_cam
     self.es_distance_msg = copy.copy(cp_es_distance.vl["ES_Distance"])
     self.es_dashstatus_msg = copy.copy(cp_cam.vl["ES_DashStatus"])
@@ -88,13 +98,21 @@ class CarState(CarStateBase):
   @staticmethod
   def get_common_global_signals():
     signals = [
+      ("COUNTER", "CruiseControl"),
+      ("Signal1", "CruiseControl"),
       ("Cruise_On", "CruiseControl"),
       ("Cruise_Activated", "CruiseControl"),
+      ("Signal2", "CruiseControl"),
       ("FL", "Wheel_Speeds"),
       ("FR", "Wheel_Speeds"),
       ("RL", "Wheel_Speeds"),
       ("RR", "Wheel_Speeds"),
+      ("COUNTER", "Brake_Status"),
+      ("Signal1", "Brake_Status"),
+      ("ES_Brake", "Brake_Status"),
+      ("Signal2", "Brake_Status"),
       ("Brake", "Brake_Status"),
+      ("Signal3", "Brake_Status"),
     ]
     checks = [
       ("CruiseControl", 20),
@@ -105,7 +123,7 @@ class CarState(CarStateBase):
     return signals, checks
 
   @staticmethod
-  def get_global_es_distance_signals():
+  def get_global_es_signals():
     signals = [
       ("COUNTER", "ES_Distance"),
       ("Signal1", "ES_Distance"),
@@ -126,9 +144,32 @@ class CarState(CarStateBase):
       ("Cruise_Set", "ES_Distance"),
       ("Cruise_Resume", "ES_Distance"),
       ("Signal6", "ES_Distance"),
+
+      ("COUNTER", "ES_Status"),
+      ("Signal1", "ES_Status"),
+      ("Cruise_Fault", "ES_Status"),
+      ("Cruise_RPM", "ES_Status"),
+      ("Signal2", "ES_Status"),
+      ("Cruise_Activated", "ES_Status"),
+      ("Brake_Lights", "ES_Status"),
+      ("Cruise_Hold", "ES_Status"),
+      ("Signal3", "ES_Status"),
+
+      ("COUNTER", "ES_Brake"),
+      ("Signal1", "ES_Brake"),
+      ("Brake_Pressure", "ES_Brake"),
+      ("Signal2", "ES_Brake"),
+      ("Cruise_Brake_Lights", "ES_Brake"),
+      ("Cruise_Brake_Fault", "ES_Brake"),
+      ("Cruise_Brake_Active", "ES_Brake"),
+      ("Cruise_Activated", "ES_Brake"),
+      ("Signal3", "ES_Brake"),
+
     ]
     checks = [
       ("ES_Distance", 20),
+      ("ES_Status", 20),
+      ("ES_Brake", 20),
     ]
 
     return signals, checks
@@ -143,6 +184,7 @@ class CarState(CarStateBase):
       ("Steer_Error_1", "Steering_Torque"),
       ("Brake_Pedal", "Brake_Pedal"),
       ("Throttle_Pedal", "Throttle"),
+      ("Throttle_Cruise", "Throttle"),
       ("LEFT_BLINKER", "Dashlights"),
       ("RIGHT_BLINKER", "Dashlights"),
       ("SEATBELT_FL", "Dashlights"),
@@ -179,6 +221,7 @@ class CarState(CarStateBase):
 
       signals += [
         ("Steer_Warning", "Steering_Torque"),
+        ("RPM", "Transmission"),
         ("UNITS", "Dashlights"),
       ]
 
@@ -248,7 +291,7 @@ class CarState(CarStateBase):
       ]
     else:
       signals = [
-        ("Counter", "ES_DashStatus"),
+        ("COUNTER", "ES_DashStatus"),
         ("PCB_Off", "ES_DashStatus"),
         ("LDW_Off", "ES_DashStatus"),
         ("Signal1", "ES_DashStatus"),
@@ -290,6 +333,7 @@ class CarState(CarStateBase):
         ("LKAS_Right_Line_Visible", "ES_LKAS_State"),
         ("LKAS_Alert", "ES_LKAS_State"),
         ("Signal3", "ES_LKAS_State"),
+
       ]
 
       checks = [
@@ -298,8 +342,8 @@ class CarState(CarStateBase):
       ]
 
       if CP.carFingerprint not in GLOBAL_GEN2:
-        signals += CarState.get_global_es_distance_signals()[0]
-        checks += CarState.get_global_es_distance_signals()[1]
+        signals += CarState.get_global_es_signals()[0]
+        checks += CarState.get_global_es_signals()[1]
 
     return CANParser(DBC[CP.carFingerprint]["pt"], signals, checks, 2)
 
@@ -307,8 +351,8 @@ class CarState(CarStateBase):
   def get_body_can_parser(CP):
     if CP.carFingerprint in GLOBAL_GEN2:
       signals, checks = CarState.get_common_global_signals()
-      signals += CarState.get_global_es_distance_signals()[0]
-      checks += CarState.get_global_es_distance_signals()[1]
+      signals += CarState.get_global_es_signals()[0]
+      checks += CarState.get_global_es_signals()[1]
       return CANParser(DBC[CP.carFingerprint]["pt"], signals, checks, 1)
 
     return None

selfdrive/car/subaru/interface.py

@@ -101,6 +101,19 @@ class CarInterface(CarInterfaceBase):
     else:
       raise ValueError(f"unknown car: {candidate}")
 
+    # longitudinal
+    ret.experimentalLongitudinalAvailable = candidate not in (GLOBAL_GEN2 | PREGLOBAL_CARS)
+    if experimental_long and ret.experimentalLongitudinalAvailable:
+      ret.longitudinalTuning.kpBP = [0., 5., 35.]
+      ret.longitudinalTuning.kpV = [0.8, 1.0, 1.5]
+      ret.longitudinalTuning.kiBP = [0., 35.]
+      ret.longitudinalTuning.kiV = [0.54, 0.36]
+
+      ret.stoppingControl = True
+      ret.openpilotLongitudinalControl = experimental_long
+      if ret.openpilotLongitudinalControl:
+        ret.safetyConfigs[0].safetyParam |= Panda.FLAG_SUBARU_LONG
+
     return ret
 
   # returns a car.CarState

selfdrive/car/subaru/subarucan.py

@@ -14,14 +14,23 @@ def create_steering_control(packer, apply_steer):
 def create_steering_status(packer):
   return packer.make_can_msg("ES_LKAS_State", 0, {})
 
-def create_es_distance(packer, es_distance_msg, bus, pcm_cancel_cmd):
+def create_es_distance(packer, es_distance_msg, bus, pcm_cancel_cmd, long_active, brake_cmd, brake_value, cruise_throttle):
+
   values = copy.copy(es_distance_msg)
-  values["COUNTER"] = (values["COUNTER"] + 1) % 0x10
+  if long_active:
+    values["Cruise_Throttle"] = cruise_throttle
   if pcm_cancel_cmd:
+    values["COUNTER"] = (values["COUNTER"] + 1) % 0x10
     values["Cruise_Cancel"] = 1
+  if brake_cmd:
+    values["Cruise_Throttle"] = 808 if brake_value >= 35 else 1818
+    values["Cruise_Brake_Active"] = 1
+  # Do not disable openpilot on Eyesight Soft Disable
+  values["Cruise_Soft_Disable"] = 0
+
   return packer.make_can_msg("ES_Distance", bus, values)
 
-def create_es_lkas(packer, es_lkas_msg, enabled, visual_alert, left_line, right_line, left_lane_depart, right_lane_depart):
+def create_es_lkas_state(packer, es_lkas_msg, enabled, visual_alert, left_line, right_line, left_lane_depart, right_lane_depart):
 
   values = copy.copy(es_lkas_msg)
 
@@ -67,8 +76,14 @@ def create_es_lkas(packer, es_lkas_msg, enabled, visual_alert, left_line, right_
 
   return packer.make_can_msg("ES_LKAS_State", 0, values)
 
-def create_es_dashstatus(packer, dashstatus_msg):
-  values = copy.copy(dashstatus_msg)
+def create_es_dashstatus(packer, es_dashstatus_msg, enabled, long_active, lead_visible):
+
+  values = copy.copy(es_dashstatus_msg)
+  if enabled and long_active:
+    values["Cruise_State"] = 0
+    values["Cruise_Activated"] = 1
+    values["Cruise_Disengaged"] = 0
+    values["Car_Follow"] = int(lead_visible)
 
   # Filter stock LKAS disabled and Keep hands on steering wheel OFF alerts
   if values["LKAS_State_Msg"] in [2, 3]:
@@ -76,6 +91,44 @@ def create_es_dashstatus(packer, dashstatus_msg):
 
   return packer.make_can_msg("ES_DashStatus", 0, values)
 
+def create_es_brake(packer, es_brake_msg, enabled, brake_cmd, brake_value):
+
+  values = copy.copy(es_brake_msg)
+  if enabled:
+    values["Cruise_Activated"] = 1
+  if brake_cmd:
+    values["Brake_Pressure"] = brake_value
+    values["Cruise_Brake_Active"] = 1
+    values["Cruise_Brake_Lights"] = 1 if brake_value >= 70 else 0
+
+  return packer.make_can_msg("ES_Brake", 0, values)
+
+def create_es_status(packer, es_status_msg, long_active, cruise_rpm):
+
+  values = copy.copy(es_status_msg)
+  if long_active:
+    values["Cruise_Activated"] = 1
+    values["Cruise_RPM"] = cruise_rpm
+
+  return packer.make_can_msg("ES_Status", 0, values)
+
+# disable cruise_activated feedback to eyesight to keep ready state
+def create_cruise_control(packer, cruise_control_msg):
+
+  values = copy.copy(cruise_control_msg)
+  values["Cruise_Activated"] = 0
+
+  return packer.make_can_msg("CruiseControl", 2, values)
+
+# disable es_brake feedback to eyesight, exempt AEB
+def create_brake_status(packer, brake_status_msg, aeb):
+
+  values = copy.copy(brake_status_msg)
+  if not aeb:
+    values["ES_Brake"] = 0
+
+  return packer.make_can_msg("Brake_Status", 2, values)
+
 # *** Subaru Pre-global ***
 
 def subaru_preglobal_checksum(packer, values, addr):

selfdrive/car/subaru/values.py

@@ -29,6 +29,21 @@ class CarControllerParams:
     else:
       self.STEER_MAX = 2047
 
+  RPM_MIN = 0                   # min cruise_rpm
+  RPM_MAX = 3200                # max cruise_rpm
+  RPM_BASE = 600                # cruise_rpm idle, from stock drive
+  RPM_SCALE = 3000              # cruise_rpm, from testing
+  RPM_DELTA = 50
+
+  THROTTLE_MIN = 0              # min cruise_throttle
+  THROTTLE_MAX = 3400           # max cruise_throttle
+  THROTTLE_BASE = 1810          # cruise_throttle, from stock drive
+  THROTTLE_SCALE = 3000         # from testing
+  THROTTLE_DELTA = 50
+
+  BRAKE_MIN = 0
+  BRAKE_MAX = 400
+  BRAKE_SCALE = 1000            # from testing
 
 class CAR:
   # Global platform