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

Safety: 2021+ Rav4 Prime #1841

Closed
wants to merge 11 commits into from
Closed

Safety: 2021+ Rav4 Prime #1841

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
11 commits
Select commit Hold shift + click to select a range
9f20563
rav4 prime
pd0wm Mar 17, 2024
896b4b8
add cruise active
pd0wm Mar 17, 2024
076cda7
add new PCM message to RX checks
pd0wm Mar 18, 2024
742a7fc
dont forward STEERING_LTA_2
pd0wm Mar 21, 2024
bfdcb2c
move to new PCM_CRUISE message
pd0wm May 5, 2024
989d0c1
try to fix misra
pd0wm May 5, 2024
2dbaea0
add flags for all the alternate signals
pd0wm May 5, 2024
09dc8d1
add comment
pd0wm May 5, 2024
0203dc0
no else if for misra
pd0wm May 5, 2024
710d2c7
fix flags
pd0wm May 5, 2024
c40ecae
one flag
pd0wm May 5, 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
67 changes: 49 additions & 18 deletions board/safety/safety_toyota.h
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -39,7 +39,7 @@ const LongitudinalLimits TOYOTA_LONG_LIMITS = {

// 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 \
Expand All @@ -59,9 +59,10 @@ const CanMsg TOYOTA_LONG_TX_MSGS[] = {
#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}}}, \

RxCheck toyota_lka_rx_checks[] = {
TOYOTA_COMMON_RX_CHECKS(false)
Expand All @@ -76,11 +77,15 @@ RxCheck toyota_lta_rx_checks[] = {
// 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;

bool toyota_alt_brake = false;
bool toyota_alt_brake_224 = false;
bool toyota_alt_brake_101 = false;
bool toyota_alt_pcm_cruise_176 = false;
bool toyota_alt_gas_pedal_116 = false;
bool toyota_stock_longitudinal = false;
bool toyota_lta = false;
int toyota_dbc_eps_torque_factor = 100; // conversion factor for STEER_TORQUE_EPS in %: see dbc file
Expand Down Expand Up @@ -143,16 +148,22 @@ static void toyota_rx_hook(const CANPacket_t *to_push) {
update_sample(&angle_meas, angle_meas_new);
}
}

// 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
Expand All @@ -169,10 +180,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
Expand Down Expand Up @@ -223,6 +239,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
Expand Down Expand Up @@ -302,8 +326,14 @@ static bool toyota_tx_hook(const CANPacket_t *to_send) {
}

static safety_config toyota_init(uint16_t param) {
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;

Expand All @@ -330,7 +360,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);
Expand Down
3 changes: 2 additions & 1 deletion python/__init__.py
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -188,9 +188,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
Expand Down
8 changes: 4 additions & 4 deletions tests/safety/test_toyota.py
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -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
Expand All @@ -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

Expand Down Expand Up @@ -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):
Expand All @@ -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)
Expand Down
Loading