Subaru: Global gen1 experimental longitudinal (#28872)

* Add longitudinal support for Subaru Crosstrek and Impreza

* Update experimentalLongitudinalAvailable check

* Update supported cars list

* bump panda

* Remove/rename es_lkas_msg to es_lkas_state_msg

* Use stockAeb for AEB passthrough

* bump panda

* bump panda

* remove stockFcw from stockAeb

* Subaru: Add FCW_Cont_Beep to stockFcw signals

* bump panda

* bump panda

* update poetry deps: shellingham

* bump panda

* bump panda

* Revert "update poetry deps: shellingham"

This reverts commit 6e9b209648.

* Merge fixes

* bump panda

* bump panda

* update supported cars list

* dont use counters for long control

* fix unittests

* submodules update

* only soft disable in long control

* use common functions and cleanup

* apply hystersis correctly

* move to comma repo

* use CanBus

* cleanup

* explicit copy

* behind a flag

* remove unrequired rpm checks

* add comment

* fix flag issue

* we still need to check rpm

* update docs

* enable long for a test route

* unit tests

* inactive throttle fix

* Update subarucan.py

* Update carcontroller.py

* Update carcontroller.py

* inactive throttle fix

* Delete settings

* fix rate limit

* bump submodules

* bump panda

* bump panda

* bump panda

* bump panda

* simplify initial implementation, remove AEB

* reduce initial complexity by not intercepting cruisecontrol or brake_status

* fix fwd hook test

* show pcb off warning

* cleanup and setup for tuning

* fix sumobuldes

* revert unrelated changes

* only whats required

* only whats required

* clean that up

* better comments

* behind the flag for now

* comments and minimize diff

* align stuff

* cleanup for PR

* apply review suggestions

---------

Co-authored-by: Martin Lillepuu <martin@mlp.ee>
old-commit-hash: d6c682b401

selfdrive/car/subaru/carcontroller.py

@@ -1,3 +1,4 @@
+from common.numpy_fast import clip, interp
 from opendbc.can.packer import CANPacker
 from selfdrive.car import apply_driver_steer_torque_limits
 from selfdrive.car.subaru import subarucan
@@ -42,6 +43,21 @@ class CarController:
 
       self.apply_steer_last = apply_steer
 
+    # *** longitudinal ***
+
+    if CC.longActive:
+      apply_throttle = int(round(interp(actuators.accel, CarControllerParams.THROTTLE_LOOKUP_BP, CarControllerParams.THROTTLE_LOOKUP_V)))
+      apply_rpm = int(round(interp(actuators.accel, CarControllerParams.RPM_LOOKUP_BP, CarControllerParams.RPM_LOOKUP_V)))
+      apply_brake = int(round(interp(actuators.accel, CarControllerParams.BRAKE_LOOKUP_BP, CarControllerParams.BRAKE_LOOKUP_V)))
+
+      # limit min and max values
+      cruise_throttle = clip(apply_throttle, CarControllerParams.THROTTLE_MIN, CarControllerParams.THROTTLE_MAX)
+      cruise_rpm = clip(apply_rpm, CarControllerParams.RPM_MIN, CarControllerParams.RPM_MAX)
+      cruise_brake = clip(apply_brake, CarControllerParams.BRAKE_MIN, CarControllerParams.BRAKE_MAX)
+    else:
+      cruise_throttle = CarControllerParams.THROTTLE_INACTIVE
+      cruise_rpm = CarControllerParams.RPM_INACTIVE
+      cruise_brake = CarControllerParams.BRAKE_MIN
 
     # *** alerts and pcm cancel ***
     if self.CP.carFingerprint in PREGLOBAL_CARS:
@@ -64,13 +80,9 @@ class CarController:
         can_sends.append(subarucan.create_preglobal_es_distance(self.packer, cruise_button, CS.es_distance_msg))
 
     else:
-      if pcm_cancel_cmd and (self.frame - self.last_cancel_frame) > 0.2:
-        bus = CanBus.alt if self.CP.carFingerprint in GLOBAL_GEN2 else CanBus.main
-        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.frame % 10 == 0:
-        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, self.CP.openpilotLongitudinalControl,
+                                                        CC.longActive, hud_control.leadVisible))
 
         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,
