Legacy and Outback port

README.md

@@ -98,6 +98,8 @@ Supported Cars
 | Lexus                | IS Hybrid 2017             | All                  | Yes     | Stock          | 0mph             | 0mph           | Toyota            |
 | Subaru               | Crosstrek 2018             | EyeSight             | Yes     | Stock          | 0mph             | 0mph           | Custom<sup>4</sup>|
 | Subaru               | Impreza 2019               | EyeSight             | Yes     | Stock          | 0mph             | 0mph           | Custom<sup>4</sup>|
+| Subaru               | Outback 2015-17            | EyeSight             | Yes     | Stock          | 0mph             | 0mph           | Custom<sup>4</sup>|
+| Subaru               | Legacy 2015-17             | EyeSight             | Yes     | Stock          | 0mph             | 0mph           | Custom<sup>4</sup>|
 | Toyota               | Avalon 2016                | TSS-P                | Yes     | Yes<sup>2</sup>| 20mph<sup>1</sup>| 0mph           | Toyota            |
 | Toyota               | Avalon 2017-18             | All                  | Yes     | Yes<sup>2</sup>| 20mph<sup>1</sup>| 0mph           | Toyota            |
 | Toyota               | Camry 2018-19              | All                  | Yes     | Stock          | 0mph<sup>5</sup> | 0mph           | Toyota            |

selfdrive/car/subaru/carcontroller.py

@@ -11,11 +11,9 @@ def __init__(self, car_fingerprint):
     self.STEER_STEP = 2                # how often we update the steer cmd
     self.STEER_DELTA_UP = 50           # torque increase per refresh, 0.8s to max
     self.STEER_DELTA_DOWN = 70         # torque decrease per refresh
-    if car_fingerprint == CAR.IMPREZA:
-      self.STEER_DRIVER_ALLOWANCE = 60   # allowed driver torque before start limiting
-      self.STEER_DRIVER_MULTIPLIER = 10   # weight driver torque heavily
-      self.STEER_DRIVER_FACTOR = 1     # from dbc
-
+    self.STEER_DRIVER_FACTOR = 1       # from dbc
+    self.STEER_DRIVER_ALLOWANCE = 60   # allowed driver torque before start limiting
+    self.STEER_DRIVER_MULTIPLIER = 10  # weight driver torque heavily
 
 
 class CarController(object):
@@ -26,6 +24,7 @@ def __init__(self, car_fingerprint):
     self.car_fingerprint = car_fingerprint
     self.es_distance_cnt = -1
     self.es_lkas_cnt = -1
+    self.counter = 0
 
     # Setup detection helper. Routes commands to
     # an appropriate CAN bus number.
@@ -44,30 +43,36 @@ def update(self, enabled, CS, frame, actuators, pcm_cancel_cmd, visual_alert, le
     ### STEER ###
 
     if (frame % P.STEER_STEP) == 0:
-
       final_steer = actuators.steer if enabled else 0.
       apply_steer = int(round(final_steer * P.STEER_MAX))
 
       # limits due to driver torque
-
-      apply_steer = int(round(apply_steer))
       apply_steer = apply_std_steer_torque_limits(apply_steer, self.apply_steer_last, CS.steer_torque_driver, P)
 
-      lkas_enabled = enabled and not CS.steer_not_allowed
+      if self.car_fingerprint in (CAR.OUTBACK, CAR.LEGACY):
 
-      if not lkas_enabled:
-        apply_steer = 0
+        # add noise to prevent lkas fault from constant torque value for over 1s
+        if enabled and apply_steer == self.apply_steer_last:
+          self.counter =+ 1
+          if self.counter == 50:
+            apply_steer = int(round(apply_steer * 0.99))
+        else:
+          self.counter = 0
 
       can_sends.append(subarucan.create_steering_control(self.packer, CS.CP.carFingerprint, apply_steer, frame, P.STEER_STEP))
 
       self.apply_steer_last = apply_steer
 
-    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))
-      self.es_distance_cnt = CS.es_distance_msg["Counter"]
+    if self.car_fingerprint == CAR.IMPREZA:
+      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))
+        self.es_distance_cnt = CS.es_distance_msg["Counter"]
+
+      if self.es_lkas_cnt != CS.es_lkas_msg["Counter"]:
+        can_sends.append(subarucan.create_es_lkas(self.packer, CS.es_lkas_msg, visual_alert, left_line, right_line))
+        self.es_lkas_cnt = CS.es_lkas_msg["Counter"]
 
