Subaru Angle-Based LKAS port without car-specific platforms

docs/CARS.md

@@ -266,6 +266,7 @@
 |Subaru|Ascent 2023|All|[Dashcam mode](#dashcam)|
 |Subaru|Crosstrek 2018-19|EyeSight Driver Assistance|[Upstream](#upstream)|
 |Subaru|Crosstrek 2020-23|EyeSight Driver Assistance|[Upstream](#upstream)|
+|Subaru|Crosstrek 2025|All|[Dashcam mode](#dashcam)|
 |Subaru|Crosstrek Hybrid 2020|EyeSight Driver Assistance|[Dashcam mode](#dashcam)|
 |Subaru|Forester 2017-18|EyeSight Driver Assistance|[Dashcam mode](#dashcam)|
 |Subaru|Forester 2019-21|All|[Upstream](#upstream)|

opendbc/car/subaru/carcontroller.py

@@ -1,7 +1,7 @@
 import numpy as np
 from opendbc.can import CANPacker
 from opendbc.car import Bus, make_tester_present_msg
-from opendbc.car.lateral import apply_driver_steer_torque_limits, common_fault_avoidance
+from opendbc.car.lateral import apply_driver_steer_torque_limits, common_fault_avoidance, apply_std_steer_angle_limits
 from opendbc.car.interfaces import CarControllerBase
 from opendbc.car.subaru import subarucan
 from opendbc.car.subaru.values import DBC, GLOBAL_ES_ADDR, CanBus, CarControllerParams, SubaruFlags
@@ -16,13 +16,56 @@ class CarController(CarControllerBase):
   def __init__(self, dbc_names, CP):
     super().__init__(dbc_names, CP)
     self.apply_torque_last = 0
+    self.apply_angle_last = 0
 
     self.cruise_button_prev = 0
     self.steer_rate_counter = 0
 
     self.p = CarControllerParams(CP)
     self.packer = CANPacker(DBC[CP.carFingerprint][Bus.pt])
 
+  def handle_angle_lateral(self, CC, CS):
+    apply_steer = apply_std_steer_angle_limits(
+          CC.actuators.steeringAngleDeg,
+          self.apply_angle_last,
+          CS.out.vEgoRaw,
+          CS.out.steeringAngleDeg,
+          CC.latActive,
+          self.p.ANGLE_LIMITS
+        )
+
+    if not CC.latActive:
+      apply_steer = CS.out.steeringAngleDeg
+
+    self.apply_angle_last = apply_steer
+    return subarucan.create_steering_control_angle(self.packer, apply_steer, CC.latActive)
+
+  def handle_torque_lateral(self, CC, CS):
+    apply_torque = int(round(CC.actuators.torque * self.p.STEER_MAX))
+
+    new_torque = int(round(apply_torque))
+    apply_torque = apply_driver_steer_torque_limits(new_torque, self.apply_torque_last, CS.out.steeringTorque, self.p)
+
+    if not CC.latActive:
+      apply_torque = 0
+
+    msg = None
+    if self.CP.flags & SubaruFlags.PREGLOBAL:
+      msg = subarucan.create_preglobal_steering_control(self.packer, self.frame // self.p.STEER_STEP, apply_torque, 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)
+
+      msg = subarucan.create_steering_control(self.packer, apply_torque, apply_steer_req)
+
+    self.apply_torque_last = apply_torque
+    return msg
+
   def update(self, CC, CS, now_nanos):
     actuators = CC.actuators
     hud_control = CC.hudControl
@@ -32,30 +75,10 @@ def update(self, CC, CS, now_nanos):
 
     # *** steering ***
     if (self.frame % self.p.STEER_STEP) == 0:
-      apply_torque = int(round(actuators.torque * self.p.STEER_MAX))
-
-      # limits due to driver torque
-
-      new_torque = int(round(apply_torque))
-      apply_torque = apply_driver_steer_torque_limits(new_torque, self.apply_torque_last, CS.out.steeringTorque, self.p)
-
-      if not CC.latActive:
-        apply_torque = 0
-
-      if self.CP.flags & SubaruFlags.PREGLOBAL:
-        can_sends.append(subarucan.create_preglobal_steering_control(self.packer, self.frame // self.p.STEER_STEP, apply_torque, CC.latActive))
+      if self.CP.flags & SubaruFlags.LKAS_ANGLE:
+        can_sends.append(self.handle_angle_lateral(CC, CS))
       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_torque, apply_steer_req))
-
-      self.apply_torque_last = apply_torque
+        can_sends.append(self.handle_torque_lateral(CC, CS))
 
     # *** longitudinal ***
 
@@ -137,6 +160,7 @@ def update(self, CC, CS, now_nanos):
           can_sends.append(subarucan.create_es_static_2(self.packer))
 
     new_actuators = actuators.as_builder()
+    new_actuators.steeringAngleDeg = self.apply_angle_last
     new_actuators.torque = self.apply_torque_last / self.p.STEER_MAX
     new_actuators.torqueOutputCan = self.apply_torque_last
 

opendbc/car/subaru/interface.py

@@ -18,7 +18,15 @@ def _get_params(ret: structs.CarParams, candidate: CAR, fingerprint, car_fw, alp
     # - replacement for ES_Distance so we can cancel the cruise control
     # - to find the Cruise_Activated bit from the car
     # - proper panda safety setup (use the correct cruise_activated bit, throttle from Throttle_Hybrid, etc)
-    ret.dashcamOnly = bool(ret.flags & (SubaruFlags.PREGLOBAL | SubaruFlags.LKAS_ANGLE | SubaruFlags.HYBRID))
+    # for LKAS_ANGLE CARS to be upstreamed, we need:
+    # - validate angle safety
+    ret.dashcamOnly = bool((ret.flags & (SubaruFlags.PREGLOBAL | SubaruFlags.HYBRID)) or
+                       ((ret.flags & SubaruFlags.LKAS_ANGLE) and is_release))
+
+    if candidate == CAR.SUBARU_FORESTER_2022:
+      # Gen 1 LKAS angle not tested, can undashcam if not release once we see a test route
+      ret.dashcamOnly = True
+
     ret.autoResumeSng = False
 
     # Detect infotainment message sent from the camera
@@ -42,6 +50,7 @@ def _get_params(ret: structs.CarParams, candidate: CAR, fingerprint, car_fw, alp
 
     if ret.flags & SubaruFlags.LKAS_ANGLE:
       ret.steerControlType = structs.CarParams.SteerControlType.angle
+      ret.safetyConfigs[0].safetyParam |= SubaruSafetyFlags.LKAS_ANGLE.value
 
     elif candidate == CAR.SUBARU_ASCENT:
       ret.steerActuatorDelay = 0.3  # end-to-end angle controller

opendbc/car/subaru/values.py

@@ -2,6 +2,7 @@
 from enum import Enum, IntFlag
 
 from opendbc.car import Bus, CarSpecs, DbcDict, PlatformConfig, Platforms, uds
+from opendbc.car.lateral import AngleSteeringLimits
 from opendbc.car.structs import CarParams
 from opendbc.car.docs_definitions import CarFootnote, CarHarness, CarDocs, CarParts, Column
 from opendbc.car.fw_query_definitions import FwQueryConfig, Request, StdQueries, p16
@@ -18,6 +19,12 @@ def __init__(self, CP):
     self.STEER_DRIVER_MULTIPLIER = 50  # weight driver torque heavily
     self.STEER_DRIVER_FACTOR = 1       # from dbc
 
+    self.ANGLE_LIMITS: AngleSteeringLimits = AngleSteeringLimits(
+      545,
+      ([0., 5., 35.], [5., .8, .15,]),
+      ([0., 5., 35.], [5., .8, .15,]),
+    )
+
     if CP.flags & SubaruFlags.GLOBAL_GEN2:
       # TODO: lower rate limits, this reaches min/max in 0.5s which negatively affects tuning
       self.STEER_MAX = 1000
@@ -57,6 +64,7 @@ class SubaruSafetyFlags(IntFlag):
   GEN2 = 1
   LONG = 2
   PREGLOBAL_REVERSED_DRIVER_TORQUE = 4
+  LKAS_ANGLE = 8
 
 
 class SubaruFlags(IntFlag):

opendbc/safety/modes/subaru.h

@@ -22,10 +22,12 @@
 #define MSG_SUBARU_Brake_Status          0x13cU
 #define MSG_SUBARU_CruiseControl         0x240U
 #define MSG_SUBARU_Throttle              0x40U
+#define MSG_SUBARU_Steering_2            0x11aU
 #define MSG_SUBARU_Steering_Torque       0x119U
 #define MSG_SUBARU_Wheel_Speeds          0x13aU
 
 #define MSG_SUBARU_ES_LKAS               0x122U
+#define MSG_SUBARU_ES_LKAS_ANGLE         0x124U
 #define MSG_SUBARU_ES_Brake              0x220U
 #define MSG_SUBARU_ES_Distance           0x221U
 #define MSG_SUBARU_ES_Status             0x222U
@@ -70,8 +72,18 @@
   {.msg = {{MSG_SUBARU_Brake_Status,    alt_bus,         8, 50U, .max_counter = 15U, .ignore_quality_flag = true}, { 0 }, { 0 }}},  \
   {.msg = {{MSG_SUBARU_CruiseControl,   alt_bus,         8, 20U, .max_counter = 15U, .ignore_quality_flag = true}, { 0 }, { 0 }}},  \
 
+#define SUBARU_LKAS_ANGLE_RX_CHECKS(alt_bus)                                                                            \
+  {.msg = {{MSG_SUBARU_Throttle,        SUBARU_MAIN_BUS, 8, 100U, .max_counter = 15U, .ignore_quality_flag = true}, { 0 }, { 0 }}},  \
+  {.msg = {{MSG_SUBARU_Steering_Torque, SUBARU_MAIN_BUS, 8, 50U,  .max_counter = 15U, .ignore_quality_flag = true},  { 0 }, { 0 }}},  \
+  {.msg = {{MSG_SUBARU_Wheel_Speeds,    alt_bus,         8, 50U,  .max_counter = 15U, .ignore_quality_flag = true},  { 0 }, { 0 }}},  \
+  {.msg = {{MSG_SUBARU_Brake_Status,    alt_bus,         8, 50U,  .max_counter = 15U, .ignore_quality_flag = true},  { 0 }, { 0 }}},  \
+  {.msg = {{MSG_SUBARU_ES_Brake,        alt_bus,         8, 50U,  .max_counter = 15U, .ignore_quality_flag = true},  { 0 }, { 0 }}},  \
+  {.msg = {{MSG_SUBARU_ES_DashStatus,   SUBARU_CAM_BUS,  8, 10U,  .max_counter = 15U, .ignore_quality_flag = true},  { 0 }, { 0 }}},  \
+  {.msg = {{MSG_SUBARU_Steering_2,      SUBARU_MAIN_BUS, 8, 50U,  .max_counter = 15U, .ignore_quality_flag = true }, { 0 }, { 0 }}},  \
+
 static bool subaru_gen2 = false;
 static bool subaru_longitudinal = false;
+static bool subaru_lkas_angle = false;
 
 static uint32_t subaru_get_checksum(const CANPacket_t *msg) {
   return (uint8_t)msg->data[0];
@@ -93,17 +105,34 @@ static uint32_t subaru_compute_checksum(const CANPacket_t *msg) {
 static void subaru_rx_hook(const CANPacket_t *msg) {
   const unsigned int alt_main_bus = subaru_gen2 ? SUBARU_ALT_BUS : SUBARU_MAIN_BUS;
 
+  if (subaru_lkas_angle) {
+    if ((msg->addr == MSG_SUBARU_Steering_2) && (msg->bus == SUBARU_MAIN_BUS)) {
+      uint32_t raw = GET_BYTES(msg, 3, 3);
+      raw &= 0x1FFFFU;
+      int angle_meas_new = -to_signed(raw, 17);
+      update_sample(&angle_meas, angle_meas_new);
+    }
+  } 
+
+  // non-zero, even for Angle-LKAS cars
   if ((msg->addr == MSG_SUBARU_Steering_Torque) && (msg->bus == SUBARU_MAIN_BUS)) {
     int torque_driver_new;
     torque_driver_new = ((GET_BYTES(msg, 0, 4) >> 16) & 0x7FFU);
     torque_driver_new = -1 * to_signed(torque_driver_new, 11);
     update_sample(&torque_driver, torque_driver_new);
   }
 
-  // enter controls on rising edge of ACC, exit controls on ACC off
-  if ((msg->addr == MSG_SUBARU_CruiseControl) && (msg->bus == alt_main_bus)) {
-    bool cruise_engaged = (msg->data[5] >> 1) & 1U;
-    pcm_cruise_check(cruise_engaged);
+  if (subaru_lkas_angle) {
+    if ((msg->addr == MSG_SUBARU_ES_Brake) && (msg->bus == SUBARU_ALT_BUS)) {
+      bool cruise_engaged = (msg->data[4] >> 7) & 1U;
+      pcm_cruise_check(cruise_engaged);
+    }
+  } else {
+    // enter controls on rising edge of ACC, exit controls on ACC off
+    if ((msg->addr == MSG_SUBARU_CruiseControl) && (msg->bus == alt_main_bus)) {
+      bool cruise_engaged = (msg->data[5] >> 1) & 1U;
+      pcm_cruise_check(cruise_engaged);
+    }
   }
 
   // update vehicle moving with any non-zero wheel speed
@@ -130,6 +159,18 @@ static void subaru_rx_hook(const CANPacket_t *msg) {
 static bool subaru_tx_hook(const CANPacket_t *msg) {
   const TorqueSteeringLimits SUBARU_STEERING_LIMITS      = SUBARU_STEERING_LIMITS_GENERATOR(2047, 50, 70);
   const TorqueSteeringLimits SUBARU_GEN2_STEERING_LIMITS = SUBARU_STEERING_LIMITS_GENERATOR(1000, 40, 40);
+  const AngleSteeringLimits SUBARU_ANGLE_STEERING_LIMITS = {
+    .max_angle = 545*100,
+    .angle_deg_to_can = 100.,
+    .angle_rate_up_lookup = {
+      {0.0, 5.0, 35.0},
+      {5.0, 0.8, 0.15}
+    },
+    .angle_rate_down_lookup = {
+      {0.0, 5.0, 35.0},
+      {5.0, 0.8, 0.15}
+    },
+  };
 
   const LongitudinalLimits SUBARU_LONG_LIMITS = {
     .min_gas = 808,       // appears to be engine braking
@@ -155,6 +196,16 @@ static bool subaru_tx_hook(const CANPacket_t *msg) {
     violation |= steer_torque_cmd_checks(desired_torque, steer_req, limits);
   }
 
+  if (subaru_lkas_angle) {
+    if (msg->addr == MSG_SUBARU_ES_LKAS_ANGLE) {
+      int desired_angle = GET_BYTES(msg, 5, 3) & 0x1FFFFU;
+      desired_angle = -1 * to_signed(desired_angle, 17);
+      bool lkas_request = GET_BIT(msg, 12U);
+
+      violation |= steer_angle_cmd_checks(desired_angle, lkas_request, SUBARU_ANGLE_STEERING_LIMITS);
+    }
+  }
+
   // check es_brake brake_pressure limits
   if (msg->addr == MSG_SUBARU_ES_Brake) {
     int es_brake_pressure = GET_BYTES(msg, 2, 2);
@@ -220,6 +271,18 @@ static safety_config subaru_init(uint16_t param) {
     SUBARU_GEN2_LONG_ADDITIONAL_TX_MSGS()
   };
 
+  static const CanMsg SUBARU_LKAS_ANGLE_TX_MSGS[] = {
+    SUBARU_BASE_TX_MSGS(SUBARU_ALT_BUS, MSG_SUBARU_ES_LKAS_ANGLE)
+    SUBARU_COMMON_TX_MSGS(SUBARU_ALT_BUS)
+  };
+
+  static const CanMsg SUBARU_LKAS_ANGLE_LONG_TX_MSGS[] = {
+    SUBARU_BASE_TX_MSGS(SUBARU_ALT_BUS, MSG_SUBARU_ES_LKAS_ANGLE) // lat
+    SUBARU_COMMON_TX_MSGS(SUBARU_ALT_BUS)
+    SUBARU_COMMON_LONG_TX_MSGS(SUBARU_ALT_BUS) // long
+    SUBARU_GEN2_LONG_ADDITIONAL_TX_MSGS()
+  };
+
   static RxCheck subaru_rx_checks[] = {
     SUBARU_COMMON_RX_CHECKS(SUBARU_MAIN_BUS)
   };
@@ -228,17 +291,26 @@ static safety_config subaru_init(uint16_t param) {
     SUBARU_COMMON_RX_CHECKS(SUBARU_ALT_BUS)
   };
 
-  const uint16_t SUBARU_PARAM_GEN2 = 1;
+  static RxCheck subaru_lkas_angle_rx_checks[] = {
+    SUBARU_LKAS_ANGLE_RX_CHECKS(SUBARU_ALT_BUS)
+  };
 
+  const uint16_t SUBARU_PARAM_GEN2 = 1;
   subaru_gen2 = GET_FLAG(param, SUBARU_PARAM_GEN2);
 
 #ifdef ALLOW_DEBUG
   const uint16_t SUBARU_PARAM_LONGITUDINAL = 2;
   subaru_longitudinal = GET_FLAG(param, SUBARU_PARAM_LONGITUDINAL);
+
+  const uint16_t SUBARU_PARAM_LKAS_ANGLE = 8;
+  subaru_lkas_angle = GET_FLAG(param, SUBARU_PARAM_LKAS_ANGLE);
 #endif
 
   safety_config ret;
-  if (subaru_gen2) {
+  if (subaru_lkas_angle) {
+    ret = subaru_longitudinal ? BUILD_SAFETY_CFG(subaru_lkas_angle_rx_checks, SUBARU_LKAS_ANGLE_LONG_TX_MSGS) : \
+                                BUILD_SAFETY_CFG(subaru_lkas_angle_rx_checks, SUBARU_LKAS_ANGLE_TX_MSGS);
+  } else if (subaru_gen2) {
     ret = subaru_longitudinal ? BUILD_SAFETY_CFG(subaru_gen2_rx_checks, SUBARU_GEN2_LONG_TX_MSGS) : \
                                 BUILD_SAFETY_CFG(subaru_gen2_rx_checks, SUBARU_GEN2_TX_MSGS);
   } else {

opendbc/safety/tests/safety_replay/helpers.py

@@ -1,5 +1,6 @@
 from opendbc.car.ford.values import FordSafetyFlags
 from opendbc.car.hyundai.values import HyundaiSafetyFlags
+from opendbc.car.subaru.values import SubaruSafetyFlags
 from opendbc.car.toyota.values import ToyotaSafetyFlags
 from opendbc.car.structs import CarParams
 from opendbc.safety.tests.libsafety import libsafety_py
@@ -29,7 +30,7 @@ def is_steering_msg(mode, param, addr):
   elif mode == CarParams.SafetyModel.chrysler:
     ret = addr == 0x292
   elif mode == CarParams.SafetyModel.subaru:
-    ret = addr == 0x122
+    ret = addr == 0x122 or addr == 0x124
   elif mode == CarParams.SafetyModel.ford:
     ret = addr == 0x3d6 if param & FordSafetyFlags.CANFD else addr == 0x3d3
   elif mode == CarParams.SafetyModel.nissan:
@@ -64,8 +65,11 @@ def get_steer_value(mode, param, msg):
   elif mode == CarParams.SafetyModel.chrysler:
     torque = (((msg.data[0] & 0x7) << 8) | msg.data[1]) - 1024
   elif mode == CarParams.SafetyModel.subaru:
-    torque = ((msg.data[3] & 0x1F) << 8) | msg.data[2]
-    torque = -to_signed(torque, 13)
+    if param & SubaruSafetyFlags.LKAS_ANGLE:
+      angle = -to_signed(((msg.data[5] | (msg.data[6] << 8) | (msg.data[7] << 16)) & 0x1FFFF), 17)
+    else:
+      torque = ((msg.data[3] & 0x1F) << 8) | msg.data[2]
+      torque = -to_signed(torque, 13)
   elif mode == CarParams.SafetyModel.ford:
     if param & FordSafetyFlags.CANFD:
       angle = ((msg.data[2] << 3) | (msg.data[3] >> 5)) - 1000