From dba4492aa0b7b773f50816c5992fbe6e272d832f Mon Sep 17 00:00:00 2001
From: Shane Smiskol <shane@smiskol.com>
Date: Tue, 1 Aug 2023 14:02:15 -0700
Subject: [PATCH] Clean up common steer fault avoidance code (#29200)

* fix spacing

* more spacing

* better names and comments

* this default doesn't make sense

* or this

* explicitly boolean since latActive is
---
 selfdrive/car/__init__.py              | 27 ++++++++++++++------------
 selfdrive/car/hyundai/carcontroller.py |  8 ++++----
 selfdrive/car/toyota/carcontroller.py  |  3 ++-
 3 files changed, 21 insertions(+), 17 deletions(-)

diff --git a/selfdrive/car/__init__.py b/selfdrive/car/__init__.py
index 5e50db45fd..10da0dc56c 100644
--- a/selfdrive/car/__init__.py
+++ b/selfdrive/car/__init__.py
@@ -132,26 +132,29 @@ def apply_std_steer_angle_limits(apply_angle, apply_angle_last, v_ego, LIMITS):
   return clip(apply_angle, apply_angle_last - angle_rate_lim, apply_angle_last + angle_rate_lim)
 
 
-def common_fault_avoidance(measured_value: float, max_value: float, request: int, current_request_frames: int = 0, 
-                                                         max_request_frames: int = 1, cut_request_frames: int = 1):
+def common_fault_avoidance(measured_value: float, max_value: float, request: bool, above_limit_frames: int,
+                           max_above_limit_frames: int, max_mismatching_frames: int = 1):
   """
   Several cars have the ability to work around their EPS limits by cutting the
   request bit of their LKAS message after a certain number of frames above the limit.
   """
-  
-  # Count up to max_request_frames, at which point we need to cut the request for cut_request_frames to avoid a fault
+
+  # Count up to max_above_limit_frames, at which point we need to cut the request for above_limit_frames to avoid a fault
   if request and abs(measured_value) >= max_value:
-    current_request_frames += 1
+    above_limit_frames += 1
   else:
-    current_request_frames = 0
+    above_limit_frames = 0
+
+  # Once we cut the request bit, count additionally to max_mismatching_frames before setting the request bit high again.
+  # Some brands do not respect our workaround without multiple messages on the bus, for example
+  if above_limit_frames > max_above_limit_frames:
+    request = False
+
+  if above_limit_frames >= max_above_limit_frames + max_mismatching_frames:
+    above_limit_frames = 0
 
-  if current_request_frames > max_request_frames:
-    request = 0
+  return above_limit_frames, request
 
-  if current_request_frames >= max_request_frames + cut_request_frames:
-    current_request_frames = 0
-  
-  return current_request_frames, request
 
 def crc8_pedal(data):
   crc = 0xFF    # standard init value
diff --git a/selfdrive/car/hyundai/carcontroller.py b/selfdrive/car/hyundai/carcontroller.py
index b1c7ce61d6..ff23d43f08 100644
--- a/selfdrive/car/hyundai/carcontroller.py
+++ b/selfdrive/car/hyundai/carcontroller.py
@@ -66,12 +66,12 @@ class CarController:
 
     # >90 degree steering fault prevention
     self.angle_limit_counter, apply_steer_req = common_fault_avoidance(CS.out.steeringAngleDeg, MAX_ANGLE, CC.latActive,
-                                                                      self.angle_limit_counter, MAX_ANGLE_FRAMES,
-                                                                      MAX_ANGLE_CONSECUTIVE_FRAMES)
-    
+                                                                       self.angle_limit_counter, MAX_ANGLE_FRAMES,
+                                                                       MAX_ANGLE_CONSECUTIVE_FRAMES)
+
     if not CC.latActive:
       apply_steer = 0
-    
+
     # Hold torque with induced temporary fault when cutting the actuation bit
     torque_fault = CC.latActive and not apply_steer_req
 
diff --git a/selfdrive/car/toyota/carcontroller.py b/selfdrive/car/toyota/carcontroller.py
index 8ef8a92b90..0a23094fa3 100644
--- a/selfdrive/car/toyota/carcontroller.py
+++ b/selfdrive/car/toyota/carcontroller.py
@@ -56,6 +56,7 @@ class CarController:
     new_steer = int(round(actuators.steer * self.params.STEER_MAX))
     apply_steer = apply_meas_steer_torque_limits(new_steer, self.last_steer, CS.out.steeringTorqueEps, self.params)
 
+    # >100 degree/sec steering fault prevention
     self.steer_rate_counter, apply_steer_req = common_fault_avoidance(CS.out.steeringRateDeg, MAX_STEER_RATE, CC.latActive,
                                                                       self.steer_rate_counter, MAX_STEER_RATE_FRAMES)
 
@@ -66,7 +67,7 @@ class CarController:
     if self.CP.steerControlType == SteerControlType.angle:
       # If using LTA control, disable LKA and set steering angle command
       apply_steer = 0
-      apply_steer_req = 0
+      apply_steer_req = False
       if self.frame % 2 == 0:
         # EPS uses the torque sensor angle to control with, offset to compensate
         apply_angle = actuators.steeringAngleDeg + CS.out.steeringAngleOffsetDeg