From ddb3698f9bf03dc6096a754c01529cddd580a976 Mon Sep 17 00:00:00 2001 From: Robbe Derks Date: Thu, 18 Nov 2021 13:50:10 +0100 Subject: [PATCH] Tesla longitudinal safety (#737) * tesla safety cleanup + long WIP * more long safety * longitudinal safety tests * fix misra * add safety for main bus DAS_control * acc_state is not enabled * fix tests * we don't want this * fix Tesla common test --- board/safety/safety_tesla.h | 131 +++++++++++++++++++++++++----------- python/__init__.py | 2 + tests/safety/common.py | 4 ++ tests/safety/test_tesla.py | 96 ++++++++++++++++++++------ 4 files changed, 173 insertions(+), 60 deletions(-) mode change 100644 => 100755 tests/safety/test_tesla.py diff --git a/board/safety/safety_tesla.h b/board/safety/safety_tesla.h index aa771d35..359c9007 100644 --- a/board/safety/safety_tesla.h +++ b/board/safety/safety_tesla.h @@ -7,18 +7,29 @@ const struct lookup_t TESLA_LOOKUP_ANGLE_RATE_DOWN = { {5., 3.5, .8}}; const int TESLA_DEG_TO_CAN = 10; +const float TESLA_MAX_ACCEL = 2.0; // m/s^2 +const float TESLA_MIN_ACCEL = -3.5; // m/s^2 + +const int TESLA_FLAG_POWERTRAIN = 1; +const int TESLA_FLAG_LONGITUDINAL_CONTROL = 2; const CanMsg TESLA_TX_MSGS[] = { {0x488, 0, 4}, // DAS_steeringControl {0x45, 0, 8}, // STW_ACTN_RQ {0x45, 2, 8}, // STW_ACTN_RQ + {0x2b9, 0, 8}, // DAS_control }; +#define TESLA_TX_LEN (sizeof(TESLA_TX_MSGS) / sizeof(TESLA_TX_MSGS[0])) + +const CanMsg TESLA_PT_TX_MSGS[] = { + {0x2bf, 0, 8}, // DAS_control +}; +#define TESLA_PT_TX_LEN (sizeof(TESLA_PT_TX_MSGS) / sizeof(TESLA_PT_TX_MSGS[0])) AddrCheckStruct tesla_addr_checks[] = { {.msg = {{0x370, 0, 8, .expected_timestep = 40000U}, { 0 }, { 0 }}}, // EPAS_sysStatus (25Hz) {.msg = {{0x108, 0, 8, .expected_timestep = 10000U}, { 0 }, { 0 }}}, // DI_torque1 (100Hz) {.msg = {{0x118, 0, 6, .expected_timestep = 10000U}, { 0 }, { 0 }}}, // DI_torque2 (100Hz) - {.msg = {{0x155, 0, 8, .expected_timestep = 20000U}, { 0 }, { 0 }}}, // ESP_B (50Hz) {.msg = {{0x20a, 0, 8, .expected_timestep = 20000U}, { 0 }, { 0 }}}, // BrakeMessage (50Hz) {.msg = {{0x368, 0, 8, .expected_timestep = 100000U}, { 0 }, { 0 }}}, // DI_state (10Hz) {.msg = {{0x318, 0, 8, .expected_timestep = 100000U}, { 0 }, { 0 }}}, // GTW_carState (10Hz) @@ -26,10 +37,20 @@ AddrCheckStruct tesla_addr_checks[] = { #define TESLA_ADDR_CHECK_LEN (sizeof(tesla_addr_checks) / sizeof(tesla_addr_checks[0])) addr_checks tesla_rx_checks = {tesla_addr_checks, TESLA_ADDR_CHECK_LEN}; -bool autopilot_enabled = false; +AddrCheckStruct tesla_pt_addr_checks[] = { + {.msg = {{0x106, 0, 8, .expected_timestep = 10000U}, { 0 }, { 0 }}}, // DI_torque1 (100Hz) + {.msg = {{0x116, 0, 6, .expected_timestep = 10000U}, { 0 }, { 0 }}}, // DI_torque2 (100Hz) + {.msg = {{0x1f8, 0, 8, .expected_timestep = 20000U}, { 0 }, { 0 }}}, // BrakeMessage (50Hz) + {.msg = {{0x256, 0, 8, .expected_timestep = 100000U}, { 0 }, { 0 }}}, // DI_state (10Hz) +}; +#define TESLA_PT_ADDR_CHECK_LEN (sizeof(tesla_pt_addr_checks) / sizeof(tesla_pt_addr_checks[0])) +addr_checks tesla_pt_rx_checks = {tesla_pt_addr_checks, TESLA_PT_ADDR_CHECK_LEN}; + +bool tesla_longitudinal = false; +bool tesla_powertrain = false; // Are we the second panda intercepting the powertrain bus? static int tesla_rx_hook(CANPacket_t *to_push) { - bool valid = addr_safety_check(to_push, &tesla_rx_checks, + bool valid = addr_safety_check(to_push, tesla_powertrain ? (&tesla_pt_rx_checks) : (&tesla_rx_checks), NULL, NULL, NULL); if(valid) { @@ -37,30 +58,32 @@ static int tesla_rx_hook(CANPacket_t *to_push) { int addr = GET_ADDR(to_push); if(bus == 0) { - if(addr == 0x370) { - // Steering angle: (0.1 * val) - 819.2 in deg. - // Store it 1/10 deg to match steering request - int angle_meas_new = (((GET_BYTE(to_push, 4) & 0x3F) << 8) | GET_BYTE(to_push, 5)) - 8192; - update_sample(&angle_meas, angle_meas_new); + if (!tesla_powertrain) { + if(addr == 0x370) { + // Steering angle: (0.1 * val) - 819.2 in deg. + // Store it 1/10 deg to match steering request + int angle_meas_new = (((GET_BYTE(to_push, 4) & 0x3F) << 8) | GET_BYTE(to_push, 5)) - 8192; + update_sample(&angle_meas, angle_meas_new); + } } - if(addr == 0x155) { - // Vehicle speed: (0.01 * val) * KPH_TO_MPS - vehicle_speed = ((GET_BYTE(to_push, 5) << 8) | (GET_BYTE(to_push, 6))) * 0.01 / 3.6; - vehicle_moving = vehicle_speed > 0.; + if(addr == (tesla_powertrain ? 0x116 : 0x118)) { + // Vehicle speed: ((0.05 * val) - 25) * MPH_TO_MPS + vehicle_speed = (((((GET_BYTE(to_push, 3) & 0x0F) << 8) | (GET_BYTE(to_push, 2))) * 0.05) - 25) * 0.447; + vehicle_moving = ABS(vehicle_speed) > 0.1; } - if(addr == 0x108) { + if(addr == (tesla_powertrain ? 0x106 : 0x108)) { // Gas pressed gas_pressed = (GET_BYTE(to_push, 6) != 0); } - if(addr == 0x20a) { + if(addr == (tesla_powertrain ? 0x1f8 : 0x20a)) { // Brake pressed brake_pressed = (((GET_BYTE(to_push, 0) & 0x0C) >> 2) != 1); } - if(addr == 0x368) { + if(addr == (tesla_powertrain ? 0x256 : 0x368)) { // Cruise state int cruise_state = (GET_BYTE(to_push, 1) >> 4); bool cruise_engaged = (cruise_state == 2) || // ENABLED @@ -79,22 +102,13 @@ static int tesla_rx_hook(CANPacket_t *to_push) { } } - if (bus == 2) { - if (addr == 0x399) { - // Autopilot status - int autopilot_status = (GET_BYTE(to_push, 0) & 0xF); - autopilot_enabled = (autopilot_status == 3) || // ACTIVE_1 - (autopilot_status == 4) || // ACTIVE_2 - (autopilot_status == 5); // ACTIVE_NAVIGATE_ON_AUTOPILOT - - if (autopilot_enabled) { - controls_allowed = 0; - } - } + if (tesla_powertrain) { + // 0x2bf: DAS_control should not be received on bus 0 + generic_rx_checks((addr == 0x2bf) && (bus == 0)); + } else { + // 0x488: DAS_steeringControl should not be received on bus 0 + generic_rx_checks((addr == 0x488) && (bus == 0)); } - - // 0x488: DAS_steeringControl should not be received on bus 0 - generic_rx_checks((addr == 0x488) && (bus == 0)); } return valid; @@ -106,11 +120,13 @@ static int tesla_tx_hook(CANPacket_t *to_send) { int addr = GET_ADDR(to_send); bool violation = false; - if(!msg_allowed(to_send, TESLA_TX_MSGS, sizeof(TESLA_TX_MSGS) / sizeof(TESLA_TX_MSGS[0]))) { + if(!msg_allowed(to_send, + tesla_powertrain ? TESLA_PT_TX_MSGS : TESLA_TX_MSGS, + tesla_powertrain ? TESLA_PT_TX_LEN : TESLA_TX_LEN)) { tx = 0; } - if(addr == 0x488) { + if(!tesla_powertrain && (addr == 0x488)) { // Steering control: (0.1 * val) - 1638.35 in deg. // We use 1/10 deg as a unit here int raw_angle_can = (((GET_BYTE(to_send, 0) & 0x7F) << 8) | GET_BYTE(to_send, 1)); @@ -146,7 +162,7 @@ static int tesla_tx_hook(CANPacket_t *to_send) { } } - if(addr == 0x45) { + if(!tesla_powertrain && (addr == 0x45)) { // No button other than cancel can be sent by us int control_lever_status = (GET_BYTE(to_send, 0) & 0x3F); if((control_lever_status != 0) && (control_lever_status != 1)) { @@ -154,6 +170,33 @@ static int tesla_tx_hook(CANPacket_t *to_send) { } } + if(addr == (tesla_powertrain ? 0x2bf : 0x2b9)) { + // DAS_control: longitudinal control message + if (tesla_longitudinal) { + // No AEB events may be sent by openpilot + int aeb_event = GET_BYTE(to_send, 2) & 0x03; + if (aeb_event != 0) { + violation = true; + } + + // Don't allow any acceleration limits above the safety limits + int raw_accel_max = ((GET_BYTE(to_send, 6) & 0x1F) << 4) | (GET_BYTE(to_send, 5) >> 4); + int raw_accel_min = ((GET_BYTE(to_send, 5) & 0x0F) << 5) | (GET_BYTE(to_send, 4) >> 3); + float accel_max = (0.04 * raw_accel_max) - 15; + float accel_min = (0.04 * raw_accel_min) - 15; + + if ((accel_max > TESLA_MAX_ACCEL) || (accel_min > TESLA_MAX_ACCEL)){ + violation = true; + } + + if ((accel_max < TESLA_MIN_ACCEL) || (accel_min < TESLA_MIN_ACCEL)){ + violation = true; + } + } else { + violation = true; + } + } + if(violation) { controls_allowed = 0; tx = 0; @@ -167,13 +210,23 @@ static int tesla_fwd_hook(int bus_num, CANPacket_t *to_fwd) { int addr = GET_ADDR(to_fwd); if(bus_num == 0) { - // Chassis to autopilot + // Chassis/PT to autopilot bus_fwd = 2; } if(bus_num == 2) { - // Autopilot to chassis - bool block_msg = ((addr == 0x488) && !autopilot_enabled); + // Autopilot to chassis/PT + int das_control_addr = (tesla_powertrain ? 0x2bf : 0x2b9); + + bool block_msg = false; + if (!tesla_powertrain && (addr == 0x488)) { + block_msg = true; + } + + if (tesla_longitudinal && (addr == das_control_addr)) { + block_msg = true; + } + if(!block_msg) { bus_fwd = 0; } @@ -183,10 +236,12 @@ static int tesla_fwd_hook(int bus_num, CANPacket_t *to_fwd) { } static const addr_checks* tesla_init(int16_t param) { - UNUSED(param); + tesla_powertrain = GET_FLAG(param, TESLA_FLAG_POWERTRAIN); + tesla_longitudinal = GET_FLAG(param, TESLA_FLAG_LONGITUDINAL_CONTROL); controls_allowed = 0; relay_malfunction_reset(); - return &tesla_rx_checks; + + return tesla_powertrain ? (&tesla_pt_rx_checks) : (&tesla_rx_checks); } const safety_hooks tesla_hooks = { diff --git a/python/__init__.py b/python/__init__.py index 97e37a2e..e491f310 100644 --- a/python/__init__.py +++ b/python/__init__.py @@ -236,6 +236,8 @@ 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 def __init__(self, serial=None, claim=True): self._serial = serial diff --git a/tests/safety/common.py b/tests/safety/common.py index bcf4363d..5528ff54 100644 --- a/tests/safety/common.py +++ b/tests/safety/common.py @@ -423,6 +423,10 @@ class PandaSafetyTest(PandaSafetyTestBase): if attr.startswith("Test") and attr != current_test: tx = getattr(getattr(test, attr), "TX_MSGS") if tx is not None: + # No point in comparing different Tesla safety modes + if 'Tesla' in attr and 'Tesla' in current_test: + continue + # TODO: Temporary, should be fixed in panda firmware, safety_honda.h if attr.startswith('TestHonda'): # exceptions for common msgs across different hondas diff --git a/tests/safety/test_tesla.py b/tests/safety/test_tesla.py old mode 100644 new mode 100755 index 5dfdbe2b..8c9b5a14 --- a/tests/safety/test_tesla.py +++ b/tests/safety/test_tesla.py @@ -2,14 +2,17 @@ import unittest import numpy as np from panda import Panda -from panda.tests.safety import libpandasafety_py import panda.tests.safety.common as common +from panda.tests.safety import libpandasafety_py from panda.tests.safety.common import CANPackerPanda ANGLE_DELTA_BP = [0., 5., 15.] ANGLE_DELTA_V = [5., .8, .15] # windup limit ANGLE_DELTA_VU = [5., 3.5, 0.4] # unwind limit +MAX_ACCEL = 2.0 +MIN_ACCEL = -3.5 + class CONTROL_LEVER_STATE: DN_1ST = 32 UP_1ST = 16 @@ -23,19 +26,14 @@ def sign(a): return 1 if a > 0 else -1 class TestTeslaSafety(common.PandaSafetyTest): - TX_MSGS = [[0x488, 0], [0x45, 0], [0x45, 2]] STANDSTILL_THRESHOLD = 0 GAS_PRESSED_THRESHOLD = 3 - RELAY_MALFUNCTION_ADDR = 0x488 RELAY_MALFUNCTION_BUS = 0 - FWD_BLACKLISTED_ADDRS = {2: [0x488]} FWD_BUS_LOOKUP = {0: 2, 2: 0} def setUp(self): - self.packer = CANPackerPanda("tesla_can") - self.safety = libpandasafety_py.libpandasafety - self.safety.set_safety_hooks(Panda.SAFETY_TESLA, 0) - self.safety.init_tests() + self.packer = None + raise unittest.SkipTest def _angle_meas_msg(self, angle): values = {"EPAS_internalSAS": angle} @@ -49,17 +47,13 @@ class TestTeslaSafety(common.PandaSafetyTest): for _ in range(6): self._rx(self._angle_meas_msg(angle)) - def _pcm_status_msg(self, enable): - values = {"DI_cruiseState": 2 if enable else 0} - return self.packer.make_can_msg_panda("DI_state", 0, values) - def _lkas_control_msg(self, angle, enabled): values = {"DAS_steeringAngleRequest": angle, "DAS_steeringControlType": 1 if enabled else 0} return self.packer.make_can_msg_panda("DAS_steeringControl", 0, values) def _speed_msg(self, speed): - values = {"ESP_vehicleSpeed": speed * 3.6} - return self.packer.make_can_msg_panda("ESP_B", 0, values) + values = {"DI_vehicleSpeed": speed / 0.447} + return self.packer.make_can_msg_panda("DI_torque2", 0, values) def _brake_msg(self, brake): values = {"driverBrakeStatus": 2 if brake else 1} @@ -73,9 +67,32 @@ class TestTeslaSafety(common.PandaSafetyTest): values = {"SpdCtrlLvr_Stat": command} return self.packer.make_can_msg_panda("STW_ACTN_RQ", 0, values) - def _autopilot_status_msg(self, status): - values = {"autopilotStatus": status} - return self.packer.make_can_msg_panda("AutopilotStatus", 2, values) + def _pcm_status_msg(self, enable): + values = {"DI_cruiseState": 2 if enable else 0} + return self.packer.make_can_msg_panda("DI_state", 0, values) + + def _long_control_msg(self, set_speed, acc_val=0, jerk_limits=(0, 0), accel_limits=(0, 0), aeb_event=0): + values = { + "DAS_setSpeed": set_speed, + "DAS_accState": acc_val, + "DAS_aebEvent": aeb_event, + "DAS_jerkMin": jerk_limits[0], + "DAS_jerkMax": jerk_limits[1], + "DAS_accelMin": accel_limits[0], + "DAS_accelMax": accel_limits[1], + } + return self.packer.make_can_msg_panda("DAS_control", 0, values) + +class TestTeslaSteeringSafety(TestTeslaSafety): + TX_MSGS = [[0x488, 0], [0x45, 0], [0x45, 2]] + RELAY_MALFUNCTION_ADDR = 0x488 + FWD_BLACKLISTED_ADDRS = {2: [0x488]} + + def setUp(self): + self.packer = CANPackerPanda("tesla_can") + self.safety = libpandasafety_py.libpandasafety + self.safety.set_safety_hooks(Panda.SAFETY_TESLA, 0) + self.safety.init_tests() def test_angle_cmd_when_enabled(self): # when controls are allowed, angle cmd rate limit is enforced @@ -155,11 +172,46 @@ class TestTeslaSafety(common.PandaSafetyTest): self._tx(self._control_lever_cmd(CONTROL_LEVER_STATE.IDLE)) self.assertTrue(self.safety.get_controls_allowed()) - def test_autopilot_passthrough(self): - for ap_status in range(16): - self.safety.set_controls_allowed(1) - self._rx(self._autopilot_status_msg(ap_status)) - self.assertEqual(self.safety.get_controls_allowed(), (ap_status not in [3, 4, 5])) +class TestTeslaLongitudinalSafety(TestTeslaSafety): + def setUp(self): + raise unittest.SkipTest + + def test_no_aeb(self): + for aeb_event in range(4): + self.assertEqual(self._tx(self._long_control_msg(10, aeb_event=aeb_event)), aeb_event == 0) + + def test_acc_accel_limits(self): + for min_accel in np.arange(MIN_ACCEL - 1, MAX_ACCEL + 1, 0.1): + for max_accel in np.arange(MIN_ACCEL - 1, MAX_ACCEL + 1, 0.1): + # floats might not hit exact boundary conditions without rounding + min_accel = round(min_accel, 2) + max_accel = round(max_accel, 2) + + self.safety.set_controls_allowed(True) + send = (MIN_ACCEL <= min_accel <= MAX_ACCEL) and (MIN_ACCEL <= max_accel <= MAX_ACCEL) + self.assertEqual(self._tx(self._long_control_msg(10, acc_val=4, accel_limits=[min_accel, max_accel])), send) + +class TestTeslaChassisLongitudinalSafety(TestTeslaLongitudinalSafety): + TX_MSGS = [[0x488, 0], [0x45, 0], [0x45, 2], [0x2B9, 0]] + RELAY_MALFUNCTION_ADDR = 0x488 + FWD_BLACKLISTED_ADDRS = {2: [0x2B9, 0x488]} + + def setUp(self): + self.packer = CANPackerPanda("tesla_can") + self.safety = libpandasafety_py.libpandasafety + self.safety.set_safety_hooks(Panda.SAFETY_TESLA, Panda.FLAG_TESLA_LONG_CONTROL) + self.safety.init_tests() + +class TestTeslaPTLongitudinalSafety(TestTeslaLongitudinalSafety): + TX_MSGS = [[0x2BF, 0]] + RELAY_MALFUNCTION_ADDR = 0x2BF + FWD_BLACKLISTED_ADDRS = {2: [0x2BF]} + + def setUp(self): + self.packer = CANPackerPanda("tesla_powertrain") + self.safety = libpandasafety_py.libpandasafety + self.safety.set_safety_hooks(Panda.SAFETY_TESLA, Panda.FLAG_TESLA_LONG_CONTROL | Panda.FLAG_TESLA_POWERTRAIN) + self.safety.init_tests() if __name__ == "__main__": unittest.main()