From 688c4e4bbbd0ad97ff164f56e418c4b3527b4d68 Mon Sep 17 00:00:00 2001 From: Jason Young Date: Wed, 25 Sep 2024 22:28:20 -0400 Subject: [PATCH] from @pd0wm commaai/panda#1841 --- board/safety/safety_toyota.h | 69 ++++++++++++++++++++++++++---------- python/__init__.py | 3 +- tests/safety/test_toyota.py | 8 ++--- 3 files changed, 57 insertions(+), 23 deletions(-) diff --git a/board/safety/safety_toyota.h b/board/safety/safety_toyota.h index 73fb730372..4ef04769d1 100644 --- a/board/safety/safety_toyota.h +++ b/board/safety/safety_toyota.h @@ -4,7 +4,7 @@ // Stock longitudinal #define TOYOTA_COMMON_TX_MSGS \ - {0x2E4, 0, 5}, {0x191, 0, 8}, {0x412, 0, 8}, {0x343, 0, 8}, {0x1D2, 0, 8}, /* LKAS + LTA + ACC & PCM cancel cmds */ \ + {0x2E4, 0, 5}, {0x2E4, 0, 8}, {0x131, 0, 8}, {0x191, 0, 8}, {0x412, 0, 8}, {0x343, 0, 8}, {0x1D2, 0, 8}, /* LKAS + LTA + ACC & PCM cancel cmds */ \ #define TOYOTA_COMMON_LONG_TX_MSGS \ TOYOTA_COMMON_TX_MSGS \ @@ -16,13 +16,18 @@ #define TOYOTA_COMMON_RX_CHECKS(lta) \ {.msg = {{ 0xaa, 0, 8, .check_checksum = false, .frequency = 83U}, { 0 }, { 0 }}}, \ {.msg = {{0x260, 0, 8, .check_checksum = true, .quality_flag = (lta), .frequency = 50U}, { 0 }, { 0 }}}, \ - {.msg = {{0x1D2, 0, 8, .check_checksum = true, .frequency = 33U}, { 0 }, { 0 }}}, \ - {.msg = {{0x224, 0, 8, .check_checksum = false, .frequency = 40U}, \ - {0x226, 0, 8, .check_checksum = false, .frequency = 40U}, { 0 }}}, \ - -static bool toyota_alt_brake = false; + {.msg = {{0x1D2, 0, 8, .check_checksum = true, .frequency = 33U}, {0x176, 0, 8, .check_checksum = true, .frequency = 32U}, { 0 }}},\ + {.msg = {{0x101, 0, 8, .check_checksum = false, .frequency = 50U}, \ + {0x224, 0, 8, .check_checksum = false, .frequency = 40U}, \ + {0x226, 0, 8, .check_checksum = false, .frequency = 40U}}}, \ + +static bool toyota_alt_brake_224 = false; +static bool toyota_alt_brake_101 = false; +static bool toyota_alt_pcm_cruise_176 = false; +static bool toyota_alt_gas_pedal_116 = false; static bool toyota_stock_longitudinal = false; static bool toyota_lta = false; + static int toyota_dbc_eps_torque_factor = 100; // conversion factor for STEER_TORQUE_EPS in %: see dbc file static uint32_t toyota_compute_checksum(const CANPacket_t *to_push) { @@ -87,14 +92,21 @@ static void toyota_rx_hook(const CANPacket_t *to_push) { } // enter controls on rising edge of ACC, exit controls on ACC off - // exit controls on rising edge of gas press - if (addr == 0x1D2) { - // 5th bit is CRUISE_ACTIVE + if (!toyota_alt_pcm_cruise_176 && (addr == 0x1D2)) { bool cruise_engaged = GET_BIT(to_push, 5U); pcm_cruise_check(cruise_engaged); + } + if (toyota_alt_pcm_cruise_176 && (addr == 0x176)) { + bool cruise_engaged = GET_BIT(to_push, 5U); + pcm_cruise_check(cruise_engaged); + } - // sample gas pedal - gas_pressed = !GET_BIT(to_push, 4U); + // exit controls on rising edge of gas press + if (!toyota_alt_gas_pedal_116 && (addr == 0x1D2)) { + gas_pressed = !GET_BIT(to_push, 4U); // GAS_PEDAL.GAS_RELEASED + } + if (toyota_alt_gas_pedal_116 && (addr == 0x116)) { + gas_pressed = GET_BYTE(to_push, 1) != 0U; // GAS_PEDAL.GAS_PEDAL_USER } // sample speed @@ -111,10 +123,15 @@ static void toyota_rx_hook(const CANPacket_t *to_push) { UPDATE_VEHICLE_SPEED(speed / 4.0 * 0.01 / 3.6); } - // most cars have brake_pressed on 0x226, corolla and rav4 on 0x224 - if (((addr == 0x224) && toyota_alt_brake) || ((addr == 0x226) && !toyota_alt_brake)) { - uint8_t bit = (addr == 0x224) ? 5U : 37U; - brake_pressed = GET_BIT(to_push, bit); + // most cars have brake_pressed on 0x226, corolla and rav4 on 0x224, rav4 prime on 0x101 + if (toyota_alt_brake_224 && !toyota_alt_brake_101 && (addr == 0x224)) { + brake_pressed = GET_BIT(to_push, 5U); + } + if (!toyota_alt_brake_224 && toyota_alt_brake_101 && (addr == 0x101)) { + brake_pressed = GET_BIT(to_push, 3U); + } + if (!toyota_alt_brake_224 && !toyota_alt_brake_101 && (addr == 0x226)) { + brake_pressed = GET_BIT(to_push, 37U); } bool stock_ecu_detected = addr == 0x2E4; // STEERING_LKA @@ -203,6 +220,14 @@ static bool toyota_tx_hook(const CANPacket_t *to_send) { } } + // LTA angle steering check for SecOC cars + if (addr == 0x131) { + // Block any form of actuation for now + if (GET_BYTE(to_send, 0) != 0U) { + tx = false; + } + } + // LTA angle steering check if (addr == 0x191) { // check the STEER_REQUEST, STEER_REQUEST_2, TORQUE_WIND_DOWN, STEER_ANGLE_CMD signals @@ -294,12 +319,19 @@ static safety_config toyota_init(uint16_t param) { // first byte is for EPS factor, second is for flags const uint32_t TOYOTA_PARAM_OFFSET = 8U; const uint32_t TOYOTA_EPS_FACTOR = (1UL << TOYOTA_PARAM_OFFSET) - 1U; - const uint32_t TOYOTA_PARAM_ALT_BRAKE = 1UL << TOYOTA_PARAM_OFFSET; + const uint32_t TOYOTA_PARAM_ALT_BRAKE_224 = 1UL << TOYOTA_PARAM_OFFSET; const uint32_t TOYOTA_PARAM_STOCK_LONGITUDINAL = 2UL << TOYOTA_PARAM_OFFSET; const uint32_t TOYOTA_PARAM_LTA = 4UL << TOYOTA_PARAM_OFFSET; + const uint32_t TOYOTA_PARAM_SECOC_CAR = 8UL << TOYOTA_PARAM_OFFSET; - toyota_alt_brake = GET_FLAG(param, TOYOTA_PARAM_ALT_BRAKE); + toyota_alt_brake_224 = GET_FLAG(param, TOYOTA_PARAM_ALT_BRAKE_224); toyota_stock_longitudinal = GET_FLAG(param, TOYOTA_PARAM_STOCK_LONGITUDINAL); + + bool secoc_car = GET_FLAG(param, TOYOTA_PARAM_SECOC_CAR); + toyota_alt_brake_101 = secoc_car; + toyota_alt_pcm_cruise_176 = secoc_car; + toyota_alt_gas_pedal_116 = secoc_car; + toyota_lta = GET_FLAG(param, TOYOTA_PARAM_LTA); toyota_dbc_eps_torque_factor = param & TOYOTA_EPS_FACTOR; @@ -339,7 +371,8 @@ static int toyota_fwd_hook(int bus_num, int addr) { if (bus_num == 2) { // block stock lkas messages and stock acc messages (if OP is doing ACC) // in TSS2, 0x191 is LTA which we need to block to avoid controls collision - bool is_lkas_msg = ((addr == 0x2E4) || (addr == 0x412) || (addr == 0x191)); + // on SecOC cars 0x131 is also LTA + bool is_lkas_msg = ((addr == 0x2E4) || (addr == 0x412) || (addr == 0x191) || (addr == 0x131)); // in TSS2 the camera does ACC as well, so filter 0x343 bool is_acc_msg = (addr == 0x343); bool block_msg = is_lkas_msg || (is_acc_msg && !toyota_stock_longitudinal); diff --git a/python/__init__.py b/python/__init__.py index d2bbb2fe45..f07bb821bd 100644 --- a/python/__init__.py +++ b/python/__init__.py @@ -184,9 +184,10 @@ class Panda: HARNESS_STATUS_FLIPPED = 2 # first byte is for EPS scaling factor - FLAG_TOYOTA_ALT_BRAKE = (1 << 8) + FLAG_TOYOTA_ALT_BRAKE_224 = (1 << 8) FLAG_TOYOTA_STOCK_LONGITUDINAL = (2 << 8) FLAG_TOYOTA_LTA = (4 << 8) + FLAG_TOYOTA_SECOC_CAR = (8 << 8) FLAG_HONDA_ALT_BRAKE = 1 FLAG_HONDA_BOSCH_LONG = 2 diff --git a/tests/safety/test_toyota.py b/tests/safety/test_toyota.py index 80bf9ce9a1..8ebe4010fc 100755 --- a/tests/safety/test_toyota.py +++ b/tests/safety/test_toyota.py @@ -9,7 +9,7 @@ import panda.tests.safety.common as common from panda.tests.safety.common import CANPackerPanda -TOYOTA_COMMON_TX_MSGS = [[0x2E4, 0], [0x191, 0], [0x412, 0], [0x343, 0], [0x1D2, 0]] # LKAS + LTA + ACC & PCM cancel cmds +TOYOTA_COMMON_TX_MSGS = [[0x2E4, 0], [0x131, 0], [0x191, 0], [0x412, 0], [0x343, 0], [0x1D2, 0]] # LKAS + LTA + ACC & PCM cancel cmds TOYOTA_COMMON_LONG_TX_MSGS = [[0x283, 0], [0x2E6, 0], [0x2E7, 0], [0x33E, 0], [0x344, 0], [0x365, 0], [0x366, 0], [0x4CB, 0], # DSU bus 0 [0x128, 1], [0x141, 1], [0x160, 1], [0x161, 1], [0x470, 1], # DSU bus 1 [0x411, 0], # PCS_HUD @@ -21,7 +21,7 @@ class TestToyotaSafetyBase(common.PandaCarSafetyTest, common.LongitudinalAccelSa TX_MSGS = TOYOTA_COMMON_TX_MSGS + TOYOTA_COMMON_LONG_TX_MSGS STANDSTILL_THRESHOLD = 0 # kph RELAY_MALFUNCTION_ADDRS = {0: (0x2E4, 0x343)} - FWD_BLACKLISTED_ADDRS = {2: [0x2E4, 0x412, 0x191, 0x343]} + FWD_BLACKLISTED_ADDRS = {2: [0x2E4, 0x412, 0x131, 0x191, 0x343]} FWD_BUS_LOOKUP = {0: 2, 2: 0} EPS_SCALE = 73 @@ -266,7 +266,7 @@ class TestToyotaAltBrakeSafety(TestToyotaSafetyTorque): def setUp(self): self.packer = CANPackerPanda("toyota_new_mc_pt_generated") self.safety = libpanda_py.libpanda - self.safety.set_safety_hooks(Panda.SAFETY_TOYOTA, self.EPS_SCALE | Panda.FLAG_TOYOTA_ALT_BRAKE) + self.safety.set_safety_hooks(Panda.SAFETY_TOYOTA, self.EPS_SCALE | Panda.FLAG_TOYOTA_ALT_BRAKE_224) self.safety.init_tests() def _user_brake_msg(self, brake): @@ -283,7 +283,7 @@ class TestToyotaStockLongitudinalBase(TestToyotaSafetyBase): TX_MSGS = TOYOTA_COMMON_TX_MSGS # Base addresses minus ACC_CONTROL (0x343) RELAY_MALFUNCTION_ADDRS = {0: (0x2E4,)} - FWD_BLACKLISTED_ADDRS = {2: [0x2E4, 0x412, 0x191]} + FWD_BLACKLISTED_ADDRS = {2: [0x2E4, 0x412, 0x191, 0x131]} def test_diagnostics(self, stock_longitudinal: bool = True): super().test_diagnostics(stock_longitudinal=stock_longitudinal)