Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Jump to bottom

Toyota: 2021+ RAV4 Prime #2042

Merged
merged 33 commits into from
Oct 4, 2024
Merged

Toyota: 2021+ RAV4 Prime #2042

Show file tree
Hide file tree
Changes from 21 commits
Commits
Show all changes
33 commits
Select commit Hold shift + click to select a range
688c4e4
from @pd0wm commaai/panda#1841
jyoung8607 Sep 26, 2024
473dde0
Merge branch 'master' of https://github.com/commaai/panda into toyota…
jyoung8607 Sep 30, 2024
bee8371
diff reduction: won't need the flag rename
jyoung8607 Oct 1, 2024
c9bce9a
run tests with new DBC and new gas/brake messages
jyoung8607 Oct 1, 2024
4ff199a
toyota_secoc_car to global
jyoung8607 Oct 1, 2024
4a7d7d0
consolidate alt_brake_101
jyoung8607 Oct 1, 2024
26c57fc
consolidate alt_pcm_cruise_176
jyoung8607 Oct 1, 2024
5750eae
consolidate alt_gas_pedal_116
jyoung8607 Oct 1, 2024
8da8161
don't allow transmit/forward for 0x131 unless SecOC
jyoung8607 Oct 1, 2024
4eda6da
cleanup and todo
jyoung8607 Oct 1, 2024
db61c11
diff reduction
jyoung8607 Oct 1, 2024
dd3d159
reorder by usage frequency
jyoung8607 Oct 1, 2024
7e03604
test for no LTA actuation
jyoung8607 Oct 1, 2024
d5f47e8
bump opendbc commit ref in Dockerfile
jyoung8607 Oct 1, 2024
b04294d
Merge branch 'master' of https://github.com/commaai/panda into toyota…
jyoung8607 Oct 2, 2024
f01d273
gate SecOC variant on ALLOW_DEBUG
jyoung8607 Oct 2, 2024
f3cd7e6
tweak gating for MISRA
jyoung8607 Oct 2, 2024
684bf3a
mutation test hates lta_angle surviving various changes
jyoung8607 Oct 2, 2024
7b0a20c
common rx checks styling
jyoung8607 Oct 2, 2024
655abf5
don't allow short version of 0x2E5 for SecOC mode
jyoung8607 Oct 2, 2024
b3cb0d3
whitespace diff reduction
jyoung8607 Oct 2, 2024
d5af11d
Merge branch 'master' of https://github.com/commaai/panda into toyota…
jyoung8607 Oct 2, 2024
3d5c258
secoc_car -> secoc
jyoung8607 Oct 2, 2024
8759f08
fix comment typo
jyoung8607 Oct 2, 2024
a85e8b0
retry CI
jyoung8607 Oct 2, 2024
4c43d25
missed a couple secoc_car -> secoc
jyoung8607 Oct 2, 2024
25e1fbc
one big secoc/not-secoc block
jyoung8607 Oct 2, 2024
b07f900
genuinely useful MISRA warning
jyoung8607 Oct 2, 2024
52fb1e7
test both STEERING_LTA and STEERING_LTA_2
jyoung8607 Oct 2, 2024
c027a6a
comment labeling for STEERING_LTA_2 signals
jyoung8607 Oct 2, 2024
a9babb1
Update board/safety/safety_toyota.h
jyoung8607 Oct 3, 2024
07ab22f
STEERING_LTA and STEERING_LTA_2 consistency
jyoung8607 Oct 3, 2024
f9102bc
update gas/brake/cruise signal annotations
jyoung8607 Oct 3, 2024
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
2 changes: 1 addition & 1 deletion Dockerfile
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -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/ && \
Expand Down
81 changes: 66 additions & 15 deletions board/safety/safety_toyota.h
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -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 \
Expand All @@ -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_car = false;
static bool toyota_alt_brake = false;
static bool toyota_stock_longitudinal = false;
static bool toyota_lta = false;
Expand Down Expand Up @@ -87,14 +98,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
jyoung8607 marked this conversation as resolved.
Show resolved Hide resolved
if (!toyota_secoc_car && (addr == 0x1D2)) {
bool cruise_engaged = GET_BIT(to_push, 5U);
pcm_cruise_check(cruise_engaged);
}
if (toyota_secoc_car && (addr == 0x176)) {
jyoung8607 marked this conversation as resolved.
Show resolved Hide resolved
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_secoc_car && (addr == 0x1D2)) {
gas_pressed = !GET_BIT(to_push, 4U); // GAS_PEDAL.GAS_RELEASED
}
if (toyota_secoc_car && (addr == 0x116)) {
gas_pressed = GET_BYTE(to_push, 1) != 0U; // GAS_PEDAL.GAS_PEDAL_USER
}

// sample speed
Expand All @@ -111,10 +129,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, secoc on 0x101
if (!toyota_alt_brake && !toyota_secoc_car && (addr == 0x226)) {
brake_pressed = GET_BIT(to_push, 37U);
}
if (toyota_alt_brake && !toyota_secoc_car && (addr == 0x224)) {
brake_pressed = GET_BIT(to_push, 5U);
}
if (!toyota_alt_brake && toyota_secoc_car && (addr == 0x101)) {
brake_pressed = GET_BIT(to_push, 3U);
}

bool stock_ecu_detected = addr == 0x2E4; // STEERING_LKA
Expand Down Expand Up @@ -203,6 +226,19 @@ static bool toyota_tx_hook(const CANPacket_t *to_send) {
}
}

