Steering fault fix

Update carcontroller.py

Steering fault fix

fault fix 1

fix conflict

Rename oops

Author: Casey Francis

panda/board/safety/safety_toyota.h

@@ -44,6 +44,11 @@ addr_checks toyota_rx_checks = {toyota_addr_checks, TOYOTA_ADDR_CHECKS_LEN};
 // global actuation limit states
 int toyota_dbc_eps_torque_factor = 100;   // conversion factor for STEER_TORQUE_EPS in %: see dbc file
 
+// steering faults occur when the angle rate is above a certain threshold for too long,
+// allow setting STEER_REQUEST bit to 0 with a non-zero desired torque when expected
+const uint8_t TOYOTA_MAX_STEER_RATE_FRAMES = 19U;
+uint8_t toyota_steer_req_matches;  // counter for steer request bit matching non-zero torque
+
 static uint8_t toyota_compute_checksum(CANPacket_t *to_push) {
   int addr = GET_ADDR(to_push);
   int len = GET_LEN(to_push);
@@ -227,13 +232,27 @@ static int toyota_tx_hook(CANPacket_t *to_send) {
         }
       }
 
+      // handle steer_req bit mismatches: we set the bit to 0 at an expected
+      // interval to bypass an EPS fault, violation if we exceed that frequency
+      bool steer_req_mismatch = (desired_torque != 0) && !steer_req;
+      if (!steer_req_mismatch) {
+        toyota_steer_req_matches = MIN(toyota_steer_req_matches + 1U, 255U);
+      } else {
+        // disallow torque cut if not enough recent matching steer_req messages
+        if (toyota_steer_req_matches < (TOYOTA_MAX_STEER_RATE_FRAMES - 1U)) {
+          violation = 1;
+        }
+        toyota_steer_req_matches = 0U;
+      }
+
       // 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) {
+        toyota_steer_req_matches = 0U;
         desired_torque_last = 0;
         rt_torque_last = 0;
         ts_last = ts;
@@ -250,6 +269,7 @@ static int toyota_tx_hook(CANPacket_t *to_send) {
 
 static const addr_checks* toyota_init(int16_t param) {
   controls_allowed = 0;
+  toyota_steer_req_matches = 0U;
   relay_malfunction_reset();
   gas_interceptor_detected = 0;
   toyota_dbc_eps_torque_factor = param;

selfdrive/car/toyota/carcontroller.py

@@ -10,6 +10,9 @@
 from opendbc.can.packer import CANPacker
 VisualAlert = car.CarControl.HUDControl.VisualAlert
 
+# constants for fault workaround
+MAX_STEER_RATE = 100  # deg/s
+MAX_STEER_RATE_FRAMES = 19
 
 class CarController():
   def __init__(self, dbc_name, CP, VM):
@@ -19,7 +22,7 @@ def __init__(self, dbc_name, CP, VM):
     self.standstill_req = False
     self.steer_rate_limited = False
     self.CP = CP
-
+    self.steer_rate_counter = 0
     self.packer = CANPacker(dbc_name)
     self.gas = 0
     self.accel = 0
@@ -51,12 +54,22 @@ def update(self, enabled, active, CS, frame, actuators, pcm_cancel_cmd, hud_aler
     apply_steer = apply_toyota_steer_torque_limits(new_steer, self.last_steer, CS.out.steeringTorqueEps, CarControllerParams)
     self.steer_rate_limited = new_steer != apply_steer
 
+    # EPS_STATUS->LKA_STATE either goes to 21 or 25 on rising edge of a steering fault and
+    # the value seems to describe how many frames the steering rate was above 100 deg/s, so
+    # cut torque with some margin for the lower state
+    if active and abs(CS.out.steeringRateDeg) >= MAX_STEER_RATE:
+      self.steer_rate_counter += 1
+    else:
+      self.rate_limit_counter = 0
+
+    apply_steer_req = 1
     # Cut steering while we're in a known fault state (2s)
-    if not active or CS.steer_state in (9, 25):
+    if not active: # or CS.steer_state in (9, 25) or abs(CS.out.steeringRateDeg) > 100 or (abs(CS.out.steeringAngleDeg) > 150 and CS.CP.carFingerprint in [CAR.RAV4H, CAR.PRIUS]):
       apply_steer = 0
       apply_steer_req = 0
-    else:
-      apply_steer_req = 1
+    elif self.steer_rate_counter > MAX_STEER_RATE_FRAMES:
+      apply_steer_req = 0
+      self.steer_rate_counter = 0
 
     # TODO: probably can delete this. CS.pcm_acc_status uses a different signal
     # than CS.cruiseState.enabled. confirm they're not meaningfully different

selfdrive/car/toyota/carstate.py

@@ -86,7 +86,10 @@ def update(self, cp, cp_cam):
     ret.steeringTorqueEps = cp.vl["STEER_TORQUE_SENSOR"]["STEER_TORQUE_EPS"] * self.eps_torque_scale
     # we could use the override bit from dbc, but it's triggered at too high torque values
     ret.steeringPressed = abs(ret.steeringTorque) > STEER_THRESHOLD
-    ret.steerWarning = cp.vl["EPS_STATUS"]["LKA_STATE"] not in (1, 5)
+    # steer rate fault, goes to 21 or 25 for 1 frame, then 9 for ~2 seconds
+    ret.steerWarning = cp.vl["EPS_STATUS"]["LKA_STATE"] in (0, 9, 21, 25)
+    # 17 is a fault from a prolonged high torque delta between cmd and user
+    ret.steerError = cp.vl["EPS_STATUS"]["LKA_STATE"] == 17
 
     if self.CP.carFingerprint in (CAR.LEXUS_IS, CAR.LEXUS_RC):
       ret.cruiseState.available = cp.vl["DSU_CRUISE"]["MAIN_ON"] != 0