Merge panda subtree

old-commit-hash: f0c5ca7227

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);
 

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;

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())

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))

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()