"Stop-and-Go" for Subaru

Co-Authored-By: martinl <148686+martinl@users.noreply.github.com>

frogpilot/common/frogpilot_variables.py

@@ -729,6 +729,8 @@ class FrogPilotVariables:
     toggle.startup_alert_top = self.get_value("StartupMessageTop", cast=str, default="")
     toggle.startup_alert_bottom = self.get_value("StartupMessageBottom", cast=str, default="")
 
+    toggle.subaru_sng = self.get_value("SubaruSNG", condition=toggle.car_make == "subaru" and not (CP.flags & SubaruFlags.GLOBAL_GEN2 or CP.flags & SubaruFlags.HYBRID))
+
     toggle.tethering_config = self.get_value("TetheringEnabled", cast=float)
 
     toyota_doors = self.get_value("ToyotaDoors", condition=toggle.car_make == "toyota")

opendbc_repo/opendbc/car/subaru/carcontroller.py

@@ -12,6 +12,8 @@ MAX_STEER_RATE = 25  # deg/s
 MAX_STEER_RATE_FRAMES = 7  # tx control frames needed before torque can be cut
 
 # FrogPilot variables
+_SNG_ACC_MIN_DIST = 3
+_SNG_ACC_MAX_DIST = 4.5
 
 
 class CarController(CarControllerBase):
@@ -26,6 +28,14 @@ class CarController(CarControllerBase):
     self.packer = CANPacker(DBC[CP.carFingerprint][Bus.pt])
 
     # FrogPilot variables
+    self.manual_hold = False
+    self.prev_standstill = False
+    self.sng_acc_resume = False
+
+    self.prev_close_distance = 0
+    self.prev_cruise_state = 0
+    self.sng_acc_resume_cnt = 0
+    self.standstill_start = 0
 
   def update(self, CC, CS, now_nanos, frogpilot_toggles):
     actuators = CC.actuators
@@ -62,6 +72,9 @@ class CarController(CarControllerBase):
       self.apply_torque_last = apply_torque
 
     # FrogPilot variables
+    # *** stop and go ***
+    if frogpilot_toggles.subaru_sng:
+      throttle_cmd, speed_cmd = self.stop_and_go(CC, CS)
 
     # *** longitudinal ***
 
@@ -100,6 +113,8 @@ class CarController(CarControllerBase):
         can_sends.append(subarucan.create_preglobal_es_distance(self.packer, cruise_button, CS.es_distance_msg))
 
       # FrogPilot variables