-    if self.es_lkas_cnt != CS.es_lkas_msg["Counter"]:
-      can_sends.append(subarucan.create_es_lkas(self.packer, CS.es_lkas_msg, visual_alert, left_line, right_line))
-      self.es_lkas_cnt = CS.es_lkas_msg["Counter"]
+    if self.car_fingerprint in (CAR.OUTBACK, CAR.LEGACY) and pcm_cancel_cmd:
+      can_sends.append(subarucan.create_door_control(self.packer))
 
     return can_sends

selfdrive/car/subaru/carstate.py

@@ -2,7 +2,7 @@
 from common.kalman.simple_kalman import KF1D
 from selfdrive.config import Conversions as CV
 from selfdrive.can.parser import CANParser
-from selfdrive.car.subaru.values import DBC, STEER_THRESHOLD
+from selfdrive.car.subaru.values import CAR, DBC, STEER_THRESHOLD
 
 def get_powertrain_can_parser(CP):
   # this function generates lists for signal, messages and initial values
@@ -25,60 +25,71 @@ def get_powertrain_can_parser(CP):
     ("DOOR_OPEN_FL", "BodyInfo", 1),
     ("DOOR_OPEN_RR", "BodyInfo", 1),
     ("DOOR_OPEN_RL", "BodyInfo", 1),
-    ("Units", "Dash_State", 1),
   ]
 
   checks = [
     # sig_address, frequency
     ("Dashlights", 10),
-    ("CruiseControl", 20),
     ("Wheel_Speeds", 50),
     ("Steering_Torque", 50),
-    ("BodyInfo", 10),
   ]
 
-  return CANParser(DBC[CP.carFingerprint]['pt'], signals, checks, 0)
+  if CP.carFingerprint == CAR.IMPREZA:
+    signals += [
+      ("Units", "Dash_State", 1),
+    ]
+    checks += [
+      ("BodyInfo", 10),
+      ("CruiseControl", 20),
+    ]
+
+  else:
+    signals += [
+      ("LKA_Lockout", "Steering_Torque", 0),
+    ]
+    checks += [
+      ("CruiseControl", 50),
+    ]
 
+  return CANParser(DBC[CP.carFingerprint]['pt'], signals, checks, 0)
 
 def get_camera_can_parser(CP):
   signals = [
     ("Cruise_Set_Speed", "ES_DashStatus", 0),
-
-    ("Counter", "ES_Distance", 0),
-    ("Signal1", "ES_Distance", 0),
-    ("Signal2", "ES_Distance", 0),
-    ("Main", "ES_Distance", 0),
-    ("Signal3", "ES_Distance", 0),
-
-    ("Checksum", "ES_LKAS_State", 0),
-    ("Counter", "ES_LKAS_State", 0),
-    ("Keep_Hands_On_Wheel", "ES_LKAS_State", 0),
-    ("Empty_Box", "ES_LKAS_State", 0),
-    ("Signal1", "ES_LKAS_State", 0),
-    ("LKAS_ACTIVE", "ES_LKAS_State", 0),
-    ("Signal2", "ES_LKAS_State", 0),
-    ("Backward_Speed_Limit_Menu", "ES_LKAS_State", 0),
-    ("LKAS_ENABLE_3", "ES_LKAS_State", 0),
-    ("Signal3", "ES_LKAS_State", 0),
-    ("LKAS_ENABLE_2", "ES_LKAS_State", 0),
-    ("Signal4", "ES_LKAS_State", 0),
-    ("LKAS_Left_Line_Visible", "ES_LKAS_State", 0),
-    ("Signal6", "ES_LKAS_State", 0),
-    ("LKAS_Right_Line_Visible", "ES_LKAS_State", 0),
-    ("Signal7", "ES_LKAS_State", 0),
-    ("FCW_Cont_Beep", "ES_LKAS_State", 0),
-    ("FCW_Repeated_Beep", "ES_LKAS_State", 0),
-    ("Throttle_Management_Activated", "ES_LKAS_State", 0),
-    ("Traffic_light_Ahead", "ES_LKAS_State", 0),
-    ("Right_Depart", "ES_LKAS_State", 0),
-    ("Signal5", "ES_LKAS_State", 0),
-
   ]
 
   checks = [
     ("ES_DashStatus", 10),
   ]
 
+  if CP.carFingerprint == CAR.IMPREZA:
+    signals += [
+      ("Counter", "ES_Distance", 0),
+      ("Signal1", "ES_Distance", 0),
+      ("Signal2", "ES_Distance", 0),
+      ("Main", "ES_Distance", 0),
+      ("Signal3", "ES_Distance", 0),
+
+      ("Checksum", "ES_LKAS_State", 0),
+      ("Counter", "ES_LKAS_State", 0),
+      ("Keep_Hands_On_Wheel", "ES_LKAS_State", 0),
+      ("Empty_Box", "ES_LKAS_State", 0),
+      ("Signal1", "ES_LKAS_State", 0),
+      ("LKAS_ACTIVE", "ES_LKAS_State", 0),
+      ("Signal2", "ES_LKAS_State", 0),
+      ("Backward_Speed_Limit_Menu", "ES_LKAS_State", 0),
+      ("LKAS_ENABLE_3", "ES_LKAS_State", 0),
+      ("Signal3", "ES_LKAS_State", 0),
+      ("LKAS_ENABLE_2", "ES_LKAS_State", 0),
+      ("Signal4", "ES_LKAS_State", 0),
+      ("FCW_Cont_Beep", "ES_LKAS_State", 0),
+      ("FCW_Repeated_Beep", "ES_LKAS_State", 0),
+      ("Throttle_Management_Activated", "ES_LKAS_State", 0),
+      ("Traffic_light_Ahead", "ES_LKAS_State", 0),
+      ("Right_Depart", "ES_LKAS_State", 0),
+      ("Signal5", "ES_LKAS_State", 0),
+    ]
+
   return CANParser(DBC[CP.carFingerprint]['pt'], signals, checks, 2)
 
 
@@ -118,12 +129,9 @@ def update(self, cp, cp_cam):
     self.v_wheel_rr = cp.vl["Wheel_Speeds"]['RR'] * CV.KPH_TO_MS
 
     self.v_cruise_pcm = cp_cam.vl["ES_DashStatus"]['Cruise_Set_Speed']
-    # 1 = imperial, 6 = metric
-    if cp.vl["Dash_State"]['Units'] == 1:
-      self.v_cruise_pcm *= CV.MPH_TO_KPH
 
     v_wheel = (self.v_wheel_fl + self.v_wheel_fr + self.v_wheel_rl + self.v_wheel_rr) / 4.
-    # Kalman filter, even though Hyundai raw wheel speed is heaviliy filtered by default
+    # Kalman filter, even though Subaru raw wheel speed is heaviliy filtered by default
     if abs(v_wheel - self.v_ego) > 2.0:  # Prevent large accelerations when car starts at non zero speed
       self.v_ego_kf.x = [[v_wheel], [0.0]]
 
@@ -149,5 +157,14 @@ def update(self, cp, cp_cam):
       cp.vl["BodyInfo"]['DOOR_OPEN_FR'],
       cp.vl["BodyInfo"]['DOOR_OPEN_FL']])
 
-    self.es_distance_msg = copy.copy(cp_cam.vl["ES_Distance"])
-    self.es_lkas_msg = copy.copy(cp_cam.vl["ES_LKAS_State"])
+    if self.car_fingerprint == CAR.IMPREZA:
+      self.v_cruise_pcm = cp_cam.vl["ES_DashStatus"]["Cruise_Set_Speed"] * CV.MPH_TO_KPH
+      self.steer_not_allowed = 0
+      self.es_distance_msg = copy.copy(cp_cam.vl["ES_Distance"])
+      self.es_lkas_msg = copy.copy(cp_cam.vl["ES_LKAS_State"])
+      # 1 = imperial, 6 = metric
+      if cp.vl["Dash_State"]['Units'] == 1:
+        self.v_cruise_pcm *= CV.MPH_TO_KPH     
+    else:
+      self.v_cruise_pcm = cp_cam.vl["ES_DashStatus"]["Cruise_Set_Speed"]
+      self.steer_not_allowed = cp.vl["Steering_Torque"]["LKA_Lockout"]

selfdrive/car/subaru/interface.py

@@ -67,9 +67,33 @@ def get_params(candidate, fingerprint, vin="", is_panda_black=False):
       ret.steerMaxBP = [0.] # m/s
       ret.steerMaxV = [1.]
 
