Subaru: Stop and Go autoresume (#289)

* Subaru: Stop and Go autoresume

* Update CHANGELOGS.md

* Update CHANGELOGS.md

* do this

* sync with subaru-community

* sync for manual parking brake

* no slot machine pls

* fix this too

* flipped

* fix panda

* comments

* Update CHANGELOGS.md

* check every 2.5 second

* move them to a function

* more logical

* frame-based frequency sends

* type hinting

* should be tuple

* better grouping

* for docs regeneration

* same as upstream

* move it down

* cleanup

* use int flag and safety param, only block message when sng is allowed

* Do it here instead

* gate everything

* move comment

* block tx if sng is not allowed on certain platforms

* bump panda

CHANGELOGS.md

@@ -11,6 +11,8 @@ sunnypilot - 0.9.5.1 (2023-09-27)
   * GAP - Driving Personality
   * DLP - Dynamic Lane Profile
   * SLC - Speed Limit Control
+* NEW❗: Subaru - Stop and Go auto-resume support thanks to martinl!
+  * Global (excluding Gen 2 and Hybrid) and Pre-Global support
 * NEW❗: Toyota - Stop and Go hack
   * Allow some Toyota/Lexus cars to auto resume during stop and go traffic
   * Only applicable to certain models and model years

common/params.cc

@@ -290,6 +290,7 @@ std::unordered_map<std::string, uint32_t> keys = {
     {"SpeedLimitOffsetType", PERSISTENT},
     {"StandStillTimer", PERSISTENT},
     {"StockLongToyota", PERSISTENT},
+    {"SubaruManualParkingBrakeSng", PERSISTENT},
     {"TorqueDeadzoneDeg", PERSISTENT},
     {"TorqueFriction", PERSISTENT},
     {"TorqueMaxLatAccel", PERSISTENT},

selfdrive/car/subaru/carcontroller.py

@@ -1,14 +1,20 @@
+from cereal import car
+from typing import Tuple
 from openpilot.common.numpy_fast import clip, interp
+from openpilot.common.params import Params
 from opendbc.can.packer import CANPacker
 from openpilot.selfdrive.car import apply_driver_steer_torque_limits, common_fault_avoidance
 from openpilot.selfdrive.car.subaru import subarucan
-from openpilot.selfdrive.car.subaru.values import DBC, GLOBAL_GEN2, PREGLOBAL_CARS, HYBRID_CARS, STEER_RATE_LIMITED, CanBus, CarControllerParams, SubaruFlags
+from openpilot.selfdrive.car.subaru.values import DBC, GLOBAL_GEN2, PREGLOBAL_CARS, HYBRID_CARS, STEER_RATE_LIMITED, CanBus, CarControllerParams, SubaruFlags, SubaruFlagsSP
 
 # FIXME: These limits aren't exact. The real limit is more than likely over a larger time period and
 # involves the total steering angle change rather than rate, but these limits work well for now
 MAX_STEER_RATE = 25  # deg/s
 MAX_STEER_RATE_FRAMES = 7  # tx control frames needed before torque can be cut
 
+_SNG_ACC_MIN_DIST = 3
+_SNG_ACC_MAX_DIST = 4.5
+
 
 class CarController:
   def __init__(self, dbc_name, CP, VM):
@@ -19,6 +25,21 @@ class CarController:
     self.cruise_button_prev = 0
     self.steer_rate_counter = 0
 
+    self.param_s = Params()
+
+    if CP.spFlags & SubaruFlagsSP.SP_SUBARU_SNG:
+      self.subaru_sng = True
+      self.manual_parking_brake = self.param_s.get_bool("SubaruManualParkingBrakeSng")
+      self.throttle_cnt = -1
+      self.brake_pedal_cnt = -1
+      self.prev_close_distance = 0
+      self.prev_standstill = False
+      self.standstill_start = 0
+      self.sng_acc_resume = False
+      self.sng_acc_resume_cnt = -1
+      self.manual_hold = False
+      self.prev_cruise_state = 0
+
     self.p = CarControllerParams(CP)
     self.packer = CANPacker(DBC[CP.carFingerprint]['pt'])
 
@@ -27,6 +48,9 @@ class CarController:
     hud_control = CC.hudControl
     pcm_cancel_cmd = CC.cruiseControl.cancel
 
+    if self.frame % 250 == 0 and self.subaru_sng:
+      self.manual_parking_brake = self.param_s.get_bool("SubaruManualParkingBrakeSng")
+
     can_sends = []
 
     # *** steering ***
@@ -56,6 +80,9 @@ class CarController:
 
       self.apply_steer_last = apply_steer
 
+    # *** stop and go ***
+    throttle_cmd, speed_cmd = self.stop_and_go(CC, CS)
+
     # *** longitudinal ***
 
     if CC.longActive:
@@ -92,6 +119,9 @@ class CarController:
 
         can_sends.append(subarucan.create_preglobal_es_distance(self.packer, cruise_button, CS.es_distance_msg))
 
+      if self.subaru_sng:
+        can_sends.append(subarucan.create_preglobal_throttle(self.packer, CS.throttle_msg, throttle_cmd))
+
     else:
       if self.frame % 10 == 0:
         can_sends.append(subarucan.create_es_dashstatus(self.packer, self.frame // 10, CS.es_dashstatus_msg, CC.enabled, self.CP.openpilotLongitudinalControl,
@@ -104,6 +134,12 @@ class CarController:
         if self.CP.flags & SubaruFlags.SEND_INFOTAINMENT:
           can_sends.append(subarucan.create_es_infotainment(self.packer, self.frame // 10, CS.es_infotainment_msg, hud_control.visualAlert))
 
+      if self.subaru_sng:
+        can_sends.append(subarucan.create_throttle(self.packer, CS.throttle_msg, throttle_cmd))
+
+        if self.frame % 2 == 0:
+          can_sends.append(subarucan.create_brake_pedal(self.packer, CS.brake_pedal_msg, speed_cmd, pcm_cancel_cmd))
+
       if self.CP.openpilotLongitudinalControl:
         if self.frame % 5 == 0:
           can_sends.append(subarucan.create_es_status(self.packer, self.frame // 5, CS.es_status_msg,
@@ -125,3 +161,58 @@ class CarController:
 
     self.frame += 1
     return new_actuators, can_sends
+
+  # Stop and Go auto-resume thanks to martinl from subaru-community
+  def stop_and_go(self, CC: car.CarControl, CS: car.CarState, throttle_cmd: bool = False, speed_cmd: bool = False) -> Tuple[bool, bool]:
+    if not self.subaru_sng:
+      return throttle_cmd, speed_cmd
+    if self.CP.carFingerprint in PREGLOBAL_CARS:
+      # Initiate the ACC resume sequence if conditions are met
+      if (CC.enabled                                          # ACC active
+        and CS.car_follow == 1                                # lead car
+        and CS.out.standstill                                 # must be standing still
+        and CS.close_distance > _SNG_ACC_MIN_DIST             # acc resume trigger low threshold
+        and CS.close_distance < _SNG_ACC_MAX_DIST             # acc resume trigger high threshold
+        and CS.close_distance > self.prev_close_distance):    # distance with lead car is increasing
+        self.sng_acc_resume = True
+    elif self.CP.carFingerprint not in (GLOBAL_GEN2 | HYBRID_CARS):
+      if self.manual_parking_brake:
+        # Send brake message with non-zero speed in standstill to avoid non-EPB ACC disengage
+        if (CC.enabled                                        # ACC active
+          and CS.car_follow == 1                              # lead car
+          and CS.out.standstill
+          and self.frame > self.standstill_start + 50):       # standstill for >0.5 second
+          speed_cmd = True
+      else:
+        # Record manual hold set while in standstill and no car in front
+        if CS.out.standstill and self.prev_cruise_state == 1 and CS.cruise_state == 3 and CS.car_follow == 0:
+          self.manual_hold = True
+        # Cancel manual hold when car starts moving
+        if not CS.out.standstill:
+          self.manual_hold = False
+        # Initiate the ACC resume sequence if conditions are met
+        if (CC.enabled                                        # ACC active
+          and not self.manual_hold
+          and CS.car_follow == 1                              # lead car
+          and CS.cruise_state == 3                            # ACC HOLD (only with EPB)
+          and CS.close_distance > _SNG_ACC_MIN_DIST           # acc resume trigger low threshold
+          and CS.close_distance < _SNG_ACC_MAX_DIST           # acc resume trigger high threshold
+          and CS.close_distance > self.prev_close_distance):  # distance with lead car is increasing
+          self.sng_acc_resume = True
+
+      if CS.out.standstill and not self.prev_standstill:
+        self.standstill_start = self.frame
+      self.prev_standstill = CS.out.standstill
+      self.prev_cruise_state = CS.cruise_state
+
+    if self.sng_acc_resume:
+      if self.sng_acc_resume_cnt < 5:
+        throttle_cmd = True
+        self.sng_acc_resume_cnt += 1
+      else:
+        self.sng_acc_resume = False
+        self.sng_acc_resume_cnt = -1
+
+    self.prev_close_distance = CS.close_distance
+
+    return throttle_cmd, speed_cmd

selfdrive/car/subaru/carstate.py

@@ -4,7 +4,7 @@ from opendbc.can.can_define import CANDefine
 from openpilot.common.conversions import Conversions as CV
 from openpilot.selfdrive.car.interfaces import CarStateBase
 from opendbc.can.parser import CANParser
-from openpilot.selfdrive.car.subaru.values import DBC, GLOBAL_GEN2, PREGLOBAL_CARS, HYBRID_CARS, CanBus, SubaruFlags
+from openpilot.selfdrive.car.subaru.values import DBC, GLOBAL_GEN2, PREGLOBAL_CARS, HYBRID_CARS, CanBus, SubaruFlags, SubaruFlagsSP
 from openpilot.selfdrive.car import CanSignalRateCalculator
 
 
@@ -123,8 +123,16 @@ class CarState(CarStateBase):
         self.es_status_msg = copy.copy(cp_es_status.vl["ES_Status"])
         self.cruise_control_msg = copy.copy(cp_cruise.vl["CruiseControl"])
 
+      if self.CP.spFlags & SubaruFlagsSP.SP_SUBARU_SNG:
+        self.cruise_state = cp_cam.vl["ES_DashStatus"]["Cruise_State"]
+        self.brake_pedal_msg = copy.copy(cp.vl["Brake_Pedal"])
+
     if self.car_fingerprint not in HYBRID_CARS:
       self.es_distance_msg = copy.copy(cp_es_distance.vl["ES_Distance"])
+      if self.CP.spFlags & SubaruFlagsSP.SP_SUBARU_SNG:
+        self.throttle_msg = copy.copy(cp.vl["Throttle"])
+        self.car_follow = cp_es_distance.vl["ES_Distance"]["Car_Follow"]
+        self.close_distance = cp_es_distance.vl["ES_Distance"]["Close_Distance"]
 
     self.es_dashstatus_msg = copy.copy(cp_cam.vl["ES_DashStatus"])
     if self.CP.flags & SubaruFlags.SEND_INFOTAINMENT:

selfdrive/car/subaru/interface.py

@@ -2,7 +2,7 @@ from cereal import car
 from panda import Panda
 from openpilot.selfdrive.car import get_safety_config, create_mads_event
 from openpilot.selfdrive.car.interfaces import CarInterfaceBase
-from openpilot.selfdrive.car.subaru.values import CAR, LKAS_ANGLE, GLOBAL_GEN2, PREGLOBAL_CARS, HYBRID_CARS, SubaruFlags
+from openpilot.selfdrive.car.subaru.values import CAR, LKAS_ANGLE, GLOBAL_GEN2, PREGLOBAL_CARS, HYBRID_CARS, SubaruFlags, SubaruFlagsSP
 
 ButtonType = car.CarState.ButtonEvent.Type
 EventName = car.CarEvent.EventName
@@ -35,6 +35,11 @@ class CarInterface(CarInterfaceBase):
       if candidate in GLOBAL_GEN2:
         ret.safetyConfigs[0].safetyParam |= Panda.FLAG_SUBARU_GEN2
 
+    if candidate not in (GLOBAL_GEN2 | HYBRID_CARS):
+      ret.autoResumeSng = True
+      ret.spFlags |= SubaruFlagsSP.SP_SUBARU_SNG.value
+      ret.safetyConfigs[0].safetyParam |= Panda.FLAG_SUBARU_SNG
+
     ret.steerLimitTimer = 0.4
     ret.steerActuatorDelay = 0.1
 

selfdrive/car/subaru/subarucan.py

@@ -291,3 +291,67 @@ def create_preglobal_es_distance(packer, cruise_button, es_distance_msg):
   values["Checksum"] = subaru_preglobal_checksum(packer, values, "ES_Distance")
 
   return packer.make_can_msg("ES_Distance", CanBus.main, values)
+
+
+def create_brake_pedal(packer, brake_pedal_msg, speed_cmd, brake_cmd):
+  values = {s: brake_pedal_msg[s] for s in [
+    "COUNTER",
+    "Signal1",
+    "Speed",
+    "Signal2",
+    "Brake_Lights",
+    "Signal3",
+    "Brake_Pedal",
+    "Signal4",
+  ]}
+
+  if speed_cmd:
+    values["Speed"] = 3
+  if brake_cmd:
+    values["Brake_Pedal"] = 5
+    values["Brake_Lights"] = 1
+
+  return packer.make_can_msg("Brake_Pedal", CanBus.camera, values)
+
+
+def create_throttle(packer, throttle_msg, throttle_cmd):
+  values = {s: throttle_msg[s] for s in [
+    "CHECKSUM",
+    "COUNTER",
+    "Signal1",
+    "Engine_RPM",
+    "Signal2",
+    "Throttle_Pedal",
+    "Throttle_Cruise",
+    "Throttle_Combo",
+    "Signal3",
+    "Off_Accel",
+  ]}
+
+  if throttle_cmd:
+    values["Throttle_Pedal"] = 5
+
+  return packer.make_can_msg("Throttle", 2, values)
+
+
+def create_preglobal_throttle(packer, throttle_msg, throttle_cmd):
+  values = {s: throttle_msg[s] for s in [
+    "Throttle_Pedal",
+    "COUNTER",
+    "Signal1",
+    "Not_Full_Throttle",
+    "Signal2",
+    "Engine_RPM",
+    "Off_Throttle",
+    "Signal3",
+    "Throttle_Cruise",
+    "Throttle_Combo",
+    "Throttle_Body",
+    "Off_Throttle_2",
+    "Signal4",
+  ]}
+
+  if throttle_cmd:
+    values["Throttle_Pedal"] = 5
+
+  return packer.make_can_msg("Throttle", 2, values)

selfdrive/car/subaru/values.py

@@ -60,6 +60,10 @@ class SubaruFlags(IntFlag):
   SEND_INFOTAINMENT = 1
 
 
+class SubaruFlagsSP(IntFlag):
+  SP_SUBARU_SNG = 1
+
+
 class CanBus:
   main = 0
   alt = 1

selfdrive/ui/qt/offroad/sunnypilot_settings.cc

@@ -562,6 +562,17 @@ SPVehiclesTogglesPanel::SPVehiclesTogglesPanel(SPVehiclesPanel *parent) : ListWi
   hkgSmoothStop->setConfirmation(true, false);
   addItem(hkgSmoothStop);
 
+  // Subaru
+  addItem(new LabelControl(tr("Subaru")));
+  auto subaruManualParkingBrakeSng = new ParamControl(
+    "SubaruManualParkingBrakeSng",
+    tr("Manual Parking Brake: Stop and Go (Beta)"),
+    tr("Experimental feature to enable stop and go for Subaru Global models with manual handbrake. Models with electric parking brake should keep this disabled. Thanks to martinl for this implementation!"),
+    "../assets/offroad/icon_blank.png"
+  );
+  subaruManualParkingBrakeSng->setConfirmation(true, false);
+  addItem(subaruManualParkingBrakeSng);
+
   // Toyota/Lexus
   addItem(new LabelControl(tr("Toyota/Lexus")));
   stockLongToyota = new ParamControl(