From a19f8dce92bfe7242e64160cb86364d728b01d97 Mon Sep 17 00:00:00 2001 From: Justin Newberry Date: Tue, 1 Aug 2023 10:27:06 -0700 Subject: [PATCH] Homogenize Steering Fault Avoidance (#29140) * fix subaru fault * try this! * wip * try this * this more or less worked * this is all under gen2 * that needs to be up there * comment * steer_angle * test * wip * wip * sync * wip * cleanup * remove print * use sets and fix unittests * common fault avoidance * common fault avoidance * cleanup * cleanup * cleanup * cleanup * cleanup * revert subaru to get this part merged * revert name change * revert name change * revert name change * same as before * add test case * also verify zero tolerance * keep the current behavior * split into multiple tests for easier debugging * added comments and remove tests --------- Co-authored-by: Shane Smiskol Co-authored-by: Comma Device --- selfdrive/car/__init__.py | 21 +++++++++++++++++++ selfdrive/car/hyundai/carcontroller.py | 28 ++++++++++---------------- selfdrive/car/toyota/carcontroller.py | 16 ++++----------- 3 files changed, 36 insertions(+), 29 deletions(-) diff --git a/selfdrive/car/__init__.py b/selfdrive/car/__init__.py index bb201bfe0..5e50db45f 100644 --- a/selfdrive/car/__init__.py +++ b/selfdrive/car/__init__.py @@ -132,6 +132,27 @@ def apply_std_steer_angle_limits(apply_angle, apply_angle_last, v_ego, LIMITS): return clip(apply_angle, apply_angle_last - angle_rate_lim, apply_angle_last + angle_rate_lim) +def common_fault_avoidance(measured_value: float, max_value: float, request: int, current_request_frames: int = 0, + max_request_frames: int = 1, cut_request_frames: int = 1): + """ + Several cars have the ability to work around their EPS limits by cutting the + request bit of their LKAS message after a certain number of frames above the limit. + """ + + # Count up to max_request_frames, at which point we need to cut the request for cut_request_frames to avoid a fault + if request and abs(measured_value) >= max_value: + current_request_frames += 1 + else: + current_request_frames = 0 + + if current_request_frames > max_request_frames: + request = 0 + + if current_request_frames >= max_request_frames + cut_request_frames: + current_request_frames = 0 + + return current_request_frames, request + def crc8_pedal(data): crc = 0xFF # standard init value poly = 0xD5 # standard crc8: x8+x7+x6+x4+x2+1 diff --git a/selfdrive/car/hyundai/carcontroller.py b/selfdrive/car/hyundai/carcontroller.py index 11ff2fb6e..b1c7ce61d 100644 --- a/selfdrive/car/hyundai/carcontroller.py +++ b/selfdrive/car/hyundai/carcontroller.py @@ -3,7 +3,7 @@ from common.conversions import Conversions as CV from common.numpy_fast import clip from common.realtime import DT_CTRL from opendbc.can.packer import CANPacker -from selfdrive.car import apply_driver_steer_torque_limits +from selfdrive.car import apply_driver_steer_torque_limits, common_fault_avoidance from selfdrive.car.hyundai import hyundaicanfd, hyundaican from selfdrive.car.hyundai.hyundaicanfd import CanBus from selfdrive.car.hyundai.values import HyundaiFlags, Buttons, CarControllerParams, CANFD_CAR, CAR @@ -64,8 +64,16 @@ class CarController: new_steer = int(round(actuators.steer * self.params.STEER_MAX)) apply_steer = apply_driver_steer_torque_limits(new_steer, self.apply_steer_last, CS.out.steeringTorque, self.params) + # >90 degree steering fault prevention + self.angle_limit_counter, apply_steer_req = common_fault_avoidance(CS.out.steeringAngleDeg, MAX_ANGLE, CC.latActive, + self.angle_limit_counter, MAX_ANGLE_FRAMES, + MAX_ANGLE_CONSECUTIVE_FRAMES) + if not CC.latActive: apply_steer = 0 + + # Hold torque with induced temporary fault when cutting the actuation bit + torque_fault = CC.latActive and not apply_steer_req self.apply_steer_last = apply_steer @@ -94,27 +102,13 @@ class CarController: if self.CP.flags & HyundaiFlags.ENABLE_BLINKERS: can_sends.append([0x7b1, 0, b"\x02\x3E\x80\x00\x00\x00\x00\x00", self.CAN.ECAN]) - # >90 degree steering fault prevention - # Count up to MAX_ANGLE_FRAMES, at which point we need to cut torque to avoid a steering fault - if CC.latActive and abs(CS.out.steeringAngleDeg) >= MAX_ANGLE: - self.angle_limit_counter += 1 - else: - self.angle_limit_counter = 0 - - # Cut steer actuation bit for two frames and hold torque with induced temporary fault - torque_fault = CC.latActive and self.angle_limit_counter > MAX_ANGLE_FRAMES - lat_active = CC.latActive and not torque_fault - - if self.angle_limit_counter >= MAX_ANGLE_FRAMES + MAX_ANGLE_CONSECUTIVE_FRAMES: - self.angle_limit_counter = 0 - # CAN-FD platforms if self.CP.carFingerprint in CANFD_CAR: hda2 = self.CP.flags & HyundaiFlags.CANFD_HDA2 hda2_long = hda2 and self.CP.openpilotLongitudinalControl # steering control - can_sends.extend(hyundaicanfd.create_steering_messages(self.packer, self.CP, self.CAN, CC.enabled, lat_active, apply_steer)) + can_sends.extend(hyundaicanfd.create_steering_messages(self.packer, self.CP, self.CAN, CC.enabled, apply_steer_req, apply_steer)) # disable LFA on HDA2 if self.frame % 5 == 0 and hda2: @@ -158,7 +152,7 @@ class CarController: can_sends.append(hyundaicanfd.create_buttons(self.packer, self.CP, self.CAN, CS.buttons_counter+1, Buttons.RES_ACCEL)) self.last_button_frame = self.frame else: - can_sends.append(hyundaican.create_lkas11(self.packer, self.frame, self.car_fingerprint, apply_steer, lat_active, + can_sends.append(hyundaican.create_lkas11(self.packer, self.frame, self.car_fingerprint, apply_steer, apply_steer_req, torque_fault, CS.lkas11, sys_warning, sys_state, CC.enabled, hud_control.leftLaneVisible, hud_control.rightLaneVisible, left_lane_warning, right_lane_warning)) diff --git a/selfdrive/car/toyota/carcontroller.py b/selfdrive/car/toyota/carcontroller.py index 905251b53..8ef8a92b9 100644 --- a/selfdrive/car/toyota/carcontroller.py +++ b/selfdrive/car/toyota/carcontroller.py @@ -1,6 +1,6 @@ from cereal import car from common.numpy_fast import clip, interp -from selfdrive.car import apply_meas_steer_torque_limits, apply_std_steer_angle_limits, \ +from selfdrive.car import apply_meas_steer_torque_limits, apply_std_steer_angle_limits, common_fault_avoidance, \ create_gas_interceptor_command, make_can_msg from selfdrive.car.toyota.toyotacan import create_steer_command, create_ui_command, \ create_accel_command, create_acc_cancel_command, \ @@ -56,19 +56,11 @@ class CarController: new_steer = int(round(actuators.steer * self.params.STEER_MAX)) apply_steer = apply_meas_steer_torque_limits(new_steer, self.last_steer, CS.out.steeringTorqueEps, self.params) - # Count up to MAX_STEER_RATE_FRAMES, at which point we need to cut torque to avoid a steering fault - if lat_active and abs(CS.out.steeringRateDeg) >= MAX_STEER_RATE: - self.steer_rate_counter += 1 - else: - self.steer_rate_counter = 0 + self.steer_rate_counter, apply_steer_req = common_fault_avoidance(CS.out.steeringRateDeg, MAX_STEER_RATE, CC.latActive, + self.steer_rate_counter, MAX_STEER_RATE_FRAMES) - apply_steer_req = 1 - if not lat_active: + if not CC.latActive: apply_steer = 0 - apply_steer_req = 0 - elif self.steer_rate_counter > MAX_STEER_RATE_FRAMES: - apply_steer_req = 0 - self.steer_rate_counter = 0 # *** steer angle *** if self.CP.steerControlType == SteerControlType.angle: