diff --git a/panda/.circleci/config.yml b/panda/.circleci/config.yml index 38042afd0a..2c7805a5bb 100644 --- a/panda/.circleci/config.yml +++ b/panda/.circleci/config.yml @@ -28,6 +28,10 @@ jobs: name: Build Panda STM image command: | docker run panda_build /bin/bash -c "cd /panda/board; make bin" + - run: + name: Build Pedal STM image + command: | + docker run panda_build /bin/bash -c "cd /panda/board/pedal; make obj/comma.bin" - run: name: Build NEO STM image command: | diff --git a/panda/board/drivers/can.h b/panda/board/drivers/can.h index 943f268a93..b837094c9a 100644 --- a/panda/board/drivers/can.h +++ b/panda/board/drivers/can.h @@ -158,7 +158,7 @@ void can_set_speed(uint8_t can_number) { // initialization mode CAN->MCR = CAN_MCR_TTCM | CAN_MCR_INRQ; while((CAN->MSR & CAN_MSR_INAK) != CAN_MSR_INAK); - + // set time quanta from defines CAN->BTR = (CAN_BTR_TS1_0 * (CAN_SEQ1-1)) | (CAN_BTR_TS2_0 * (CAN_SEQ2-1)) | @@ -468,8 +468,10 @@ void can_send(CAN_FIFOMailBox_TypeDef *to_push, uint8_t bus_number) { // bus number isn't passed through to_push->RDTR &= 0xF; if (bus_number == 3 && can_num_lookup[3] == 0xFF) { + #ifdef PANDA // TODO: why uint8 bro? only int8? bitbang_gmlan(to_push); + #endif } else { can_push(can_queues[bus_number], to_push); process_can(CAN_NUM_FROM_BUS_NUM(bus_number)); diff --git a/panda/board/pedal/obj/.gitkeep b/panda/board/pedal/obj/.gitkeep new file mode 100644 index 0000000000..e69de29bb2 diff --git a/panda/board/safety/safety_cadillac.h b/panda/board/safety/safety_cadillac.h index 86549d96eb..2a2d8b9857 100644 --- a/panda/board/safety/safety_cadillac.h +++ b/panda/board/safety/safety_cadillac.h @@ -125,7 +125,7 @@ const safety_hooks cadillac_hooks = { .init = cadillac_init, .rx = cadillac_rx_hook, .tx = cadillac_tx_hook, - .tx_lin = alloutput_tx_lin_hook, + .tx_lin = nooutput_tx_lin_hook, .ignition = cadillac_ign_hook, .fwd = alloutput_fwd_hook, }; diff --git a/panda/board/safety/safety_defaults.h b/panda/board/safety/safety_defaults.h index 8042cc3ce0..196df65d2f 100644 --- a/panda/board/safety/safety_defaults.h +++ b/panda/board/safety/safety_defaults.h @@ -57,4 +57,3 @@ const safety_hooks alloutput_hooks = { .ignition = default_ign_hook, .fwd = alloutput_fwd_hook, }; - diff --git a/panda/board/safety/safety_ford.h b/panda/board/safety/safety_ford.h index 804064842b..075029fb62 100644 --- a/panda/board/safety/safety_ford.h +++ b/panda/board/safety/safety_ford.h @@ -83,24 +83,11 @@ static int ford_tx_hook(CAN_FIFOMailBox_TypeDef *to_send) { return true; } -static int ford_tx_lin_hook(int lin_num, uint8_t *data, int len) { - // TODO: add safety if using LIN - return true; -} - -static void ford_init(int16_t param) { - controls_allowed = 0; -} - -static int ford_fwd_hook(int bus_num, CAN_FIFOMailBox_TypeDef *to_fwd) { - return -1; -} - const safety_hooks ford_hooks = { - .init = ford_init, + .init = nooutput_init, .rx = ford_rx_hook, .tx = ford_tx_hook, - .tx_lin = ford_tx_lin_hook, + .tx_lin = nooutput_tx_lin_hook, .ignition = default_ign_hook, - .fwd = ford_fwd_hook, + .fwd = nooutput_fwd_hook, }; diff --git a/panda/board/safety/safety_gm.h b/panda/board/safety/safety_gm.h index 2a6e8af067..5e38e00572 100644 --- a/panda/board/safety/safety_gm.h +++ b/panda/board/safety/safety_gm.h @@ -222,11 +222,6 @@ static int gm_tx_hook(CAN_FIFOMailBox_TypeDef *to_send) { return true; } -static int gm_tx_lin_hook(int lin_num, uint8_t *data, int len) { - // LIN is not used in Volt - return false; -} - static void gm_init(int16_t param) { controls_allowed = 0; gm_ignition_started = 0; @@ -236,16 +231,12 @@ static int gm_ign_hook() { return gm_ignition_started; } -static int gm_fwd_hook(int bus_num, CAN_FIFOMailBox_TypeDef *to_fwd) { - return -1; -} - const safety_hooks gm_hooks = { .init = gm_init, .rx = gm_rx_hook, .tx = gm_tx_hook, - .tx_lin = gm_tx_lin_hook, + .tx_lin = nooutput_tx_lin_hook, .ignition = gm_ign_hook, - .fwd = gm_fwd_hook, + .fwd = nooutput_fwd_hook, }; diff --git a/panda/board/safety/safety_honda.h b/panda/board/safety/safety_honda.h index b64e45ce6d..82e6eadda2 100644 --- a/panda/board/safety/safety_honda.h +++ b/panda/board/safety/safety_honda.h @@ -119,7 +119,7 @@ static int honda_tx_hook(CAN_FIFOMailBox_TypeDef *to_send) { if ((to_send->RDLR & 0xFFFF0000) != to_send->RDLR) return 0; } } - + // FORCE CANCEL: safety check only relevant when spamming the cancel button in Bosch HW // ensuring that only the cancel button press is sent (VAL 2) when controls are off. // This avoids unintended engagements while still allowing resume spam @@ -132,28 +132,19 @@ static int honda_tx_hook(CAN_FIFOMailBox_TypeDef *to_send) { return true; } -static int honda_tx_lin_hook(int lin_num, uint8_t *data, int len) { - // TODO: add safety if using LIN - return true; -} - static void honda_init(int16_t param) { controls_allowed = 0; bosch_hardware = false; honda_alt_brake_msg = false; } -static int honda_fwd_hook(int bus_num, CAN_FIFOMailBox_TypeDef *to_fwd) { - return -1; -} - const safety_hooks honda_hooks = { .init = honda_init, .rx = honda_rx_hook, .tx = honda_tx_hook, - .tx_lin = honda_tx_lin_hook, + .tx_lin = nooutput_tx_lin_hook, .ignition = default_ign_hook, - .fwd = honda_fwd_hook, + .fwd = nooutput_fwd_hook, }; static void honda_bosch_init(int16_t param) { @@ -175,7 +166,7 @@ const safety_hooks honda_bosch_hooks = { .init = honda_bosch_init, .rx = honda_rx_hook, .tx = honda_tx_hook, - .tx_lin = honda_tx_lin_hook, + .tx_lin = nooutput_tx_lin_hook, .ignition = default_ign_hook, .fwd = honda_bosch_fwd_hook, }; diff --git a/panda/board/safety/safety_hyundai.h b/panda/board/safety/safety_hyundai.h index e23dc7372a..d474eaeaf8 100644 --- a/panda/board/safety/safety_hyundai.h +++ b/panda/board/safety/safety_hyundai.h @@ -1,35 +1,156 @@ -int hyundai_giraffe_switch_1 = 0; // is giraffe switch 1 high? +const int HYUNDAI_MAX_STEER = 250; +const int HYUNDAI_MAX_RT_DELTA = 112; // max delta torque allowed for real time checks +const int32_t HYUNDAI_RT_INTERVAL = 250000; // 250ms between real time checks +const int HYUNDAI_MAX_RATE_UP = 3; +const int HYUNDAI_MAX_RATE_DOWN = 7; +const int HYUNDAI_DRIVER_TORQUE_ALLOWANCE = 50; +const int HYUNDAI_DRIVER_TORQUE_FACTOR = 2; +int hyundai_camera_detected = 0; +int hyundai_giraffe_switch_2 = 0; // is giraffe switch 2 high? +int hyundai_rt_torque_last = 0; +int hyundai_desired_torque_last = 0; +int hyundai_cruise_engaged_last = 0; +uint32_t hyundai_ts_last = 0; +struct sample_t hyundai_torque_driver; // last few driver torques measured static void hyundai_rx_hook(CAN_FIFOMailBox_TypeDef *to_push) { + int bus = (to_push->RDTR >> 4) & 0xFF; + uint32_t addr; + if (to_push->RIR & 4) { + // Extended + // Not looked at, but have to be separated + // to avoid address collision + addr = to_push->RIR >> 3; + } else { + // Normal + addr = to_push->RIR >> 21; + } + + if (addr == 897) { + int torque_driver_new = ((to_push->RDLR >> 11) & 0xfff) - 2048; + // update array of samples + update_sample(&hyundai_torque_driver, torque_driver_new); + } + + // check if stock camera ECU is still online + if (bus == 0 && addr == 832) { + hyundai_camera_detected = 1; + controls_allowed = 0; + } + + // enter controls on rising edge of ACC, exit controls on ACC off + if ((to_push->RIR>>21) == 1057) { + // 2 bits: 13-14 + int cruise_engaged = (to_push->RDLR >> 13) & 0x3; + if (cruise_engaged && !hyundai_cruise_engaged_last) { + controls_allowed = 1; + } else if (!cruise_engaged) { + controls_allowed = 0; + } + hyundai_cruise_engaged_last = cruise_engaged; + } - int bus = (to_push->RDTR >> 4) & 0xF; - // 832 is lkas cmd. If it is on bus 0, then giraffe switch 1 is high and we want stock - if ((to_push->RIR>>21) == 832 && (bus == 0)) { - hyundai_giraffe_switch_1 = 1; + // 832 is lkas cmd. If it is on bus 2, then giraffe switch 2 is high + if ((to_push->RIR>>21) == 832 && (bus == 2)) { + hyundai_giraffe_switch_2 = 1; } } -static void hyundai_init(int16_t param) { - controls_allowed = 0; - hyundai_giraffe_switch_1 = 0; +static int hyundai_tx_hook(CAN_FIFOMailBox_TypeDef *to_send) { + + // There can be only one! (camera) + if (hyundai_camera_detected) { + return 0; + } + + uint32_t addr; + if (to_send->RIR & 4) { + // Extended + addr = to_send->RIR >> 3; + } else { + // Normal + addr = to_send->RIR >> 21; + } + + // LKA STEER: safety check + if (addr == 832) { + int desired_torque = ((to_send->RDLR >> 16) & 0x7ff) - 1024; + uint32_t ts = TIM2->CNT; + int violation = 0; + + if (controls_allowed) { + + // *** global torque limit check *** + violation |= max_limit_check(desired_torque, HYUNDAI_MAX_STEER, -HYUNDAI_MAX_STEER); + + // *** torque rate limit check *** + violation |= driver_limit_check(desired_torque, hyundai_desired_torque_last, &hyundai_torque_driver, + HYUNDAI_MAX_STEER, HYUNDAI_MAX_RATE_UP, HYUNDAI_MAX_RATE_DOWN, + HYUNDAI_DRIVER_TORQUE_ALLOWANCE, HYUNDAI_DRIVER_TORQUE_FACTOR); + + // used next time + hyundai_desired_torque_last = desired_torque; + + // *** torque real time rate limit check *** + violation |= rt_rate_limit_check(desired_torque, hyundai_rt_torque_last, HYUNDAI_MAX_RT_DELTA); + + // every RT_INTERVAL set the new limits + uint32_t ts_elapsed = get_ts_elapsed(ts, hyundai_ts_last); + if (ts_elapsed > HYUNDAI_RT_INTERVAL) { + hyundai_rt_torque_last = desired_torque; + hyundai_ts_last = ts; + } + } + + // no torque if controls is not allowed + if (!controls_allowed && (desired_torque != 0)) { + violation = 1; + } + + // reset to 0 if either controls is not allowed or there's a violation + if (violation || !controls_allowed) { + hyundai_desired_torque_last = 0; + hyundai_rt_torque_last = 0; + hyundai_ts_last = ts; + } + + if (violation) { + return false; + } + } + + // FORCE CANCEL: safety check only relevant when spamming the cancel button. + // ensuring that only the cancel button press is sent (VAL 4) when controls are off. + // This avoids unintended engagements while still allowing resume spam + if (((to_send->RIR>>21) == 1265) && !controls_allowed && ((to_send->RDTR >> 4) & 0xFF) == 0) { + if ((to_send->RDLR & 0x7) != 4) return 0; + } + + // 1 allows the message through + return true; } static int hyundai_fwd_hook(int bus_num, CAN_FIFOMailBox_TypeDef *to_fwd) { - // forward camera to car and viceversa, excpet for lkas11 and mdps12 - if ((bus_num == 0 || bus_num == 2) && !hyundai_giraffe_switch_1) { + // forward cam to ccan and viceversa, except lkas cmd + if ((bus_num == 0 || bus_num == 2) && hyundai_giraffe_switch_2) { int addr = to_fwd->RIR>>21; - bool is_lkas_msg = (addr == 832 && bus_num == 2) || (addr == 593 && bus_num == 0); + bool is_lkas_msg = addr == 832 && bus_num == 2; return is_lkas_msg? -1 : (uint8_t)(~bus_num & 0x2); } return -1; } +static void hyundai_init(int16_t param) { + controls_allowed = 0; + hyundai_giraffe_switch_2 = 0; +} + const safety_hooks hyundai_hooks = { .init = hyundai_init, .rx = hyundai_rx_hook, - .tx = nooutput_tx_hook, + .tx = hyundai_tx_hook, .tx_lin = nooutput_tx_lin_hook, .ignition = default_ign_hook, .fwd = hyundai_fwd_hook, diff --git a/panda/board/safety/safety_toyota.h b/panda/board/safety/safety_toyota.h index d51a12a441..6ddf2952d1 100644 --- a/panda/board/safety/safety_toyota.h +++ b/panda/board/safety/safety_toyota.h @@ -107,7 +107,7 @@ static int toyota_tx_hook(CAN_FIFOMailBox_TypeDef *to_send) { violation |= max_limit_check(desired_torque, TOYOTA_MAX_TORQUE, -TOYOTA_MAX_TORQUE); // *** torque rate limit check *** - violation |= dist_to_meas_check(desired_torque, toyota_desired_torque_last, + violation |= dist_to_meas_check(desired_torque, toyota_desired_torque_last, &toyota_torque_meas, TOYOTA_MAX_RATE_UP, TOYOTA_MAX_RATE_DOWN, TOYOTA_MAX_TORQUE_ERROR); // used next time @@ -123,7 +123,7 @@ static int toyota_tx_hook(CAN_FIFOMailBox_TypeDef *to_send) { toyota_ts_last = ts; } } - + // no torque if controls is not allowed if (!controls_allowed && (desired_torque != 0)) { violation = 1; @@ -146,11 +146,6 @@ static int toyota_tx_hook(CAN_FIFOMailBox_TypeDef *to_send) { return true; } -static int toyota_tx_lin_hook(int lin_num, uint8_t *data, int len) { - // TODO: add safety if using LIN - return true; -} - static void toyota_init(int16_t param) { controls_allowed = 0; toyota_actuation_limits = 1; @@ -173,7 +168,7 @@ const safety_hooks toyota_hooks = { .init = toyota_init, .rx = toyota_rx_hook, .tx = toyota_tx_hook, - .tx_lin = toyota_tx_lin_hook, + .tx_lin = nooutput_tx_lin_hook, .ignition = default_ign_hook, .fwd = toyota_fwd_hook, }; @@ -189,7 +184,7 @@ const safety_hooks toyota_nolimits_hooks = { .init = toyota_nolimits_init, .rx = toyota_rx_hook, .tx = toyota_tx_hook, - .tx_lin = toyota_tx_lin_hook, + .tx_lin = nooutput_tx_lin_hook, .ignition = default_ign_hook, .fwd = toyota_fwd_hook, }; diff --git a/panda/board/safety/safety_toyota_ipas.h b/panda/board/safety/safety_toyota_ipas.h index 1627ece3a5..ff4158e3c7 100644 --- a/panda/board/safety/safety_toyota_ipas.h +++ b/panda/board/safety/safety_toyota_ipas.h @@ -149,7 +149,7 @@ const safety_hooks toyota_ipas_hooks = { .init = toyota_init, .rx = toyota_ipas_rx_hook, .tx = toyota_ipas_tx_hook, - .tx_lin = toyota_tx_lin_hook, + .tx_lin = nooutput_tx_lin_hook, .ignition = default_ign_hook, .fwd = toyota_fwd_hook, }; diff --git a/panda/tests/safety/libpandasafety_py.py b/panda/tests/safety/libpandasafety_py.py index 0af95ae47a..dfd396045b 100644 --- a/panda/tests/safety/libpandasafety_py.py +++ b/panda/tests/safety/libpandasafety_py.py @@ -41,6 +41,7 @@ void set_timer(int t); void set_toyota_torque_meas(int min, int max); void set_cadillac_torque_driver(int min, int max); void set_gm_torque_driver(int min, int max); +void set_hyundai_torque_driver(int min, int max); void set_toyota_rt_torque_last(int t); void set_toyota_desired_torque_last(int t); int get_toyota_torque_meas_min(void); @@ -70,6 +71,13 @@ int gm_tx_hook(CAN_FIFOMailBox_TypeDef *to_send); void set_gm_desired_torque_last(int t); void set_gm_rt_torque_last(int t); +void init_tests_hyundai(void); +void nooutput_init(int16_t param); +void hyundai_rx_hook(CAN_FIFOMailBox_TypeDef *to_push); +int hyundai_tx_hook(CAN_FIFOMailBox_TypeDef *to_send); +void set_hyundai_desired_torque_last(int t); +void set_hyundai_rt_torque_last(int t); + void toyota_ipas_rx_hook(CAN_FIFOMailBox_TypeDef *to_push); int toyota_ipas_tx_hook(CAN_FIFOMailBox_TypeDef *to_send); diff --git a/panda/tests/safety/test.c b/panda/tests/safety/test.c index 3d4288116e..c3f280644b 100644 --- a/panda/tests/safety/test.c +++ b/panda/tests/safety/test.c @@ -25,6 +25,7 @@ typedef struct struct sample_t toyota_torque_meas; struct sample_t cadillac_torque_driver; struct sample_t gm_torque_driver; +struct sample_t hyundai_torque_driver; TIM_TypeDef timer; TIM_TypeDef *TIM2 = &timer; @@ -75,6 +76,11 @@ void set_gm_torque_driver(int min, int max){ gm_torque_driver.max = max; } +void set_hyundai_torque_driver(int min, int max){ + hyundai_torque_driver.min = min; + hyundai_torque_driver.max = max; +} + int get_toyota_torque_meas_min(void){ return toyota_torque_meas.min; } @@ -95,6 +101,10 @@ void set_gm_rt_torque_last(int t){ gm_rt_torque_last = t; } +void set_hyundai_rt_torque_last(int t){ + hyundai_rt_torque_last = t; +} + void set_toyota_desired_torque_last(int t){ toyota_desired_torque_last = t; } @@ -107,6 +117,9 @@ void set_gm_desired_torque_last(int t){ gm_desired_torque_last = t; } +void set_hyundai_desired_torque_last(int t){ + hyundai_desired_torque_last = t; +} int get_ego_speed(void){ return ego_speed; @@ -155,6 +168,15 @@ void init_tests_gm(void){ set_timer(0); } +void init_tests_hyundai(void){ + hyundai_torque_driver.min = 0; + hyundai_torque_driver.max = 0; + hyundai_desired_torque_last = 0; + hyundai_rt_torque_last = 0; + hyundai_ts_last = 0; + set_timer(0); +} + void init_tests_honda(void){ ego_speed = 0; gas_interceptor_detected = 0; diff --git a/panda/tests/safety/test_gm.py b/panda/tests/safety/test_gm.py index f11360dbe2..346fc07381 100644 --- a/panda/tests/safety/test_gm.py +++ b/panda/tests/safety/test_gm.py @@ -84,10 +84,6 @@ class TestGmSafety(unittest.TestCase): to_send[0].RDHR = (((t >> 8) & 0x7) << 16) | ((t & 0xFF) << 24) return to_send - def _torque_driver_msg_array(self, torque): - for i in range(3): - self.safety.gm_ipas_rx_hook(self._torque_driver_msg(torque)) - def _torque_msg(self, torque): to_send = libpandasafety_py.ffi.new('CAN_FIFOMailBox_TypeDef *') to_send[0].RIR = 384 << 21 @@ -117,7 +113,7 @@ class TestGmSafety(unittest.TestCase): self.safety.gm_rx_hook(self._button_msg(CANCEL_BTN)) self.assertFalse(self.safety.get_controls_allowed()) - def test_disengage_on_brake(self): + def test_disengage_on_brake(self): self.safety.set_controls_allowed(1) self.safety.gm_rx_hook(self._brake_msg(True)) self.assertFalse(self.safety.get_controls_allowed()) diff --git a/panda/tests/safety/test_honda.py b/panda/tests/safety/test_honda.py index 143916d609..48b678eaae 100755 --- a/panda/tests/safety/test_honda.py +++ b/panda/tests/safety/test_honda.py @@ -108,7 +108,7 @@ class TestHondaSafety(unittest.TestCase): self.safety.set_controls_allowed(1) self.safety.honda_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.honda_rx_hook(self._alt_brake_msg(1)) diff --git a/panda/tests/safety/test_hyundai.py b/panda/tests/safety/test_hyundai.py new file mode 100644 index 0000000000..c53a85c053 --- /dev/null +++ b/panda/tests/safety/test_hyundai.py @@ -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()