// SecOC cars block any form of LTA actuation for now
if (toyota_secoc_car && (addr == 0x131)) {
bool lta_request = GET_BIT(to_send, 3U);
bool lta_request2 = GET_BIT(to_send, 0U);
int lta_angle_msb = GET_BYTE(to_send, 2);
int lta_angle_lsb = GET_BYTE(to_send, 3);

bool actuation = lta_request || lta_request2 || (lta_angle_msb != 0) || (lta_angle_lsb != 0);
if (actuation) {
tx = false;
jyoung8607 marked this conversation as resolved.
Show resolved Hide resolved
}
}

// LTA angle steering check
jyoung8607 marked this conversation as resolved.
Show resolved Hide resolved
if (addr == 0x191) {
// check the STEER_REQUEST, STEER_REQUEST_2, TORQUE_WIND_DOWN, STEER_ANGLE_CMD signals
Expand Down Expand Up @@ -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
};
Expand All @@ -298,14 +338,23 @@ 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_CAR = 8UL << TOYOTA_PARAM_OFFSET;
toyota_secoc_car = GET_FLAG(param, TOYOTA_PARAM_SECOC_CAR);
#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);
toyota_dbc_eps_torque_factor = param & TOYOTA_EPS_FACTOR;

safety_config ret;
if (toyota_stock_longitudinal) {
SET_TX_MSGS(TOYOTA_TX_MSGS, ret);
if (toyota_secoc_car) {
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);
}
Expand Down Expand Up @@ -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_car && (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);
Expand Down
1 change: 1 addition & 0 deletions python/__init__.py
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -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_CAR = (8 << 8)
jyoung8607 marked this conversation as resolved.
Show resolved Hide resolved

FLAG_HONDA_ALT_BRAKE = 1
FLAG_HONDA_BOSCH_LONG = 2
Expand Down
35 changes: 34 additions & 1 deletion tests/safety/test_toyota.py
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -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
Expand Down Expand Up @@ -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
Expand Down Expand Up @@ -324,5 +326,36 @@ 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_CAR)
self.safety.init_tests()

# This platform also has alternate brake and PCM messages, but same naming in the DBC, so same packers orks
jyoung8607 marked this conversation as resolved.
Show resolved Hide resolved

def _user_gas_msg(self, gas):
values = {"GAS_PEDAL_USER": gas}
return self.packer.make_can_msg_panda("GAS_PEDAL", 0, values)

def _lta_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)
Copy link
Contributor

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

If we still use STEERING_LTA, do we no longer test that?

Copy link
Collaborator Author

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

We do send both. Both are present on the car, not sure why. Your point is correct. Reverted to Draft, I'll fix up the tests in the AM to verify no-actuation on both.

Copy link
Contributor

@sshane sshane Oct 2, 2024 •
edited
Loading

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

Did we ever check what the stock system uses? We were able to send LKA on some 2023 RAV4 but because the stock system used LTA for lane keeping (not just centering) we chose to switch off the unused API.

Copy link
Contributor

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

If I recall correctly the stock system uses the LTA for lane keep steering, but the torque message works fine too. So it's like most TSS2 cars in this regard.

They split up the LTA message into two parts, the old one which contains a bunch of values and flags (with no space for a MAC), and a new one that contains just the actual requested steering angle + a SecOC MAC. They did something similar for longitudinal.

Copy link
Collaborator Author

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

Now testing both messages, thanks for spotting that!

Verified by breaking the 0x191 check offline:

bool steer_control_enabled = lta_request; //|| lta_request2;

Which was properly picked up by both normal and SecOC tests:

FAILED test_toyota.py::TestToyotaSecOcSafety::test_lta_steer_cmd - AssertionError: False != True : req=0 req2=1 angle=0.0 torque_wind_down=0
FAILED test_toyota.py::TestToyotaStockLongitudinalTorque::test_lta_steer_cmd - AssertionError: False != True : req=0 req2=1 angle=0.0 torque_wind_down=0
FAILED test_toyota.py::TestToyotaSafetyTorque::test_lta_steer_cmd - AssertionError: False != True : req=0 req2=1 angle=0.0 torque_wind_down=0

Copy link
Contributor

@sshane sshane Oct 2, 2024 •
edited
Loading

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

@adeebshihadeh do you want to merge this with LKA when we specifically didn't want to merge LKA for the new RAV4s which don't use it but could? commaai/openpilot#29978 for reference.

Copy link
Collaborator Author

@jyoung8607 jyoung8607 Oct 2, 2024 •
edited
Loading

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

It's probably possible to switch to LTA, the SecOC part should just carry over. However, it will take multiple round-trips with test owners to plot how the stock camera drives the two LTA messages in tandem and then replicate, test, and validate safety for that behavior.

I don't have a preference on whether to make the switch, the comma openpilot team knows more depth and breadth of Toyota history than I do. If we do want to make the switch, my preference is to ship what we have and create an issue to track migrating to LTA.

Copy link
Contributor

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

If I recall correctly the stock system uses the LTA for lane keep steering, but the torque message works fine too. So it's like most TSS2 cars in this regard.

Sorry, forgot lane keep is what you should call the safety nudges to put you back in the lane, not lane centering. I did not check what the stock system uses for LKA.

Copy link
Collaborator Author

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

image

Verified the stock system actuates on both STEERING_LKA (torque) and STEERING_LTA_2 (angle) depending on conditions.


# Only allow LTA msgs with no actuation
def test_lta_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_msg(req, req2, angle)), f"{req=} {req2=} {angle=}")


if __name__ == "__main__":
unittest.main()
Loading