From fdc36cc8d9cc8e4bdefdd9c201601e8aeec4690a Mon Sep 17 00:00:00 2001 From: HaraldSchafer Date: Fri, 1 Jul 2022 19:14:21 -0700 Subject: [PATCH] Torque control: max torque warning (#25018) * New steer torque warning * typo --- selfdrive/controls/controlsd.py | 11 +++++++++-- 1 file changed, 9 insertions(+), 2 deletions(-) diff --git a/selfdrive/controls/controlsd.py b/selfdrive/controls/controlsd.py index 0bccb404fb0e13..46e122d93ee2d5 100755 --- a/selfdrive/controls/controlsd.py +++ b/selfdrive/controls/controlsd.py @@ -300,7 +300,7 @@ def update_events(self, CS): self.events.add(EventName.commIssue) elif not self.sm.all_freq_ok(): self.events.add(EventName.commIssueAvgFreq) - else: # invalid or can_rcv_error. + else: # invalid or can_rcv_error. self.events.add(EventName.commIssue) if not self.logged_comm_issue: @@ -588,7 +588,14 @@ def state_control(self, CS): lac_log.saturated = abs(actuators.steer) >= 0.9 # Send a "steering required alert" if saturation count has reached the limit - if lac_log.active and lac_log.saturated and not CS.steeringPressed: + if lac_log.active and not CS.steeringPressed and self.CP.lateralTuning.which() == 'torque': + undershooting = abs(lac_log.desiredLateralAccel) / abs(1e-3 + lac_log.actualLateralAccel) > 1.2 + turning = abs(lac_log.desiredLateralAccel) > 1.0 + good_speed = CS.vEgo > 5 + max_torque = abs(self.last_actuators.steer) > 0.99 + if undershooting and turning and good_speed and max_torque: + self.events.add(EventName.steerSaturated) + elif lac_log.active and lac_log.saturated and not CS.steeringPressed: dpath_points = lat_plan.dPathPoints if len(dpath_points): # Check if we deviated from the path