From 7d9fdd1108ae1c8136a527d1369d25bb890783fb Mon Sep 17 00:00:00 2001 From: Adeeb Shihadeh Date: Tue, 6 Apr 2021 16:56:03 -0700 Subject: [PATCH] Toyota: whitelist LTA message (#638) * Toyota: whitelist LTA message * LTA safety * simple test * little more test * misra --- board/safety/safety_toyota.h | 21 ++++++++++++++++++--- tests/safety/test_toyota.py | 31 ++++++++++++++++++++++++++++--- 2 files changed, 46 insertions(+), 6 deletions(-) diff --git a/board/safety/safety_toyota.h b/board/safety/safety_toyota.h index 2ffe58b..241e579 100644 --- a/board/safety/safety_toyota.h +++ b/board/safety/safety_toyota.h @@ -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); diff --git a/tests/safety/test_toyota.py b/tests/safety/test_toyota.py index f1b4fea..d546025 100755 --- a/tests/safety/test_toyota.py +++ b/tests/safety/test_toyota.py @@ -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"]: