Skip to content

Commit

Permalink
from @pd0wm #1841
Browse files Browse the repository at this point in the history
  • Loading branch information
@jyoung8607
jyoung8607 committed Sep 26, 2024
1 parent 38cab7d commit 688c4e4
Show file tree
Hide file tree
Showing 3 changed files with 57 additions and 23 deletions.
69 changes: 51 additions & 18 deletions board/safety/safety_toyota.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -4,7 +4,7 @@

// 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 @@ -16,13 +16,18 @@
#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 }}}, \

static bool toyota_alt_brake = false;
{.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_alt_brake_224 = false;
static bool toyota_alt_brake_101 = false;
static bool toyota_alt_pcm_cruise_176 = false;
static bool toyota_alt_gas_pedal_116 = false;
static bool toyota_stock_longitudinal = false;
static bool toyota_lta = false;

static int toyota_dbc_eps_torque_factor = 100; // conversion factor for STEER_TORQUE_EPS in %: see dbc file

static uint32_t toyota_compute_checksum(const CANPacket_t *to_push) {
Expand Down Expand Up @@ -87,14 +92,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
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 @@ -111,10 +123,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 @@ -203,6 +220,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 @@ -294,12 +319,19 @@ static safety_config toyota_init(uint16_t param) {
// 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;

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 Down Expand Up @@ -339,7 +371,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
Original file line number Diff line number Diff line change
Expand Up @@ -184,9 +184,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
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

0 comments on commit 688c4e4

Please sign in to comment.