Torque request bit: support multiple invalid frames (commaai#1094)

* h90d safety * quicker rate * update comment * support multi frames? * stash * stash * might work? * stash * clean up * revert that * add test * fixup tests * clean up * remove test * try to combine tests * Revert "try to combine tests" This reverts commit f147955. * seems simpler * more explicit * shouldn't be needed * no hyundai stuff * no line * consecutively * comment * comment * comment * revert * comment * check earlier check earlier * ensure we don't set a negative value * add todo comments * for loop
budney · Oct 11, 2022 · 0a819ad · 0a819ad
1 parent 3644d94
commit 0a819ad
Show file tree

Hide file tree

Showing 6 changed files with 80 additions and 25 deletions.
diff --git a/board/safety.h b/board/safety.h
@@ -336,6 +336,7 @@ int set_safety_hooks(uint16_t mode, uint16_t param) {
   ts_torque_check_last = 0;
   ts_steer_req_mismatch_last = 0;
   valid_steer_req_count = 0;
+  invalid_steer_req_count = 0;
 
   torque_meas.max = 0;
   torque_meas.max = 0;
@@ -522,36 +523,47 @@ bool steer_torque_cmd_checks(int desired_torque, int steer_req, const SteeringLi
     violation = true;
   }
 
-  // certain safety modes set their steer request bit low for one frame at a
+  // certain safety modes set their steer request bit low for one or more frame at a
   // predefined max frequency to avoid steering faults in certain situations
   bool steer_req_mismatch = (steer_req == 0) && (desired_torque != 0);
-  if (steer_req_mismatch) {
-    // no torque if request bit isn't high
-    if (!limits.has_steer_req_tolerance) {
+  if (!limits.has_steer_req_tolerance) {
+    if (steer_req_mismatch) {
       violation = true;
+    }
 
-    } else {
-      // disallow torque cut if not enough recent matching steer_req messages
-      if (valid_steer_req_count < limits.min_valid_request_frames) {
-        violation = true;
-      }
+  } else {
+    if (steer_req_mismatch) {
+      if (invalid_steer_req_count == 0) {
+        // disallow torque cut if not enough recent matching steer_req messages
+        if (valid_steer_req_count < limits.min_valid_request_frames) {
+          violation = true;
+        }
 
-      // or we've cut torque too recently in time
-      uint32_t ts_elapsed = get_ts_elapsed(ts, ts_steer_req_mismatch_last);
-      if (ts_elapsed < limits.min_valid_request_rt_interval) {
-        violation = true;
+        // or we've cut torque too recently in time
+        uint32_t ts_elapsed = get_ts_elapsed(ts, ts_steer_req_mismatch_last);
+        if (ts_elapsed < limits.min_valid_request_rt_interval) {
+          violation = true;
+        }
+      } else {
+        // or we're cutting more frames consecutively than allowed
+        if (invalid_steer_req_count >= limits.max_invalid_request_frames) {
+          violation = true;
+        }
       }
 
       valid_steer_req_count = 0;
       ts_steer_req_mismatch_last = ts;
+      invalid_steer_req_count = MIN(invalid_steer_req_count + 1, limits.max_invalid_request_frames);
+    } else {
+      valid_steer_req_count = MIN(valid_steer_req_count + 1, limits.min_valid_request_frames);
+      invalid_steer_req_count = 0;
     }
-  } else {
-    valid_steer_req_count = MIN(valid_steer_req_count + 1, limits.min_valid_request_frames);
   }
 
   // reset to 0 if either controls is not allowed or there's a violation
   if (violation || !controls_allowed) {
     valid_steer_req_count = 0;
+    invalid_steer_req_count = 0;
     desired_torque_last = 0;
     rt_torque_last = 0;
     ts_torque_check_last = ts;

diff --git a/board/safety/safety_toyota.h b/board/safety/safety_toyota.h
@@ -10,6 +10,7 @@ const SteeringLimits TOYOTA_STEERING_LIMITS = {
   // the EPS faults when the steering angle rate is above a certain threshold for too long. to prevent this,
   // we allow setting STEER_REQUEST bit to 0 while maintaining the requested torque value for a single frame
   .min_valid_request_frames = 18,
+  .max_invalid_request_frames = 1,
   .min_valid_request_rt_interval = 170000,  // 170ms; a ~10% buffer on cutting every 19 frames
   .has_steer_req_tolerance = true,
 };

diff --git a/board/safety_declarations.h b/board/safety_declarations.h
@@ -49,6 +49,7 @@ typedef struct {
 
   // safety around steer req bit
   const int min_valid_request_frames;
+  const int max_invalid_request_frames;
   const uint32_t min_valid_request_rt_interval;
   const bool has_steer_req_tolerance;
 } SteeringLimits;
@@ -152,6 +153,7 @@ bool safety_rx_checks_invalid = false;
 int desired_torque_last = 0;       // last desired steer torque
 int rt_torque_last = 0;            // last desired torque for real time check
 int valid_steer_req_count = 0;     // counter for steer request bit matching non-zero torque
+int invalid_steer_req_count = 0;   // counter to allow multiple frames of mismatching torque request bit
 struct sample_t torque_meas;       // last 6 motor torques produced by the eps
 struct sample_t torque_driver;     // last 6 driver torques measured
 uint32_t ts_torque_check_last = 0;

diff --git a/tests/safety/common.py b/tests/safety/common.py
@@ -145,7 +145,10 @@ class TorqueSteeringSafetyTestBase(PandaSafetyTestBase):
   MAX_TORQUE = 0
   MAX_RT_DELTA = 0
   RT_INTERVAL = 0
+
+  # Safety around steering req bit
   MIN_VALID_STEERING_FRAMES = 0
+  MAX_INVALID_STEERING_FRAMES = 0
   MIN_VALID_STEERING_RT_INTERVAL = 0
 
   @classmethod
@@ -194,21 +197,23 @@ def test_steer_req_bit_frames(self):
         - We can't cut torque until at least the minimum number of matching steer_req messages
         - We can always recover from violations if steer_req=1
     """
-    if self.MIN_VALID_STEERING_FRAMES == 0:
-      raise unittest.SkipTest("Safety mode does not implement tolerance for steer request bit safety")
 
-    for steer_rate_frames in range(self.MIN_VALID_STEERING_FRAMES * 2):
+    for min_valid_steer_frames in range(self.MIN_VALID_STEERING_FRAMES * 2):
       # Reset match count and rt timer to allow cut (valid_steer_req_count, ts_steer_req_mismatch_last)
       self.safety.init_tests()
       self.safety.set_timer(self.MIN_VALID_STEERING_RT_INTERVAL)
 
+      # Allow torque cut
       self.safety.set_controls_allowed(True)
       self._set_prev_torque(self.MAX_TORQUE)
-      for _ in range(steer_rate_frames):
+      for _ in range(min_valid_steer_frames):
         self.assertTrue(self._tx(self._torque_cmd_msg(self.MAX_TORQUE, steer_req=1)))
 
-      should_tx = steer_rate_frames >= self.MIN_VALID_STEERING_FRAMES
-      self.assertEqual(should_tx, self._tx(self._torque_cmd_msg(self.MAX_TORQUE, steer_req=0)))
+      # should tx if we've sent enough valid frames, and we're not cutting torque for too many frames consecutively
+      should_tx = min_valid_steer_frames >= self.MIN_VALID_STEERING_FRAMES
+      for idx in range(self.MAX_INVALID_STEERING_FRAMES * 2):
+        tx = self._tx(self._torque_cmd_msg(self.MAX_TORQUE, steer_req=0))
+        self.assertEqual(should_tx and idx < self.MAX_INVALID_STEERING_FRAMES, tx)
 
       # Keep blocking after one steer_req mismatch
       for _ in range(100):
@@ -219,12 +224,42 @@ def test_steer_req_bit_frames(self):
       self._set_prev_torque(self.MAX_TORQUE)
       self.assertTrue(self._tx(self._torque_cmd_msg(self.MAX_TORQUE, steer_req=1)))
 
+  def test_steer_req_bit_multi_invalid(self):
+    """
+      For safety modes allowing multiple consecutive invalid frames, this ensures that once a valid frame
+      is sent after an invalid frame (even without sending the max number of allowed invalid frames),
+      all counters are reset.
+    """
+    # TODO: Add safety around steer request bits for all safety modes and remove exception
+    if self.MIN_VALID_STEERING_FRAMES == 0:
+      raise unittest.SkipTest("Safety mode does not implement tolerance for steer request bit safety")
+
+    for max_invalid_steer_frames in range(1, self.MAX_INVALID_STEERING_FRAMES * 2):
+      self.safety.init_tests()
+      self.safety.set_timer(self.MIN_VALID_STEERING_RT_INTERVAL)
+
+      # Allow torque cut
+      self.safety.set_controls_allowed(True)
+      self._set_prev_torque(self.MAX_TORQUE)
+      for _ in range(self.MIN_VALID_STEERING_FRAMES):
+        self.assertTrue(self._tx(self._torque_cmd_msg(self.MAX_TORQUE, steer_req=1)))
+
+      # Send partial amount of allowed invalid frames
+      for _ in range(max_invalid_steer_frames):
+        self.assertTrue(self._tx(self._torque_cmd_msg(self.MAX_TORQUE, steer_req=0)))
+
+      # Send one valid frame, and subsequent invalid should now be blocked
+      self.assertTrue(self._tx(self._torque_cmd_msg(self.MAX_TORQUE, steer_req=1)))
+      for _ in range(self.MIN_VALID_STEERING_FRAMES + 1):
+        self.assertFalse(self._tx(self._torque_cmd_msg(self.MAX_TORQUE, steer_req=0)))
+
   def test_steer_req_bit_realtime(self):
     """
       Realtime safety for cutting steer request bit. This tests:
         - That we allow messages with mismatching steer request bit if time from last is >= MIN_VALID_STEERING_RT_INTERVAL
         - That frame mismatch safety does not interfere with this test
     """
+    # TODO: Add safety around steer request bits for all safety modes and remove exception
     if self.MIN_VALID_STEERING_RT_INTERVAL == 0:
       raise unittest.SkipTest("Safety mode does not implement tolerance for steer request bit safety")
 
@@ -239,9 +274,10 @@ def test_steer_req_bit_realtime(self):
         self.assertTrue(self._tx(self._torque_cmd_msg(self.MAX_TORQUE, steer_req=1)))
 
       # Normally, sending MIN_VALID_STEERING_FRAMES valid frames should always allow
-      self.safety.set_timer(rt_us)
+      self.safety.set_timer(max(rt_us, 0))
       should_tx = rt_us >= self.MIN_VALID_STEERING_RT_INTERVAL
-      self.assertEqual(should_tx, self._tx(self._torque_cmd_msg(self.MAX_TORQUE, steer_req=0)))
+      for idx in range(self.MAX_INVALID_STEERING_FRAMES):
+        self.assertEqual(should_tx, self._tx(self._torque_cmd_msg(self.MAX_TORQUE, steer_req=0)))
 
       # Keep blocking after one steer_req mismatch
       for _ in range(100):

diff --git a/tests/safety/test.c b/tests/safety/test.c
@@ -225,6 +225,7 @@ void init_tests(void){
   set_timer(0);
   ts_steer_req_mismatch_last = 0;
   valid_steer_req_count = 0;
+  invalid_steer_req_count = 0;
 }
 
 void init_tests_honda(void){

diff --git a/tests/safety/test_toyota.py b/tests/safety/test_toyota.py
@@ -41,11 +41,14 @@ class TestToyotaSafety(common.PandaSafetyTest, common.InterceptorSafetyTest,
   MAX_RT_DELTA = 450
   RT_INTERVAL = 250000
   MAX_TORQUE_ERROR = 350
-  MIN_VALID_STEERING_FRAMES = 18
-  MIN_VALID_STEERING_RT_INTERVAL = 170000  # a ~10% buffer, can send steer up to 110Hz
   TORQUE_MEAS_TOLERANCE = 1  # toyota safety adds one to be conservative for rounding
   EPS_SCALE = 73
 
+  # Safety around steering req bit
+  MIN_VALID_STEERING_FRAMES = 18
+  MAX_INVALID_STEERING_FRAMES = 1
+  MIN_VALID_STEERING_RT_INTERVAL = 170000  # a ~10% buffer, can send steer up to 110Hz
+
   def setUp(self):
     self.packer = CANPackerPanda("toyota_nodsu_pt_generated")
     self.safety = libpandasafety_py.libpandasafety