+    if candidate in [CAR.OUTBACK]:
+      ret.mass = 1568 + STD_CARGO_KG
+      ret.wheelbase = 2.67
+      ret.centerToFront = ret.wheelbase * 0.5
+      ret.steerRatio = 20           # learned, 14 stock
+      ret.steerActuatorDelay = 0.2
+      ret.steerRateCost = 0.4
+      ret.lateralTuning.pid.kf = 0.00005
+      ret.lateralTuning.pid.kiBP, ret.lateralTuning.pid.kpBP = [[0., 10., 20.], [0., 10., 20.]]
+      ret.lateralTuning.pid.kpV, ret.lateralTuning.pid.kiV = [[0.01, 0.05, 0.18], [0.003, 0.018, 0.025]]
+      ret.steerMaxBP = [0.] # m/s
+      ret.steerMaxV = [1.]
+
+    if candidate in [CAR.LEGACY]:
+      ret.mass = 1568 + STD_CARGO_KG
+      ret.wheelbase = 2.67
+      ret.centerToFront = ret.wheelbase * 0.5
+      ret.steerRatio = 12.5   #14.5 stock
+      ret.steerActuatorDelay = 0.15
+      ret.lateralTuning.pid.kf = 0.00005
+      ret.lateralTuning.pid.kiBP, ret.lateralTuning.pid.kpBP = [[0., 20.], [0., 20.]]
+      ret.lateralTuning.pid.kpV, ret.lateralTuning.pid.kiV = [[0.1, 0.2], [0.01, 0.02]]
+      ret.steerMaxBP = [0.] # m/s
+      ret.steerMaxV = [1.]
+
     ret.steerControlType = car.CarParams.SteerControlType.torque
     ret.steerRatioRear = 0.
-    # testing tuning
 
     # No long control in subaru
     ret.gasMaxBP = [0.]
@@ -161,6 +185,9 @@ def update(self, c, can_strings):
 
 
     events = []
+    if self.CS.steer_not_allowed:
+      events.append(create_event('steerUnavailable', [ET.NO_ENTRY, ET.IMMEDIATE_DISABLE, ET.PERMANENT]))
+
     if ret.seatbeltUnlatched:
       events.append(create_event('seatbeltNotLatched', [ET.NO_ENTRY, ET.SOFT_DISABLE]))
 

selfdrive/car/subaru/subarucan.py

@@ -23,6 +23,28 @@ def create_steering_control(packer, car_fingerprint, apply_steer, frame, steer_s
     }
     values["Checksum"] = subaru_checksum(packer, values, 0x122)
 
+  if car_fingerprint in (CAR.OUTBACK, CAR.LEGACY):
+
+    if apply_steer != 0:
+      chksm_steer = apply_steer * -1
+      chksm_engage = 1
+    else:
+      chksm_steer = 0
+      chksm_engage = 0
+
+    #counts from 0 to 7 then back to 0
+    idx = (frame / steer_step) % 8
+    steer2 = (chksm_steer >> 8) & 0x1F
+    steer1 =  chksm_steer - (steer2 << 8)
+    checksum = (idx + steer2 + steer1 + chksm_engage) % 256
+
+    values = {
+      "Counter": idx,
+      "LKAS_Command": apply_steer,
+      "LKAS_Active": 1 if apply_steer != 0 else 0,
+      "Checksum": checksum
+    }
+
   return packer.make_can_msg("ES_LKAS", 0, values)
 
 def create_steering_status(packer, car_fingerprint, apply_steer, frame, steer_step):
@@ -55,3 +77,10 @@ def create_es_lkas(packer, es_lkas_msg, visual_alert, left_line, right_line):
   values["Checksum"] = subaru_checksum(packer, values, 802)
 
   return packer.make_can_msg("ES_LKAS_State", 0, values)
+
+def create_door_control(packer):
+  values = {
+    "DOOR_OPEN_FR": 1,
+    "_UNKNOWN": 5,
+  }
+  return packer.make_can_msg("BodyInfo", 2, values)

selfdrive/car/subaru/values.py

@@ -2,21 +2,44 @@
 
 class CAR:
   IMPREZA = "SUBARU IMPREZA LIMITED 2019"
