WIP: Car Port: Subaru Ascent 2023

opendbc/car/subaru/carcontroller.py

@@ -1,5 +1,5 @@
 from opendbc.can.packer import CANPacker
-from opendbc.car import Bus, apply_driver_steer_torque_limits, common_fault_avoidance, make_tester_present_msg
+from opendbc.car import Bus, apply_driver_steer_torque_limits, common_fault_avoidance, make_tester_present_msg, apply_std_steer_angle_limits
 from opendbc.car.common.numpy_fast import clip, interp
 from opendbc.car.interfaces import CarControllerBase
 from opendbc.car.subaru import subarucan
@@ -31,28 +31,38 @@ 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))
+      apply_steer = 0
+      if self.CP.flags & SubaruFlags.LKAS_ANGLE:
+        apply_steer = apply_std_steer_angle_limits(actuators.steeringAngleDeg, self.apply_steer_last, CS.out.vEgoRaw, self.p)
 
-      # limits due to driver torque
+        if not CC.latActive:
+          apply_steer = CS.out.steeringAngleDeg
 
-      new_steer = int(round(apply_steer))
-      apply_steer = apply_driver_steer_torque_limits(new_steer, self.apply_steer_last, CS.out.steeringTorque, self.p)
+        can_sends.append(subarucan.create_steering_control_angle(self.packer, apply_steer, CC.latActive))
 
