commit
f0c5ca7227
16 changed files with 373 additions and 71 deletions
@ -0,0 +1,186 @@ |
|||||||
|
#!/usr/bin/env python2 |
||||||
|
import unittest |
||||||
|
import numpy as np |
||||||
|
import libpandasafety_py |
||||||
|
|
||||||
|
MAX_RATE_UP = 3 |
||||||
|
MAX_RATE_DOWN = 7 |
||||||
|
MAX_STEER = 250 |
||||||
|
|
||||||
|
MAX_RT_DELTA = 112 |
||||||
|
RT_INTERVAL = 250000 |
||||||
|
|
||||||
|
DRIVER_TORQUE_ALLOWANCE = 50; |
||||||
|
DRIVER_TORQUE_FACTOR = 2; |
||||||
|
|
||||||
|
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 |
||||||
|
|
||||||
|
class TestHyundaiSafety(unittest.TestCase): |
||||||
|
@classmethod |
||||||
|
def setUp(cls): |
||||||
|
cls.safety = libpandasafety_py.libpandasafety |
||||||
|
cls.safety.nooutput_init(0) |
||||||
|
cls.safety.init_tests_hyundai() |
||||||
|
|
||||||
|
def _button_msg(self, buttons): |
||||||
|
to_send = libpandasafety_py.ffi.new('CAN_FIFOMailBox_TypeDef *') |
||||||
|
to_send[0].RIR = 1265 << 21 |
||||||
|
to_send[0].RDLR = buttons |
||||||
|
return to_send |
||||||
|
|
||||||
|
def _set_prev_torque(self, t): |
||||||
|
self.safety.set_hyundai_desired_torque_last(t) |
||||||
|
self.safety.set_hyundai_rt_torque_last(t) |
||||||
|
|
||||||
|
def _torque_driver_msg(self, torque): |
||||||
|
to_send = libpandasafety_py.ffi.new('CAN_FIFOMailBox_TypeDef *') |
||||||
|
to_send[0].RIR = 897 << 21 |
||||||
|
to_send[0].RDLR = (torque + 2048) << 11 |
||||||
|
return to_send |
||||||
|
|
||||||
|
def _torque_msg(self, torque): |
||||||
|
to_send = libpandasafety_py.ffi.new('CAN_FIFOMailBox_TypeDef *') |
||||||
|
to_send[0].RIR = 832 << 21 |
||||||
|
to_send[0].RDLR = (torque + 1024) << 16 |
||||||
|
return to_send |
||||||
|
|
||||||
|
def test_default_controls_not_allowed(self): |
||||||
|
self.assertFalse(self.safety.get_controls_allowed()) |
||||||
|
|
||||||
|
def test_steer_safety_check(self): |
||||||
|
for enabled in [0, 1]: |
||||||
|
for t in range(-0x200, 0x200): |
||||||
|
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.hyundai_tx_hook(self._torque_msg(t))) |
||||||
|
else: |
||||||
|
self.assertTrue(self.safety.hyundai_tx_hook(self._torque_msg(t))) |
||||||
|
|
||||||
|
def test_manually_enable_controls_allowed(self): |
||||||
|
self.safety.set_controls_allowed(1) |
||||||
|
self.assertTrue(self.safety.get_controls_allowed()) |
||||||
|
self.safety.set_controls_allowed(0) |
||||||
|
self.assertFalse(self.safety.get_controls_allowed()) |
||||||
|
|
||||||
|
def test_enable_control_allowed_from_cruise(self): |
||||||
|
to_push = libpandasafety_py.ffi.new('CAN_FIFOMailBox_TypeDef *') |
||||||
|
to_push[0].RIR = 1057 << 21 |
||||||
|
to_push[0].RDLR = 1 << 13 |
||||||
|
|
||||||
|
self.safety.hyundai_rx_hook(to_push) |
||||||
|
self.assertTrue(self.safety.get_controls_allowed()) |
||||||
|
|
||||||
|
def test_disable_control_allowed_from_cruise(self): |
||||||
|
to_push = libpandasafety_py.ffi.new('CAN_FIFOMailBox_TypeDef *') |
||||||
|
to_push[0].RIR = 1057 << 21 |
||||||
|
to_push[0].RDLR = 0 |
||||||
|
|
||||||
|
self.safety.set_controls_allowed(1) |
||||||
|
self.safety.hyundai_rx_hook(to_push) |
||||||
|
self.assertFalse(self.safety.get_controls_allowed()) |
||||||
|
|
||||||
|
def test_non_realtime_limit_up(self): |
||||||
|
self.safety.set_hyundai_torque_driver(0, 0) |
||||||
|
self.safety.set_controls_allowed(True) |
||||||
|
|
||||||
|
self._set_prev_torque(0) |
||||||
|
self.assertTrue(self.safety.hyundai_tx_hook(self._torque_msg(MAX_RATE_UP))) |
||||||
|
self._set_prev_torque(0) |
||||||
|
self.assertTrue(self.safety.hyundai_tx_hook(self._torque_msg(-MAX_RATE_UP))) |
||||||
|
|
||||||
|
self._set_prev_torque(0) |
||||||
|
self.assertFalse(self.safety.hyundai_tx_hook(self._torque_msg(MAX_RATE_UP + 1))) |
||||||
|
self.safety.set_controls_allowed(True) |
||||||
|
self._set_prev_torque(0) |
||||||
|
self.assertFalse(self.safety.hyundai_tx_hook(self._torque_msg(-MAX_RATE_UP - 1))) |
||||||
|
|
||||||
|
def test_non_realtime_limit_down(self): |
||||||
|
self.safety.set_hyundai_torque_driver(0, 0) |
||||||
|
self.safety.set_controls_allowed(True) |
||||||
|
|
||||||
|
def test_against_torque_driver(self): |
||||||
|
self.safety.set_controls_allowed(True) |
||||||
|
|
||||||
|
for sign in [-1, 1]: |
||||||
|
for t in np.arange(0, DRIVER_TORQUE_ALLOWANCE + 1, 1): |
||||||
|
t *= -sign |
||||||
|
self.safety.set_hyundai_torque_driver(t, t) |
||||||
|
self._set_prev_torque(MAX_STEER * sign) |
||||||
|
self.assertTrue(self.safety.hyundai_tx_hook(self._torque_msg(MAX_STEER * sign))) |
||||||
|
|
||||||
|
self.safety.set_hyundai_torque_driver(DRIVER_TORQUE_ALLOWANCE + 1, DRIVER_TORQUE_ALLOWANCE + 1) |
||||||
|
self.assertFalse(self.safety.hyundai_tx_hook(self._torque_msg(-MAX_STEER))) |
||||||
|
|
||||||
|
# spot check some individual cases |
||||||
|
for sign in [-1, 1]: |
||||||
|
driver_torque = (DRIVER_TORQUE_ALLOWANCE + 10) * sign |
||||||
|
torque_desired = (MAX_STEER - 10 * DRIVER_TORQUE_FACTOR) * sign |
||||||
|
delta = 1 * sign |
||||||
|
self._set_prev_torque(torque_desired) |
||||||
|
self.safety.set_hyundai_torque_driver(-driver_torque, -driver_torque) |
||||||
|
self.assertTrue(self.safety.hyundai_tx_hook(self._torque_msg(torque_desired))) |
||||||
|
self._set_prev_torque(torque_desired + delta) |
||||||
|
self.safety.set_hyundai_torque_driver(-driver_torque, -driver_torque) |
||||||
|
self.assertFalse(self.safety.hyundai_tx_hook(self._torque_msg(torque_desired + delta))) |
||||||
|
|
||||||
|
self._set_prev_torque(MAX_STEER * sign) |
||||||
|
self.safety.set_hyundai_torque_driver(-MAX_STEER * sign, -MAX_STEER * sign) |
||||||
|
self.assertTrue(self.safety.hyundai_tx_hook(self._torque_msg((MAX_STEER - MAX_RATE_DOWN) * sign))) |
||||||
|
self._set_prev_torque(MAX_STEER * sign) |
||||||
|
self.safety.set_hyundai_torque_driver(-MAX_STEER * sign, -MAX_STEER * sign) |
||||||
|
self.assertTrue(self.safety.hyundai_tx_hook(self._torque_msg(0))) |
||||||
|
self._set_prev_torque(MAX_STEER * sign) |
||||||
|
self.safety.set_hyundai_torque_driver(-MAX_STEER * sign, -MAX_STEER * sign) |
||||||
|
self.assertFalse(self.safety.hyundai_tx_hook(self._torque_msg((MAX_STEER - MAX_RATE_DOWN + 1) * sign))) |
||||||
|
|
||||||
|
|
||||||
|
def test_realtime_limits(self): |
||||||
|
self.safety.set_controls_allowed(True) |
||||||
|
|
||||||
|
for sign in [-1, 1]: |
||||||
|
self.safety.init_tests_hyundai() |
||||||
|
self._set_prev_torque(0) |
||||||
|
self.safety.set_hyundai_torque_driver(0, 0) |
||||||
|
for t in np.arange(0, MAX_RT_DELTA, 1): |
||||||
|
t *= sign |
||||||
|
self.assertTrue(self.safety.hyundai_tx_hook(self._torque_msg(t))) |
||||||
|
self.assertFalse(self.safety.hyundai_tx_hook(self._torque_msg(sign * (MAX_RT_DELTA + 1)))) |
||||||
|
|
||||||
|
self._set_prev_torque(0) |
||||||
|
for t in np.arange(0, MAX_RT_DELTA, 1): |
||||||
|
t *= sign |
||||||
|
self.assertTrue(self.safety.hyundai_tx_hook(self._torque_msg(t))) |
||||||
|
|
||||||
|
# Increase timer to update rt_torque_last |
||||||
|
self.safety.set_timer(RT_INTERVAL + 1) |
||||||
|
self.assertTrue(self.safety.hyundai_tx_hook(self._torque_msg(sign * (MAX_RT_DELTA - 1)))) |
||||||
|
self.assertTrue(self.safety.hyundai_tx_hook(self._torque_msg(sign * (MAX_RT_DELTA + 1)))) |
||||||
|
|
||||||
|
|
||||||
|
def test_spam_cancel_safety_check(self): |
||||||
|
RESUME_BTN = 1 |
||||||
|
SET_BTN = 2 |
||||||
|
CANCEL_BTN = 4 |
||||||
|
BUTTON_MSG = 1265 |
||||||
|
self.safety.set_controls_allowed(0) |
||||||
|
self.assertTrue(self.safety.hyundai_tx_hook(self._button_msg(CANCEL_BTN))) |
||||||
|
self.assertFalse(self.safety.hyundai_tx_hook(self._button_msg(RESUME_BTN))) |
||||||
|
self.assertFalse(self.safety.hyundai_tx_hook(self._button_msg(SET_BTN))) |
||||||
|
# do not block resume if we are engaged already |
||||||
|
self.safety.set_controls_allowed(1) |
||||||
|
self.assertTrue(self.safety.hyundai_tx_hook(self._button_msg(RESUME_BTN))) |
||||||
|
|
||||||
|
|
||||||
|
if __name__ == "__main__": |
||||||
|
unittest.main() |
Loading…
Reference in new issue