Add support for Subaru Forester 2022

.gitmodules

@@ -1,6 +1,7 @@
 [submodule "panda"]
   path = panda
-  url = ../../commaai/panda.git
+  url = ../../martinl/panda.git
+  branch = forester-2022-PR
 [submodule "opendbc"]
   path = opendbc
   url = ../../commaai/opendbc.git

RELEASES.md

@@ -11,6 +11,7 @@ Version 0.9.4 (2023-XX-XX)
   * Alerts are shown inside the border. Black/grey means info, orange means warning, and red means critical alert
 * Bookmarked segments are preserved on the device's storage
 * Ford Focus 2018 support
+* Subaru Forester 2022 support thanks to martinl!
 * Kia Carnival 2023 support thanks to sunnyhaibin!
 
 Version 0.9.3 (2023-06-29)

docs/CARS.md

@@ -4,7 +4,7 @@
 
 A supported vehicle is one that just works when you install a comma three. All supported cars provide a better experience than any stock system. Supported vehicles reference the US market unless otherwise specified.
 
-# 255 Supported Cars
+# 256 Supported Cars
 
 |Make|Model|Supported Package|ACC|No ACC accel below|No ALC below|Steering Torque|Resume from stop|<a href="##"><img width=2000></a>Hardware Needed<br>&nbsp;|Video|
 |---|---|---|:---:|:---:|:---:|:---:|:---:|:---:|:---:|