-      if not CC.latActive:
-        apply_steer = 0
-
-      if self.CP.flags & SubaruFlags.PREGLOBAL:
-        can_sends.append(subarucan.create_preglobal_steering_control(self.packer, self.frame // self.p.STEER_STEP, apply_steer, CC.latActive))
       else:
-        apply_steer_req = CC.latActive
+        apply_steer = int(round(actuators.steer * self.p.STEER_MAX))
+
+        # limits due to driver torque
 
-        if self.CP.flags & SubaruFlags.STEER_RATE_LIMITED:
-          # Steering rate fault prevention
-          self.steer_rate_counter, apply_steer_req = \
-            common_fault_avoidance(abs(CS.out.steeringRateDeg) > MAX_STEER_RATE, apply_steer_req,
-                                  self.steer_rate_counter, MAX_STEER_RATE_FRAMES)
+        new_steer = int(round(apply_steer))
+        apply_steer = apply_driver_steer_torque_limits(new_steer, self.apply_steer_last, CS.out.steeringTorque, self.p)
 
-        can_sends.append(subarucan.create_steering_control(self.packer, apply_steer, apply_steer_req))
+        if not CC.latActive:
+          apply_steer = 0
+
+        if self.CP.flags & SubaruFlags.PREGLOBAL:
+          can_sends.append(subarucan.create_preglobal_steering_control(self.packer, self.frame // self.p.STEER_STEP, apply_steer, CC.latActive))
+        else:
+          apply_steer_req = CC.latActive
+
+          if self.CP.flags & SubaruFlags.STEER_RATE_LIMITED:
+            # Steering rate fault prevention
+            self.steer_rate_counter, apply_steer_req = \
+              common_fault_avoidance(abs(CS.out.steeringRateDeg) > MAX_STEER_RATE, apply_steer_req,
+                                      self.steer_rate_counter, MAX_STEER_RATE_FRAMES)
+
+          can_sends.append(subarucan.create_steering_control(self.packer, apply_steer, apply_steer_req))
 
       self.apply_steer_last = apply_steer
 
@@ -136,8 +146,11 @@ def update(self, CC, CS, now_nanos):
           can_sends.append(subarucan.create_es_static_2(self.packer))
 
     new_actuators = actuators.as_builder()
-    new_actuators.steer = self.apply_steer_last / self.p.STEER_MAX
-    new_actuators.steerOutputCan = self.apply_steer_last
+    if self.CP.flags & SubaruFlags.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

opendbc/car/subaru/carstate.py

@@ -78,12 +78,18 @@ def update(self, can_parsers) -> structs.CarState:
     ret.steeringPressed = abs(ret.steeringTorque) > steer_threshold
 
     cp_cruise = cp_alt if self.CP.flags & SubaruFlags.GLOBAL_GEN2 else cp
-    if self.CP.flags & SubaruFlags.HYBRID:
+    if self.CP.flags & SubaruFlags.LKAS_ANGLE:
       ret.cruiseState.enabled = cp_cam.vl["ES_DashStatus"]['Cruise_Activated'] != 0
       ret.cruiseState.available = cp_cam.vl["ES_DashStatus"]['Cruise_On'] != 0
+
     else:
-      ret.cruiseState.enabled = cp_cruise.vl["CruiseControl"]["Cruise_Activated"] != 0
-      ret.cruiseState.available = cp_cruise.vl["CruiseControl"]["Cruise_On"] != 0
+      if self.CP.flags & SubaruFlags.HYBRID:
+        ret.cruiseState.enabled = cp_cam.vl["ES_DashStatus"]['Cruise_Activated'] != 0
+        ret.cruiseState.available = cp_cam.vl["ES_DashStatus"]['Cruise_On'] != 0
+      else:
+        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.CP.flags & SubaruFlags.PREGLOBAL and cp.vl["Dash_State2"]["UNITS"] == 1) or \

opendbc/car/subaru/fingerprints.py

@@ -42,13 +42,17 @@
     ],
     (Ecu.fwdCamera, 0x787, None): [
       b'\x05!\x08\x1dK\x05!\x08\x01/',
-    ],
-    (Ecu.engine, 0x7a2, None): [
-      b'\xe5,\xa0P\x07',
-    ],
-    (Ecu.transmission, 0x7a3, None): [
-      b'\x04\xfe\xf3\x00\x00',
-    ],
+      b'\x05!\x08\x1dK\x00\x00\x00\x00\x00',
+    ],
+    # XXX: No longer showing up?!
+    #(Ecu.engine, 0x7a2, None): [
+    #  b'\xe5,\xa0P\x07',
+    #  b'\xe5,\xa0p\x07',
+    #],
+    #(Ecu.transmission, 0x7a3, None): [
+    #  b'\x04\xfe\xf3\x00\x00',
+    #  b'\x04\xfe\xf6\x00\x00',
+    #],
   },
   CAR.SUBARU_LEGACY: {
     (Ecu.abs, 0x7b0, None): [

opendbc/car/subaru/interface.py

@@ -30,6 +30,8 @@ def _get_params(ret: structs.CarParams, candidate: CAR, fingerprint, car_fw, exp
       ret.safetyConfigs = [get_safety_config(structs.CarParams.SafetyModel.subaru)]
       if ret.flags & SubaruFlags.GLOBAL_GEN2:
         ret.safetyConfigs[0].safetyParam |= Panda.FLAG_SUBARU_GEN2
+      if ret.flags & SubaruFlags.LKAS_ANGLE:
+        ret.safetyConfigs[0].safetyParam |= Panda.FLAG_SUBARU_LKAS_ANGLE
 
     ret.steerLimitTimer = 0.4
     ret.steerActuatorDelay = 0.1
@@ -39,13 +41,23 @@ def _get_params(ret: structs.CarParams, candidate: CAR, fingerprint, car_fw, exp
     else:
       CarInterfaceBase.configure_torque_tune(candidate, ret.lateralTuning)
 
-    if candidate in (CAR.SUBARU_ASCENT, CAR.SUBARU_ASCENT_2023):
+    if candidate == CAR.SUBARU_ASCENT:
       ret.steerActuatorDelay = 0.3  # end-to-end angle controller
       ret.lateralTuning.init('pid')
       ret.lateralTuning.pid.kf = 0.00003
       ret.lateralTuning.pid.kiBP, ret.lateralTuning.pid.kpBP = [[0., 20.], [0., 20.]]
       ret.lateralTuning.pid.kpV, ret.lateralTuning.pid.kiV = [[0.0025, 0.1], [0.00025, 0.01]]
 
+    elif candidate == CAR.SUBARU_ASCENT_2023:
+      ret.dashcamOnly = False
+      ret.steerActuatorDelay = 0.3  # end-to-end angle controller
+      ret.lateralTuning.init('pid')
+      ret.lateralTuning.pid.kf = 0.00003
+      ret.lateralTuning.pid.kpBP = [0., 20.]
+      ret.lateralTuning.pid.kiBP = [0., 20.]
+      ret.lateralTuning.pid.kpV = [0.0025, 0.1]
+      ret.lateralTuning.pid.kiV = [0.00025, 0.01]
+
     elif candidate == CAR.SUBARU_IMPREZA:
       ret.steerActuatorDelay = 0.4  # end-to-end angle controller
       ret.lateralTuning.init('pid')

opendbc/car/subaru/values.py

@@ -2,7 +2,7 @@
 from enum import Enum, IntFlag
 
 from panda import uds
-from opendbc.car import Bus, CarSpecs, DbcDict, PlatformConfig, Platforms
+from opendbc.car import Bus, CarSpecs, DbcDict, PlatformConfig, Platforms, AngleRateLimit
 from opendbc.car.structs import CarParams
 from opendbc.car.docs_definitions import CarFootnote, CarHarness, CarDocs, CarParts, Tool, Column
 from opendbc.car.fw_query_definitions import FwQueryConfig, Request, StdQueries, p16
@@ -53,6 +53,9 @@ def __init__(self, CP):
   BRAKE_LOOKUP_BP = [-3.5, 0]
   BRAKE_LOOKUP_V = [BRAKE_MAX, BRAKE_MIN]
 
+  ANGLE_RATE_LIMIT_UP = AngleRateLimit(speed_bp=[0.], angle_v=[1.])
+  ANGLE_RATE_LIMIT_DOWN = AngleRateLimit(speed_bp=[0.], angle_v=[1.])
+
 
 class SubaruFlags(IntFlag):
   # Detected flags
@@ -271,7 +274,7 @@ class CAR(Platforms):
   # We don't get the EPS from non-OBD queries on GEN2 cars. Note that we still attempt to match when it exists
   non_essential_ecus={
     Ecu.eps: list(CAR.with_flags(SubaruFlags.GLOBAL_GEN2)),
-  }
+  },
 )
 
 DBC = CAR.create_dbc_map()