Add Subaru SNG support

martinl · martinl · commit c249cbd7b7c5 · 2022-01-04T17:13:48.000+02:00
diff --git a/selfdrive/car/subaru/carcontroller.py b/selfdrive/car/subaru/carcontroller.py
@@ -9,8 +9,14 @@ def __init__(self, dbc_name, CP, VM):
     self.apply_steer_last = 0
     self.es_distance_cnt = -1
     self.es_lkas_cnt = -1
+    self.throttle_cnt = -1
     self.cruise_button_prev = 0
     self.steer_rate_limited = False
+    self.sng_acc_resume = False
+    self.sng_acc_resume_cnt = -1
+    self.manual_hold = False
+    self.prev_cruise_state = 0
+    self.prev_close_distance = 0
 
     self.p = CarControllerParams(CP)
     self.packer = CANPacker(DBC[CP.carFingerprint]['pt'])
@@ -40,6 +46,49 @@ def update(self, enabled, CS, frame, actuators, pcm_cancel_cmd, visual_alert, le
 
       self.apply_steer_last = apply_steer
 
+    # *** stop and go ***
+
+    throttle_cmd = False
+
+    if CS.CP.carFingerprint in PREGLOBAL_CARS:
+      if (enabled                                            # ACC active
+          and CS.car_follow == 1                             # lead car
+          and CS.out.standstill                              # must be standing still
+          and CS.close_distance > 3                          # acc resume trigger threshold
+          and CS.close_distance < 4.5                        # max operating distance to filter false positives
+          and CS.close_distance > self.prev_close_distance): # distance with lead car is increasing
+        self.sng_acc_resume = True
+    elif CS.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
+      # Cancel manual hold when car starts moving
+      if not CS.out.standstill:
+        self.manual_hold = False
+      if (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.out.standstill                              # must be standing still
+          and CS.close_distance > 150                        # acc resume trigger threshold
+          and CS.close_distance < 255                        # ignore max value
+          and CS.close_distance > self.prev_close_distance): # distance with lead car is increasing
+        self.sng_acc_resume = True
+      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
+
+    # Cancel ACC if stopped, brake pressed and not stopped behind another car
+    if enabled and CS.out.brakePressed and CS.car_follow == 0 and CS.out.standstill:
+      pcm_cancel_cmd = True
+
+    self.prev_close_distance = CS.close_distance
 
     # *** alerts and pcm cancel ***
 
@@ -63,6 +112,10 @@ def update(self, enabled, CS, frame, actuators, pcm_cancel_cmd, visual_alert, le
         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 self.es_distance_cnt != CS.es_distance_msg["Counter"]:
         can_sends.append(subarucan.create_es_distance(self.packer, CS.es_distance_msg, pcm_cancel_cmd))
@@ -72,6 +125,10 @@ def update(self, enabled, CS, frame, actuators, pcm_cancel_cmd, visual_alert, le
         can_sends.append(subarucan.create_es_lkas(self.packer, CS.es_lkas_msg, enabled, visual_alert, left_line, right_line, left_lane_depart, right_lane_depart))
         self.es_lkas_cnt = CS.es_lkas_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
 
diff --git a/selfdrive/car/subaru/carstate.py b/selfdrive/car/subaru/carstate.py
@@ -71,6 +71,10 @@ def update(self, cp, cp_cam):
       ret.steerWarning = cp.vl["Steering_Torque"]["Steer_Warning"] == 1
       ret.cruiseState.nonAdaptive = cp_cam.vl["ES_DashStatus"]["Conventional_Cruise"] == 1
       self.es_lkas_msg = copy.copy(cp_cam.vl["ES_LKAS_State"])
+      self.cruise_state = cp_cam.vl["ES_DashStatus"]["Cruise_State"]
+    self.car_follow = cp_cam.vl["ES_Distance"]["Car_Follow"]
+    self.close_distance = cp_cam.vl["ES_Distance"]["Close_Distance"]
+    self.throttle_msg = copy.copy(cp.vl["Throttle"])
     self.es_distance_msg = copy.copy(cp_cam.vl["ES_Distance"])
 
     return ret
@@ -85,7 +89,6 @@ def get_can_parser(CP):
       ("Steer_Error_1", "Steering_Torque", 0),
       ("Cruise_On", "CruiseControl", 0),
       ("Cruise_Activated", "CruiseControl", 0),
-      ("Brake_Pedal", "Brake_Pedal", 0),
       ("Throttle_Pedal", "Throttle", 0),
       ("LEFT_BLINKER", "Dashlights", 0),
       ("RIGHT_BLINKER", "Dashlights", 0),
@@ -105,7 +108,6 @@ def get_can_parser(CP):
       # sig_address, frequency
       ("Throttle", 100),
       ("Dashlights", 10),
-      ("Brake_Pedal", 50),
       ("Wheel_Speeds", 50),
       ("Transmission", 100),
       ("Steering_Torque", 50),
@@ -125,6 +127,16 @@ def get_can_parser(CP):
 
     if CP.carFingerprint not in PREGLOBAL_CARS:
       signals += [
+        ("Counter", "Throttle", 0),
+        ("Signal1", "Throttle", 0),
+        ("Engine_RPM", "Throttle", 0),
+        ("Signal2", "Throttle", 0),
+        ("Throttle_Pedal", "Throttle", 0),
+        ("Throttle_Cruise", "Throttle", 0),
+        ("Throttle_Combo", "Throttle", 0),
+        ("Signal1", "Throttle", 0),
+        ("Off_Accel", "Throttle", 0),
+
         ("Steer_Warning", "Steering_Torque", 0),
         ("Brake", "Brake_Status", 0),
         ("UNITS", "Dashlights", 0),
@@ -138,11 +150,27 @@ def get_can_parser(CP):
       ]
     else:
       signals += [
+        ("Throttle_Pedal", "Throttle", 0),
+        ("Counter", "Throttle", 0),
+        ("Signal1", "Throttle", 0),
+        ("Not_Full_Throttle", "Throttle", 0),
+        ("Signal2", "Throttle", 0),
+        ("Engine_RPM", "Throttle", 0),
+        ("Off_Throttle", "Throttle", 0),
+        ("Signal3", "Throttle", 0),
+        ("Throttle_Cruise", "Throttle", 0),
+        ("Throttle_Combo", "Throttle", 0),
+        ("Throttle_Body", "Throttle", 0),
+        ("Off_Throttle_2", "Throttle", 0),
+        ("Signal4", "Throttle", 0),
+
         ("UNITS", "Dash_State2", 0),
+        ("Brake_Pedal", "Brake_Pedal", 0),
       ]
 
       checks += [
         ("Dash_State2", 1),
+        ("Brake_Pedal", 50),
       ]
 
     if CP.carFingerprint == CAR.FORESTER_PREGLOBAL:
@@ -194,6 +222,7 @@ def get_cam_can_parser(CP):
       signals = [
         ("Cruise_Set_Speed", "ES_DashStatus", 0),
         ("Conventional_Cruise", "ES_DashStatus", 0),
+        ("Cruise_State", "ES_DashStatus", 0),
 
         ("Counter", "ES_Distance", 0),
         ("Signal1", "ES_Distance", 0),
diff --git a/selfdrive/car/subaru/subarucan.py b/selfdrive/car/subaru/subarucan.py
@@ -61,6 +61,15 @@ def create_es_lkas(packer, es_lkas_msg, enabled, visual_alert, left_line, right_
 
   return packer.make_can_msg("ES_LKAS_State", 0, values)
 
+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 ***
 
 def subaru_preglobal_checksum(packer, values, addr):
@@ -88,3 +97,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)
