From 13a68ecc568fe1c0cd1f6f0b5f7ff6560efaf9e0 Mon Sep 17 00:00:00 2001 From: Shane Smiskol Date: Sun, 8 May 2022 03:19:04 -0700 Subject: [PATCH] cut steer for two frames --- selfdrive/car/toyota/carcontroller.py | 15 ++++++++++++--- 1 file changed, 12 insertions(+), 3 deletions(-) diff --git a/selfdrive/car/toyota/carcontroller.py b/selfdrive/car/toyota/carcontroller.py index c4174abf2a..a8cfa034d7 100644 --- a/selfdrive/car/toyota/carcontroller.py +++ b/selfdrive/car/toyota/carcontroller.py @@ -26,6 +26,8 @@ class CarController: self.steer_rate_limited = False self.rate_limit_counter = 0 + self.cut_steer_frames = 0 + self.cut_steer = False self.packer = CANPacker(dbc_name) self.gas = 0 @@ -67,14 +69,21 @@ class CarController: else: # TODO: unclear if it resets its internal state at another value self.rate_limit_counter = 0 + self.cut_steer_frames = 0 + + if self.rate_limit_counter > STEER_FAULT_MAX_FRAMES: + self.cut_steer = True + elif self.cut_steer_frames > 1: + self.cut_steer_frames = 0 + self.cut_steer = False apply_steer_req = 1 if not CC.latActive: - apply_steer = 0 apply_steer_req = 0 - elif self.rate_limit_counter > STEER_FAULT_MAX_FRAMES: + apply_steer = 0 + elif self.cut_steer: + self.cut_steer_frames += 1 apply_steer_req = 0 - self.rate_limit_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