mirror of
https://github.com/infiniteCable2/panda.git
synced 2026-02-19 01:33:52 +08:00
232 lines
8.0 KiB
Python
Executable File
232 lines
8.0 KiB
Python
Executable File
#!/usr/bin/env python3
|
|
import unittest
|
|
import numpy as np
|
|
from panda import Panda
|
|
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
|
|
DN_2ND = 8
|
|
UP_2ND = 4
|
|
RWD = 2
|
|
FWD = 1
|
|
IDLE = 0
|
|
|
|
def sign(a):
|
|
return 1 if a > 0 else -1
|
|
|
|
class TestTeslaSafety(common.PandaSafetyTest):
|
|
STANDSTILL_THRESHOLD = 0
|
|
GAS_PRESSED_THRESHOLD = 3
|
|
RELAY_MALFUNCTION_BUS = 0
|
|
FWD_BUS_LOOKUP = {0: 2, 2: 0}
|
|
|
|
def setUp(self):
|
|
self.packer = None
|
|
raise unittest.SkipTest
|
|
|
|
def _angle_meas_msg(self, angle):
|
|
values = {"EPAS_internalSAS": angle}
|
|
return self.packer.make_can_msg_panda("EPAS_sysStatus", 0, values)
|
|
|
|
def _set_prev_angle(self, t):
|
|
t = int(t * 10)
|
|
self.safety.set_desired_angle_last(t)
|
|
|
|
def _angle_meas_msg_array(self, angle):
|
|
for _ in range(6):
|
|
self._rx(self._angle_meas_msg(angle))
|
|
|
|
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 = {"DI_vehicleSpeed": speed / 0.447}
|
|
return self.packer.make_can_msg_panda("DI_torque2", 0, values)
|
|
|
|
def _user_brake_msg(self, brake):
|
|
values = {"driverBrakeStatus": 2 if brake else 1}
|
|
return self.packer.make_can_msg_panda("BrakeMessage", 0, values)
|
|
|
|
def _user_gas_msg(self, gas):
|
|
values = {"DI_pedalPos": gas}
|
|
return self.packer.make_can_msg_panda("DI_torque1", 0, values)
|
|
|
|
def _control_lever_cmd(self, command):
|
|
values = {"SpdCtrlLvr_Stat": command}
|
|
return self.packer.make_can_msg_panda("STW_ACTN_RQ", 0, 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
|
|
speeds = [0., 1., 5., 10., 15., 50.]
|
|
angles = [-300, -100, -10, 0, 10, 100, 300]
|
|
for a in angles:
|
|
for s in speeds:
|
|
max_delta_up = np.interp(s, ANGLE_DELTA_BP, ANGLE_DELTA_V)
|
|
max_delta_down = np.interp(s, ANGLE_DELTA_BP, ANGLE_DELTA_VU)
|
|
|
|
# first test against false positives
|
|
self._angle_meas_msg_array(a)
|
|
self._rx(self._speed_msg(s))
|
|
|
|
self._set_prev_angle(a)
|
|
self.safety.set_controls_allowed(1)
|
|
|
|
# Stay within limits
|
|
# Up
|
|
self.assertEqual(True, self._tx(self._lkas_control_msg(a + sign(a) * max_delta_up, 1)))
|
|
self.assertTrue(self.safety.get_controls_allowed())
|
|
|
|
# Don't change
|
|
self.assertEqual(True, self._tx(self._lkas_control_msg(a, 1)))
|
|
self.assertTrue(self.safety.get_controls_allowed())
|
|
|
|
# Down
|
|
self.assertEqual(True, self._tx(self._lkas_control_msg(a - sign(a) * max_delta_down, 1)))
|
|
self.assertTrue(self.safety.get_controls_allowed())
|
|
|
|
# Inject too high rates
|
|
# Up
|
|
self.assertEqual(False, self._tx(self._lkas_control_msg(a + sign(a) * (max_delta_up + 1.1), 1)))
|
|
|
|
# Don't change
|
|
self.safety.set_controls_allowed(1)
|
|
self._set_prev_angle(a)
|
|
self.assertTrue(self.safety.get_controls_allowed())
|
|
self.assertEqual(True, self._tx(self._lkas_control_msg(a, 1)))
|
|
self.assertTrue(self.safety.get_controls_allowed())
|
|
|
|
# Down
|
|
self.assertEqual(False, self._tx(self._lkas_control_msg(a - sign(a) * (max_delta_down + 1.1), 1)))
|
|
|
|
# Check desired steer should be the same as steer angle when controls are off
|
|
self.safety.set_controls_allowed(0)
|
|
self.assertEqual(True, self._tx(self._lkas_control_msg(a, 0)))
|
|
|
|
def test_angle_cmd_when_disabled(self):
|
|
self.safety.set_controls_allowed(0)
|
|
|
|
self._set_prev_angle(0)
|
|
self.assertFalse(self._tx(self._lkas_control_msg(0, 1)))
|
|
self.assertFalse(self.safety.get_controls_allowed())
|
|
|
|
def test_acc_buttons(self):
|
|
"""
|
|
FWD (cancel) always allowed.
|
|
"""
|
|
btns = [
|
|
(CONTROL_LEVER_STATE.FWD, True),
|
|
(CONTROL_LEVER_STATE.RWD, False),
|
|
(CONTROL_LEVER_STATE.UP_1ST, False),
|
|
(CONTROL_LEVER_STATE.UP_2ND, False),
|
|
(CONTROL_LEVER_STATE.DN_1ST, False),
|
|
(CONTROL_LEVER_STATE.DN_2ND, False),
|
|
(CONTROL_LEVER_STATE.IDLE, False),
|
|
]
|
|
for btn, should_tx in btns:
|
|
for controls_allowed in (True, False):
|
|
self.safety.set_controls_allowed(controls_allowed)
|
|
tx = self._tx(self._control_lever_cmd(btn))
|
|
self.assertEqual(tx, should_tx)
|
|
|
|
|
|
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_stock_aeb_passthrough(self):
|
|
no_aeb_msg = self._long_control_msg(10, aeb_event=0)
|
|
aeb_msg = self._long_control_msg(10, aeb_event=1)
|
|
|
|
# stock system sends no AEB -> no forwarding, and OP is allowed to TX
|
|
self.assertEqual(-1, self.safety.safety_fwd_hook(2, no_aeb_msg))
|
|
self.assertEqual(True, self._tx(no_aeb_msg))
|
|
|
|
# stock system sends AEB -> forwarding, and OP is not allowed to TX
|
|
self.assertEqual(0, self.safety.safety_fwd_hook(2, aeb_msg))
|
|
self.assertEqual(False, self._tx(no_aeb_msg))
|
|
|
|
# reset global state for next tests
|
|
self.assertEqual(-1, self.safety.safety_fwd_hook(2, no_aeb_msg))
|
|
|
|
def test_acc_accel_limits(self):
|
|
for controls_allowed in [True, False]:
|
|
self.safety.set_controls_allowed(controls_allowed)
|
|
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)
|
|
if controls_allowed:
|
|
send = (MIN_ACCEL <= min_accel <= MAX_ACCEL) and (MIN_ACCEL <= max_accel <= MAX_ACCEL)
|
|
else:
|
|
send = np.all(np.isclose([min_accel, max_accel], 0, atol=0.0001))
|
|
self.assertEqual(send, self._tx(self._long_control_msg(10, acc_val=4, accel_limits=[min_accel, max_accel])))
|
|
|
|
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()
|