diff --git a/Dockerfile b/Dockerfile index 670283dd47..68362f8cfd 100644 --- a/Dockerfile +++ b/Dockerfile @@ -37,7 +37,7 @@ RUN pip3 install --break-system-packages --no-cache-dir $PYTHONPATH/panda/[dev] # TODO: this should be a "pip install" or not even in this repo at all RUN git config --global --add safe.directory $PYTHONPATH/panda -ENV OPENDBC_REF="5ed7a834a4e0e24c3968dd1e98ceb4b9d5f9791a" +ENV OPENDBC_REF="e1ce3619a5db661ef2b406ccf258a253baf6eebc" RUN cd /tmp/ && \ git clone --depth 1 https://github.com/commaai/opendbc opendbc_repo && \ cd opendbc_repo && git fetch origin $OPENDBC_REF && git checkout FETCH_HEAD && rm -rf .git/ && \ diff --git a/board/safety/safety_toyota.h b/board/safety/safety_toyota.h index 73fb730372..7008bf8419 100644 --- a/board/safety/safety_toyota.h +++ b/board/safety/safety_toyota.h @@ -3,8 +3,16 @@ #include "safety_declarations.h" // 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 */ \ +#define TOYOTA_BASE_TX_MSGS \ + {0x191, 0, 8}, {0x412, 0, 8}, {0x343, 0, 8}, {0x1D2, 0, 8}, /* LKAS + LTA + ACC & PCM cancel cmds */ \ + +#define TOYOTA_COMMON_TX_MSGS \ + TOYOTA_BASE_TX_MSGS \ + {0x2E4, 0, 5}, \ + +#define TOYOTA_COMMON_SECOC_TX_MSGS \ + TOYOTA_BASE_TX_MSGS \ + {0x2E4, 0, 8}, {0x131, 0, 8}, \ #define TOYOTA_COMMON_LONG_TX_MSGS \ TOYOTA_COMMON_TX_MSGS \ @@ -16,10 +24,13 @@ #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 }}}, \ + {.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_secoc = false; static bool toyota_alt_brake = false; static bool toyota_stock_longitudinal = false; static bool toyota_lta = false; @@ -87,14 +98,31 @@ 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 - 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 not alternative experience + // exit controls on rising edge of brake press + if (toyota_secoc) { + if (addr == 0x176) { + bool cruise_engaged = GET_BIT(to_push, 5U); // PCM_CRUISE.CRUISE_ACTIVE + pcm_cruise_check(cruise_engaged); + } + if (addr == 0x116) { + gas_pressed = GET_BYTE(to_push, 1) != 0U; // GAS_PEDAL.GAS_PEDAL_USER + } + if (addr == 0x101) { + brake_pressed = GET_BIT(to_push, 3U); // BRAKE_MODULE.BRAKE_PRESSED (toyota_rav4_prime_generated.dbc) + } + } else { + if (addr == 0x1D2) { + bool cruise_engaged = GET_BIT(to_push, 5U); // PCM_CRUISE.CRUISE_ACTIVE + pcm_cruise_check(cruise_engaged); + gas_pressed = !GET_BIT(to_push, 4U); // PCM_CRUISE.GAS_RELEASED + } + if (!toyota_alt_brake && (addr == 0x226)) { + brake_pressed = GET_BIT(to_push, 37U); // BRAKE_MODULE.BRAKE_PRESSED (toyota_nodsu_pt_generated.dbc) + } + if (toyota_alt_brake && (addr == 0x224)) { + brake_pressed = GET_BIT(to_push, 5U); // BRAKE_MODULE.BRAKE_PRESSED (toyota_new_mc_pt_generated.dbc) + } } // sample speed @@ -111,12 +139,6 @@ 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); - } - bool stock_ecu_detected = addr == 0x2E4; // STEERING_LKA if (!toyota_stock_longitudinal && (addr == 0x343)) { stock_ecu_detected = true; // ACC_CONTROL @@ -203,7 +225,7 @@ static bool toyota_tx_hook(const CANPacket_t *to_send) { } } - // LTA angle steering check + // STEERING_LTA angle steering check if (addr == 0x191) { // check the STEER_REQUEST, STEER_REQUEST_2, TORQUE_WIND_DOWN, STEER_ANGLE_CMD signals bool lta_request = GET_BIT(to_send, 0U); @@ -251,6 +273,20 @@ static bool toyota_tx_hook(const CANPacket_t *to_send) { } } + // STEERING_LTA_2 angle steering check (SecOC) + if (toyota_secoc && (addr == 0x131)) { + // SecOC cars block any form of LTA actuation for now + bool lta_request = GET_BIT(to_send, 3U); // STEERING_LTA_2.STEER_REQUEST + bool lta_request2 = GET_BIT(to_send, 0U); // STEERING_LTA_2.STEER_REQUEST_2 + int lta_angle_msb = GET_BYTE(to_send, 2); // STEERING_LTA_2.STEER_ANGLE_CMD (MSB) + int lta_angle_lsb = GET_BYTE(to_send, 3); // STEERING_LTA_2.STEER_ANGLE_CMD (LSB) + + bool actuation = lta_request || lta_request2 || (lta_angle_msb != 0) || (lta_angle_lsb != 0); + if (actuation) { + tx = false; + } + } + // STEER: safety check on bytes 2-3 if (addr == 0x2E4) { int desired_torque = (GET_BYTE(to_send, 1) << 8) | GET_BYTE(to_send, 2); @@ -286,6 +322,10 @@ static safety_config toyota_init(uint16_t param) { TOYOTA_COMMON_TX_MSGS }; + static const CanMsg TOYOTA_SECOC_TX_MSGS[] = { + TOYOTA_COMMON_SECOC_TX_MSGS + }; + static const CanMsg TOYOTA_LONG_TX_MSGS[] = { TOYOTA_COMMON_LONG_TX_MSGS }; @@ -298,6 +338,11 @@ static safety_config toyota_init(uint16_t param) { const uint32_t TOYOTA_PARAM_STOCK_LONGITUDINAL = 2UL << TOYOTA_PARAM_OFFSET; const uint32_t TOYOTA_PARAM_LTA = 4UL << TOYOTA_PARAM_OFFSET; +#ifdef ALLOW_DEBUG + const uint32_t TOYOTA_PARAM_SECOC = 8UL << TOYOTA_PARAM_OFFSET; + toyota_secoc = GET_FLAG(param, TOYOTA_PARAM_SECOC); +#endif + toyota_alt_brake = GET_FLAG(param, TOYOTA_PARAM_ALT_BRAKE); toyota_stock_longitudinal = GET_FLAG(param, TOYOTA_PARAM_STOCK_LONGITUDINAL); toyota_lta = GET_FLAG(param, TOYOTA_PARAM_LTA); @@ -305,7 +350,11 @@ static safety_config toyota_init(uint16_t param) { safety_config ret; if (toyota_stock_longitudinal) { - SET_TX_MSGS(TOYOTA_TX_MSGS, ret); + if (toyota_secoc) { + SET_TX_MSGS(TOYOTA_SECOC_TX_MSGS, ret); + } else { + SET_TX_MSGS(TOYOTA_TX_MSGS, ret); + } } else { SET_TX_MSGS(TOYOTA_LONG_TX_MSGS, ret); } @@ -340,6 +389,8 @@ static int toyota_fwd_hook(int bus_num, int addr) { // 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 + is_lkas_msg |= toyota_secoc && (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..692691e914 100644 --- a/python/__init__.py +++ b/python/__init__.py @@ -187,6 +187,7 @@ class Panda: FLAG_TOYOTA_ALT_BRAKE = (1 << 8) FLAG_TOYOTA_STOCK_LONGITUDINAL = (2 << 8) FLAG_TOYOTA_LTA = (4 << 8) + FLAG_TOYOTA_SECOC = (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..e60b29c5c2 100755 --- a/tests/safety/test_toyota.py +++ b/tests/safety/test_toyota.py @@ -10,6 +10,7 @@ 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_SECOC_TX_MSGS = [[0x131, 0]] + TOYOTA_COMMON_TX_MSGS 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 @@ -107,7 +108,8 @@ def test_lta_steer_cmd(self): self.safety.set_controls_allowed(engaged) should_tx = not req and not req2 and angle == 0 and torque_wind_down == 0 - self.assertEqual(should_tx, self._tx(self._lta_msg(req, req2, angle, torque_wind_down))) + self.assertEqual(should_tx, self._tx(self._lta_msg(req, req2, angle, torque_wind_down)), + f"{req=} {req2=} {angle=} {torque_wind_down=}") def test_rx_hook(self): # checksum checks @@ -324,5 +326,38 @@ def setUp(self): self.safety.init_tests() +class TestToyotaSecOcSafety(TestToyotaStockLongitudinalBase): + + TX_MSGS = TOYOTA_SECOC_TX_MSGS + RELAY_MALFUNCTION_ADDRS = {0: (0x2E4,)} + FWD_BLACKLISTED_ADDRS = {2: [0x2E4, 0x412, 0x191, 0x131]} + + def setUp(self): + self.packer = CANPackerPanda("toyota_rav4_prime_generated") + self.safety = libpanda_py.libpanda + self.safety.set_safety_hooks(Panda.SAFETY_TOYOTA, self.EPS_SCALE | Panda.FLAG_TOYOTA_STOCK_LONGITUDINAL | Panda.FLAG_TOYOTA_SECOC) + self.safety.init_tests() + + # This platform also has alternate brake and PCM messages, but same naming in the DBC, so same packers work + + def _user_gas_msg(self, gas): + values = {"GAS_PEDAL_USER": gas} + return self.packer.make_can_msg_panda("GAS_PEDAL", 0, values) + + # This platform sends both STEERING_LTA (same as other Toyota) and STEERING_LTA_2 (SecOC signed) + # STEERING_LTA is checked for no-actuation by the base class, STEERING_LTA_2 is checked for no-actuation below + + def _lta_2_msg(self, req, req2, angle_cmd, torque_wind_down=100): + values = {"STEER_REQUEST": req, "STEER_REQUEST_2": req2, "STEER_ANGLE_CMD": angle_cmd} + return self.packer.make_can_msg_panda("STEERING_LTA_2", 0, values) + + def test_lta_2_steer_cmd(self): + for engaged, req, req2, angle in itertools.product([True, False], [0, 1], [0, 1], np.linspace(-20, 20, 5)): + self.safety.set_controls_allowed(engaged) + + should_tx = not req and not req2 and angle == 0 + self.assertEqual(should_tx, self._tx(self._lta_2_msg(req, req2, angle)), f"{req=} {req2=} {angle=}") + + if __name__ == "__main__": unittest.main()