Safety Test Refactor: Subaru (#502)
* subaru global kind of works * all global uses can packer * fix legacy * legacy uses can packer * bump opendbc * i guess docker caches thatmaster
Adeeb
GitHub
parent
57cc954f24
commit
6f105e827b
|
|
@ -34,11 +34,11 @@ RUN pip install -r /tmp/requirements.txt
|
|||
WORKDIR /openpilot
|
||||
RUN git clone https://github.com/commaai/opendbc.git || true
|
||||
WORKDIR /openpilot/opendbc
|
||||
RUN git checkout 36c471e59eaac3760a00125d557ef19af091f289
|
||||
RUN git pull && git checkout a57e7ddbd72c92241d5d6442da9d47c55e95a8cf
|
||||
WORKDIR /openpilot
|
||||
RUN git clone https://github.com/commaai/cereal.git
|
||||
WORKDIR /openpilot/cereal
|
||||
RUN git checkout e370f79522ff7fc0b16f33f4fef420be48061206
|
||||
RUN git pull && git checkout e370f79522ff7fc0b16f33f4fef420be48061206
|
||||
RUN /openpilot/cereal/install_capnp.sh
|
||||
|
||||
RUN pip install -r /openpilot/opendbc/requirements.txt
|
||||
|
|
|
|||
|
|
@ -3,7 +3,8 @@ import unittest
|
|||
import numpy as np
|
||||
from panda import Panda
|
||||
from panda.tests.safety import libpandasafety_py
|
||||
from panda.tests.safety.common import StdTest, make_msg, UNSAFE_MODE
|
||||
import panda.tests.safety.common as common
|
||||
from panda.tests.safety.common import CANPackerPanda
|
||||
|
||||
MAX_RATE_UP = 50
|
||||
MAX_RATE_DOWN = 70
|
||||
|
|
@ -12,174 +13,77 @@ MAX_STEER = 2047
|
|||
MAX_RT_DELTA = 940
|
||||
RT_INTERVAL = 250000
|
||||
|
||||
DRIVER_TORQUE_ALLOWANCE = 60;
|
||||
DRIVER_TORQUE_FACTOR = 10;
|
||||
|
||||
SPEED_THRESHOLD = 20 # 1kph (see dbc file)
|
||||
|
||||
TX_MSGS = [[0x122, 0], [0x221, 0], [0x322, 0]]
|
||||
TX_L_MSGS = [[0x164, 0], [0x221, 0], [0x322, 0]]
|
||||
|
||||
def twos_comp(val, bits):
|
||||
if val >= 0:
|
||||
return val
|
||||
else:
|
||||
return (2**bits) + val
|
||||
|
||||
def sign(a):
|
||||
if a > 0:
|
||||
return 1
|
||||
else:
|
||||
return -1
|
||||
|
||||
def subaru_checksum(msg, addr, len_msg):
|
||||
checksum = addr + (addr >> 8)
|
||||
for i in range(len_msg):
|
||||
if i < 4:
|
||||
checksum += (msg.RDLR >> (8 * i))
|
||||
else:
|
||||
checksum += (msg.RDHR >> (8 * (i - 4)))
|
||||
return checksum & 0xff
|
||||
DRIVER_TORQUE_ALLOWANCE = 60
|
||||
DRIVER_TORQUE_FACTOR = 10
|
||||
|
||||
|
||||
class TestSubaruSafety(unittest.TestCase):
|
||||
class TestSubaruSafety(common.PandaSafetyTest):
|
||||
cnt_gas = 0
|
||||
cnt_torque_driver = 0
|
||||
cnt_cruise = 0
|
||||
cnt_speed = 0
|
||||
cnt_brake = 0
|
||||
|
||||
@classmethod
|
||||
def setUp(cls):
|
||||
cls.safety = libpandasafety_py.libpandasafety
|
||||
cls.safety.set_safety_hooks(Panda.SAFETY_SUBARU, 0)
|
||||
cls.safety.init_tests_subaru()
|
||||
TX_MSGS = [[0x122, 0], [0x221, 0], [0x322, 0]]
|
||||
STANDSTILL_THRESHOLD = 20 # 1kph (see dbc file)
|
||||
RELAY_MALFUNCTION_ADDR = 0x122
|
||||
RELAY_MALFUNCTION_BUS = 0
|
||||
FWD_BLACKLISTED_ADDRS = {2: [290, 545, 802]}
|
||||
FWD_BUS_LOOKUP = {0: 2, 2: 0}
|
||||
|
||||
def setUp(self):
|
||||
self.packer = CANPackerPanda("subaru_global_2017")
|
||||
self.safety = libpandasafety_py.libpandasafety
|
||||
self.safety.set_safety_hooks(Panda.SAFETY_SUBARU, 0)
|
||||
self.safety.init_tests_subaru()
|
||||
|
||||
def _set_prev_torque(self, t):
|
||||
self.safety.set_subaru_desired_torque_last(t)
|
||||
self.safety.set_subaru_rt_torque_last(t)
|
||||
|
||||
def _torque_driver_msg(self, torque):
|
||||
t = twos_comp(torque, 11)
|
||||
if self.safety.get_subaru_global():
|
||||
to_send = make_msg(0, 0x119)
|
||||
to_send[0].RDLR = ((t & 0x7FF) << 16)
|
||||
to_send[0].RDLR |= (self.cnt_torque_driver & 0xF) << 8
|
||||
to_send[0].RDLR |= subaru_checksum(to_send, 0x119, 8)
|
||||
self.__class__.cnt_torque_driver += 1
|
||||
else:
|
||||
to_send = make_msg(0, 0x371)
|
||||
to_send[0].RDLR = (t & 0x7) << 29
|
||||
to_send[0].RDHR = (t >> 3) & 0xFF
|
||||
return to_send
|
||||
values = {"Steer_Torque_Sensor": -torque, "counter": self.cnt_torque_driver % 4}
|
||||
self.__class__.cnt_torque_driver += 1
|
||||
return self.packer.make_can_msg_panda("Steering_Torque", 0, values)
|
||||
|
||||
def _speed_msg(self, speed):
|
||||
speed &= 0x1FFF
|
||||
to_send = make_msg(0, 0x13a)
|
||||
to_send[0].RDLR = speed << 12
|
||||
to_send[0].RDHR = speed << 6
|
||||
to_send[0].RDLR |= (self.cnt_speed & 0xF) << 8
|
||||
to_send[0].RDLR |= subaru_checksum(to_send, 0x13a, 8)
|
||||
# subaru safety doesn't use the scaled value, so undo the scaling
|
||||
values = {s: speed*0.057 for s in ["FR", "FL", "RR", "RL"]}
|
||||
values["Counter"] = self.cnt_speed % 4
|
||||
self.__class__.cnt_speed += 1
|
||||
return to_send
|
||||
return self.packer.make_can_msg_panda("Wheel_Speeds", 0, values)
|
||||
|
||||
def _brake_msg(self, brake):
|
||||
to_send = make_msg(0, 0x139)
|
||||
to_send[0].RDHR = (brake << 4) & 0xFFF
|
||||
to_send[0].RDLR |= (self.cnt_brake & 0xF) << 8
|
||||
to_send[0].RDLR |= subaru_checksum(to_send, 0x139, 8)
|
||||
values = {"Brake_Pedal": brake, "Counter": self.cnt_brake % 4}
|
||||
self.__class__.cnt_brake += 1
|
||||
return to_send
|
||||
return self.packer.make_can_msg_panda("Brake_Pedal", 0, values)
|
||||
|
||||
def _torque_msg(self, torque):
|
||||
t = twos_comp(torque, 13)
|
||||
if self.safety.get_subaru_global():
|
||||
to_send = make_msg(0, 0x122)
|
||||
to_send[0].RDLR = (t << 16)
|
||||
else:
|
||||
to_send = make_msg(0, 0x164)
|
||||
to_send[0].RDLR = (t << 8)
|
||||
return to_send
|
||||
values = {"LKAS_Output": -torque}
|
||||
return self.packer.make_can_msg_panda("ES_LKAS", 0, values)
|
||||
|
||||
def _gas_msg(self, gas):
|
||||
if self.safety.get_subaru_global():
|
||||
to_send = make_msg(0, 0x40)
|
||||
to_send[0].RDHR = gas & 0xFF
|
||||
to_send[0].RDLR |= (self.cnt_gas & 0xF) << 8
|
||||
to_send[0].RDLR |= subaru_checksum(to_send, 0x40, 8)
|
||||
self.__class__.cnt_gas += 1
|
||||
else:
|
||||
to_send = make_msg(0, 0x140)
|
||||
to_send[0].RDLR = gas & 0xFF
|
||||
return to_send
|
||||
values = {"Throttle_Pedal": gas, "Counter": self.cnt_gas % 4}
|
||||
self.__class__.cnt_gas += 1
|
||||
return self.packer.make_can_msg_panda("Throttle", 0, values)
|
||||
|
||||
def _cruise_msg(self, cruise):
|
||||
if self.safety.get_subaru_global():
|
||||
to_send = make_msg(0, 0x240)
|
||||
to_send[0].RDHR = cruise << 9
|
||||
to_send[0].RDLR |= (self.cnt_cruise & 0xF) << 8
|
||||
to_send[0].RDLR |= subaru_checksum(to_send, 0x240, 8)
|
||||
self.__class__.cnt_cruise += 1
|
||||
else:
|
||||
to_send = make_msg(0, 0x144)
|
||||
to_send[0].RDHR = cruise << 17
|
||||
return to_send
|
||||
def _pcm_status_msg(self, cruise):
|
||||
values = {"Cruise_Activated": cruise, "Counter": self.cnt_cruise % 4}
|
||||
self.__class__.cnt_cruise += 1
|
||||
return self.packer.make_can_msg_panda("CruiseControl", 0, values)
|
||||
|
||||
def _set_torque_driver(self, min_t, max_t):
|
||||
for i in range(0, 5):
|
||||
self.safety.safety_rx_hook(self._torque_driver_msg(min_t))
|
||||
self.safety.safety_rx_hook(self._torque_driver_msg(max_t))
|
||||
|
||||
def test_spam_can_buses(self):
|
||||
StdTest.test_spam_can_buses(self, TX_MSGS if self.safety.get_subaru_global() else TX_L_MSGS)
|
||||
|
||||
def test_relay_malfunction(self):
|
||||
StdTest.test_relay_malfunction(self, 0x122 if self.safety.get_subaru_global() else 0x164)
|
||||
|
||||
def test_default_controls_not_allowed(self):
|
||||
self.assertFalse(self.safety.get_controls_allowed())
|
||||
|
||||
def test_enable_control_allowed_from_cruise(self):
|
||||
self.safety.safety_rx_hook(self._cruise_msg(True))
|
||||
self.assertTrue(self.safety.get_controls_allowed())
|
||||
|
||||
def test_disable_control_allowed_from_cruise(self):
|
||||
self.safety.set_controls_allowed(1)
|
||||
self.safety.safety_rx_hook(self._cruise_msg(False))
|
||||
self.assertFalse(self.safety.get_controls_allowed())
|
||||
|
||||
def test_disengage_on_gas(self):
|
||||
self.safety.set_controls_allowed(True)
|
||||
self.safety.safety_rx_hook(self._gas_msg(0))
|
||||
self.assertTrue(self.safety.get_controls_allowed())
|
||||
self.safety.safety_rx_hook(self._gas_msg(1))
|
||||
self.assertFalse(self.safety.get_controls_allowed())
|
||||
|
||||
def test_unsafe_mode_no_disengage_on_gas(self):
|
||||
self.safety.safety_rx_hook(self._gas_msg(0))
|
||||
self.safety.set_controls_allowed(True)
|
||||
self.safety.set_unsafe_mode(UNSAFE_MODE.DISABLE_DISENGAGE_ON_GAS)
|
||||
self.safety.safety_rx_hook(self._gas_msg(1))
|
||||
self.assertTrue(self.safety.get_controls_allowed())
|
||||
self.safety.set_unsafe_mode(UNSAFE_MODE.DEFAULT)
|
||||
|
||||
def test_brake_disengage(self):
|
||||
if (self.safety.get_subaru_global()):
|
||||
StdTest.test_allow_brake_at_zero_speed(self)
|
||||
StdTest.test_not_allow_brake_when_moving(self, SPEED_THRESHOLD)
|
||||
|
||||
def test_steer_safety_check(self):
|
||||
for enabled in [0, 1]:
|
||||
for t in range(-3000, 3000):
|
||||
self.safety.set_controls_allowed(enabled)
|
||||
self._set_prev_torque(t)
|
||||
if abs(t) > MAX_STEER or (not enabled and abs(t) > 0):
|
||||
self.assertFalse(self.safety.safety_tx_hook(self._torque_msg(t)))
|
||||
else:
|
||||
self.assertTrue(self.safety.safety_tx_hook(self._torque_msg(t)))
|
||||
|
||||
def test_manually_enable_controls_allowed(self):
|
||||
StdTest.test_manually_enable_controls_allowed(self)
|
||||
block = abs(t) > MAX_STEER or (not enabled and abs(t) > 0)
|
||||
self.assertEqual(not block, self.safety.safety_tx_hook(self._torque_msg(t)))
|
||||
|
||||
def test_non_realtime_limit_up(self):
|
||||
self._set_torque_driver(0, 0)
|
||||
|
|
@ -262,28 +166,43 @@ class TestSubaruSafety(unittest.TestCase):
|
|||
self.assertTrue(self.safety.safety_tx_hook(self._torque_msg(sign * (MAX_RT_DELTA + 1))))
|
||||
|
||||
|
||||
def test_fwd_hook(self):
|
||||
buss = list(range(0x0, 0x3))
|
||||
msgs = list(range(0x1, 0x800))
|
||||
blocked_msgs = [290, 545, 802] if self.safety.get_subaru_global() else [356, 545, 802]
|
||||
for b in buss:
|
||||
for m in msgs:
|
||||
if b == 0:
|
||||
fwd_bus = 2
|
||||
elif b == 1:
|
||||
fwd_bus = -1
|
||||
elif b == 2:
|
||||
fwd_bus = -1 if m in blocked_msgs else 0
|
||||
|
||||
# assume len 8
|
||||
self.assertEqual(fwd_bus, self.safety.safety_fwd_hook(b, make_msg(b, m, 8)))
|
||||
|
||||
class TestSubaruLegacySafety(TestSubaruSafety):
|
||||
@classmethod
|
||||
def setUp(cls):
|
||||
cls.safety = libpandasafety_py.libpandasafety
|
||||
cls.safety.set_safety_hooks(Panda.SAFETY_SUBARU_LEGACY, 0)
|
||||
cls.safety.init_tests_subaru()
|
||||
|
||||
TX_MSGS = [[0x164, 0], [0x221, 0], [0x322, 0]]
|
||||
RELAY_MALFUNCTION_ADDR = 0x164
|
||||
RELAY_MALFUNCTION_BUS = 0
|
||||
FWD_BLACKLISTED_ADDRS = {2: [356, 545, 802]}
|
||||
FWD_BUS_LOOKUP = {0: 2, 2: 0}
|
||||
|
||||
def setUp(self):
|
||||
self.packer = CANPackerPanda("subaru_outback_2015_eyesight")
|
||||
self.safety = libpandasafety_py.libpandasafety
|
||||
self.safety.set_safety_hooks(Panda.SAFETY_SUBARU_LEGACY, 0)
|
||||
self.safety.init_tests_subaru()
|
||||
|
||||
# subaru legacy safety doesn't have brake checks
|
||||
def test_prev_brake(self): pass
|
||||
def test_not_allow_brake_when_moving(self): pass
|
||||
def test_allow_brake_at_zero_speed(self): pass
|
||||
|
||||
def _torque_driver_msg(self, torque):
|
||||
# TODO: figure out if this scaling factor from the DBC is right.
|
||||
# if it is, remove the scaling from here and put it in the safety code
|
||||
values = {"Steer_Torque_Sensor": torque*8}
|
||||
return self.packer.make_can_msg_panda("Steering_Torque", 0, values)
|
||||
|
||||
def _torque_msg(self, torque):
|
||||
values = {"LKAS_Command": -torque}
|
||||
return self.packer.make_can_msg_panda("ES_LKAS", 0, values)
|
||||
|
||||
def _gas_msg(self, gas):
|
||||
values = {"Throttle_Pedal": gas}
|
||||
return self.packer.make_can_msg_panda("Throttle", 0, values)
|
||||
|
||||
def _pcm_status_msg(self, cruise):
|
||||
values = {"Cruise_Activated": cruise}
|
||||
return self.packer.make_can_msg_panda("CruiseControl", 0, values)
|
||||
|
||||
|
||||
if __name__ == "__main__":
|
||||
unittest.main()
|
||||
|
|
|
|||
Loading…
Reference in New Issue