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 <shane@smiskol.com> Co-authored-by: Comma Device <device@comma.ai> old-commit-hash: a19f8dce92bfe7242e64160cb86364d728b01d97
This commit is contained in:
Justin Newberry
@@ -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
|
||||
|
||||
@@ -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))
|
||||
|
||||
@@ -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:
|
||||
|
||||
Reference in New Issue
Block a user