#!/usr/bin/env python3 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.common import CANPackerPanda MAX_RATE_UP = 3 MAX_RATE_DOWN = 7 MAX_STEER = 255 MAX_RT_DELTA = 112 RT_INTERVAL = 250000 DRIVER_TORQUE_ALLOWANCE = 50 DRIVER_TORQUE_FACTOR = 2 # 4 bit checkusm used in some hyundai messages # lives outside the can packer because we never send this msg def checksum(msg): addr, t, dat, bus = msg chksum = 0 for i, b in enumerate(dat): if addr in [608, 1057] and i == 7: b &= 0x0F if addr == 1057 else 0xF0 elif addr == 916 and i == 6: b &= 0xF0 chksum += sum(divmod(b, 16)) chksum = (16 - chksum) % 16 ret = bytearray(dat) ret[6 if addr == 916 else 7] |= chksum << (4 if addr == 1057 else 0) return addr, t, ret, bus class TestHyundaiSafety(common.PandaSafetyTest): TX_MSGS = [[832, 0], [1265, 0], [1157, 0]] STANDSTILL_THRESHOLD = 30 # ~1kph RELAY_MALFUNCTION_ADDR = 832 RELAY_MALFUNCTION_BUS = 0 FWD_BLACKLISTED_ADDRS = {2: [832, 1157]} FWD_BUS_LOOKUP = {0: 2, 2: 0} cnt_gas = 0 cnt_speed = 0 cnt_brake = 0 cnt_cruise = 0 def setUp(self): self.packer = CANPackerPanda("hyundai_kia_generic") self.safety = libpandasafety_py.libpandasafety self.safety.set_safety_hooks(Panda.SAFETY_HYUNDAI, 0) self.safety.init_tests() def _button_msg(self, buttons): values = {"CF_Clu_CruiseSwState": buttons} return self.packer.make_can_msg_panda("CLU11", 0, values) def _gas_msg(self, gas): values = {"CF_Ems_AclAct": gas, "AliveCounter": self.cnt_gas % 4} self.__class__.cnt_gas += 1 return self.packer.make_can_msg_panda("EMS16", 0, values, fix_checksum=checksum) def _brake_msg(self, brake): values = {"DriverBraking": brake, "AliveCounterTCS": self.cnt_brake % 8} self.__class__.cnt_brake += 1 return self.packer.make_can_msg_panda("TCS13", 0, values, fix_checksum=checksum) def _speed_msg(self, speed): # panda safety doesn't scale, so undo the scaling values = {"WHL_SPD_%s" % s: speed * 0.03125 for s in ["FL", "FR", "RL", "RR"]} values["WHL_SPD_AliveCounter_LSB"] = (self.cnt_speed % 16) & 0x3 values["WHL_SPD_AliveCounter_MSB"] = (self.cnt_speed % 16) >> 2 self.__class__.cnt_speed += 1 return self.packer.make_can_msg_panda("WHL_SPD11", 0, values) def _pcm_status_msg(self, enable): values = {"ACCMode": enable, "CR_VSM_Alive": self.cnt_cruise % 16} self.__class__.cnt_cruise += 1 return self.packer.make_can_msg_panda("SCC12", 0, values, fix_checksum=checksum) def _set_prev_torque(self, t): self.safety.set_desired_torque_last(t) self.safety.set_rt_torque_last(t) # TODO: this is unused def _torque_driver_msg(self, torque): values = {"CR_Mdps_StrColTq": torque} return self.packer.make_can_msg_panda("MDPS12", 0, values) def _torque_msg(self, torque): values = {"CR_Lkas_StrToqReq": torque} return self.packer.make_can_msg_panda("LKAS11", 0, values) 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._tx(self._torque_msg(t))) else: self.assertTrue(self._tx(self._torque_msg(t))) def test_non_realtime_limit_up(self): self.safety.set_torque_driver(0, 0) self.safety.set_controls_allowed(True) self._set_prev_torque(0) self.assertTrue(self._tx(self._torque_msg(MAX_RATE_UP))) self._set_prev_torque(0) self.assertTrue(self._tx(self._torque_msg(-MAX_RATE_UP))) self._set_prev_torque(0) self.assertFalse(self._tx(self._torque_msg(MAX_RATE_UP + 1))) self.safety.set_controls_allowed(True) self._set_prev_torque(0) self.assertFalse(self._tx(self._torque_msg(-MAX_RATE_UP - 1))) def test_non_realtime_limit_down(self): self.safety.set_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_torque_driver(t, t) self._set_prev_torque(MAX_STEER * sign) self.assertTrue(self._tx(self._torque_msg(MAX_STEER * sign))) self.safety.set_torque_driver(DRIVER_TORQUE_ALLOWANCE + 1, DRIVER_TORQUE_ALLOWANCE + 1) self.assertFalse(self._tx(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_torque_driver(-driver_torque, -driver_torque) self.assertTrue(self._tx(self._torque_msg(torque_desired))) self._set_prev_torque(torque_desired + delta) self.safety.set_torque_driver(-driver_torque, -driver_torque) self.assertFalse(self._tx(self._torque_msg(torque_desired + delta))) self._set_prev_torque(MAX_STEER * sign) self.safety.set_torque_driver(-MAX_STEER * sign, -MAX_STEER * sign) self.assertTrue(self._tx(self._torque_msg((MAX_STEER - MAX_RATE_DOWN) * sign))) self._set_prev_torque(MAX_STEER * sign) self.safety.set_torque_driver(-MAX_STEER * sign, -MAX_STEER * sign) self.assertTrue(self._tx(self._torque_msg(0))) self._set_prev_torque(MAX_STEER * sign) self.safety.set_torque_driver(-MAX_STEER * sign, -MAX_STEER * sign) self.assertFalse(self._tx(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() self._set_prev_torque(0) self.safety.set_torque_driver(0, 0) for t in np.arange(0, MAX_RT_DELTA, 1): t *= sign self.assertTrue(self._tx(self._torque_msg(t))) self.assertFalse(self._tx(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._tx(self._torque_msg(t))) # Increase timer to update rt_torque_last self.safety.set_timer(RT_INTERVAL + 1) self.assertTrue(self._tx(self._torque_msg(sign * (MAX_RT_DELTA - 1)))) self.assertTrue(self._tx(self._torque_msg(sign * (MAX_RT_DELTA + 1)))) def test_spam_cancel_safety_check(self): RESUME_BTN = 1 SET_BTN = 2 CANCEL_BTN = 4 self.safety.set_controls_allowed(0) self.assertTrue(self._tx(self._button_msg(CANCEL_BTN))) self.assertFalse(self._tx(self._button_msg(RESUME_BTN))) self.assertFalse(self._tx(self._button_msg(SET_BTN))) # do not block resume if we are engaged already self.safety.set_controls_allowed(1) self.assertTrue(self._tx(self._button_msg(RESUME_BTN))) if __name__ == "__main__": unittest.main()