Subaru pre-global: add support for Subaru Legacy 2015-18 (#1805)

* Add support for Subaru Legacy 2015-18

* syntax fix

* Add Legacy 2018 FPv1

* Add Subaru Ascent from upstream

* Use GLOBAL_CAR and LEGACY_CAR lists

* Change LEGACY_2015 to LEGACY_PREGLOBAL

* Add LEGACY_CAR to carstate

* Change LEGACY_2015 to LEGACY_PREGLOBAL in test_car_models

* Add missing SafetyModel to Ascent

* Use GLOBAL_CAR and LEGACY_CAR to set safetyModel

* Change LEGACY_CAR to PREGLOBAL_CARS, remove GLOBAL_CAR

* Fix PREGLOBAL_CARS in carstate and subarucan

* Minor cleanups

* Add accelCruise button event

* Change Preglobal Driver Torque limit to match Global

* Match comments to upstream

* Use Steer_Warning and Steer_Error_1 only for Global

* Change mph units to match upstream values

* Increase Preglobal brakePressed  threshold to 2

* Add DashcamOnly to LEGACY_PREGLOBAL

* Fix typo in variable name

* Update README, add create_preglobal_steering_control

* cleanup carcontroller

* cleanup values

* missed that one

* Update STEER_STEP

* Update STEER_MAX

* Add preglobal signal frequency checks

* remove PREGLOBAL_CARS from subarucan

* Remove whitespace

* Use common frequency checks

* cleanup carstate

* cleanup subarucan

Co-authored-by: Adeeb Shihadeh <adeebshihadeh@gmail.com>

release/files_common

@@ -524,6 +524,7 @@ opendbc/nissan_x_trail_2017.dbc
 opendbc/nissan_leaf_2018.dbc
 
 opendbc/subaru_global_2017_generated.dbc
+opendbc/subaru_outback_2015_generated.dbc
 
 opendbc/toyota_rav4_hybrid_2017_pt_generated.dbc
 opendbc/toyota_rav4_2017_pt_generated.dbc

selfdrive/car/subaru/carcontroller.py

@@ -1,7 +1,6 @@
-#from common.numpy_fast import clip
 from selfdrive.car import apply_std_steer_torque_limits
 from selfdrive.car.subaru import subarucan
-from selfdrive.car.subaru.values import DBC
+from selfdrive.car.subaru.values import DBC, PREGLOBAL_CARS
 from opendbc.can.packer import CANPacker
 
 
@@ -18,43 +17,65 @@ class CarControllerParams():
 
 class CarController():
   def __init__(self, dbc_name, CP, VM):
-    self.lkas_active = False
     self.apply_steer_last = 0
     self.es_distance_cnt = -1
+    self.es_accel_cnt = -1
     self.es_lkas_cnt = -1
+    self.fake_button_prev = 0
     self.steer_rate_limited = False
 
     self.params = CarControllerParams()
     self.packer = CANPacker(DBC[CP.carFingerprint]['pt'])
 
   def update(self, enabled, CS, frame, actuators, pcm_cancel_cmd, visual_alert, left_line, right_line):
-    """ Controls thread """
 
-    P = self.params
-
-    # Send CAN commands.
     can_sends = []
 
-    ### STEER ###
-
-    if (frame % P.STEER_STEP) == 0:
+    # *** steering ***
+    if (frame % self.params.STEER_STEP) == 0:
 
-      final_steer = actuators.steer if enabled else 0.
-      apply_steer = int(round(final_steer * P.STEER_MAX))
+      apply_steer = int(round(actuators.steer * self.params.STEER_MAX))
 
       # limits due to driver torque
 
       new_steer = int(round(apply_steer))
-      apply_steer = apply_std_steer_torque_limits(new_steer, self.apply_steer_last, CS.out.steeringTorque, P)
+      apply_steer = apply_std_steer_torque_limits(new_steer, self.apply_steer_last, CS.out.steeringTorque, self.params)
       self.steer_rate_limited = new_steer != apply_steer
 
       if not enabled:
         apply_steer = 0
 
-      can_sends.append(subarucan.create_steering_control(self.packer, apply_steer, frame, P.STEER_STEP))
+      if CS.CP.carFingerprint in PREGLOBAL_CARS:
+        can_sends.append(subarucan.create_preglobal_steering_control(self.packer, apply_steer, frame, self.params.STEER_STEP))
+      else:
+        can_sends.append(subarucan.create_steering_control(self.packer, apply_steer, frame, self.params.STEER_STEP))
 
       self.apply_steer_last = apply_steer
 
+
+    # *** alerts and pcm cancel ***
+
+    if CS.CP.carFingerprint in PREGLOBAL_CARS:
+      if self.es_accel_cnt != CS.es_accel_msg["Counter"]:
+        # 1 = main, 2 = set shallow, 3 = set deep, 4 = resume shallow, 5 = resume deep
+        # disengage ACC when OP is disengaged
+        if pcm_cancel_cmd:
+          fake_button = 1
+        # turn main on if off and past start-up state
+        elif not CS.out.cruiseState.available and CS.ready:
+          fake_button = 1
+        else:
+          fake_button = CS.button
+
+        # unstick previous mocked button press
+        if fake_button == 1 and self.fake_button_prev == 1:
+          fake_button = 0
+        self.fake_button_prev = fake_button
+
+        can_sends.append(subarucan.create_es_throttle_control(self.packer, fake_button, CS.es_accel_msg))
+        self.es_accel_cnt = CS.es_accel_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))
         self.es_distance_cnt = CS.es_distance_msg["Counter"]