@@ -79,6 +91,20 @@ class CarController:
         if self.CP.flags & SubaruFlags.SEND_INFOTAINMENT:
           can_sends.append(subarucan.create_es_infotainment(self.packer, CS.es_infotainment_msg, hud_control.visualAlert))
 
+      if self.CP.openpilotLongitudinalControl:
+        if self.frame % 5 == 0:
+          can_sends.append(subarucan.create_es_status(self.packer, CS.es_status_msg, self.CP.openpilotLongitudinalControl, CC.longActive, cruise_rpm))
+
+          can_sends.append(subarucan.create_es_brake(self.packer, CS.es_brake_msg, CC.enabled, cruise_brake))
+
+          can_sends.append(subarucan.create_es_distance(self.packer, CS.es_distance_msg, 0, pcm_cancel_cmd,
+                                                        self.CP.openpilotLongitudinalControl, cruise_brake > 0, cruise_throttle))
+      else:
+        if pcm_cancel_cmd and (self.frame - self.last_cancel_frame) > 0.2:
+          bus = CanBus.alt if self.CP.carFingerprint in GLOBAL_GEN2 else CanBus.main
+          can_sends.append(subarucan.create_es_distance(self.packer, CS.es_distance_msg, bus, pcm_cancel_cmd))
+          self.last_cancel_frame = self.frame
+
     new_actuators = actuators.copy()
     new_actuators.steer = self.apply_steer_last / self.p.STEER_MAX
     new_actuators.steerOutputCan = self.apply_steer_last

selfdrive/car/subaru/carstate.py

@@ -91,6 +91,12 @@ class CarState(CarStateBase):
       ret.stockAeb = (cp_es_distance.vl["ES_Brake"]["AEB_Status"] == 8) and \
                      (cp_es_distance.vl["ES_Brake"]["Brake_Pressure"] != 0)
       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.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"])
 
     self.es_distance_msg = copy.copy(cp_es_distance.vl["ES_Distance"])
     self.es_dashstatus_msg = copy.copy(cp_cam.vl["ES_DashStatus"])
@@ -114,6 +120,8 @@ class CarState(CarStateBase):
     messages = [
       ("ES_Brake", 20),
       ("ES_Distance", 20),
+      ("ES_Status", 20),
+      ("ES_Brake", 20),
     ]
 
     return messages

selfdrive/car/subaru/interface.py

@@ -107,6 +107,18 @@ class CarInterface(CarInterfaceBase):
     else:
       raise ValueError(f"unknown car: {candidate}")
 
+    #ret.experimentalLongitudinalAvailable = candidate not in (GLOBAL_GEN2 | PREGLOBAL_CARS | LKAS_ANGLE)
+    ret.openpilotLongitudinalControl = experimental_long and ret.experimentalLongitudinalAvailable
+
+    if ret.openpilotLongitudinalControl:
+      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.safetyConfigs[0].safetyParam |= Panda.FLAG_SUBARU_LONG
+
     return ret
 
   # returns a car.CarState

selfdrive/car/subaru/subarucan.py

@@ -16,8 +16,7 @@ def create_steering_control(packer, apply_steer, steer_req):
 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, long_enabled = False, brake_cmd = False, cruise_throttle = 0):
-def create_es_distance(packer, es_distance_msg, bus, pcm_cancel_cmd):
   values = {s: es_distance_msg[s] for s in [
     "CHECKSUM",
     "COUNTER",
@@ -41,9 +40,20 @@ def create_es_distance(packer, es_distance_msg, bus, pcm_cancel_cmd):
     "Signal6",
   ]}
   values["COUNTER"] = (values["COUNTER"] + 1) % 0x10
+
+  if long_enabled:
+    values["Cruise_Throttle"] = cruise_throttle
+
+    # Do not disable openpilot on Eyesight Soft Disable, if openpilot is controlling long
+    values["Cruise_Soft_Disable"] = 0
+
+    if brake_cmd:
+      values["Cruise_Brake_Active"] = 1
+
   if pcm_cancel_cmd:
     values["Cruise_Cancel"] = 1
     values["Cruise_Throttle"] = 1818 # inactive throttle
+
   return packer.make_can_msg("ES_Distance", bus, values)
 
 
