Toyota: generic steer fault workaround safety (commaai#939)

* toyota steer fault safety * fix * alternative safety * no comment * should be good * same behavior, a bit simpler * better tests * fix comment * update safety comment * const is actual number of messages * Fix bug * misra * Fix test * clean up logic a bit clean up logic a bit fix * fix fix * clean up tests * unsigned * forgot to rename message when merged * Comments Co-authored-by: Adeeb Shihadeh <adeebshihadeh@gmail.com> * Update names Co-authored-by: Adeeb Shihadeh <adeebshihadeh@gmail.com> * rename rest of variables * real time checks * clean up safety tests * revert * add this * clean up * better name * use common steering checks * reverse order * make common * re-organize the safety * clean up safety_toyota * more clean up * add comment back * 19 * recover * some variable name clean up * rename and reset `valid_steering_msg_count`, another recover message * move comment * remove reset_toyota_timer, minor test clean up * common test * use init_tests * threshold used to be: frame you can cut steer on, now it's min num of valid frames (next frame you can cut, 18+1) * Update tests/safety/test_toyota.py Co-authored-by: Adeeb Shihadeh <adeebshihadeh@gmail.com> * fix realtime * Update board/safety/safety_toyota.h * Apply suggestions from code review * Update board/safety/safety_toyota.h Co-authored-by: Adeeb Shihadeh <adeebshihadeh@gmail.com>
budney · Sep 7, 2022 · 0ca23b6 · 0ca23b6
1 parent 671b5ae
commit 0ca23b6
Show file tree

Hide file tree

Showing 7 changed files with 122 additions and 11 deletions.
diff --git a/board/safety.h b/board/safety.h
@@ -318,7 +318,9 @@ int set_safety_hooks(uint16_t mode, uint16_t param) {
   rt_torque_last = 0;
   ts_angle_last = 0;
   desired_angle_last = 0;
-  ts_last = 0;
+  ts_torque_check_last = 0;
+  ts_steer_req_mismatch_last = 0;
+  valid_steer_req_count = 0;
 
   torque_meas.max = 0;
   torque_meas.max = 0;
@@ -492,10 +494,10 @@ bool steer_torque_cmd_checks(int desired_torque, int steer_req, const SteeringLi
     violation |= rt_rate_limit_check(desired_torque, rt_torque_last, limits.max_rt_delta);
 
     // every RT_INTERVAL set the new limits
-    uint32_t ts_elapsed = get_ts_elapsed(ts, ts_last);
+    uint32_t ts_elapsed = get_ts_elapsed(ts, ts_torque_check_last);
     if (ts_elapsed > limits.max_rt_interval) {
       rt_torque_last = desired_torque;
-      ts_last = ts;
+      ts_torque_check_last = ts;
     }
   }
 
@@ -504,16 +506,40 @@ bool steer_torque_cmd_checks(int desired_torque, int steer_req, const SteeringLi
     violation = true;
   }
 
-  // no torque if request bit isn't high
-  if ((steer_req == 0) && (desired_torque != 0)) {
-    violation = true;
+  // certain safety modes set their steer request bit low for one 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) {
+      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;
+      }
+
+      // 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;
+      }
+
+      valid_steer_req_count = 0;
+      ts_steer_req_mismatch_last = ts;
+    }
+  } 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;
     desired_torque_last = 0;
     rt_torque_last = 0;
-    ts_last = ts;
+    ts_torque_check_last = ts;
+    ts_steer_req_mismatch_last = ts;
   }
 
   return violation;

diff --git a/board/safety/safety_gm.h b/board/safety/safety_gm.h
@@ -189,10 +189,10 @@ static int gm_tx_hook(CANPacket_t *to_send, bool longitudinal_allowed) {
       violation |= rt_rate_limit_check(desired_torque, rt_torque_last, GM_MAX_RT_DELTA);
 
       // every RT_INTERVAL set the new limits
-      uint32_t ts_elapsed = get_ts_elapsed(ts, ts_last);
+      uint32_t ts_elapsed = get_ts_elapsed(ts, ts_torque_check_last);
       if (ts_elapsed > GM_RT_INTERVAL) {
         rt_torque_last = desired_torque;
-        ts_last = ts;
+        ts_torque_check_last = ts;
       }
     }
 
