retropilot
/
panda
1
0
Fork 0
Code Issues Pull Requests Projects Releases Wiki Activity

Toyota: whitelist LTA message (#638) 

* Toyota: whitelist LTA message

* LTA safety

* simple test

* little more test

* misra
master
Adeeb Shihadeh 2021-04-06 16:56:03 -07:00 committed by GitHub
parent c416419c16
commit 7d9fdd1108
No known key found for this signature in database
GPG Key ID: 4AEE18F83AFDEB23
2 changed files with 46 additions and 6 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

21
board/safety/safety_toyota.h
View File

@ -30,9 +30,9 @@ const int TOYOTA_GAS_INTERCEPTOR_THRSLD = 845;
#define TOYOTA_GET_INTERCEPTOR(msg) (((GET_BYTE((msg), 0) << 8) + GET_BYTE((msg), 1) + (GET_BYTE((msg), 2) << 8) + GET_BYTE((msg), 3)) / 2) // avg between 2 tracks
const CanMsg TOYOTA_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
{0x2E4, 0, 5}, {0x411, 0, 8}, {0x412, 0, 8}, {0x343, 0, 8}, {0x1D2, 0, 8}, // LKAS + ACC
{0x200, 0, 6}}; // interceptor
{0x128, 1, 6}, {0x141, 1, 4}, {0x160, 1, 8}, {0x161, 1, 7}, {0x470, 1, 4}, // DSU bus 1
{0x2E4, 0, 5}, {0x191, 0, 8}, {0x411, 0, 8}, {0x412, 0, 8}, {0x343, 0, 8}, {0x1D2, 0, 8}, // LKAS + ACC
{0x200, 0, 6}}; // interceptor
AddrCheckStruct toyota_rx_checks[] = {
{.msg = {{ 0xaa, 0, 8, .check_checksum = false, .expected_timestep = 12000U}}},
@ -180,6 +180,21 @@ static int toyota_tx_hook(CAN_FIFOMailBox_TypeDef *to_send) {
}
}
// LTA steering check
// only sent to prevent dash errors, no actuation is accepted
if (addr == 0x191) {
// check the STEER_REQUEST, STEER_REQUEST_2, and STEER_ANGLE_CMD signals
bool lta_request = (GET_BYTE(to_send, 0) & 1) != 0;
bool lta_request2 = ((GET_BYTE(to_send, 3) >> 1) & 1) != 0;
int lta_angle = (GET_BYTE(to_send, 1) << 8) | GET_BYTE(to_send, 2);
lta_angle = to_signed(lta_angle, 16);
// block LTA msgs with actuation requests
if (lta_request || lta_request2 || (lta_angle != 0)) {
tx = 0;
}
}
// STEER: safety check on bytes 2-3
if (addr == 0x2E4) {
int desired_torque = (GET_BYTE(to_send, 1) << 8) | GET_BYTE(to_send, 2);

31
tests/safety/test_toyota.py
View File

@ -1,6 +1,7 @@
#!/usr/bin/env python3
import unittest
import numpy as np
import random
from panda import Panda
from panda.tests.safety import libpandasafety_py
import panda.tests.safety.common as common
@ -17,7 +18,7 @@ class TestToyotaSafety(common.PandaSafetyTest, common.InterceptorSafetyTest,
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
[0x2E4, 0], [0x411, 0], [0x412, 0], [0x343, 0], [0x1D2, 0], # LKAS + ACC
[0x2E4, 0], [0x191, 0], [0x411, 0], [0x412, 0], [0x343, 0], [0x1D2, 0], # LKAS + ACC
[0x200, 0], [0x750, 0]] # interceptor + blindspot monitor
STANDSTILL_THRESHOLD = 1 # 1kph
RELAY_MALFUNCTION_ADDR = 0x2E4
@ -35,9 +36,9 @@ class TestToyotaSafety(common.PandaSafetyTest, common.InterceptorSafetyTest,
TORQUE_MEAS_TOLERANCE = 1 # toyota safety adds one to be conversative for rounding
def setUp(self):
self.packer = CANPackerPanda("toyota_prius_2017_pt_generated")
self.packer = CANPackerPanda("toyota_nodsu_pt_generated")
self.safety = libpandasafety_py.libpandasafety
self.safety.set_safety_hooks(Panda.SAFETY_TOYOTA, 66)
self.safety.set_safety_hooks(Panda.SAFETY_TOYOTA, 73)
self.safety.init_tests()
def _torque_meas_msg(self, torque):
@ -48,6 +49,10 @@ class TestToyotaSafety(common.PandaSafetyTest, common.InterceptorSafetyTest,
values = {"STEER_TORQUE_CMD": torque}
return self.packer.make_can_msg_panda("STEERING_LKA", 0, values)
def _lta_msg(self, req, req2, angle_cmd):
values = {"STEER_REQUEST": req, "STEER_REQUEST_2": req2, "STEER_ANGLE_CMD": angle_cmd}
return self.packer.make_can_msg_panda("STEERING_LTA", 0, values)
def _accel_msg(self, accel):
values = {"ACCEL_CMD": accel}
return self.packer.make_can_msg_panda("ACC_CONTROL", 0, values)
@ -91,6 +96,26 @@ class TestToyotaSafety(common.PandaSafetyTest, common.InterceptorSafetyTest,
should_tx = np.isclose(accel, 0, atol=0.0001)
self.assertEqual(should_tx, self._tx(self._accel_msg(accel)))
# Only allow LTA msgs with no actuation
def test_lta_steer_cmd(self):
for engaged in [True, False]:
self.safety.set_controls_allowed(engaged)
# good msg
self.assertTrue(self._tx(self._lta_msg(0, 0, 0)))
# bad msgs
self.assertFalse(self._tx(self._lta_msg(1, 0, 0)))
self.assertFalse(self._tx(self._lta_msg(0, 1, 0)))
self.assertFalse(self._tx(self._lta_msg(0, 0, 1)))
for _ in range(20):
req = random.choice([0, 1])
req2 = random.choice([0, 1])
angle = random.randint(-50, 50)
should_tx = not req and not req2 and angle == 0
self.assertEqual(should_tx, self._tx(self._lta_msg(req, req2, angle)))
def test_rx_hook(self):
# checksum checks
for msg in ["trq", "pcm"]: