From 8bd3e938139d131192da2da9d6c20fa029694fb2 Mon Sep 17 00:00:00 2001 From: Justin Newberry Date: Tue, 15 Aug 2023 18:51:13 -0700 Subject: [PATCH] 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 2a5058d858b36ff7dc5477119a54f0aab137b266. * Revert "cleanup fw filename conventions (#1434)" This reverts commit 4dd2735e38b63940ca1e1f42b4522ca4c1a0a1cc. * 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 * 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 Co-authored-by: Adeeb Shihadeh Co-authored-by: Shane Smiskol --- board/safety/safety_subaru.h | 66 ++++++++++++++++++++++++++++++++---- python/__init__.py | 1 + tests/safety/common.py | 3 +- tests/safety/test_subaru.py | 42 +++++++++++++++++++++++ 4 files changed, 105 insertions(+), 7 deletions(-) diff --git a/board/safety/safety_subaru.h b/board/safety/safety_subaru.h index 864d041d..7a643625 100644 --- 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 { diff --git a/python/__init__.py b/python/__init__.py index 40066af4..aa9be43a 100644 --- 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 diff --git a/tests/safety/common.py b/tests/safety/common.py index 2ff771d5..36df296e 100644 --- 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 diff --git a/tests/safety/test_subaru.py b/tests/safety/test_subaru.py index bbf9143f..f5c30e6e 100755 --- 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()