Skip to content

Commit

Permalink
Merge remote-tracking branch 'jyoung/secoc-long' into secoc-long
Browse files Browse the repository at this point in the history
  • Loading branch information
@chrispypatt
chrispypatt committed Nov 2, 2024
2 parents 4c4ef05 + 1418744 commit 0a67998
Show file tree
Hide file tree
Showing 3 changed files with 59 additions and 35 deletions.
2 changes: 1 addition & 1 deletion Dockerfile
View file
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="e1ce3619a5db661ef2b406ccf258a253baf6eebc"
ENV OPENDBC_REF="b4c0dfeb4a449a315bbccbda526355f5dae261d3"
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
53 changes: 25 additions & 28 deletions board/safety/safety_toyota.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -12,7 +12,9 @@

#define TOYOTA_COMMON_SECOC_TX_MSGS \
TOYOTA_BASE_TX_MSGS \
{0x2E4, 0, 8}, {0x131, 0, 8}, \
{0x2E4, 0, 8}, {0x131, 0, 8}, /* STEERING_LKA (longer message for SecOC), STEERING_LTA_2 */ \
{0x183, 0, 8}, {0x411, 0, 8}, /* ACC_CONTROL_2, PCS_HUD */ \
{0x750, 0, 8}, /* radar diagnostic address */ \

#define TOYOTA_COMMON_LONG_TX_MSGS \
TOYOTA_COMMON_TX_MSGS \
Expand All @@ -21,14 +23,6 @@
{0x411, 0, 8}, /* PCS_HUD */ \
{0x750, 0, 8}, /* radar diagnostic address */ \

#define TOYOTA_COMMON_LONG_SECOC_TX_MSGS \
TOYOTA_COMMON_SECOC_TX_MSGS \
{0x283, 0, 7}, {0x2E6, 0, 8}, {0x2E7, 0, 8}, {0x33E, 0, 7}, {0x344, 0, 8}, {0x365, 0, 7}, {0x366, 0, 7}, {0x4CB, 0, 8}, /* DSU bus 0 */ \
{0x128, 1, 6}, {0x141, 1, 4}, {0x160, 1, 8}, {0x161, 1, 7}, {0x470, 1, 4}, /* DSU bus 1 */ \
{0x411, 0, 8}, /* PCS_HUD */ \
{0x750, 0, 8}, /* radar diagnostic address */ \
{0x183, 0, 8}, \

#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 }}}, \
Expand Down Expand Up @@ -148,8 +142,8 @@ static void toyota_rx_hook(const CANPacket_t *to_push) {
}

bool stock_ecu_detected = addr == 0x2E4; // STEERING_LKA
if (!toyota_stock_longitudinal && (addr == 0x343)) {
stock_ecu_detected = true; // ACC_CONTROL
if (!toyota_stock_longitudinal && ((addr == 0x343) || (toyota_secoc && (addr == 0x183)))) {
stock_ecu_detected = true; // ACC_CONTROL or ACC_CONTROL_2
}
generic_rx_checks(stock_ecu_detected);
}
Expand Down Expand Up @@ -206,6 +200,7 @@ static bool toyota_tx_hook(const CANPacket_t *to_send) {
desired_accel = to_signed(desired_accel, 16);

bool violation = false;
violation |= toyota_secoc && (desired_accel != 0); // SecOC cars still use 0x343, but accel itself is in 0x183
violation |= longitudinal_accel_checks(desired_accel, TOYOTA_LONG_LIMITS);

// only ACC messages that cancel are allowed when openpilot is not controlling longitudinal
Expand All @@ -224,6 +219,18 @@ static bool toyota_tx_hook(const CANPacket_t *to_send) {
}
}

if (addr == 0x183) {
int desired_accel = (GET_BYTE(to_send, 0) << 8) | GET_BYTE(to_send, 1);
desired_accel = to_signed(desired_accel, 16);

bool violation = !toyota_secoc; // Only SecOC cars may transmit this message
violation |= longitudinal_accel_checks(desired_accel, TOYOTA_LONG_LIMITS);

if (violation) {
tx = false;
}
}

// AEB: block all actuation. only used when DSU is unplugged
if (addr == 0x283) {
// only allow the checksum, which is the last byte
Expand Down Expand Up @@ -338,10 +345,6 @@ static safety_config toyota_init(uint16_t param) {
TOYOTA_COMMON_LONG_TX_MSGS
};

static const CanMsg TOYOTA_SECOC_LONG_TX_MSGS[] = {
TOYOTA_COMMON_LONG_SECOC_TX_MSGS
};

// safety param flags
// first byte is for EPS factor, second is for flags
const uint32_t TOYOTA_PARAM_OFFSET = 8U;
Expand All @@ -361,18 +364,12 @@ static safety_config toyota_init(uint16_t param) {
toyota_dbc_eps_torque_factor = param & TOYOTA_EPS_FACTOR;

safety_config ret;
if (toyota_stock_longitudinal) {
if (toyota_secoc) {
SET_TX_MSGS(TOYOTA_SECOC_TX_MSGS, ret);
} else {
SET_TX_MSGS(TOYOTA_TX_MSGS, ret);
}
if (toyota_secoc) {
SET_TX_MSGS(TOYOTA_SECOC_TX_MSGS, ret);
} else if (toyota_stock_longitudinal) {
SET_TX_MSGS(TOYOTA_TX_MSGS, ret);
} else {
if (toyota_secoc) {
SET_TX_MSGS(TOYOTA_SECOC_LONG_TX_MSGS, ret);
} else {
SET_TX_MSGS(TOYOTA_LONG_TX_MSGS, ret);
}
SET_TX_MSGS(TOYOTA_LONG_TX_MSGS, ret);
}

if (toyota_lta) {
Expand Down Expand Up @@ -405,10 +402,10 @@ 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);
// SecOC cars use additional (not alternate) messages for lateral and longitudinal actuation
is_lkas_msg |= toyota_secoc && (addr == 0x131);
is_acc_msg |= toyota_secoc && (addr == 0x183);
bool block_msg = is_lkas_msg || (is_acc_msg && !toyota_stock_longitudinal);
if (!block_msg) {
Expand Down
39 changes: 33 additions & 6 deletions tests/safety/test_toyota.py
View file
Original file line number Diff line number Diff line change
Expand Up @@ -4,13 +4,13 @@
import unittest
import itertools

from panda import Panda
from panda import Panda, ALTERNATIVE_EXPERIENCE
from panda.tests.libpanda import libpanda_py
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_SECOC_TX_MSGS = [[0x131, 0]] + TOYOTA_COMMON_TX_MSGS
TOYOTA_SECOC_TX_MSGS = [[0x131, 0], [0x183, 0], [0x411, 0], [0x750, 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 @@ -326,18 +326,22 @@ def setUp(self):
self.safety.init_tests()


class TestToyotaSecOcSafety(TestToyotaStockLongitudinalBase):
class TestToyotaSecOcSafety(TestToyotaSafetyBase):

TX_MSGS = TOYOTA_SECOC_TX_MSGS
RELAY_MALFUNCTION_ADDRS = {0: (0x2E4,)}
FWD_BLACKLISTED_ADDRS = {2: [0x2E4, 0x412, 0x191, 0x131]}
RELAY_MALFUNCTION_ADDRS = {0: (0x2E4, 0x343, 0x183)}
FWD_BLACKLISTED_ADDRS = {2: [0x2E4, 0x412, 0x191, 0x131, 0x343, 0x183]}

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.set_safety_hooks(Panda.SAFETY_TOYOTA, self.EPS_SCALE | Panda.FLAG_TOYOTA_SECOC)
self.safety.init_tests()

@unittest.skip("test not applicable for cars without a DSU")
def test_block_aeb(self, stock_longitudinal: bool = False):
pass

# 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):
Expand All @@ -358,6 +362,29 @@ def test_lta_2_steer_cmd(self):
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=}")

def _accel_2_msg(self, accel, cancel_req=0):
values = {"ACCEL_CMD": accel}
return self.packer.make_can_msg_panda("ACC_CONTROL_2", 0, values)

# FIXME: Replaces common test, refactor common tests to handle this situation better?
def test_accel_actuation_limits(self, stock_longitudinal=False):
limits = ((self.MIN_ACCEL, self.MAX_ACCEL, ALTERNATIVE_EXPERIENCE.DEFAULT),
(self.MIN_ACCEL, self.MAX_ACCEL, ALTERNATIVE_EXPERIENCE.RAISE_LONGITUDINAL_LIMITS_TO_ISO_MAX))

for min_accel, max_accel, alternative_experience in limits:
# enforce we don't skip over 0 or inactive accel
for accel in np.concatenate((np.arange(min_accel - 1, max_accel + 1, 0.05), [0, self.INACTIVE_ACCEL])):
accel = round(accel, 2) # floats might not hit exact boundary conditions without rounding
for controls_allowed in [True, False]:
self.safety.set_controls_allowed(controls_allowed)
self.safety.set_alternative_experience(alternative_experience)
# On a SecOC vehicle, we still transmit ACC_CONTROL but the accel value moves to ACC_CONTROL_2
# Verify that all non-idle accel values in ACC_CONTROL are rejected, verify ACC_CONTROL_2 accel normally
should_tx_1 = accel == self.INACTIVE_ACCEL
should_tx_2 = (controls_allowed and min_accel <= accel <= max_accel) or accel == self.INACTIVE_ACCEL
self.assertEqual(should_tx_1, self._tx(self._accel_msg(accel)))
self.assertEqual(should_tx_2, self._tx(self._accel_2_msg(accel)))


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

0 comments on commit 0a67998

Please sign in to comment.