@@ -171,6 +171,7 @@ A supported vehicle is one that just works when you install a comma three. All s
 |Subaru|Crosstrek 2018-19|EyeSight Driver Assistance[<sup>7</sup>](#footnotes)|Stock|0 mph|0 mph|[![star](assets/icon-star-empty.svg)](##)|[![star](assets/icon-star-empty.svg)](##)|<details><summary>View</summary><sub>- 1 RJ45 cable (7 ft)<br>- 1 Subaru A connector<br>- 1 comma power v2<br>- 1 comma three<br>- 1 harness box<br>- 1 mount<br>- 1 right angle OBD-C cable (1.5 ft)<br><a href="https://comma.ai/shop/comma-three.html?make=Subaru&model=Crosstrek 2018-19">Buy Here</a></sub></details>|<a href="https://youtu.be/Agww7oE1k-s?t=26" target="_blank"><img height="18px" src="assets/icon-youtube.svg"></img></a>|
 |Subaru|Crosstrek 2020-23|EyeSight Driver Assistance[<sup>7</sup>](#footnotes)|Stock|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-empty.svg)](##)|<details><summary>View</summary><sub>- 1 RJ45 cable (7 ft)<br>- 1 Subaru A connector<br>- 1 comma power v2<br>- 1 comma three<br>- 1 harness box<br>- 1 mount<br>- 1 right angle OBD-C cable (1.5 ft)<br><a href="https://comma.ai/shop/comma-three.html?make=Subaru&model=Crosstrek 2020-23">Buy Here</a></sub></details>||
 |Subaru|Forester 2019-21|All[<sup>7</sup>](#footnotes)|Stock|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-empty.svg)](##)|<details><summary>View</summary><sub>- 1 RJ45 cable (7 ft)<br>- 1 Subaru A connector<br>- 1 comma power v2<br>- 1 comma three<br>- 1 harness box<br>- 1 mount<br>- 1 right angle OBD-C cable (1.5 ft)<br><a href="https://comma.ai/shop/comma-three.html?make=Subaru&model=Forester 2019-21">Buy Here</a></sub></details>||
+|Subaru|Forester 2022|EyeSight Driver Assistance[<sup>7</sup>](#footnotes)|Stock|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-empty.svg)](##)|<details><summary>View</summary><sub>- 1 RJ45 cable (7 ft)<br>- 1 Subaru C connector<br>- 1 comma power v2<br>- 1 comma three<br>- 1 harness box<br>- 1 mount<br>- 1 right angle OBD-C cable (1.5 ft)<br><a href="https://comma.ai/shop/comma-three.html?make=Subaru&model=Forester 2022">Buy Here</a></sub></details>||
 |Subaru|Impreza 2017-19|EyeSight Driver Assistance[<sup>7</sup>](#footnotes)|Stock|0 mph|0 mph|[![star](assets/icon-star-empty.svg)](##)|[![star](assets/icon-star-empty.svg)](##)|<details><summary>View</summary><sub>- 1 RJ45 cable (7 ft)<br>- 1 Subaru A connector<br>- 1 comma power v2<br>- 1 comma three<br>- 1 harness box<br>- 1 mount<br>- 1 right angle OBD-C cable (1.5 ft)<br><a href="https://comma.ai/shop/comma-three.html?make=Subaru&model=Impreza 2017-19">Buy Here</a></sub></details>||
 |Subaru|Impreza 2020-22|EyeSight Driver Assistance[<sup>7</sup>](#footnotes)|Stock|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-empty.svg)](##)|<details><summary>View</summary><sub>- 1 RJ45 cable (7 ft)<br>- 1 Subaru A connector<br>- 1 comma power v2<br>- 1 comma three<br>- 1 harness box<br>- 1 mount<br>- 1 right angle OBD-C cable (1.5 ft)<br><a href="https://comma.ai/shop/comma-three.html?make=Subaru&model=Impreza 2020-22">Buy Here</a></sub></details>||
 |Subaru|Legacy 2020-22|All[<sup>7</sup>](#footnotes)|Stock|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-empty.svg)](##)|<details><summary>View</summary><sub>- 1 RJ45 cable (7 ft)<br>- 1 Subaru B connector<br>- 1 comma power v2<br>- 1 comma three<br>- 1 harness box<br>- 1 mount<br>- 1 right angle OBD-C cable (1.5 ft)<br><a href="https://comma.ai/shop/comma-three.html?make=Subaru&model=Legacy 2020-22">Buy Here</a></sub></details>||

selfdrive/car/docs_definitions.py

@@ -87,6 +87,7 @@ class CarHarness(EnumBase):
   toyota = BaseCarHarness("Toyota connector")
   subaru_a = BaseCarHarness("Subaru A connector")
   subaru_b = BaseCarHarness("Subaru B connector")
+  subaru_c = BaseCarHarness("Subaru C connector")
   fca = BaseCarHarness("FCA connector")
   ram = BaseCarHarness("Ram connector")
   vw = BaseCarHarness("VW connector")

selfdrive/car/subaru/carcontroller.py

@@ -1,7 +1,7 @@
 from opendbc.can.packer import CANPacker
-from selfdrive.car import apply_driver_steer_torque_limits
+from selfdrive.car import apply_driver_steer_torque_limits, apply_std_steer_angle_limits
 from selfdrive.car.subaru import subarucan
-from selfdrive.car.subaru.values import DBC, GLOBAL_GEN2, PREGLOBAL_CARS, CanBus, CarControllerParams, SubaruFlags
+from selfdrive.car.subaru.values import DBC, GLOBAL_GEN2, LKAS_ANGLE, PREGLOBAL_CARS, CanBus, CarControllerParams, SubaruFlags
 
 
 class CarController:
@@ -25,23 +25,31 @@ def update(self, CC, CS, now_nanos):
 
     # *** steering ***
     if (self.frame % self.p.STEER_STEP) == 0:
-      apply_steer = int(round(actuators.steer * self.p.STEER_MAX))
+      # angle based steering
+      if self.CP.carFingerprint in LKAS_ANGLE:
+        apply_steer = apply_std_steer_angle_limits(actuators.steeringAngleDeg, self.apply_steer_last, CS.out.vEgoRaw, self.p)
+
+        if not CC.latActive:
+          apply_steer = CS.out.steeringAngleDeg
 
-      # limits due to driver torque
+        can_sends.append(subarucan.create_steering_control_angle(self.packer, apply_steer, CC.latActive))
 
-      new_steer = int(round(apply_steer))
-      apply_steer = apply_driver_steer_torque_limits(new_steer, self.apply_steer_last, CS.out.steeringTorque, self.p)
+      # torque based steering
+      else:
+        apply_steer = int(round(actuators.steer * self.p.STEER_MAX))
 
-      if not CC.latActive:
-        apply_steer = 0
+        # limits due to driver torque
+        apply_steer = apply_driver_steer_torque_limits(apply_steer, self.apply_steer_last, CS.out.steeringTorque, self.p)
 
-      if self.CP.carFingerprint in PREGLOBAL_CARS:
-        can_sends.append(subarucan.create_preglobal_steering_control(self.packer, apply_steer, CC.latActive))
-      else:
-        can_sends.append(subarucan.create_steering_control(self.packer, apply_steer, CC.latActive))
+        if not CC.latActive:
+          apply_steer = 0
 
-      self.apply_steer_last = apply_steer
+        if self.CP.carFingerprint in PREGLOBAL_CARS:
+          can_sends.append(subarucan.create_preglobal_steering_control(self.packer, apply_steer, CC.latActive))
+        else:
+          can_sends.append(subarucan.create_steering_control(self.packer, apply_steer, CC.latActive))
 
+      self.apply_steer_last = apply_steer
 
     # *** alerts and pcm cancel ***
     if self.CP.carFingerprint in PREGLOBAL_CARS:
@@ -80,8 +88,10 @@ def update(self, CC, CS, now_nanos):
           can_sends.append(subarucan.create_es_infotainment(self.packer, CS.es_infotainment_msg, hud_control.visualAlert))
 
     new_actuators = actuators.copy()
-    new_actuators.steer = self.apply_steer_last / self.p.STEER_MAX
-    new_actuators.steerOutputCan = self.apply_steer_last
-
+    if self.CP.carFingerprint in LKAS_ANGLE:
+      new_actuators.steeringAngleDeg = self.apply_steer_last
+    else:
+      new_actuators.steer = self.apply_steer_last / self.p.STEER_MAX
+      new_actuators.steerOutputCan = self.apply_steer_last
     self.frame += 1
     return new_actuators, can_sends

selfdrive/car/subaru/carstate.py

@@ -4,7 +4,7 @@
 from common.conversions import Conversions as CV
 from selfdrive.car.interfaces import CarStateBase
 from opendbc.can.parser import CANParser
-from selfdrive.car.subaru.values import DBC, CAR, GLOBAL_GEN2, PREGLOBAL_CARS, CanBus, SubaruFlags
+from selfdrive.car.subaru.values import DBC, CAR, GLOBAL_GEN2, ES_STATUS, PREGLOBAL_CARS, CanBus, SubaruFlags
 
 
 class CarState(CarStateBase):
@@ -53,9 +53,13 @@ def update(self, cp, cp_cam, cp_body):
     steer_threshold = 75 if self.CP.carFingerprint in PREGLOBAL_CARS else 80
     ret.steeringPressed = abs(ret.steeringTorque) > steer_threshold
 
-    cp_cruise = cp_body if self.car_fingerprint in GLOBAL_GEN2 else cp
-    ret.cruiseState.enabled = cp_cruise.vl["CruiseControl"]["Cruise_Activated"] != 0
-    ret.cruiseState.available = cp_cruise.vl["CruiseControl"]["Cruise_On"] != 0
+    if self.car_fingerprint in ES_STATUS:
+      ret.cruiseState.enabled = cp_cam.vl["ES_Status"]['Cruise_Activated'] != 0
+      ret.cruiseState.available = cp_cam.vl["ES_DashStatus"]['Cruise_On'] != 0
+    else:
+      cp_cruise = cp_body if self.car_fingerprint in GLOBAL_GEN2 else cp
+      ret.cruiseState.enabled = cp_cruise.vl["CruiseControl"]["Cruise_Activated"] != 0
+      ret.cruiseState.available = cp_cruise.vl["CruiseControl"]["Cruise_On"] != 0
     ret.cruiseState.speed = cp_cam.vl["ES_DashStatus"]["Cruise_Set_Speed"] * CV.KPH_TO_MS
 
     if (self.car_fingerprint in PREGLOBAL_CARS and cp.vl["Dash_State2"]["UNITS"] == 1) or \
@@ -135,11 +139,13 @@ def get_common_global_es_signals():
       ("Cruise_Set", "ES_Distance"),
       ("Cruise_Resume", "ES_Distance"),
       ("Signal6", "ES_Distance"),
+      ("Cruise_Activated", "ES_Status"),
     ]
 
     checks = [
       ("ES_Brake", 20),
       ("ES_Distance", 20),
+      ("ES_Status", 20),
     ]
 
     return signals, checks

selfdrive/car/subaru/interface.py

@@ -3,7 +3,7 @@
 from panda import Panda
 from selfdrive.car import STD_CARGO_KG, get_safety_config
 from selfdrive.car.interfaces import CarInterfaceBase
-from selfdrive.car.subaru.values import CAR, GLOBAL_GEN2, PREGLOBAL_CARS, SubaruFlags
+from selfdrive.car.subaru.values import LKAS_ANGLE, CAR, ES_STATUS, GLOBAL_GEN2, PREGLOBAL_CARS, SubaruFlags
 
 
 class CarInterface(CarInterfaceBase):
@@ -27,6 +27,10 @@ def _get_params(ret, candidate, fingerprint, car_fw, experimental_long, docs):
       ret.safetyConfigs = [get_safety_config(car.CarParams.SafetyModel.subaru)]
       if candidate in GLOBAL_GEN2:
         ret.safetyConfigs[0].safetyParam |= Panda.FLAG_SUBARU_GEN2
+      if candidate in LKAS_ANGLE:
+        ret.safetyConfigs[0].safetyParam |= Panda.FLAG_SUBARU_LKAS_ANGLE
+      if candidate in ES_STATUS:
+        ret.safetyConfigs[0].safetyParam |= Panda.FLAG_SUBARU_ES_STATUS
 
     ret.steerLimitTimer = 0.4
     ret.steerActuatorDelay = 0.1
@@ -64,7 +68,7 @@ def _get_params(ret, candidate, fingerprint, car_fw, experimental_long, docs):
       ret.lateralTuning.pid.kiBP, ret.lateralTuning.pid.kpBP = [[0., 14., 23.], [0., 14., 23.]]
       ret.lateralTuning.pid.kpV, ret.lateralTuning.pid.kiV = [[0.045, 0.042, 0.20], [0.04, 0.035, 0.045]]
 
-    elif candidate == CAR.FORESTER:
+    elif candidate in (CAR.FORESTER, CAR.FORESTER_2022):
       ret.mass = 1568. + STD_CARGO_KG
       ret.wheelbase = 2.67
       ret.centerToFront = ret.wheelbase * 0.5

selfdrive/car/subaru/subarucan.py

@@ -12,6 +12,13 @@ def create_steering_control(packer, apply_steer, steer_req):
   }
   return packer.make_can_msg("ES_LKAS", 0, values)
 
+def create_steering_control_angle(packer, apply_steer, steer_req):
+  values = {
+    "LKAS_Output": apply_steer,
+    "LKAS_Request": steer_req,
+    "SET_3": 3
+  }
+  return packer.make_can_msg("ES_LKAS_ANGLE", 0, values)
 
 def create_steering_status(packer):
   return packer.make_can_msg("ES_LKAS_State", 0, {})

selfdrive/car/subaru/values.py

@@ -4,7 +4,7 @@
 
 from cereal import car
 from panda.python import uds
-from selfdrive.car import dbc_dict
+from selfdrive.car import AngleRateLimit, dbc_dict
 from selfdrive.car.docs_definitions import CarFootnote, CarHarness, CarInfo, CarParts, Column
 from selfdrive.car.fw_query_definitions import FwQueryConfig, Request, StdQueries, p16
 
@@ -19,15 +19,19 @@ def __init__(self, CP):
     self.STEER_DRIVER_ALLOWANCE = 60   # allowed driver torque before start limiting
     self.STEER_DRIVER_MULTIPLIER = 50  # weight driver torque heavily
     self.STEER_DRIVER_FACTOR = 1       # from dbc
-
-    if CP.carFingerprint in GLOBAL_GEN2:
-      self.STEER_MAX = 1000
-      self.STEER_DELTA_UP = 40
-      self.STEER_DELTA_DOWN = 40
-    elif CP.carFingerprint == CAR.IMPREZA_2020:
-      self.STEER_MAX = 1439
+
+    if CP.carFingerprint in LKAS_ANGLE:
+      self.ANGLE_RATE_LIMIT_UP = AngleRateLimit(speed_bp=[0.], angle_v=[1.])
+      self.ANGLE_RATE_LIMIT_DOWN = AngleRateLimit(speed_bp=[0.], angle_v=[1.])
     else:
-      self.STEER_MAX = 2047
+      if CP.carFingerprint in GLOBAL_GEN2:
+        self.STEER_MAX = 1000
+        self.STEER_DELTA_UP = 40
+        self.STEER_DELTA_DOWN = 40
+      elif CP.carFingerprint == CAR.IMPREZA_2020:
+        self.STEER_MAX = 1439
+      else:
+        self.STEER_MAX = 2047
 
 
 class SubaruFlags(IntFlag):
@@ -46,6 +50,7 @@ class CAR:
   IMPREZA = "SUBARU IMPREZA LIMITED 2019"
   IMPREZA_2020 = "SUBARU IMPREZA SPORT 2020"
   FORESTER = "SUBARU FORESTER 2019"
+  FORESTER_2022 = "SUBARU FORESTER 2022"
   OUTBACK = "SUBARU OUTBACK 6TH GEN"
   LEGACY = "SUBARU LEGACY 7TH GEN"
 
@@ -84,6 +89,7 @@ class SubaruCarInfo(CarInfo):
     SubaruCarInfo("Subaru XV 2020-21"),
   ],
   CAR.FORESTER: SubaruCarInfo("Subaru Forester 2019-21", "All"),
+  CAR.FORESTER_2022: SubaruCarInfo("Subaru Forester 2022", car_parts=CarParts.common([CarHarness.subaru_c])),
   CAR.FORESTER_PREGLOBAL: SubaruCarInfo("Subaru Forester 2017-18"),
   CAR.LEGACY_PREGLOBAL: SubaruCarInfo("Subaru Legacy 2015-18"),
   CAR.OUTBACK_PREGLOBAL: SubaruCarInfo("Subaru Outback 2015-17"),
@@ -331,6 +337,35 @@ class SubaruCarInfo(CarInfo):
       b'\x1a\xe6F1\x00',
     ],
   },
+  CAR.FORESTER_2022: {
+    (Ecu.abs, 0x7b0, None): [
+      b'\xa3 !x\x00',
+      b'\xa3 !v\x00',
+      b'\xa3 "v\x00',
+      b'\xa3 "x\x00',
+    ],
+    (Ecu.eps, 0x746, None): [
+      b'-\xc0%0',
+      b'-\xc0\x040',
+      b'=\xc0%\x02',
+      b'=\xc04\x02',
+    ],
+    (Ecu.fwdCamera, 0x787, None): [
+      b'\x04!\x01\x1eD\x07!\x00\x04,'
+    ],
+    (Ecu.engine, 0x7e0, None): [
+      b'\xd5"a0\x07',
+      b'\xd5"`0\x07',
+      b'\xf1"aq\x07',
+      b'\xf1"`q\x07',
+    ],
+    (Ecu.transmission, 0x7e1, None): [
+      b'\x1d\x86B0\x00',
+      b'\x1d\xf6B0\x00',
+      b'\x1e\x86B0\x00',
+      b'\x1e\xf6D0\x00',
+    ],
+  },
   CAR.FORESTER_PREGLOBAL: {
     (Ecu.abs, 0x7b0, None): [
       b'\x7d\x97\x14\x40',
@@ -543,6 +578,7 @@ class SubaruCarInfo(CarInfo):
   CAR.IMPREZA: dbc_dict('subaru_global_2017_generated', None),
   CAR.IMPREZA_2020: dbc_dict('subaru_global_2017_generated', None),
   CAR.FORESTER: dbc_dict('subaru_global_2017_generated', None),
+  CAR.FORESTER_2022: dbc_dict('subaru_global_2017_generated', None),
   CAR.OUTBACK: dbc_dict('subaru_global_2017_generated', None),
   CAR.LEGACY: dbc_dict('subaru_global_2017_generated', None),
   CAR.FORESTER_PREGLOBAL: dbc_dict('subaru_forester_2017_generated', None),
@@ -551,5 +587,8 @@ class SubaruCarInfo(CarInfo):
   CAR.OUTBACK_PREGLOBAL_2018: dbc_dict('subaru_outback_2019_generated', None),
 }
 
+LKAS_ANGLE = (CAR.FORESTER_2022,)
+ES_STATUS = (CAR.FORESTER_2022,)
+
 GLOBAL_GEN2 = (CAR.OUTBACK, CAR.LEGACY)
 PREGLOBAL_CARS = (CAR.FORESTER_PREGLOBAL, CAR.LEGACY_PREGLOBAL, CAR.OUTBACK_PREGLOBAL, CAR.OUTBACK_PREGLOBAL_2018)

selfdrive/car/tests/routes.py

@@ -234,6 +234,8 @@
   CarTestRoute("8bf7e79a3ce64055|2021-05-24--09-36-27", SUBARU.IMPREZA_2020),
   CarTestRoute("1bbe6bf2d62f58a8|2022-07-14--17-11-43", SUBARU.OUTBACK, segment=10),
   CarTestRoute("c56e69bbc74b8fad|2022-08-18--09-43-51", SUBARU.LEGACY, segment=3),
+  CarTestRoute("7fd1e4f3a33c1673|2022-12-04--15-09-53", SUBARU.FORESTER_2022, segment=4),
+
   # Pre-global, dashcam
   CarTestRoute("95441c38ae8c130e|2020-06-08--12-10-17", SUBARU.FORESTER_PREGLOBAL),
   CarTestRoute("df5ca7660000fba8|2020-06-16--17-37-19", SUBARU.LEGACY_PREGLOBAL),

selfdrive/car/torque_data/substitute.yaml

@@ -77,6 +77,7 @@ SEAT LEON 3RD GEN: VOLKSWAGEN GOLF 7TH GEN
 SEAT ATECA 1ST GEN: VOLKSWAGEN GOLF 7TH GEN
 
 SUBARU LEGACY 7TH GEN: SUBARU OUTBACK 6TH GEN
+SUBARU FORESTER 2022: SUBARU FORESTER 2019
 
 # Old subarus don't have much data guessing it's like low torque impreza
 SUBARU OUTBACK 2018 - 2019: SUBARU IMPREZA LIMITED 2019