selfdrive/car/subaru/carstate.py

@@ -4,7 +4,7 @@ from opendbc.can.can_define import CANDefine
 from selfdrive.config import Conversions as CV
 from selfdrive.car.interfaces import CarStateBase
 from opendbc.can.parser import CANParser
-from selfdrive.car.subaru.values import DBC, STEER_THRESHOLD
+from selfdrive.car.subaru.values import DBC, STEER_THRESHOLD, CAR, PREGLOBAL_CARS
 
 
 class CarState(CarStateBase):
@@ -20,6 +20,9 @@ class CarState(CarStateBase):
 
     ret.gas = cp.vl["Throttle"]['Throttle_Pedal'] / 255.
     ret.gasPressed = ret.gas > 1e-5
+    if self.car_fingerprint in PREGLOBAL_CARS:
+      ret.brakePressed = cp.vl["Brake_Pedal"]['Brake_Pedal'] > 2
+    else:
       ret.brakePressed = cp.vl["Brake_Pedal"]['Brake_Pedal'] > 1e-5
     ret.brakeLights = ret.brakePressed
 
@@ -48,15 +51,15 @@ class CarState(CarStateBase):
     ret.steeringTorque = cp.vl["Steering_Torque"]['Steer_Torque_Sensor']
     ret.steeringPressed = abs(ret.steeringTorque) > STEER_THRESHOLD[self.car_fingerprint]
 
-    ret.steerError = cp.vl["Steering_Torque"]['Steer_Error_1'] == 1
-    ret.steerWarning = cp.vl["Steering_Torque"]['Steer_Warning'] == 1
-
     ret.cruiseState.enabled = cp.vl["CruiseControl"]['Cruise_Activated'] != 0
     ret.cruiseState.available = cp.vl["CruiseControl"]['Cruise_On'] != 0
     ret.cruiseState.speed = cp_cam.vl["ES_DashStatus"]['Cruise_Set_Speed'] * CV.KPH_TO_MS
-    ret.cruiseState.nonAdaptive = cp_cam.vl["ES_DashStatus"]['Conventional_Cruise'] == 1
-    # EDM Impreza: 1,2 = mph, UDM Forester: 7 = mph
-    if cp.vl["Dash_State"]['Units'] in [1, 2, 7]:
+
+    # UDM Legacy: mph = 0
+    if self.car_fingerprint == CAR.LEGACY_PREGLOBAL and cp.vl["Dash_State"]['Units'] == 0:
+      ret.cruiseState.speed *= CV.MPH_TO_KPH
+    # EDM Global: mph = 1, 2; UDM Forester: 7 = mph
+    elif self.car_fingerprint != CAR.LEGACY_PREGLOBAL and cp.vl["Dash_State"]['Units'] in [1, 2, 7]:
       ret.cruiseState.speed *= CV.MPH_TO_KPH
 
     ret.seatbeltUnlatched = cp.vl["Dashlights"]['SEATBELT_FL'] == 1
@@ -65,6 +68,15 @@ class CarState(CarStateBase):
                         cp.vl["BodyInfo"]['DOOR_OPEN_FR'],
                         cp.vl["BodyInfo"]['DOOR_OPEN_FL']])
 
+    if self.car_fingerprint in PREGLOBAL_CARS:
+      ret.steerError = cp.vl["Steering_Torque"]["LKA_Lockout"] == 1
+      self.button = cp_cam.vl["ES_CruiseThrottle"]["Button"]
+      self.ready = not cp_cam.vl["ES_DashStatus"]["Not_Ready_Startup"]
+      self.es_accel_msg = copy.copy(cp_cam.vl["ES_CruiseThrottle"])
+    else:
+      ret.steerError = cp.vl["Steering_Torque"]['Steer_Error_1'] == 1
+      ret.steerWarning = cp.vl["Steering_Torque"]['Steer_Warning'] == 1
+      ret.cruiseState.nonAdaptive = cp_cam.vl["ES_DashStatus"]['Conventional_Cruise'] == 1
       self.es_distance_msg = copy.copy(cp_cam.vl["ES_Distance"])
       self.es_lkas_msg = copy.copy(cp_cam.vl["ES_LKAS_State"])
 
@@ -77,8 +89,6 @@ class CarState(CarStateBase):
       # sig_name, sig_address, default
       ("Steer_Torque_Sensor", "Steering_Torque", 0),
       ("Steering_Angle", "Steering_Torque", 0),
-      ("Steer_Error_1", "Steering_Torque", 0),
-      ("Steer_Warning", "Steering_Torque", 0),
       ("Cruise_On", "CruiseControl", 0),
       ("Cruise_Activated", "CruiseControl", 0),
       ("Brake_Pedal", "Brake_Pedal", 0),
@@ -104,17 +114,64 @@ class CarState(CarStateBase):
 
     checks = [
       # sig_address, frequency
+      ("Throttle", 100),
       ("Dashlights", 10),
-      ("CruiseControl", 20),
+      ("Brake_Pedal", 50),
       ("Wheel_Speeds", 50),
+      ("Transmission", 100),
       ("Steering_Torque", 50),
+    ]
+
+    if CP.carFingerprint in PREGLOBAL_CARS:
+      signals += [
+        ("LKA_Lockout", "Steering_Torque", 0),
+      ]
+      checks += [
+        ("CruiseControl", 50),
+      ]
+    else:
+      signals += [
+        ("Steer_Error_1", "Steering_Torque", 0),
+        ("Steer_Warning", "Steering_Torque", 0),
+      ]
+      checks += [
         ("BodyInfo", 10),
+        ("CruiseControl", 20),
       ]
 
     return CANParser(DBC[CP.carFingerprint]['pt'], signals, checks, 0)
 
   @staticmethod
   def get_cam_can_parser(CP):