+  OUTBACK = "SUBARU OUTBACK 2015 - 2017"
+  LEGACY = "SUBARU LEGACY 2015 - 2017"
+
 
 FINGERPRINTS = {
   CAR.IMPREZA: [{
     2: 8, 64: 8, 65: 8, 72: 8, 73: 8, 280: 8, 281: 8, 290: 8, 312: 8, 313: 8, 314: 8, 315: 8, 316: 8, 326: 8, 544: 8, 545: 8, 546: 8, 552: 8, 554: 8, 557: 8, 576: 8, 577: 8, 722: 8, 801: 8, 802: 8, 805: 8, 808: 8, 816: 8, 826: 8, 837: 8, 838: 8, 839: 8, 842: 8, 912: 8, 915: 8, 940: 8, 1614: 8, 1617: 8, 1632: 8, 1650: 8, 1657: 8, 1658: 8, 1677: 8, 1697: 8, 1722: 8, 1743: 8, 1759: 8, 1786: 5, 1787: 5, 1788: 8, 1809: 8, 1813: 8, 1817: 8, 1821: 8, 1840: 8, 1848: 8, 1924: 8, 1932: 8, 1952: 8, 1960: 8
   },
-  # Crosstrek 2018 (same platform as Subaru)
+  # Crosstrek 2018 (same platform as Impreza)
   {
     2: 8, 64: 8, 65: 8, 72: 8, 73: 8, 256: 8, 280: 8, 281: 8, 290: 8, 312: 8, 313: 8, 314: 8, 315: 8, 316: 8, 326: 8, 372: 8, 544: 8, 545: 8, 546: 8, 554: 8, 557: 8, 576: 8, 577: 8, 722: 8, 801: 8, 802: 8, 805: 8, 808: 8, 811: 8, 826: 8, 837: 8, 838: 8, 839: 8, 842: 8, 912: 8, 915: 8, 940: 8, 1614: 8, 1617: 8, 1632: 8, 1650: 8, 1657: 8, 1658: 8, 1677: 8, 1697: 8, 1759: 8, 1786: 5, 1787: 5, 1788: 8
   }],
+  # OUTBACK PREMIUM 2.5i 2015
+  CAR.OUTBACK: [{
+    2: 8, 208: 8, 209: 4, 210: 8, 211: 7, 212: 8, 320: 8, 321: 8, 324: 8, 328: 8, 329: 8, 336: 2, 338: 8, 342: 8, 346: 8, 352: 8, 353: 8, 354: 8, 356: 8, 358: 8, 359: 8, 392: 8, 640: 8, 642: 8, 644: 8, 864: 8, 865: 8, 866: 8, 872: 8, 880: 8, 881: 8, 882: 8, 884: 8, 977: 8, 1632: 8, 1745: 8, 1786: 5
+  }],
+  #LEGACY LIMITED 2.5i 2015
+  CAR.LEGACY: [{
+    2: 8, 208: 8, 209: 4, 210: 8, 211: 7, 212: 8, 320: 8, 321: 8, 324: 8, 328: 8, 329: 8, 336: 2, 338: 8, 342: 8, 352: 8, 353: 8, 354: 8, 356: 8, 358: 8, 359: 8, 392: 8, 604: 8, 640: 8, 642: 8, 644: 8, 864: 8, 865: 8, 866: 8, 872: 8, 880: 8, 881: 8, 882: 8, 884: 8, 977: 8, 1632: 8, 1745: 8, 1786: 5
+  },
+  #LEGACY 2.5i 2017
+  {
+    2: 8, 208: 8, 209: 4, 210: 8, 211: 7, 212: 8, 320: 8, 321: 8, 324: 8, 328: 8, 329: 8, 336: 2, 338: 8, 342: 8, 392: 8, 604: 8, 640: 8, 642: 8, 864: 8, 865: 8, 866: 8, 872: 8, 880: 8, 881: 8, 882: 8, 884: 8, 885: 8, 977: 8, 1632: 8, 1640: 8, 1736: 8, 1745: 8, 1785: 5, 1786: 5, 1787: 5, 1788: 8
+  },
+  #LEGACY 2018
+  {
+    2: 8, 208: 8, 209: 4, 210: 8, 211: 7, 212: 8, 316: 8, 320: 8, 321: 8, 324: 8, 328: 8, 329: 8, 336: 2, 338: 8, 342: 8, 392: 8, 604: 8, 640: 8, 642: 8, 864: 8, 865: 8, 866: 8, 872: 8, 880: 8, 881: 8, 882: 8, 884: 8, 885: 8, 977: 8, 1614: 8, 1632: 8, 1640: 8, 1657: 8, 1658: 8, 1672: 8, 1722: 8, 1743: 8, 1745: 8, 1778: 8, 1785: 5, 1786: 5, 1787: 5, 1788: 8, 2015: 8, 2016: 8, 2024: 8
+  }],
 }
 
 STEER_THRESHOLD = {
   CAR.IMPREZA: 80,
+  CAR.OUTBACK: 350,
+  CAR.LEGACY: 350,
 }
 
 DBC = {
   CAR.IMPREZA: dbc_dict('subaru_global_2017', None),
+  CAR.OUTBACK: dbc_dict('subaru_outback_2015_eyesight', None),
+  CAR.LEGACY: dbc_dict('subaru_outback_2015_eyesight', None),
 }