diff --git a/board/safety/safety_toyota.h b/board/safety/safety_toyota.h index 73ad6e76..395c9817 100644 --- a/board/safety/safety_toyota.h +++ b/board/safety/safety_toyota.h @@ -43,9 +43,11 @@ addr_checks toyota_rx_checks = {toyota_addr_checks, TOYOTA_ADDR_CHECKS_LEN}; // first byte is for eps factor, second is for flags const uint32_t TOYOTA_PARAM_OFFSET = 8U; const uint32_t TOYOTA_EPS_FACTOR = (1U << TOYOTA_PARAM_OFFSET) - 1U; -const uint32_t TOYOTA_ALT_BRAKE = 1U << TOYOTA_PARAM_OFFSET; +const uint32_t TOYOTA_PARAM_ALT_BRAKE = 1U << TOYOTA_PARAM_OFFSET; +const uint32_t TOYOTA_PARAM_STOCK_LONGITUDINAL = 2U << TOYOTA_PARAM_OFFSET; bool toyota_alt_brake = false; +bool toyota_stock_longitudinal = false; int toyota_dbc_eps_torque_factor = 100; // conversion factor for STEER_TORQUE_EPS in %: see dbc file static uint8_t toyota_compute_checksum(CANPacket_t *to_push) { @@ -164,11 +166,20 @@ static int toyota_tx_hook(CANPacket_t *to_send, bool longitudinal_allowed) { if (addr == 0x343) { int desired_accel = (GET_BYTE(to_send, 0) << 8) | GET_BYTE(to_send, 1); desired_accel = to_signed(desired_accel, 16); - if (!longitudinal_allowed) { + if (!longitudinal_allowed || toyota_stock_longitudinal) { if (desired_accel != 0) { tx = 0; } } + + // only ACC messages that cancel are allowed when openpilot is not controlling longitudinal + if (toyota_stock_longitudinal) { + bool cancel_req = GET_BIT(to_send, 24U) != 0U; + if (!cancel_req) { + tx = 0; + } + } + bool violation = max_limit_check(desired_accel, TOYOTA_MAX_ACCEL, TOYOTA_MIN_ACCEL); if (violation) { @@ -256,7 +267,8 @@ static const addr_checks* toyota_init(uint32_t param) { controls_allowed = 0; relay_malfunction_reset(); gas_interceptor_detected = 0; - toyota_alt_brake = GET_FLAG(param, TOYOTA_ALT_BRAKE); + toyota_alt_brake = GET_FLAG(param, TOYOTA_PARAM_ALT_BRAKE); + toyota_stock_longitudinal = GET_FLAG(param, TOYOTA_PARAM_STOCK_LONGITUDINAL); toyota_dbc_eps_torque_factor = param & TOYOTA_EPS_FACTOR; return &toyota_rx_checks; } diff --git a/python/__init__.py b/python/__init__.py index b292a8f7..d3a96cbe 100644 --- a/python/__init__.py +++ b/python/__init__.py @@ -180,7 +180,9 @@ class Panda(object): CLOCK_SOURCE_MODE_DISABLED = 0 CLOCK_SOURCE_MODE_FREE_RUNNING = 1 + # first byte is for EPS scaling factor FLAG_TOYOTA_ALT_BRAKE = (1 << 8) + FLAG_TOYOTA_STOCK_LONGITUDINAL = (2 << 8) FLAG_HONDA_ALT_BRAKE = 1 FLAG_HONDA_BOSCH_LONG = 2 @@ -189,6 +191,7 @@ class Panda(object): FLAG_HYUNDAI_EV_GAS = 1 FLAG_HYUNDAI_HYBRID_GAS = 2 FLAG_HYUNDAI_LONG = 4 + FLAG_TESLA_POWERTRAIN = 1 FLAG_TESLA_LONG_CONTROL = 2 diff --git a/tests/safety/common.py b/tests/safety/common.py index 2999dd67..888bc2a5 100644 --- a/tests/safety/common.py +++ b/tests/safety/common.py @@ -450,7 +450,7 @@ class PandaSafetyTest(PandaSafetyTestBase): if 'Tesla' in attr and 'Tesla' in current_test: continue - if {attr, current_test} == {'TestToyotaSafety', 'TestToyotaAltBrakeSafety'}: + if {attr, current_test}.issubset({'TestToyotaSafety', 'TestToyotaAltBrakeSafety', 'TestToyotaStockLongitudinal'}): continue # TODO: Temporary, should be fixed in panda firmware, safety_honda.h diff --git a/tests/safety/test_toyota.py b/tests/safety/test_toyota.py index ae7c5433..f5cef45e 100755 --- a/tests/safety/test_toyota.py +++ b/tests/safety/test_toyota.py @@ -1,7 +1,8 @@ #!/usr/bin/env python3 -import unittest import numpy as np import random +import unittest + from panda import Panda from panda.tests.safety import libpandasafety_py import panda.tests.safety.common as common @@ -40,17 +41,17 @@ class TestToyotaSafety(common.PandaSafetyTest, common.InterceptorSafetyTest, MAX_RT_DELTA = 450 RT_INTERVAL = 250000 MAX_TORQUE_ERROR = 350 - TORQUE_MEAS_TOLERANCE = 1 # toyota safety adds one to be conversative for rounding - EPS_SCALE = 0.73 + TORQUE_MEAS_TOLERANCE = 1 # toyota safety adds one to be conservative for rounding + EPS_SCALE = 73 def setUp(self): self.packer = CANPackerPanda("toyota_nodsu_pt_generated") self.safety = libpandasafety_py.libpandasafety - self.safety.set_safety_hooks(Panda.SAFETY_TOYOTA, 73) + self.safety.set_safety_hooks(Panda.SAFETY_TOYOTA, self.EPS_SCALE) self.safety.init_tests() def _torque_meas_msg(self, torque): - values = {"STEER_TORQUE_EPS": (torque/self.EPS_SCALE)} + values = {"STEER_TORQUE_EPS": (torque / self.EPS_SCALE) * 100.} return self.packer.make_can_msg_panda("STEER_TORQUE_SENSOR", 0, values) def _torque_msg(self, torque): @@ -61,8 +62,8 @@ class TestToyotaSafety(common.PandaSafetyTest, common.InterceptorSafetyTest, 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} + def _accel_msg(self, accel, cancel_req=0): + values = {"ACCEL_CMD": accel, "CANCEL_REQ": cancel_req} return self.packer.make_can_msg_panda("ACC_CONTROL", 0, values) def _speed_msg(self, speed): @@ -99,7 +100,7 @@ class TestToyotaSafety(common.PandaSafetyTest, common.InterceptorSafetyTest, msg = common.package_can_msg([0x283, 0, bytes(dat), 0]) self.assertEqual(not bad, self._tx(msg)) - def test_accel_actuation_limits(self): + def test_accel_actuation_limits(self, stock_longitudinal=False): limits = ((MIN_ACCEL, MAX_ACCEL, ALTERNATIVE_EXPERIENCE.DEFAULT), (MIN_ACCEL, MAX_ACCEL, ALTERNATIVE_EXPERIENCE.RAISE_LONGITUDINAL_LIMITS_TO_ISO_MAX)) @@ -108,7 +109,9 @@ class TestToyotaSafety(common.PandaSafetyTest, common.InterceptorSafetyTest, for controls_allowed in [True, False]: self.safety.set_controls_allowed(controls_allowed) self.safety.set_alternative_experience(alternative_experience) - if controls_allowed: + if stock_longitudinal: + should_tx = False + elif controls_allowed: should_tx = int(min_accel * 1000) <= int(accel * 1000) <= int(max_accel * 1000) else: should_tx = np.isclose(accel, 0, atol=0.0001) @@ -155,7 +158,7 @@ class TestToyotaAltBrakeSafety(TestToyotaSafety): def setUp(self): self.packer = CANPackerPanda("toyota_new_mc_pt_generated") self.safety = libpandasafety_py.libpandasafety - self.safety.set_safety_hooks(Panda.SAFETY_TOYOTA, 73 | Panda.FLAG_TOYOTA_ALT_BRAKE) + self.safety.set_safety_hooks(Panda.SAFETY_TOYOTA, self.EPS_SCALE | Panda.FLAG_TOYOTA_ALT_BRAKE) self.safety.init_tests() def _user_brake_msg(self, brake): @@ -167,5 +170,27 @@ class TestToyotaAltBrakeSafety(TestToyotaSafety): pass +class TestToyotaStockLongitudinal(TestToyotaSafety): + def setUp(self): + self.packer = CANPackerPanda("toyota_nodsu_pt_generated") + self.safety = libpandasafety_py.libpandasafety + self.safety.set_safety_hooks(Panda.SAFETY_TOYOTA, self.EPS_SCALE | Panda.FLAG_TOYOTA_STOCK_LONGITUDINAL) + self.safety.init_tests() + + def test_accel_actuation_limits(self, stock_longitudinal=True): + super().test_accel_actuation_limits(stock_longitudinal=stock_longitudinal) + + def test_acc_cancel(self): + """ + Regardless of controls allowed, never allow ACC_CONTROL if cancel bit isn't set + """ + for controls_allowed in [True, False]: + self.safety.set_controls_allowed(controls_allowed) + for accel in np.arange(MIN_ACCEL - 1, MAX_ACCEL + 1, 0.1): + self.assertFalse(self._tx(self._accel_msg(accel))) + should_tx = np.isclose(accel, 0, atol=0.0001) + self.assertEqual(should_tx, self._tx(self._accel_msg(accel, cancel_req=1))) + + if __name__ == "__main__": unittest.main()