From 50d117ed9a0c75a7374e76ec6e42d1a603bb11b0 Mon Sep 17 00:00:00 2001 From: Shane Smiskol Date: Tue, 6 Sep 2022 19:30:34 -0700 Subject: [PATCH] =?UTF-8?q?Toyota:=20remove=20100=C2=B0/sec=20steering=20l?= =?UTF-8?q?ockout=20(#24067)?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit * try a method to kill those faults * cut torque for 1 frame * sign doesn't seem to matter * clean up * better name * Toyota allows you to keep your apply_steer, better control * the logic was wrong entire time? * cut steer for two frames * Revert "cut steer for two frames" This reverts commit 13a68ecc568fe1c0cd1f6f0b5f7ff6560efaf9e0. * better variable names and comments better variable names and comments * should be good * add safety * actual number of frames * constant * bump panda * bump panda * bump panda * bump panda * bump panda * bump panda * bump panda * fix to use min valid frames * rm line * simplify check * bump panda * bump panda to master --- panda | 2 +- selfdrive/car/toyota/carcontroller.py | 17 +++++++++++++++-- 2 files changed, 16 insertions(+), 3 deletions(-) diff --git a/panda b/panda index 13d64d4cc3..0ca23b6778 160000 --- a/panda +++ b/panda @@ -1 +1 @@ -Subproject commit 13d64d4cc38295401ff1ffcf6a233a4b9625fd9f +Subproject commit 0ca23b67786d0fa2a3162371aadeca52d24a8958 diff --git a/selfdrive/car/toyota/carcontroller.py b/selfdrive/car/toyota/carcontroller.py index 3288bcaedf..511017a551 100644 --- a/selfdrive/car/toyota/carcontroller.py +++ b/selfdrive/car/toyota/carcontroller.py @@ -10,6 +10,10 @@ 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 = 18 # control frames allowed where steering rate >= MAX_STEER_RATE + class CarController: def __init__(self, dbc_name, CP, VM): @@ -20,6 +24,7 @@ class CarController: self.alert_active = False self.last_standstill = False self.standstill_req = False + self.steer_rate_counter = 0 self.packer = CANPacker(dbc_name) self.gas = 0 @@ -52,11 +57,19 @@ class CarController: new_steer = int(round(actuators.steer * CarControllerParams.STEER_MAX)) 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: + self.steer_rate_counter += 1 + else: + self.steer_rate_counter = 0 + + apply_steer_req = 1 if not CC.latActive: 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