@@ -106,8 +116,7 @@ def create_es_lkas_state(packer, es_lkas_state_msg, enabled, visual_alert, left_
 
   return packer.make_can_msg("ES_LKAS_State", CanBus.main, values)
 
-
+def create_es_dashstatus(packer, dashstatus_msg, enabled, long_enabled, long_active, lead_visible):
-def create_es_dashstatus(packer, dashstatus_msg):
   values = {s: dashstatus_msg[s] for s in [
     "CHECKSUM",
     "COUNTER",
@@ -138,12 +147,68 @@ def create_es_dashstatus(packer, dashstatus_msg):
     "Cruise_State",
   ]}
 
+  if enabled and long_active:
+    values["Cruise_State"] = 0
+    values["Cruise_Activated"] = 1
+    values["Cruise_Disengaged"] = 0
+    values["Car_Follow"] = int(lead_visible)
+
+  if long_enabled:
+    values["PCB_Off"] = 1 # AEB is not presevered, so show the PCB_Off on dash
+
   # Filter stock LKAS disabled and Keep hands on steering wheel OFF alerts
   if values["LKAS_State_Msg"] in (2, 3):
     values["LKAS_State_Msg"] = 0
 
   return packer.make_can_msg("ES_DashStatus", CanBus.main, values)
 
+def create_es_brake(packer, es_brake_msg, enabled, brake_value):
+  values = {s: es_brake_msg[s] for s in [
+    "CHECKSUM",
+    "COUNTER",
+    "Signal1",
+    "Brake_Pressure",
+    "AEB_Status",
+    "Cruise_Brake_Lights",
+    "Cruise_Brake_Fault",
+    "Cruise_Brake_Active",
+    "Cruise_Activated",
+    "Signal3",
+  ]}
+
+  if enabled:
+    values["Cruise_Activated"] = 1
+
+  values["Brake_Pressure"] = brake_value
+
+  if brake_value > 0:
+    values["Cruise_Brake_Active"] = 1
+    values["Cruise_Brake_Lights"] = 1 if brake_value >= 70 else 0
+
+  return packer.make_can_msg("ES_Brake", CanBus.main, values)
+
+def create_es_status(packer, es_status_msg, long_enabled, long_active, cruise_rpm):
+  values = {s: es_status_msg[s] for s in [
+    "CHECKSUM",
+    "COUNTER",
+    "Signal1",
+    "Cruise_Fault",
+    "Cruise_RPM",
+    "Signal2",
+    "Cruise_Activated",
+    "Brake_Lights",
+    "Cruise_Hold",
+    "Signal3",
+  ]}
+
+  if long_enabled:
+    values["Cruise_RPM"] = cruise_rpm
+
+    if long_active:
+      values["Cruise_Activated"] = 1
+
+  return packer.make_can_msg("ES_Status", CanBus.main, values)
+
 
 def create_es_infotainment(packer, es_infotainment_msg, visual_alert):
   # Filter stock LKAS disabled and Keep hands on steering wheel OFF alerts

selfdrive/car/subaru/values.py

@@ -29,6 +29,29 @@ class CarControllerParams:
     else:
       self.STEER_MAX = 2047
 
+  THROTTLE_MIN = 808
+  THROTTLE_MAX = 3400
+
+  THROTTLE_INACTIVE     = 1818   # corresponds to zero acceleration
+  THROTTLE_ENGINE_BRAKE = 808    # while braking, eyesight sets throttle to this, probably for engine braking
+
+  BRAKE_MIN = 0
+  BRAKE_MAX = 600                # about -3.5m/s2 from testing
+
+  RPM_MIN = 0
+  RPM_MAX = 2400
+
+  RPM_INACTIVE = 600             # a good base rpm for zero acceleration
+
+  THROTTLE_LOOKUP_BP = [0, 1]
+  THROTTLE_LOOKUP_V = [THROTTLE_INACTIVE, THROTTLE_MAX]
+
+  RPM_LOOKUP_BP = [0, 1]
+  RPM_LOOKUP_V = [RPM_INACTIVE, RPM_MAX]
+
+  BRAKE_LOOKUP_BP = [-1, 0]
+  BRAKE_LOOKUP_V = [BRAKE_MAX, BRAKE_MIN]
+
 
 class SubaruFlags(IntFlag):
   SEND_INFOTAINMENT = 1