+      if frogpilot_toggles.subaru_sng:
+        can_sends.append(subarucan.create_preglobal_throttle(self.packer, CS.throttle_msg["COUNTER"] + 1, 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,
@@ -113,6 +128,10 @@ class CarController(CarControllerBase):
           can_sends.append(subarucan.create_es_infotainment(self.packer, self.frame // 10, CS.es_infotainment_msg, hud_control.visualAlert))
 
       # FrogPilot variables
+      if frogpilot_toggles.subaru_sng:
+        can_sends.append(subarucan.create_throttle(self.packer, CS.throttle_msg["COUNTER"] + 1, CS.throttle_msg, throttle_cmd))
+        if self.frame % 2 == 0:
+          can_sends.append(subarucan.create_brake_pedal(self.packer, self.frame // 2, CS.brake_pedal_msg, speed_cmd, pcm_cancel_cmd))
 
       if self.CP.openpilotLongitudinalControl:
         if self.frame % 5 == 0:
@@ -153,3 +172,49 @@ class CarController(CarControllerBase):
     return new_actuators, can_sends
 
   # FrogPilot variables
+  def stop_and_go(self, CC, CS, speed_cmd=False, throttle_cmd=False):
+    if self.CP.flags & SubaruFlags.PREGLOBAL:
+      trigger_resume = CC.enabled
+      trigger_resume &= CS.car_follow == 1
+      trigger_resume &= CS.close_distance > self.prev_close_distance
+      trigger_resume &= CS.out.standstill
+      trigger_resume &= _SNG_ACC_MIN_DIST < CS.close_distance < _SNG_ACC_MAX_DIST
+
+      if trigger_resume:
+        self.sng_acc_resume = True
+    else:
+      if CS.car_follow == 0 and CS.cruise_state == 3 and CS.out.standstill and self.prev_cruise_state == 1:
+        self.manual_hold = True
+
+      if not CS.out.standstill:
+        self.manual_hold = False
+
+      trigger_resume = CC.enabled
+      trigger_resume &= CS.car_follow == 1
+      trigger_resume &= CS.close_distance > self.prev_close_distance
+      trigger_resume &= CS.cruise_state == 3
+      trigger_resume &= not self.manual_hold
+      trigger_resume &= _SNG_ACC_MIN_DIST < CS.close_distance < _SNG_ACC_MAX_DIST
+
+      if trigger_resume:
+        self.sng_acc_resume = True
+
+      if CC.enabled and CS.car_follow == 1 and CS.out.standstill and self.frame > self.standstill_start + 50:
+        speed_cmd = 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

opendbc_repo/opendbc/car/subaru/carstate.py

@@ -127,6 +127,13 @@ class CarState(CarStateBase):
     # FrogPilot variables
     fp_ret = custom.FrogPilotCarState.new_message()
 
+    if frogpilot_toggles.subaru_sng:
+      self.brake_pedal_msg = copy.copy(cp.vl["Brake_Pedal"])
+      self.car_follow = cp_es_distance.vl["ES_Distance"]["Car_Follow"]
+      self.close_distance = cp_es_distance.vl["ES_Distance"]["Close_Distance"]
+      self.cruise_state = cp_cam.vl["ES_DashStatus"]["Cruise_State"]
+      self.throttle_msg = copy.copy(cp.vl["Throttle"])
+
     return ret, fp_ret
 
   @staticmethod

opendbc_repo/opendbc/car/subaru/interface.py

@@ -19,7 +19,7 @@ class CarInterface(CarInterfaceBase):
     # - to find the Cruise_Activated bit from the car
     # - proper panda safety setup (use the correct cruise_activated bit, throttle from Throttle_Hybrid, etc)
     ret.dashcamOnly = bool(ret.flags & (SubaruFlags.PREGLOBAL | SubaruFlags.LKAS_ANGLE | SubaruFlags.HYBRID))
-    ret.autoResumeSng = False
+    ret.autoResumeSng = not (ret.flags & SubaruFlags.GLOBAL_GEN2 or ret.flags & SubaruFlags.HYBRID)
 
     # Detect infotainment message sent from the camera
     if not (ret.flags & SubaruFlags.PREGLOBAL) and 0x323 in fingerprint[2]:

opendbc_repo/opendbc/car/subaru/subarucan.py

@@ -337,3 +337,68 @@ def subaru_checksum(address: int, sig, d: bytearray) -> int:
 
 
 # FrogPilot variables
+def create_brake_pedal(packer, frame, brake_pedal_msg, speed_cmd, brake_cmd):
+  values = {s: brake_pedal_msg[s] for s in sorted([
+    "Brake_Lights",
+    "Brake_Pedal",
+    "Signal1",
+    "Signal2",
+    "Signal3",
+    "Signal4",
+    "Speed",
+  ])}
+
+  values["COUNTER"] = frame % 0x10
+
+  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, frame, throttle_msg, throttle_cmd):
+  values = {s: throttle_msg[s] for s in sorted([
+    "CHECKSUM",
+    "Engine_RPM",
+    "Off_Accel",
+    "Signal1",
+    "Signal2",
+    "Signal3",
+    "Throttle_Combo",
+    "Throttle_Cruise",
+    "Throttle_Pedal",
+  ])}
+
+  values["COUNTER"] = frame % 0x10
+
+  if throttle_cmd:
+    values["Throttle_Pedal"] = 5
+
+  return packer.make_can_msg("Throttle", 2, values)
+
+
+def create_preglobal_throttle(packer, frame, throttle_msg, throttle_cmd):
+  values = {s: throttle_msg[s] for s in sorted([
+    "Engine_RPM",
+    "Not_Full_Throttle",
+    "Off_Throttle",
+    "Off_Throttle_2",
+    "Signal1",
+    "Signal2",
+    "Signal3",
+    "Signal4",
+    "Throttle_Body",
+    "Throttle_Combo",
+    "Throttle_Cruise",
+    "Throttle_Pedal",
+  ])}
+
+  values["COUNTER"] = frame % 0x10
+
+  if throttle_cmd:
+    values["Throttle_Pedal"] = 5
+
+  return packer.make_can_msg("Throttle", 2, values)