+    if CP.carFingerprint in PREGLOBAL_CARS:
+      signals = [
+        ("Cruise_Set_Speed", "ES_DashStatus", 0),
+        ("Not_Ready_Startup", "ES_DashStatus", 0),
+
+        ("Throttle_Cruise", "ES_CruiseThrottle", 0),
+        ("Signal1", "ES_CruiseThrottle", 0),
+        ("Cruise_Activated", "ES_CruiseThrottle", 0),
+        ("Signal2", "ES_CruiseThrottle", 0),
+        ("Brake_On", "ES_CruiseThrottle", 0),
+        ("DistanceSwap", "ES_CruiseThrottle", 0),
+        ("Standstill", "ES_CruiseThrottle", 0),
+        ("Signal3", "ES_CruiseThrottle", 0),
+        ("CloseDistance", "ES_CruiseThrottle", 0),
+        ("Signal4", "ES_CruiseThrottle", 0),
+        ("Standstill_2", "ES_CruiseThrottle", 0),
+        ("ES_Error", "ES_CruiseThrottle", 0),
+        ("Signal5", "ES_CruiseThrottle", 0),
+        ("Counter", "ES_CruiseThrottle", 0),
+        ("Signal6", "ES_CruiseThrottle", 0),
+        ("Button", "ES_CruiseThrottle", 0),
+        ("Signal7", "ES_CruiseThrottle", 0),
+      ]
+
+      checks = [
+        ("ES_DashStatus", 20),
+        ("ES_CruiseThrottle", 20),
+      ]
+    else:
       signals = [
         ("Cruise_Set_Speed", "ES_DashStatus", 0),
         ("Conventional_Cruise", "ES_DashStatus", 0),

selfdrive/car/subaru/interface.py

@@ -1,6 +1,6 @@
 #!/usr/bin/env python3
 from cereal import car
-from selfdrive.car.subaru.values import CAR
+from selfdrive.car.subaru.values import CAR, PREGLOBAL_CARS
 from selfdrive.car import STD_CARGO_KG, scale_rot_inertia, scale_tire_stiffness, gen_empty_fingerprint
 from selfdrive.car.interfaces import CarInterfaceBase
 
@@ -16,11 +16,17 @@ class CarInterface(CarInterfaceBase):
 
     ret.carName = "subaru"
     ret.radarOffCan = True
+
+    if candidate in PREGLOBAL_CARS:
+      ret.safetyModel = car.CarParams.SafetyModel.subaruLegacy
+    else:
       ret.safetyModel = car.CarParams.SafetyModel.subaru
 
     # Subaru port is a community feature, since we don't own one to test
     ret.communityFeature = True
 
+    ret.dashcamOnly = candidate in PREGLOBAL_CARS
+
     # force openpilot to fake the stock camera, since car harness is not supported yet and old style giraffe (with switches)
     # was never released
     ret.enableCamera = True
@@ -58,6 +64,16 @@ class CarInterface(CarInterfaceBase):
       ret.lateralTuning.pid.kiBP, ret.lateralTuning.pid.kpBP = [[0., 14., 23.], [0., 14., 23.]]
       ret.lateralTuning.pid.kpV, ret.lateralTuning.pid.kiV = [[0.01, 0.065, 0.2], [0.001, 0.015, 0.025]]
 
+    if candidate == CAR.LEGACY_PREGLOBAL:
+      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]]
+
     # TODO: get actual value, for now starting with reasonable value for
     # civic and scaling by mass and wheelbase
     ret.rotationalInertia = scale_rot_inertia(ret.mass, ret.wheelbase)

selfdrive/car/subaru/subarucan.py

@@ -5,8 +5,7 @@ VisualAlert = car.CarControl.HUDControl.VisualAlert
 
 def create_steering_control(packer, apply_steer, frame, steer_step):
 