@@ -205,7 +205,7 @@ static int gm_tx_hook(CANPacket_t *to_send, bool longitudinal_allowed) {
     if (violation || !current_controls_allowed) {
       desired_torque_last = 0;
       rt_torque_last = 0;
-      ts_last = ts;
+      ts_torque_check_last = ts;
     }
 
     if (violation) {

diff --git a/board/safety/safety_toyota.h b/board/safety/safety_toyota.h
@@ -6,6 +6,12 @@ const SteeringLimits TOYOTA_STEERING_LIMITS = {
   .max_rt_delta = 450,        // the real time limit is 1800/sec, a 20% buffer
   .max_rt_interval = 250000,
   .type = TorqueMotorLimited,
+
+  // 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,
+  .min_valid_request_rt_interval = 170000,  // 170ms; a ~10% buffer on cutting every 19 frames
+  .has_steer_req_tolerance = true,
 };
 
 // longitudinal limits

diff --git a/board/safety_declarations.h b/board/safety_declarations.h
@@ -46,6 +46,11 @@ typedef struct {
 
   // motor torque limits
   const int max_torque_error;
+
+  // safety around steer req bit
+  const int min_valid_request_frames;
+  const uint32_t min_valid_request_rt_interval;
+  const bool has_steer_req_tolerance;
 } SteeringLimits;
 
 typedef struct {
@@ -143,9 +148,11 @@ int cruise_button_prev = 0;
 // for safety modes with torque steering control
 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
 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_last = 0;
+uint32_t ts_torque_check_last = 0;
+uint32_t ts_steer_req_mismatch_last = 0;  // last timestamp steer req was mismatched with torque
 
 // state for controls_allowed timeout logic
 bool heartbeat_engaged = false;             // openpilot enabled, passed in heartbeat USB command

diff --git a/tests/safety/common.py b/tests/safety/common.py
@@ -123,6 +123,8 @@ class TorqueSteeringSafetyTestBase(PandaSafetyTestBase):
   MAX_TORQUE = 0
   MAX_RT_DELTA = 0
   RT_INTERVAL = 0
+  MIN_VALID_STEERING_FRAMES = 0
+  MIN_VALID_STEERING_RT_INTERVAL = 0
 
   @classmethod
   def setUpClass(cls):
@@ -162,6 +164,72 @@ def test_non_realtime_limit_up(self):
     self._set_prev_torque(0)
     self.assertFalse(self._tx(self._torque_cmd_msg(-self.MAX_RATE_UP - 1)))
 
+  def test_steer_req_bit_frames(self):
+    """
+      Certain safety modes implement some tolerance on their steer request bits matching the
+      requested torque to avoid a steering fault or lockout and maintain torque. This tests:
+        - We can't cut torque for more than one frame
+        - 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):
+      # 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)
+
+      self.safety.set_controls_allowed(True)
+      self._set_prev_torque(self.MAX_TORQUE)
+      for _ in range(steer_rate_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)))
+
+      # Keep blocking after one steer_req mismatch
+      for _ in range(100):
+        self.assertFalse(self._tx(self._torque_cmd_msg(self.MAX_TORQUE, steer_req=0)))
+
+      # Make sure we can recover
+      self.assertTrue(self._tx(self._torque_cmd_msg(0, steer_req=1)))
+      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_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
+    """
+    if self.MIN_VALID_STEERING_RT_INTERVAL == 0:
+      raise unittest.SkipTest("Safety mode does not implement tolerance for steer request bit safety")
+
+    for rt_us in np.arange(self.MIN_VALID_STEERING_RT_INTERVAL - 50000, self.MIN_VALID_STEERING_RT_INTERVAL + 50000, 10000):
+      # Reset match count and rt timer (valid_steer_req_count, ts_steer_req_mismatch_last)
+      self.safety.init_tests()
+
+      # Make sure valid_steer_req_count doesn't affect this test
+      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)))
+
+      # Normally, sending MIN_VALID_STEERING_FRAMES valid frames should always allow
+      self.safety.set_timer(rt_us)
+      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)))
+
+      # Keep blocking after one steer_req mismatch
+      for _ in range(100):
+        self.assertFalse(self._tx(self._torque_cmd_msg(self.MAX_TORQUE, steer_req=0)))
+
+      # Make sure we can recover
+      self.assertTrue(self._tx(self._torque_cmd_msg(0, steer_req=1)))
+      self._set_prev_torque(self.MAX_TORQUE)
+      self.assertTrue(self._tx(self._torque_cmd_msg(self.MAX_TORQUE, steer_req=1)))
+
 
 class DriverTorqueSteeringSafetyTest(TorqueSteeringSafetyTestBase):
 

diff --git a/tests/safety/test.c b/tests/safety/test.c
@@ -219,6 +219,8 @@ void init_tests(void){
   safety_mode_cnt = 2U;  // avoid ignoring relay_malfunction logic
   alternative_experience = 0;
   set_timer(0);
+  ts_steer_req_mismatch_last = 0;
+  valid_steer_req_count = 0;
 }
 
 void init_tests_honda(void){

diff --git a/tests/safety/test_toyota.py b/tests/safety/test_toyota.py
@@ -41,6 +41,8 @@ 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