Clean up common steer fault avoidance code (#29200)

* fix spacing

* more spacing

* better names and comments

* this default doesn't make sense

* or this

* explicitly boolean since latActive is
pull/29204/head
Shane Smiskol 2 years ago committed by GitHub
parent 42e106dd47
commit dba4492aa0
No known key found for this signature in database
GPG Key ID: 4AEE18F83AFDEB23
  1. 23
      selfdrive/car/__init__.py
  2. 3
      selfdrive/car/toyota/carcontroller.py

@ -132,26 +132,29 @@ 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) 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, def common_fault_avoidance(measured_value: float, max_value: float, request: bool, above_limit_frames: int,
max_request_frames: int = 1, cut_request_frames: int = 1): max_above_limit_frames: int, max_mismatching_frames: int = 1):
""" """
Several cars have the ability to work around their EPS limits by cutting the 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. 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 # Count up to max_above_limit_frames, at which point we need to cut the request for above_limit_frames to avoid a fault
if request and abs(measured_value) >= max_value: if request and abs(measured_value) >= max_value:
current_request_frames += 1 above_limit_frames += 1
else: else:
current_request_frames = 0 above_limit_frames = 0
if current_request_frames > max_request_frames: # Once we cut the request bit, count additionally to max_mismatching_frames before setting the request bit high again.
request = 0 # Some brands do not respect our workaround without multiple messages on the bus, for example
if above_limit_frames > max_above_limit_frames:
request = False
if current_request_frames >= max_request_frames + cut_request_frames: if above_limit_frames >= max_above_limit_frames + max_mismatching_frames:
current_request_frames = 0 above_limit_frames = 0
return above_limit_frames, request
return current_request_frames, request
def crc8_pedal(data): def crc8_pedal(data):
crc = 0xFF # standard init value crc = 0xFF # standard init value

@ -56,6 +56,7 @@ class CarController:
new_steer = int(round(actuators.steer * self.params.STEER_MAX)) 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) apply_steer = apply_meas_steer_torque_limits(new_steer, self.last_steer, CS.out.steeringTorqueEps, self.params)
# >100 degree/sec steering fault prevention
self.steer_rate_counter, apply_steer_req = common_fault_avoidance(CS.out.steeringRateDeg, MAX_STEER_RATE, CC.latActive, 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) self.steer_rate_counter, MAX_STEER_RATE_FRAMES)
@ -66,7 +67,7 @@ class CarController:
if self.CP.steerControlType == SteerControlType.angle: if self.CP.steerControlType == SteerControlType.angle:
# If using LTA control, disable LKA and set steering angle command # If using LTA control, disable LKA and set steering angle command
apply_steer = 0 apply_steer = 0
apply_steer_req = 0 apply_steer_req = False
if self.frame % 2 == 0: if self.frame % 2 == 0:
# EPS uses the torque sensor angle to control with, offset to compensate # EPS uses the torque sensor angle to control with, offset to compensate
apply_angle = actuators.steeringAngleDeg + CS.out.steeringAngleOffsetDeg apply_angle = actuators.steeringAngleDeg + CS.out.steeringAngleOffsetDeg

Loading…
Cancel
Save