-  # counts from 0 to 15 then back to 0 + 16 for enable bit
-  idx = ((frame // steer_step) % 16)
+  idx = (frame / steer_step) % 16
 
   values = {
     "Counter": idx,
@@ -38,3 +37,31 @@ def create_es_lkas(packer, es_lkas_msg, visual_alert, left_line, right_line):
   values["LKAS_Right_Line_Visible"] = int(right_line)
 
   return packer.make_can_msg("ES_LKAS_State", 0, values)
+
+# *** Subaru Pre-global ***
+
+def subaru_preglobal_checksum(packer, values, addr):
+  dat = packer.make_can_msg(addr, 0, values)[2]
+  return (sum(dat[:7])) % 256
+
+def create_preglobal_steering_control(packer, apply_steer, frame, steer_step):
+
+  idx = (frame / steer_step) % 8
+
+  values = {
+    "Counter": idx,
+    "LKAS_Command": apply_steer,
+    "LKAS_Active": 1 if apply_steer != 0 else 0
+  }
+  values["Checksum"] = subaru_preglobal_checksum(packer, values, "ES_LKAS")
+
+  return packer.make_can_msg("ES_LKAS", 0, values)
+
+def create_es_throttle_control(packer, fake_button, es_accel_msg):
+
+  values = copy.copy(es_accel_msg)
+  values["Button"] = fake_button
+
+  values["Checksum"] = subaru_preglobal_checksum(packer, values, "ES_CruiseThrottle")
+
+  return packer.make_can_msg("ES_CruiseThrottle", 0, values)

selfdrive/car/subaru/values.py

@@ -8,14 +8,15 @@ class CAR:
   ASCENT = "SUBARU ASCENT LIMITED 2019"
   IMPREZA = "SUBARU IMPREZA LIMITED 2019"
   FORESTER = "SUBARU FORESTER 2019"
+  LEGACY_PREGLOBAL = "SUBARU LEGACY 2015 - 2018"
 
 FINGERPRINTS = {
-  CAR.ASCENT: [
+  CAR.ASCENT: [{
   # SUBARU ASCENT LIMITED 2019
-  {
     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, 811: 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, 1722: 8, 1743: 8, 1759: 8, 1785: 5, 1786: 5, 1787: 5, 1788: 8
   }],
   CAR.IMPREZA: [{
+  # SUBARU IMPREZA LIMITED 2019
     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 Impreza)
@@ -30,12 +31,25 @@ FINGERPRINTS = {
   {
     2: 8, 64: 8, 65: 8, 72: 8, 73: 8, 280: 8, 281: 8, 282: 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, 803: 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, 1651: 8, 1657: 8, 1658: 8, 1677: 8, 1722: 8, 1759: 8, 1787: 5, 1788: 8
   }],
+  CAR.LEGACY_PREGLOBAL: [{
+  # 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, 352: 8, 353: 8, 354: 8, 356: 8, 358: 8, 359: 8, 644: 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
+  },
+  # 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, 352: 8, 353: 8, 354: 8, 356: 8, 358: 8, 359: 8, 392: 8, 554: 8, 604: 8, 640: 8, 642: 8, 805: 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, 1785: 5, 1786: 5, 1787: 5, 1788: 8, 2015: 8, 2016: 8, 2024: 8
+  }],
 }
 
 STEER_THRESHOLD = {
   CAR.ASCENT: 80,
   CAR.IMPREZA: 80,
   CAR.FORESTER: 80,
+  CAR.LEGACY_PREGLOBAL: 75,
 }
 
 ECU_FINGERPRINT = {
@@ -46,4 +60,7 @@ DBC = {
   CAR.ASCENT: dbc_dict('subaru_global_2017_generated', None),
   CAR.IMPREZA: dbc_dict('subaru_global_2017_generated', None),
   CAR.FORESTER: dbc_dict('subaru_global_2017_generated', None),
+  CAR.LEGACY_PREGLOBAL: dbc_dict('subaru_outback_2015_generated', None),
 }
+
+PREGLOBAL_CARS = [CAR.LEGACY_PREGLOBAL]

selfdrive/test/test_car_models.py

@@ -389,6 +389,10 @@ routes = {
     'carFingerprint': SUBARU.IMPREZA,
     'enableCamera': True,
   },
+  "df5ca7660000fba8|2020-06-16--17-37-19": {
+    'carFingerprint': SUBARU.LEGACY_PREGLOBAL,
+    'enableCamera': True,
+  },
   "fbbfa6af821552b9|2020-03-03--08-09-43": {
     'carFingerprint': NISSAN.XTRAIL,
     'enableCamera': True,