293 lines
11 KiB
Python
Executable File
293 lines
11 KiB
Python
Executable File
#!/usr/bin/env python3
|
|
import unittest
|
|
import numpy as np
|
|
import libpandasafety_py
|
|
from panda import Panda
|
|
|
|
MAX_BRAKE = 255
|
|
|
|
INTERCEPTOR_THRESHOLD = 328
|
|
|
|
class TestHondaSafety(unittest.TestCase):
|
|
@classmethod
|
|
def setUp(cls):
|
|
cls.safety = libpandasafety_py.libpandasafety
|
|
cls.safety.safety_set_mode(Panda.SAFETY_HONDA, 0)
|
|
cls.safety.init_tests_honda()
|
|
|
|
def _send_msg(self, bus, addr, length):
|
|
to_send = libpandasafety_py.ffi.new('CAN_FIFOMailBox_TypeDef *')
|
|
to_send[0].RIR = addr << 21
|
|
to_send[0].RDTR = length
|
|
to_send[0].RDTR = bus << 4
|
|
|
|
return to_send
|
|
|
|
def _speed_msg(self, speed):
|
|
to_send = libpandasafety_py.ffi.new('CAN_FIFOMailBox_TypeDef *')
|
|
to_send[0].RIR = 0x158 << 21
|
|
to_send[0].RDLR = speed
|
|
|
|
return to_send
|
|
|
|
def _button_msg(self, buttons, msg):
|
|
to_send = libpandasafety_py.ffi.new('CAN_FIFOMailBox_TypeDef *')
|
|
to_send[0].RIR = msg << 21
|
|
to_send[0].RDLR = buttons << 5
|
|
is_panda_black = self.safety.get_hw_type() == 3 # black_panda
|
|
honda_bosch_hardware = self.safety.get_honda_bosch_hardware()
|
|
bus = 1 if is_panda_black and honda_bosch_hardware else 0
|
|
to_send[0].RDTR = bus << 4
|
|
|
|
return to_send
|
|
|
|
def _brake_msg(self, brake):
|
|
to_send = libpandasafety_py.ffi.new('CAN_FIFOMailBox_TypeDef *')
|
|
to_send[0].RIR = 0x17C << 21
|
|
to_send[0].RDHR = 0x200000 if brake else 0
|
|
|
|
return to_send
|
|
|
|
def _alt_brake_msg(self, brake):
|
|
to_send = libpandasafety_py.ffi.new('CAN_FIFOMailBox_TypeDef *')
|
|
to_send[0].RIR = 0x1BE << 21
|
|
to_send[0].RDLR = 0x10 if brake else 0
|
|
|
|
return to_send
|
|
|
|
def _gas_msg(self, gas):
|
|
to_send = libpandasafety_py.ffi.new('CAN_FIFOMailBox_TypeDef *')
|
|
to_send[0].RIR = 0x17C << 21
|
|
to_send[0].RDLR = 1 if gas else 0
|
|
|
|
return to_send
|
|
|
|
def _send_brake_msg(self, brake):
|
|
to_send = libpandasafety_py.ffi.new('CAN_FIFOMailBox_TypeDef *')
|
|
to_send[0].RIR = 0x1FA << 21
|
|
to_send[0].RDLR = ((brake & 0x3) << 14) | ((brake & 0x3FF) >> 2)
|
|
|
|
return to_send
|
|
|
|
def _send_interceptor_msg(self, gas, addr):
|
|
to_send = libpandasafety_py.ffi.new('CAN_FIFOMailBox_TypeDef *')
|
|
to_send[0].RIR = addr << 21
|
|
to_send[0].RDTR = 6
|
|
gas2 = gas * 2
|
|
to_send[0].RDLR = ((gas & 0xff) << 8) | ((gas & 0xff00) >> 8) | \
|
|
((gas2 & 0xff) << 24) | ((gas2 & 0xff00) << 8)
|
|
|
|
return to_send
|
|
|
|
def _send_steer_msg(self, steer):
|
|
to_send = libpandasafety_py.ffi.new('CAN_FIFOMailBox_TypeDef *')
|
|
to_send[0].RIR = 0xE4 << 21
|
|
to_send[0].RDLR = steer
|
|
|
|
return to_send
|
|
|
|
def test_default_controls_not_allowed(self):
|
|
self.assertFalse(self.safety.get_controls_allowed())
|
|
|
|
def test_resume_button(self):
|
|
RESUME_BTN = 4
|
|
self.safety.set_controls_allowed(0)
|
|
self.safety.safety_rx_hook(self._button_msg(RESUME_BTN, 0x1A6))
|
|
self.assertTrue(self.safety.get_controls_allowed())
|
|
|
|
def test_set_button(self):
|
|
SET_BTN = 3
|
|
self.safety.set_controls_allowed(0)
|
|
self.safety.safety_rx_hook(self._button_msg(SET_BTN, 0x1A6))
|
|
self.assertTrue(self.safety.get_controls_allowed())
|
|
|
|
def test_cancel_button(self):
|
|
CANCEL_BTN = 2
|
|
self.safety.set_controls_allowed(1)
|
|
self.safety.safety_rx_hook(self._button_msg(CANCEL_BTN, 0x1A6))
|
|
self.assertFalse(self.safety.get_controls_allowed())
|
|
|
|
def test_sample_speed(self):
|
|
self.assertEqual(0, self.safety.get_honda_moving())
|
|
self.safety.safety_rx_hook(self._speed_msg(100))
|
|
self.assertEqual(1, self.safety.get_honda_moving())
|
|
|
|
def test_prev_brake(self):
|
|
self.assertFalse(self.safety.get_honda_brake_pressed_prev())
|
|
self.safety.safety_rx_hook(self._brake_msg(True))
|
|
self.assertTrue(self.safety.get_honda_brake_pressed_prev())
|
|
|
|
def test_disengage_on_brake(self):
|
|
self.safety.set_controls_allowed(1)
|
|
self.safety.safety_rx_hook(self._brake_msg(1))
|
|
self.assertFalse(self.safety.get_controls_allowed())
|
|
|
|
def test_alt_disengage_on_brake(self):
|
|
self.safety.set_honda_alt_brake_msg(1)
|
|
self.safety.set_controls_allowed(1)
|
|
self.safety.safety_rx_hook(self._alt_brake_msg(1))
|
|
self.assertFalse(self.safety.get_controls_allowed())
|
|
|
|
self.safety.set_honda_alt_brake_msg(0)
|
|
self.safety.set_controls_allowed(1)
|
|
self.safety.safety_rx_hook(self._alt_brake_msg(1))
|
|
self.assertTrue(self.safety.get_controls_allowed())
|
|
|
|
def test_allow_brake_at_zero_speed(self):
|
|
# Brake was already pressed
|
|
self.safety.safety_rx_hook(self._brake_msg(True))
|
|
self.safety.set_controls_allowed(1)
|
|
|
|
self.safety.safety_rx_hook(self._brake_msg(True))
|
|
self.assertTrue(self.safety.get_controls_allowed())
|
|
self.safety.safety_rx_hook(self._brake_msg(False)) # reset no brakes
|
|
|
|
def test_not_allow_brake_when_moving(self):
|
|
# Brake was already pressed
|
|
self.safety.safety_rx_hook(self._brake_msg(True))
|
|
self.safety.safety_rx_hook(self._speed_msg(100))
|
|
self.safety.set_controls_allowed(1)
|
|
|
|
self.safety.safety_rx_hook(self._brake_msg(True))
|
|
self.assertFalse(self.safety.get_controls_allowed())
|
|
|
|
def test_prev_gas(self):
|
|
self.safety.safety_rx_hook(self._gas_msg(False))
|
|
self.assertFalse(self.safety.get_honda_gas_prev())
|
|
self.safety.safety_rx_hook(self._gas_msg(True))
|
|
self.assertTrue(self.safety.get_honda_gas_prev())
|
|
|
|
def test_prev_gas_interceptor(self):
|
|
self.safety.safety_rx_hook(self._send_interceptor_msg(0x0, 0x201))
|
|
self.assertFalse(self.safety.get_gas_interceptor_prev())
|
|
self.safety.safety_rx_hook(self._send_interceptor_msg(0x1000, 0x201))
|
|
self.assertTrue(self.safety.get_gas_interceptor_prev())
|
|
self.safety.safety_rx_hook(self._send_interceptor_msg(0x0, 0x201))
|
|
self.safety.set_gas_interceptor_detected(False)
|
|
|
|
def test_disengage_on_gas(self):
|
|
for long_controls_allowed in [0, 1]:
|
|
self.safety.set_long_controls_allowed(long_controls_allowed)
|
|
self.safety.safety_rx_hook(self._gas_msg(0))
|
|
self.safety.set_controls_allowed(1)
|
|
self.safety.safety_rx_hook(self._gas_msg(1))
|
|
if long_controls_allowed:
|
|
self.assertFalse(self.safety.get_controls_allowed())
|
|
else:
|
|
self.assertTrue(self.safety.get_controls_allowed())
|
|
self.safety.set_long_controls_allowed(True)
|
|
|
|
def test_allow_engage_with_gas_pressed(self):
|
|
self.safety.safety_rx_hook(self._gas_msg(1))
|
|
self.safety.set_controls_allowed(1)
|
|
self.safety.safety_rx_hook(self._gas_msg(1))
|
|
self.assertTrue(self.safety.get_controls_allowed())
|
|
|
|
def test_disengage_on_gas_interceptor(self):
|
|
for long_controls_allowed in [0, 1]:
|
|
for g in range(0, 0x1000):
|
|
self.safety.set_long_controls_allowed(long_controls_allowed)
|
|
self.safety.safety_rx_hook(self._send_interceptor_msg(0, 0x201))
|
|
self.safety.set_controls_allowed(True)
|
|
self.safety.safety_rx_hook(self._send_interceptor_msg(g, 0x201))
|
|
remain_enabled = (not long_controls_allowed or g <= INTERCEPTOR_THRESHOLD)
|
|
self.assertEqual(remain_enabled, self.safety.get_controls_allowed())
|
|
self.safety.safety_rx_hook(self._send_interceptor_msg(0, 0x201))
|
|
self.safety.set_gas_interceptor_detected(False)
|
|
self.safety.set_long_controls_allowed(True)
|
|
|
|
def test_allow_engage_with_gas_interceptor_pressed(self):
|
|
self.safety.safety_rx_hook(self._send_interceptor_msg(0x1000, 0x201))
|
|
self.safety.set_controls_allowed(1)
|
|
self.safety.safety_rx_hook(self._send_interceptor_msg(0x1000, 0x201))
|
|
self.assertTrue(self.safety.get_controls_allowed())
|
|
self.safety.safety_rx_hook(self._send_interceptor_msg(0, 0x201))
|
|
self.safety.set_gas_interceptor_detected(False)
|
|
|
|
def test_brake_safety_check(self):
|
|
for fwd_brake in [False, True]:
|
|
self.safety.set_honda_fwd_brake(fwd_brake)
|
|
for long_controls_allowed in [0, 1]:
|
|
self.safety.set_long_controls_allowed(long_controls_allowed)
|
|
for brake in np.arange(0, MAX_BRAKE + 10, 1):
|
|
for controls_allowed in [True, False]:
|
|
self.safety.set_controls_allowed(controls_allowed)
|
|
if fwd_brake:
|
|
send = False # block openpilot brake msg when fwd'ing stock msg
|
|
elif controls_allowed and long_controls_allowed:
|
|
send = MAX_BRAKE >= brake >= 0
|
|
else:
|
|
send = brake == 0
|
|
self.assertEqual(send, self.safety.safety_tx_hook(self._send_brake_msg(brake)))
|
|
self.safety.set_long_controls_allowed(True)
|
|
self.safety.set_honda_fwd_brake(False)
|
|
|
|
def test_gas_interceptor_safety_check(self):
|
|
for long_controls_allowed in [0, 1]:
|
|
self.safety.set_long_controls_allowed(long_controls_allowed)
|
|
for gas in np.arange(0, 4000, 100):
|
|
for controls_allowed in [True, False]:
|
|
self.safety.set_controls_allowed(controls_allowed)
|
|
if controls_allowed and long_controls_allowed:
|
|
send = True
|
|
else:
|
|
send = gas == 0
|
|
self.assertEqual(send, self.safety.safety_tx_hook(self._send_interceptor_msg(gas, 0x200)))
|
|
self.safety.set_long_controls_allowed(True)
|
|
|
|
def test_steer_safety_check(self):
|
|
self.safety.set_controls_allowed(0)
|
|
self.assertTrue(self.safety.safety_tx_hook(self._send_steer_msg(0x0000)))
|
|
self.assertFalse(self.safety.safety_tx_hook(self._send_steer_msg(0x1000)))
|
|
|
|
def test_spam_cancel_safety_check(self):
|
|
RESUME_BTN = 4
|
|
SET_BTN = 3
|
|
CANCEL_BTN = 2
|
|
BUTTON_MSG = 0x296
|
|
self.safety.set_honda_bosch_hardware(1)
|
|
self.safety.set_controls_allowed(0)
|
|
self.assertTrue(self.safety.safety_tx_hook(self._button_msg(CANCEL_BTN, BUTTON_MSG)))
|
|
self.assertFalse(self.safety.safety_tx_hook(self._button_msg(RESUME_BTN, BUTTON_MSG)))
|
|
self.assertFalse(self.safety.safety_tx_hook(self._button_msg(SET_BTN, BUTTON_MSG)))
|
|
# do not block resume if we are engaged already
|
|
self.safety.set_controls_allowed(1)
|
|
self.assertTrue(self.safety.safety_tx_hook(self._button_msg(RESUME_BTN, BUTTON_MSG)))
|
|
|
|
def test_fwd_hook(self):
|
|
buss = list(range(0x0, 0x3))
|
|
msgs = list(range(0x1, 0x800))
|
|
long_controls_allowed = [0, 1]
|
|
fwd_brake = [False, True]
|
|
|
|
self.safety.set_honda_bosch_hardware(0)
|
|
|
|
for f in fwd_brake:
|
|
self.safety.set_honda_fwd_brake(f)
|
|
for l in long_controls_allowed:
|
|
self.safety.set_long_controls_allowed(l)
|
|
blocked_msgs = [0xE4, 0x194, 0x33D]
|
|
if l:
|
|
blocked_msgs += [0x30C, 0x39F]
|
|
if not f:
|
|
blocked_msgs += [0x1FA]
|
|
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, self._send_msg(b, m, 8)))
|
|
|
|
self.safety.set_long_controls_allowed(True)
|
|
self.safety.set_honda_fwd_brake(False)
|
|
|
|
|
|
|
|
if __name__ == "__main__":
|
|
unittest.main()
|