Subaru: Global gen1 longitudinal safety (#1500)

* Add subaru long TX and RX messages * misra c2012 fix * fix tests * Reorder signals * increase max steering torque * merge fix * merge fix, remove cnt from test * Move subaru logitudinal behind flag, add safety test * update subaru_longitudinal checks * cleanup * Add subaru longitudinal checks * misra fix * Add subaru rpm and throttle rate limit checks * temporary disable cruise throttle and cruise rpm checks * update subaru long test * update longitudinal safety and add tests * fix tests * fix misra * subaru long signals limits checks * Add controls_allowed to long tests * remove unused variables * Add AEB passthru and tests * Add bus * Update AEB logic * Add GEN2 AEB support * syntax fix * Update AEB tests * Add comment for subaru_aeb * Do not check cruise_throttle and cruise_rpm limits when gas pressed * use long limits struct * Subaru: longitudinal checks only when controls_allowed, use ES_LKAS_State LKAS_Alert for AEB * fix misra, set controls_allowed for es_brake test * fix misra * Fix es_brake violation check * Add 0x323 to longitudinal test * Remove stock fcw from aeb signals * Use GET_BYTES * Revert "export FW_PATH" This reverts commit 2a5058d858. * Revert "cleanup fw filename conventions (#1434)" This reverts commit 4dd2735e38. * Revert "Revert "export FW_PATH"" This reverts commit 86e8d321d055c5f89cde7ee26a0014df1bcd5b39. * Revert "Revert "cleanup fw filename conventions (#1434)"" This reverts commit 5aae6716fa5506aac49dadd287229a45f0a3b94e. * cleaned up safety * cleanup subaru long safety * subaru_aeb -> stock_aeb for other platforms * fix unittests * pretty * need to upload this route * remove AEB stuff for now * remove unrequired rpm checks * add comment * added comment about acceleration and use throttle limit for rpm too * inactive_throttle_fix * Update board/safety/safety_subaru.h Co-authored-by: Shane Smiskol <shane@smiskol.com> * added comments about long limits * increase max brake * revert that * cleanup * rename throttle to gas * add safety replay * remove todo * rename throttle to gas * move subaru stuff out of common test * PR cleanup * added min gas * reduce initial complexity by not intercepting cruisecontrol or brake_status * create common gas brake safety test * remove unrelated subaru reference * also test below min_value * fix fwd hook test * better name and hardcoded * use same as acceleration limits * revert gm * remove callable * also limit rpm * remove safety replay for now * revert unrelated changes * pr cleanup * remove unused gasbrake safety test * fix ruff linter * PR cleanup * pr cleanup? * added common brake test and moved generic test to base safety class * remove duplicate test * wip * wip * rename longitudinalaccelsafety * revert limits correct test * fix controls allowed test * move gm over to long gas brake test * assert that limits are reasonable * fix typing * fix linter again * fix linter again * fix linter again * like to make it clear * typo * fix from merge * clearer names * dont need thesemore * subaru: use common test * fix merge issue * inactive gas from long limits * fix tests * linter * fix gen2 * fix linter * also test torque when doing long control * renamre transmision rpm * consistent whitespace * rename declarations too * rename transmission rpm * same line * actually is transmission rpm --------- Co-authored-by: Martin Lillepuu <martin@mlp.ee> Co-authored-by: Adeeb Shihadeh <adeebshihadeh@gmail.com> Co-authored-by: Shane Smiskol <shane@smiskol.com>
2026-02-18 17:23:52 +08:00 · 2023-08-15 18:51:13 -07:00
parent f4e63e7da5
commit 8bd3e93813
4 changed files with 105 additions and 7 deletions
--- a/board/safety/safety_subaru.h
+++ b/board/safety/safety_subaru.h
@@ -14,6 +14,16 @@ const SteeringLimits SUBARU_STEERING_LIMITS = SUBARU_STEERING_LIMITS_GENERATOR(2
 const SteeringLimits SUBARU_GEN2_STEERING_LIMITS = SUBARU_STEERING_LIMITS_GENERATOR(1000, 40, 40);


+const LongitudinalLimits SUBARU_LONG_LIMITS = {
+  .min_gas = 808,       // appears to be engine braking
+  .max_gas = 3400,      // approx  2 m/s^2 when maxing cruise_rpm and cruise_throttle
+  .inactive_gas = 1818, // this is zero acceleration
+  .max_brake = 600,     // approx -3.5 m/s^2
+
+  .min_transmission_rpm = 0,
+  .max_transmission_rpm = 2400,
+};
+
 #define MSG_SUBARU_Brake_Status          0x13c
 #define MSG_SUBARU_CruiseControl         0x240
 #define MSG_SUBARU_Throttle              0x40
@@ -39,6 +49,10 @@ const SteeringLimits SUBARU_GEN2_STEERING_LIMITS = SUBARU_STEERING_LIMITS_GENERA
  {MSG_SUBARU_ES_LKAS_State,   SUBARU_MAIN_BUS, 8}, \
  {MSG_SUBARU_ES_Infotainment, SUBARU_MAIN_BUS, 8}, \

+#define SUBARU_COMMON_LONG_TX_MSGS(alt_bus)         \
+  {MSG_SUBARU_ES_Brake,        SUBARU_MAIN_BUS, 8}, \
+  {MSG_SUBARU_ES_Status,       SUBARU_MAIN_BUS, 8}, \
+
 #define SUBARU_COMMON_ADDR_CHECKS(alt_bus)                                                                                                            \
  {.msg = {{MSG_SUBARU_Throttle,        SUBARU_MAIN_BUS, 8, .check_checksum = true, .max_counter = 15U, .expected_timestep = 10000U}, { 0 }, { 0 }}}, \
  {.msg = {{MSG_SUBARU_Steering_Torque, SUBARU_MAIN_BUS, 8, .check_checksum = true, .max_counter = 15U, .expected_timestep = 20000U}, { 0 }, { 0 }}}, \
@@ -51,6 +65,12 @@ const CanMsg SUBARU_TX_MSGS[] = {
 };
 #define SUBARU_TX_MSGS_LEN (sizeof(SUBARU_TX_MSGS) / sizeof(SUBARU_TX_MSGS[0]))

+const CanMsg SUBARU_LONG_TX_MSGS[] = {
+  SUBARU_COMMON_TX_MSGS(SUBARU_MAIN_BUS)
+  SUBARU_COMMON_LONG_TX_MSGS(SUBARU_MAIN_BUS)
+};
+#define SUBARU_LONG_TX_MSGS_LEN (sizeof(SUBARU_LONG_TX_MSGS) / sizeof(SUBARU_LONG_TX_MSGS[0]))
+
 const CanMsg SUBARU_GEN2_TX_MSGS[] = {
  SUBARU_COMMON_TX_MSGS(SUBARU_ALT_BUS)
 };
@@ -70,7 +90,10 @@ addr_checks subaru_gen2_rx_checks = {subaru_gen2_addr_checks, SUBARU_GEN2_ADDR_C


 const uint16_t SUBARU_PARAM_GEN2 = 1;
+const uint16_t SUBARU_PARAM_LONGITUDINAL = 2;
+
 bool subaru_gen2 = false;
+bool subaru_longitudinal = false;


 static uint32_t subaru_get_checksum(CANPacket_t *to_push) {
@@ -153,6 +176,8 @@ static int subaru_tx_hook(CANPacket_t *to_send) {

  if (subaru_gen2) {
    tx = msg_allowed(to_send, SUBARU_GEN2_TX_MSGS, SUBARU_GEN2_TX_MSGS_LEN);
+  } else if (subaru_longitudinal) {
+    tx = msg_allowed(to_send, SUBARU_LONG_TX_MSGS, SUBARU_LONG_TX_MSGS_LEN);
  } else {
    tx = msg_allowed(to_send, SUBARU_TX_MSGS, SUBARU_TX_MSGS_LEN);
  }
@@ -166,12 +191,31 @@ static int subaru_tx_hook(CANPacket_t *to_send) {
    violation |= steer_torque_cmd_checks(desired_torque, -1, limits);
  }

-  // Only allow ES_Distance when Cruise_Cancel is true, and Cruise_Throttle is "inactive" (1818)
-  if (addr == MSG_SUBARU_ES_Distance){
+  // check es_brake brake_pressure limits
+  if (addr == MSG_SUBARU_ES_Brake) {
+    int es_brake_pressure = GET_BYTES(to_send, 2, 2);
+    violation |= longitudinal_brake_checks(es_brake_pressure, SUBARU_LONG_LIMITS);
+  }
+
+  // check es_distance cruise_throttle limits
+  if (addr == MSG_SUBARU_ES_Distance) {
    int cruise_throttle = (GET_BYTES(to_send, 2, 2) & 0xFFFU);
    bool cruise_cancel = GET_BIT(to_send, 56U) != 0U;
-    violation |= (cruise_throttle != 1818);
-    violation |= (!cruise_cancel);
+    
+    if (subaru_longitudinal) {
+      violation |= longitudinal_gas_checks(cruise_throttle, SUBARU_LONG_LIMITS);
+    } else {
+      // If openpilot is not controlling long, only allow ES_Distance for cruise cancel requests,
+      // (when Cruise_Cancel is true, and Cruise_Throttle is inactive)
+      violation |= (cruise_throttle != SUBARU_LONG_LIMITS.inactive_gas);
+      violation |= (!cruise_cancel);
+    }
+  }
+
+  // check es_status transmission_rpm limits
+  if (addr == MSG_SUBARU_ES_Status) {
+    int transmission_rpm = (GET_BYTES(to_send, 2, 2) & 0xFFFU);
+    violation |= longitudinal_transmission_rpm_checks(transmission_rpm, SUBARU_LONG_LIMITS);
  }

  if (violation){
@@ -184,7 +228,7 @@ static int subaru_fwd_hook(int bus_num, int addr) {
  int bus_fwd = -1;

  if (bus_num == SUBARU_MAIN_BUS) {
-    bus_fwd = SUBARU_CAM_BUS;  // forward to camera
+    bus_fwd = SUBARU_CAM_BUS;  // to the eyesight camera
  }

  if (bus_num == SUBARU_CAM_BUS) {
@@ -193,7 +237,13 @@ static int subaru_fwd_hook(int bus_num, int addr) {
                       (addr == MSG_SUBARU_ES_DashStatus) ||
                       (addr == MSG_SUBARU_ES_LKAS_State) ||
                       (addr == MSG_SUBARU_ES_Infotainment));
-    if (!block_lkas) {
+
+    bool block_long = ((addr == MSG_SUBARU_ES_Brake) ||
+                       (addr == MSG_SUBARU_ES_Distance) ||
+                       (addr == MSG_SUBARU_ES_Status));
+
+    bool block_msg = block_lkas || (subaru_longitudinal && block_long);
+    if (!block_msg) {
      bus_fwd = SUBARU_MAIN_BUS;  // Main CAN
    }
  }
@@ -204,6 +254,10 @@ static int subaru_fwd_hook(int bus_num, int addr) {
 static const addr_checks* subaru_init(uint16_t param) {
  subaru_gen2 = GET_FLAG(param, SUBARU_PARAM_GEN2);

+#ifdef ALLOW_DEBUG
+  subaru_longitudinal = GET_FLAG(param, SUBARU_PARAM_LONGITUDINAL) && !subaru_gen2;
+#endif
+
  if (subaru_gen2) {
    subaru_rx_checks = (addr_checks){subaru_gen2_addr_checks, SUBARU_GEN2_ADDR_CHECK_LEN};
  } else {
--- a/python/init.py
+++ b/python/init.py
@@ -248,6 +248,7 @@ class Panda:
  FLAG_CHRYSLER_RAM_HD = 2

  FLAG_SUBARU_GEN2 = 1
+  FLAG_SUBARU_LONG = 2

  FLAG_GM_HW_CAM = 1
  FLAG_GM_HW_CAM_LONG = 2
--- a/tests/safety/common.py
+++ b/tests/safety/common.py
@@ -954,7 +954,8 @@ class PandaSafetyTest(PandaSafetyTestBase):
              continue
            if attr.startswith('TestToyota') and current_test.startswith('TestToyota'):
              continue
-            if {attr, current_test}.issubset({'TestSubaruGen1TorqueStockLongitudinalSafety', 'TestSubaruGen2TorqueStockLongitudinalSafety'}):
+            if {attr, current_test}.issubset({'TestSubaruGen1TorqueStockLongitudinalSafety', 'TestSubaruGen2TorqueStockLongitudinalSafety',
+                                              'TestSubaruGen1LongitudinalSafety'}):
              continue
            if {attr, current_test}.issubset({'TestVolkswagenPqSafety', 'TestVolkswagenPqStockSafety', 'TestVolkswagenPqLongSafety'}):
              continue
--- a/tests/safety/test_subaru.py
+++ b/tests/safety/test_subaru.py
@@ -32,6 +32,9 @@ def lkas_tx_msgs(alt_bus):
          [MSG_SUBARU_ES_LKAS_State,    SUBARU_MAIN_BUS],
          [MSG_SUBARU_ES_Infotainment,  SUBARU_MAIN_BUS]]

+def long_tx_msgs():
+  return [[MSG_SUBARU_ES_Brake,         SUBARU_MAIN_BUS],
+          [MSG_SUBARU_ES_Status,        SUBARU_MAIN_BUS]]

 class TestSubaruSafetyBase(common.PandaSafetyTest, MeasurementSafetyTest):
  FLAGS = 0
@@ -100,6 +103,40 @@ class TestSubaruStockLongitudinalSafetyBase(TestSubaruSafetyBase):
      self._generic_limit_safety_check(partial(self._cancel_msg, cancel), self.INACTIVE_GAS, self.INACTIVE_GAS, 0, 2**12, 1, self.INACTIVE_GAS, cancel)


+class TestSubaruLongitudinalSafetyBase(TestSubaruSafetyBase, common.LongitudinalGasBrakeSafetyTest):
+  MIN_GAS = 808
+  MAX_GAS = 3400
+  INACTIVE_GAS = 1818
+  MAX_POSSIBLE_GAS = 2**12
+
+  MIN_BRAKE = 0
+  MAX_BRAKE = 600
+  MAX_POSSIBLE_BRAKE = 2**16
+
+  MIN_RPM = 0
+  MAX_RPM = 2400
+  MAX_POSSIBLE_RPM = 2**12
+
+  FWD_BLACKLISTED_ADDRS = {2: [MSG_SUBARU_ES_LKAS, MSG_SUBARU_ES_Brake, MSG_SUBARU_ES_Distance,
+                               MSG_SUBARU_ES_Status, MSG_SUBARU_ES_DashStatus,
+                               MSG_SUBARU_ES_LKAS_State, MSG_SUBARU_ES_Infotainment]}
+
+  def test_rpm_safety_check(self):
+    self._generic_limit_safety_check(self._send_rpm_msg, self.MIN_RPM, self.MAX_RPM, 0, self.MAX_POSSIBLE_RPM, 1)
+
+  def _send_brake_msg(self, brake):
+    values = {"Brake_Pressure": brake}
+    return self.packer.make_can_msg_panda("ES_Brake", self.ALT_BUS, values)
+
+  def _send_gas_msg(self, gas):
+    values = {"Cruise_Throttle": gas}
+    return self.packer.make_can_msg_panda("ES_Distance", self.ALT_BUS, values)
+
+  def _send_rpm_msg(self, rpm):
+    values = {"Cruise_RPM": rpm}
+    return self.packer.make_can_msg_panda("ES_Status", self.ALT_BUS, values)
+
+
 class TestSubaruTorqueSafetyBase(TestSubaruSafetyBase, common.DriverTorqueSteeringSafetyTest):
  MAX_RATE_UP = 50
  MAX_RATE_DOWN = 70
@@ -126,5 +163,10 @@ class TestSubaruGen2TorqueStockLongitudinalSafety(TestSubaruStockLongitudinalSaf
  TX_MSGS = lkas_tx_msgs(SUBARU_ALT_BUS)


+class TestSubaruGen1LongitudinalSafety(TestSubaruLongitudinalSafetyBase, TestSubaruTorqueSafetyBase):
+  FLAGS = Panda.FLAG_SUBARU_LONG
+  TX_MSGS = lkas_tx_msgs(SUBARU_MAIN_BUS) + long_tx_msgs()
+
+
 if __name__ == "__main__":
  unittest.main()