Subaru: stop and go autoresume

.gitmodules

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

selfdrive/car/subaru/carcontroller.py

@@ -15,7 +15,10 @@ def __init__(self, dbc_name, CP, VM):
     self.es_dashstatus_cnt = -1
     self.infotainmentstatus_cnt = -1
     self.cruise_button_prev = 0
+    self.prev_cruise_state = 0
     self.last_cancel_frame = 0
+    self.throttle_cnt = -1
+    self.manual_hold = False
 
     self.p = CarControllerParams(CP)
     self.packer = CANPacker(DBC[CP.carFingerprint]['pt'])
@@ -47,6 +50,17 @@ def update(self, CC, CS, now_nanos):
 
       self.apply_steer_last = apply_steer
 
+    # *** stop and go ***
+
+    if self.CP.carFingerprint not in PREGLOBAL_CARS:
+      # 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
+      if not CS.out.standstill:
+        self.manual_hold = False
+      self.prev_cruise_state = CS.cruise_state
+
+    throttle_cmd = True if CC.enabled and CC.cruiseControl.resume and not self.manual_hold else False
 
     # *** alerts and pcm cancel ***
 
@@ -70,6 +84,10 @@ def update(self, CC, CS, now_nanos):
         can_sends.append(subarucan.create_preglobal_es_distance(self.packer, cruise_button, CS.es_distance_msg))
         self.es_distance_cnt = CS.es_distance_msg["COUNTER"]
 
+      if self.throttle_cnt != CS.throttle_msg["COUNTER"]:
+        can_sends.append(subarucan.create_preglobal_throttle(self.packer, CS.throttle_msg, throttle_cmd))
+        self.throttle_cnt = CS.throttle_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
@@ -90,6 +108,10 @@ def update(self, CC, CS, now_nanos):
         can_sends.append(subarucan.create_infotainmentstatus(self.packer, CS.es_infotainmentstatus_msg, hud_control.visualAlert))
         self.infotainmentstatus_cnt = CS.es_infotainmentstatus_msg["COUNTER"]
 
+      if self.throttle_cnt != CS.throttle_msg["COUNTER"]:
+        can_sends.append(subarucan.create_throttle(self.packer, CS.throttle_msg, throttle_cmd))
+        self.throttle_cnt = CS.throttle_msg["COUNTER"]
+
     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

@@ -78,8 +78,11 @@ def update(self, cp, cp_cam, cp_body):
       ret.cruiseState.standstill = cp_cam.vl["ES_DashStatus"]["Cruise_State"] == 3
       ret.stockFcw = cp_cam.vl["ES_LKAS_State"]["LKAS_Alert"] == 2
       self.es_lkas_state_msg = copy.copy(cp_cam.vl["ES_LKAS_State"])
+      self.cruise_state = cp_cam.vl["ES_DashStatus"]["Cruise_State"]
 
+    self.throttle_msg = copy.copy(cp.vl["Throttle"])
     cp_es_distance = cp_body if self.car_fingerprint in GLOBAL_GEN2 else cp_cam
+    self.car_follow = cp_es_distance.vl["ES_Distance"]["Car_Follow"]
     self.es_distance_msg = copy.copy(cp_es_distance.vl["ES_Distance"])
     self.es_dashstatus_msg = copy.copy(cp_cam.vl["ES_DashStatus"])
     if self.CP.flags & SubaruFlags.SEND_INFOTAINMENT:
@@ -180,6 +183,16 @@ def get_can_parser(CP):
         checks += CarState.get_common_global_signals()[1]
 
       signals += [
+        ("COUNTER", "Throttle"),
+        ("Signal1", "Throttle"),
+        ("Engine_RPM", "Throttle"),
+        ("Signal2", "Throttle"),
+        ("Throttle_Pedal", "Throttle"),
+        ("Throttle_Cruise", "Throttle"),
+        ("Throttle_Combo", "Throttle"),
+        ("Signal1", "Throttle"),
+        ("Off_Accel", "Throttle"),
+
         ("Steer_Warning", "Steering_Torque"),
         ("UNITS", "Dashlights"),
       ]
@@ -190,6 +203,20 @@ def get_can_parser(CP):
       ]
     else:
       signals += [
+        ("Throttle_Pedal", "Throttle"),
+        ("COUNTER", "Throttle"),
+        ("Signal1", "Throttle"),
+        ("Not_Full_Throttle", "Throttle"),
+        ("Signal2", "Throttle"),
+        ("Engine_RPM", "Throttle"),
+        ("Off_Throttle", "Throttle"),
+        ("Signal3", "Throttle"),
+        ("Throttle_Cruise", "Throttle"),
+        ("Throttle_Combo", "Throttle"),
+        ("Throttle_Body", "Throttle"),
+        ("Off_Throttle_2", "Throttle"),
+        ("Signal4", "Throttle"),
+
         ("FL", "Wheel_Speeds"),
         ("FR", "Wheel_Speeds"),
         ("RL", "Wheel_Speeds"),

selfdrive/car/subaru/subarucan.py

@@ -67,7 +67,6 @@ def create_es_lkas_state(packer, es_lkas_state_msg, enabled, visual_alert, left_
 
   return packer.make_can_msg("ES_LKAS_State", 0, values)
 
-
 def create_es_dashstatus(packer, dashstatus_msg):
   values = copy.copy(dashstatus_msg)
 
@@ -77,7 +76,6 @@ def create_es_dashstatus(packer, dashstatus_msg):
 
   return packer.make_can_msg("ES_DashStatus", 0, values)
 
-
 def create_infotainmentstatus(packer, infotainmentstatus_msg, visual_alert):
   # Filter stock LKAS disabled and Keep hands on steering wheel OFF alerts
   if infotainmentstatus_msg["LKAS_State_Infotainment"] in (3, 4):
@@ -93,6 +91,13 @@ def create_infotainmentstatus(packer, infotainmentstatus_msg, visual_alert):
 
   return packer.make_can_msg("INFOTAINMENT_STATUS", 0, infotainmentstatus_msg)
 
+def create_throttle(packer, throttle_msg, throttle_cmd):
+
+  values = copy.copy(throttle_msg)
+  if throttle_cmd:
+    values["Throttle_Pedal"] = 5
+
+  return packer.make_can_msg("Throttle", 2, values)
 
 # *** Subaru Pre-global ***
 
@@ -118,3 +123,13 @@ 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", 0, values)
+
+def create_preglobal_throttle(packer, throttle_msg, throttle_cmd):
+
+  values = copy.copy(throttle_msg)
+  if throttle_cmd:
+    values["Throttle_Pedal"] = 5
+
+  values["Checksum"] = subaru_preglobal_checksum(packer, values, "Throttle")
+
+  return packer.make_can_msg("Throttle", 2, values)