From c5dc10bd2f063e2233795a66e45b6f17dffbd5c1 Mon Sep 17 00:00:00 2001 From: Shane Smiskol Date: Wed, 7 Sep 2022 00:18:01 -0700 Subject: [PATCH] Toyota: fix permanent LKAS faults (#25620) * Fix permanent LKAS Toyota faults * comment and global variable * EPS * comments * move up * comment --- selfdrive/car/toyota/carcontroller.py | 12 ++++++++---- 1 file changed, 8 insertions(+), 4 deletions(-) diff --git a/selfdrive/car/toyota/carcontroller.py b/selfdrive/car/toyota/carcontroller.py index 511017a551..b34a31a01c 100644 --- a/selfdrive/car/toyota/carcontroller.py +++ b/selfdrive/car/toyota/carcontroller.py @@ -10,9 +10,12 @@ from opendbc.can.packer import CANPacker VisualAlert = car.CarControl.HUDControl.VisualAlert -# constants for fault workaround +# EPS faults if you apply torque while the steering rate is above 100 deg/s for too long MAX_STEER_RATE = 100 # deg/s -MAX_STEER_RATE_FRAMES = 18 # control frames allowed where steering rate >= MAX_STEER_RATE +MAX_STEER_RATE_FRAMES = 18 # tx control frames needed before torque can be cut + +# EPS allows user torque above threshold for 50 frames before permanently faulting +MAX_USER_TORQUE = 500 class CarController: @@ -34,6 +37,7 @@ class CarController: actuators = CC.actuators hud_control = CC.hudControl pcm_cancel_cmd = CC.cruiseControl.cancel + lat_active = CC.latActive and abs(CS.out.steeringTorque) < MAX_USER_TORQUE # gas and brake if self.CP.enableGasInterceptor and CC.longActive: @@ -58,13 +62,13 @@ class CarController: apply_steer = apply_toyota_steer_torque_limits(new_steer, self.last_steer, CS.out.steeringTorqueEps, self.torque_rate_limits) # Count up to MAX_STEER_RATE_FRAMES, at which point we need to cut torque to avoid a steering fault - if CC.latActive and abs(CS.out.steeringRateDeg) >= MAX_STEER_RATE: + if lat_active and abs(CS.out.steeringRateDeg) >= MAX_STEER_RATE: self.steer_rate_counter += 1 else: self.steer_rate_counter = 0 apply_steer_req = 1 - if not CC.latActive: + if not lat_active: apply_steer = 0 apply_steer_req = 0 elif self.steer_rate_counter > MAX_STEER_RATE_FRAMES: