diff --git a/panda/board/safety/safety_toyota.h b/panda/board/safety/safety_toyota.h index 648d10d768f4bd..a81df4cf35a2fd 100644 --- a/panda/board/safety/safety_toyota.h +++ b/panda/board/safety/safety_toyota.h @@ -43,6 +43,10 @@ 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); @@ -227,6 +231,19 @@ 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; diff --git a/selfdrive/car/toyota/carcontroller.py b/selfdrive/car/toyota/carcontroller.py index e15440da0dcc45..40da75d1e7a07b 100644 --- a/selfdrive/car/toyota/carcontroller.py +++ b/selfdrive/car/toyota/carcontroller.py @@ -9,8 +9,13 @@ from selfdrive.car.toyota.interface import CarInterface from opendbc.can.packer import CANPacker VisualAlert = car.CarControl.HUDControl.VisualAlert + CREEP_SPEED = 2.3 +# constants for fault workaround +MAX_STEER_RATE = 100 # deg/s +MAX_STEER_RATE_FRAMES = 19 + class CarController(): def __init__(self, dbc_name, CP, VM): self.last_steer = 0 @@ -20,6 +25,7 @@ def __init__(self, dbc_name, CP, VM): self.steer_rate_limited = False self.CP = CP + self.steer_rate_counter = 0 self.packer = CANPacker(dbc_name) self.gas = 0 self.accel = 0 @@ -56,12 +62,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 diff --git a/selfdrive/car/toyota/carstate.py b/selfdrive/car/toyota/carstate.py index ac033f4b1191de..2d1b18d6cda8ae 100644 --- a/selfdrive/car/toyota/carstate.py +++ b/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