Skip to content

Commit abdc418

Browse files
jyoung8607sshane
jyoung8607
and
sshane
authored
Toyota: 2021+ RAV4 Prime (commaai#2042)
* from @pd0wm commaai#1841 * diff reduction: won't need the flag rename * run tests with new DBC and new gas/brake messages * toyota_secoc_car to global * consolidate alt_brake_101 * consolidate alt_pcm_cruise_176 * consolidate alt_gas_pedal_116 * don't allow transmit/forward for 0x131 unless SecOC * cleanup and todo * diff reduction * reorder by usage frequency * test for no LTA actuation * bump opendbc commit ref in Dockerfile * gate SecOC variant on ALLOW_DEBUG * tweak gating for MISRA * mutation test hates lta_angle surviving various changes * common rx checks styling * don't allow short version of 0x2E5 for SecOC mode * whitespace diff reduction * secoc_car -> secoc * fix comment typo * retry CI * missed a couple secoc_car -> secoc * one big secoc/not-secoc block * genuinely useful MISRA warning * test both STEERING_LTA and STEERING_LTA_2 * comment labeling for STEERING_LTA_2 signals * Update board/safety/safety_toyota.h Co-authored-by: Shane Smiskol <[email protected]> * STEERING_LTA and STEERING_LTA_2 consistency * update gas/brake/cruise signal annotations --------- Co-authored-by: Shane Smiskol <[email protected]>
1 parent c77d383 commit abdc418

File tree

4 files changed

+110
-23
lines changed

4 files changed

+110
-23
lines changed

‎Dockerfile

+1-1
Original file line numberDiff line numberDiff line change
@@ -37,7 +37,7 @@ RUN pip3 install --break-system-packages --no-cache-dir $PYTHONPATH/panda/[dev]
3737

3838
# TODO: this should be a "pip install" or not even in this repo at all
3939
RUN git config --global --add safe.directory $PYTHONPATH/panda
40-
ENV OPENDBC_REF="5ed7a834a4e0e24c3968dd1e98ceb4b9d5f9791a"
40+
ENV OPENDBC_REF="e1ce3619a5db661ef2b406ccf258a253baf6eebc"
4141
RUN cd /tmp/ && \
4242
git clone --depth 1 https://github.com/commaai/opendbc opendbc_repo && \
4343
cd opendbc_repo && git fetch origin $OPENDBC_REF && git checkout FETCH_HEAD && rm -rf .git/ && \

‎board/safety/safety_toyota.h

+72-21
Original file line numberDiff line numberDiff line change
@@ -3,8 +3,16 @@
33
#include "safety_declarations.h"
44

55
// Stock longitudinal
6-
#define TOYOTA_COMMON_TX_MSGS \
7-
{0x2E4, 0, 5}, {0x191, 0, 8}, {0x412, 0, 8}, {0x343, 0, 8}, {0x1D2, 0, 8}, /* LKAS + LTA + ACC & PCM cancel cmds */ \
6+
#define TOYOTA_BASE_TX_MSGS \
7+
{0x191, 0, 8}, {0x412, 0, 8}, {0x343, 0, 8}, {0x1D2, 0, 8}, /* LKAS + LTA + ACC & PCM cancel cmds */ \
8+
9+
#define TOYOTA_COMMON_TX_MSGS \
10+
TOYOTA_BASE_TX_MSGS \
11+
{0x2E4, 0, 5}, \
12+
13+
#define TOYOTA_COMMON_SECOC_TX_MSGS \
14+
TOYOTA_BASE_TX_MSGS \
15+
{0x2E4, 0, 8}, {0x131, 0, 8}, \
816

917
#define TOYOTA_COMMON_LONG_TX_MSGS \
1018
TOYOTA_COMMON_TX_MSGS \
@@ -16,10 +24,13 @@
1624
#define TOYOTA_COMMON_RX_CHECKS(lta) \
1725
{.msg = {{ 0xaa, 0, 8, .check_checksum = false, .frequency = 83U}, { 0 }, { 0 }}}, \
1826
{.msg = {{0x260, 0, 8, .check_checksum = true, .quality_flag = (lta), .frequency = 50U}, { 0 }, { 0 }}}, \
19-
{.msg = {{0x1D2, 0, 8, .check_checksum = true, .frequency = 33U}, { 0 }, { 0 }}}, \
20-
{.msg = {{0x224, 0, 8, .check_checksum = false, .frequency = 40U}, \
21-
{0x226, 0, 8, .check_checksum = false, .frequency = 40U}, { 0 }}}, \
27+
{.msg = {{0x1D2, 0, 8, .check_checksum = true, .frequency = 33U}, \
28+
{0x176, 0, 8, .check_checksum = true, .frequency = 32U}, { 0 }}}, \
29+
{.msg = {{0x101, 0, 8, .check_checksum = false, .frequency = 50U}, \
30+
{0x224, 0, 8, .check_checksum = false, .frequency = 40U}, \
31+
{0x226, 0, 8, .check_checksum = false, .frequency = 40U}}}, \
2232

33+
static bool toyota_secoc = false;
2334
static bool toyota_alt_brake = false;
2435
static bool toyota_stock_longitudinal = false;
2536
static bool toyota_lta = false;
@@ -87,14 +98,31 @@ static void toyota_rx_hook(const CANPacket_t *to_push) {
8798
}
8899

89100
// enter controls on rising edge of ACC, exit controls on ACC off
90-
// exit controls on rising edge of gas press
91-
if (addr == 0x1D2) {
92-
// 5th bit is CRUISE_ACTIVE
93-
bool cruise_engaged = GET_BIT(to_push, 5U);
94-
pcm_cruise_check(cruise_engaged);
95-
96-
// sample gas pedal
97-
gas_pressed = !GET_BIT(to_push, 4U);
101+
// exit controls on rising edge of gas press, if not alternative experience
102+
// exit controls on rising edge of brake press
103+
if (toyota_secoc) {
104+
if (addr == 0x176) {
105+
bool cruise_engaged = GET_BIT(to_push, 5U); // PCM_CRUISE.CRUISE_ACTIVE
106+
pcm_cruise_check(cruise_engaged);
107+
}
108+
if (addr == 0x116) {
109+
gas_pressed = GET_BYTE(to_push, 1) != 0U; // GAS_PEDAL.GAS_PEDAL_USER
110+
}
111+
if (addr == 0x101) {
112+
brake_pressed = GET_BIT(to_push, 3U); // BRAKE_MODULE.BRAKE_PRESSED (toyota_rav4_prime_generated.dbc)
113+
}
114+
} else {
115+
if (addr == 0x1D2) {
116+
bool cruise_engaged = GET_BIT(to_push, 5U); // PCM_CRUISE.CRUISE_ACTIVE
117+
pcm_cruise_check(cruise_engaged);
118+
gas_pressed = !GET_BIT(to_push, 4U); // PCM_CRUISE.GAS_RELEASED
119+
}
120+
if (!toyota_alt_brake && (addr == 0x226)) {
121+
brake_pressed = GET_BIT(to_push, 37U); // BRAKE_MODULE.BRAKE_PRESSED (toyota_nodsu_pt_generated.dbc)
122+
}
123+
if (toyota_alt_brake && (addr == 0x224)) {
124+
brake_pressed = GET_BIT(to_push, 5U); // BRAKE_MODULE.BRAKE_PRESSED (toyota_new_mc_pt_generated.dbc)
125+
}
98126
}
99127

100128
// sample speed
@@ -111,12 +139,6 @@ static void toyota_rx_hook(const CANPacket_t *to_push) {
111139
UPDATE_VEHICLE_SPEED(speed / 4.0 * 0.01 / 3.6);
112140
}
113141

114-
// most cars have brake_pressed on 0x226, corolla and rav4 on 0x224
115-
if (((addr == 0x224) && toyota_alt_brake) || ((addr == 0x226) && !toyota_alt_brake)) {
116-
uint8_t bit = (addr == 0x224) ? 5U : 37U;
117-
brake_pressed = GET_BIT(to_push, bit);
118-
}
119-
120142
bool stock_ecu_detected = addr == 0x2E4; // STEERING_LKA
121143
if (!toyota_stock_longitudinal && (addr == 0x343)) {
122144
stock_ecu_detected = true; // ACC_CONTROL
@@ -203,7 +225,7 @@ static bool toyota_tx_hook(const CANPacket_t *to_send) {
203225
}
204226
}
205227

206-
// LTA angle steering check
228+
// STEERING_LTA angle steering check
207229
if (addr == 0x191) {
208230
// check the STEER_REQUEST, STEER_REQUEST_2, TORQUE_WIND_DOWN, STEER_ANGLE_CMD signals
209231
bool lta_request = GET_BIT(to_send, 0U);
@@ -251,6 +273,20 @@ static bool toyota_tx_hook(const CANPacket_t *to_send) {
251273
}
252274
}
253275

276+
// STEERING_LTA_2 angle steering check (SecOC)
277+
if (toyota_secoc && (addr == 0x131)) {
278+
// SecOC cars block any form of LTA actuation for now
279+
bool lta_request = GET_BIT(to_send, 3U); // STEERING_LTA_2.STEER_REQUEST
280+
bool lta_request2 = GET_BIT(to_send, 0U); // STEERING_LTA_2.STEER_REQUEST_2
281+
int lta_angle_msb = GET_BYTE(to_send, 2); // STEERING_LTA_2.STEER_ANGLE_CMD (MSB)
282+
int lta_angle_lsb = GET_BYTE(to_send, 3); // STEERING_LTA_2.STEER_ANGLE_CMD (LSB)
283+
284+
bool actuation = lta_request || lta_request2 || (lta_angle_msb != 0) || (lta_angle_lsb != 0);
285+
if (actuation) {
286+
tx = false;
287+
}
288+
}
289+
254290
// STEER: safety check on bytes 2-3
255291
if (addr == 0x2E4) {
256292
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) {
286322
TOYOTA_COMMON_TX_MSGS
287323
};
288324

325+
static const CanMsg TOYOTA_SECOC_TX_MSGS[] = {
326+
TOYOTA_COMMON_SECOC_TX_MSGS
327+
};
328+
289329
static const CanMsg TOYOTA_LONG_TX_MSGS[] = {
290330
TOYOTA_COMMON_LONG_TX_MSGS
291331
};
@@ -298,14 +338,23 @@ static safety_config toyota_init(uint16_t param) {
298338
const uint32_t TOYOTA_PARAM_STOCK_LONGITUDINAL = 2UL << TOYOTA_PARAM_OFFSET;
299339
const uint32_t TOYOTA_PARAM_LTA = 4UL << TOYOTA_PARAM_OFFSET;
300340

341+
#ifdef ALLOW_DEBUG
342+
const uint32_t TOYOTA_PARAM_SECOC = 8UL << TOYOTA_PARAM_OFFSET;
343+
toyota_secoc = GET_FLAG(param, TOYOTA_PARAM_SECOC);
344+
#endif
345+
301346
toyota_alt_brake = GET_FLAG(param, TOYOTA_PARAM_ALT_BRAKE);
302347
toyota_stock_longitudinal = GET_FLAG(param, TOYOTA_PARAM_STOCK_LONGITUDINAL);
303348
toyota_lta = GET_FLAG(param, TOYOTA_PARAM_LTA);
304349
toyota_dbc_eps_torque_factor = param & TOYOTA_EPS_FACTOR;
305350

306351
safety_config ret;
307352
if (toyota_stock_longitudinal) {
308-
SET_TX_MSGS(TOYOTA_TX_MSGS, ret);
353+
if (toyota_secoc) {
354+
SET_TX_MSGS(TOYOTA_SECOC_TX_MSGS, ret);
355+
} else {
356+
SET_TX_MSGS(TOYOTA_TX_MSGS, ret);
357+
}
309358
} else {
310359
SET_TX_MSGS(TOYOTA_LONG_TX_MSGS, ret);
311360
}
@@ -340,6 +389,8 @@ static int toyota_fwd_hook(int bus_num, int addr) {
340389
// block stock lkas messages and stock acc messages (if OP is doing ACC)
341390
// in TSS2, 0x191 is LTA which we need to block to avoid controls collision
342391
bool is_lkas_msg = ((addr == 0x2E4) || (addr == 0x412) || (addr == 0x191));
392+
// on SecOC cars 0x131 is also LTA
393+
is_lkas_msg |= toyota_secoc && (addr == 0x131);
343394
// in TSS2 the camera does ACC as well, so filter 0x343
344395
bool is_acc_msg = (addr == 0x343);
345396
bool block_msg = is_lkas_msg || (is_acc_msg && !toyota_stock_longitudinal);

‎python/__init__.py

+1
Original file line numberDiff line numberDiff line change
@@ -187,6 +187,7 @@ class Panda:
187187
FLAG_TOYOTA_ALT_BRAKE = (1 << 8)
188188
FLAG_TOYOTA_STOCK_LONGITUDINAL = (2 << 8)
189189
FLAG_TOYOTA_LTA = (4 << 8)
190+
FLAG_TOYOTA_SECOC = (8 << 8)
190191

191192
FLAG_HONDA_ALT_BRAKE = 1
192193
FLAG_HONDA_BOSCH_LONG = 2

‎tests/safety/test_toyota.py

+36-1
Original file line numberDiff line numberDiff line change
@@ -10,6 +10,7 @@
1010
from panda.tests.safety.common import CANPackerPanda
1111

1212
TOYOTA_COMMON_TX_MSGS = [[0x2E4, 0], [0x191, 0], [0x412, 0], [0x343, 0], [0x1D2, 0]] # LKAS + LTA + ACC & PCM cancel cmds
13+
TOYOTA_SECOC_TX_MSGS = [[0x131, 0]] + TOYOTA_COMMON_TX_MSGS
1314
TOYOTA_COMMON_LONG_TX_MSGS = [[0x283, 0], [0x2E6, 0], [0x2E7, 0], [0x33E, 0], [0x344, 0], [0x365, 0], [0x366, 0], [0x4CB, 0], # DSU bus 0
1415
[0x128, 1], [0x141, 1], [0x160, 1], [0x161, 1], [0x470, 1], # DSU bus 1
1516
[0x411, 0], # PCS_HUD
@@ -107,7 +108,8 @@ def test_lta_steer_cmd(self):
107108
self.safety.set_controls_allowed(engaged)
108109

109110
should_tx = not req and not req2 and angle == 0 and torque_wind_down == 0
110-
self.assertEqual(should_tx, self._tx(self._lta_msg(req, req2, angle, torque_wind_down)))
111+
self.assertEqual(should_tx, self._tx(self._lta_msg(req, req2, angle, torque_wind_down)),
112+
f"{req=} {req2=} {angle=} {torque_wind_down=}")
111113

112114
def test_rx_hook(self):
113115
# checksum checks
@@ -324,5 +326,38 @@ def setUp(self):
324326
self.safety.init_tests()
325327

326328

329+
class TestToyotaSecOcSafety(TestToyotaStockLongitudinalBase):
330+
331+
TX_MSGS = TOYOTA_SECOC_TX_MSGS
332+
RELAY_MALFUNCTION_ADDRS = {0: (0x2E4,)}
333+
FWD_BLACKLISTED_ADDRS = {2: [0x2E4, 0x412, 0x191, 0x131]}
334+
335+
def setUp(self):
336+
self.packer = CANPackerPanda("toyota_rav4_prime_generated")
337+
self.safety = libpanda_py.libpanda
338+
self.safety.set_safety_hooks(Panda.SAFETY_TOYOTA, self.EPS_SCALE | Panda.FLAG_TOYOTA_STOCK_LONGITUDINAL | Panda.FLAG_TOYOTA_SECOC)
339+
self.safety.init_tests()
340+
341+
# This platform also has alternate brake and PCM messages, but same naming in the DBC, so same packers work
342+
343+
def _user_gas_msg(self, gas):
344+
values = {"GAS_PEDAL_USER": gas}
345+
return self.packer.make_can_msg_panda("GAS_PEDAL", 0, values)
346+
347+
# This platform sends both STEERING_LTA (same as other Toyota) and STEERING_LTA_2 (SecOC signed)
348+
# STEERING_LTA is checked for no-actuation by the base class, STEERING_LTA_2 is checked for no-actuation below
349+
350+
def _lta_2_msg(self, req, req2, angle_cmd, torque_wind_down=100):
351+
values = {"STEER_REQUEST": req, "STEER_REQUEST_2": req2, "STEER_ANGLE_CMD": angle_cmd}
352+
return self.packer.make_can_msg_panda("STEERING_LTA_2", 0, values)
353+
354+
def test_lta_2_steer_cmd(self):
355+
for engaged, req, req2, angle in itertools.product([True, False], [0, 1], [0, 1], np.linspace(-20, 20, 5)):
356+
self.safety.set_controls_allowed(engaged)
357+
358+
should_tx = not req and not req2 and angle == 0
359+
self.assertEqual(should_tx, self._tx(self._lta_2_msg(req, req2, angle)), f"{req=} {req2=} {angle=}")
360+
361+
327362
if __name__ == "__main__":
328363
unittest.main()

0 commit comments

Comments
 (0)