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>
2 years ago · d6c682b401
parent e8e03197ca
commit d6c682b401
5 changed files with 144 additions and 10 deletions
--- a/selfdrive/car/subaru/carcontroller.py
+++ b/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
--- a/selfdrive/car/subaru/carstate.py
+++ b/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
--- a/selfdrive/car/subaru/interface.py
+++ b/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
--- a/selfdrive/car/subaru/subarucan.py
+++ b/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):
+def create_es_distance(packer, es_distance_msg, bus, pcm_cancel_cmd, long_enabled = False, brake_cmd = False, cruise_throttle = 0):
  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):
+def create_es_dashstatus(packer, dashstatus_msg, enabled, long_enabled, long_active, lead_visible):
  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
--- a/selfdrive/car/subaru/values.py
+++ b/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