sadmen
/
openpilot_comma
mirror of https://github.com/commaai/openpilot.git
1
0
Fork 0
Code Issues Packages Projects Releases Wiki Activity

clean up carcontroller a bit

Browse Source
pull/27494/head
Shane Smiskol 2 years ago
parent 548c801313
commit 7e0cd9357d
1 changed files with 32 additions and 44 deletions
Whitespace
Show all changes Ignore whitespace when comparing lines Ignore changes in amount of whitespace Ignore changes in whitespace at EOL
Unified View
Diff Options
Show Stats Download Patch File Download Diff File
  1. 76
      selfdrive/car/toyota/carcontroller.py

76
selfdrive/car/toyota/carcontroller.py
Unescape View File

@ -14,9 +14,9 @@ from common.op_params import opParams
SteerControlType = car.CarParams.SteerControlType SteerControlType = car.CarParams.SteerControlType
VisualAlert = car.CarControl.HUDControl.VisualAlert VisualAlert = car.CarControl.HUDControl.VisualAlert
# EPS faults if you apply torque while the steering rate is above 100 deg/s for too long # EPS faults if you request steer while the steering rate is above 100 deg/s for too long
MAX_STEER_RATE = 100 # deg/s MAX_STEER_RATE = 100 # deg/s
MAX_STEER_RATE_FRAMES = 18 # tx control frames needed before torque can be cut MAX_STEER_RATE_FRAMES = 18 # tx control frames needed before steer can be cut
# EPS allows user torque above threshold for 50 frames before permanently faulting # EPS allows user torque above threshold for 50 frames before permanently faulting
MAX_USER_TORQUE = 500 MAX_USER_TORQUE = 500
@ -63,36 +63,39 @@ class CarController:
interceptor_gas_cmd = 0. interceptor_gas_cmd = 0.
pcm_accel_cmd = clip(actuators.accel, self.params.ACCEL_MIN, self.params.ACCEL_MAX) pcm_accel_cmd = clip(actuators.accel, self.params.ACCEL_MIN, self.params.ACCEL_MAX)
# - steer torque apply_angle = 0
new_steer = int(round(actuators.steer * self.params.STEER_MAX)) apply_steer = 0
apply_steer = apply_toyota_steer_torque_limits(new_steer, self.last_steer, CS.out.steeringTorqueEps, self.params)
# - steer angle if self.CP.steerControlType != SteerControlType.angle:
# 1a. angle command is in terms of the angle signal in the torque sensor message (may or may not have an offset) # - steer torque
# 1b. clip to max angle limits (EPS ignores messages outside of these bounds and causes PCS faults) new_steer = int(round(actuators.steer * self.params.STEER_MAX))
# 2. rate limit angle command apply_steer = apply_toyota_steer_torque_limits(new_steer, self.last_steer, CS.out.steeringTorqueEps, self.params)
# 3. limit max angle error between cmd and actual to reduce EPS integral windup # TODO: can we configure this with a signal? else:
# - steer angle
# angle command is in terms of the torque sensor angle (may or may not have an offset)
apply_angle = actuators.steeringAngleDeg + CS.out.steeringAngleOffsetDeg
torque_sensor_angle = CS.out.steeringAngleDeg + CS.out.steeringAngleOffsetDeg
# angle command is in terms of the torque sensor angle (may or may not have an offset) # driver torque blending
apply_angle = actuators.steeringAngleDeg + CS.out.steeringAngleOffsetDeg percentage = interp(abs(CS.out.steeringTorque), [40, 100], [100, 0])
torque_sensor_angle = CS.out.steeringAngleDeg + CS.out.steeringAngleOffsetDeg apply_angle = interp(percentage, [-10, 100], [torque_sensor_angle, apply_angle])
percentage = interp(abs(CS.out.steeringTorque), [40, 100], [100, 0]) # Angular rate limit based on speed
apply_angle = interp(percentage, [-10, 100], [torque_sensor_angle, apply_angle]) apply_angle = apply_std_steer_angle_limits(apply_angle, self.last_angle, CS.out.vEgo, self.params)
# Angular rate limit based on speed (from TESLA) # limit max angle error between cmd and actual to reduce EPS integral windup
apply_angle = apply_std_steer_angle_limits(apply_angle, self.last_angle, CS.out.vEgo, self.params) # TODO: can we configure this with a signal?
# apply_angle = clip(apply_angle,
# torque_sensor_angle - self.params.ANGLE_DELTA_MAX,
# torque_sensor_angle + self.params.ANGLE_DELTA_MAX)
# limit max angle error between cmd and actual to reduce EPS integral windup # clip to max angle limits (EPS ignores messages outside of these bounds and causes PCS faults)
# TODO: can we configure this with a signal? apply_angle = clip(apply_angle, -94.9461, 94.9461)
# apply_steer = clip(apply_steer,
# torque_sensor_angle - self.params.ANGLE_DELTA_MAX,
# torque_sensor_angle + self.params.ANGLE_DELTA_MAX)
# clip to max angle limits (EPS ignores messages outside of these bounds and causes PCS faults) if not lat_active:
apply_angle = clip(apply_angle, -94.9461, 94.9461) apply_angle = clip(torque_sensor_angle, -94.9461, 94.9461)
# Count up to MAX_STEER_RATE_FRAMES, at which point we need to cut torque to avoid a steering fault # Count up to MAX_STEER_RATE_FRAMES, at which point we need to cut steer request bit to avoid a steering fault
if lat_active and abs(CS.out.steeringRateDeg) >= MAX_STEER_RATE: if lat_active and abs(CS.out.steeringRateDeg) >= MAX_STEER_RATE:
self.steer_rate_counter += 1 self.steer_rate_counter += 1
else: else:
@ -100,18 +103,12 @@ class CarController:
apply_steer_req = 1 apply_steer_req = 1
if not lat_active: if not lat_active:
apply_angle = clip(torque_sensor_angle, -94.9461, 94.9461)
apply_steer = 0 apply_steer = 0
apply_steer_req = 0 apply_steer_req = 0
elif self.steer_rate_counter > MAX_STEER_RATE_FRAMES: elif self.steer_rate_counter > MAX_STEER_RATE_FRAMES:
apply_steer_req = 0 apply_steer_req = 0
self.steer_rate_counter = 0 self.steer_rate_counter = 0
if self.CP.steerControlType == SteerControlType.angle:
apply_steer = 0
else:
apply_angle = 0
# TODO: probably can delete this. CS.pcm_acc_status uses a different signal # TODO: probably can delete this. CS.pcm_acc_status uses a different signal
# than CS.cruiseState.enabled. confirm they're not meaningfully different # than CS.cruiseState.enabled. confirm they're not meaningfully different
if not CC.enabled and CS.pcm_acc_status: if not CC.enabled and CS.pcm_acc_status:
@ -132,19 +129,10 @@ class CarController:
# toyota can trace shows this message at 42Hz, with counter adding alternatively 1 and 2; # toyota can trace shows this message at 42Hz, with counter adding alternatively 1 and 2;
# sending it at 100Hz seem to allow a higher rate limit, as the rate limit seems imposed # sending it at 100Hz seem to allow a higher rate limit, as the rate limit seems imposed
# on consecutive messages # on consecutive messages
angle_control = self.CP.steerControlType == SteerControlType.angle
if self.CP.steerControlType == SteerControlType.angle: can_sends.append(create_steer_command(self.packer, apply_steer, apply_steer_req and not angle_control))
apply_steer = 0 if TSS2_CAR:
# LTA mode. Set ret.steerControlType = car.CarParams.SteerControlType.angle and whitelist 0x191 in the panda can_sends.append(create_lta_steer_command(self.packer, apply_angle, apply_steer_req and angle_control, self.op_params))
can_sends.append(create_steer_command(self.packer, 0, 0))
# On TSS2 cars, the LTA and STEER_TORQUE_SENSOR messages use relative steering angle signals that start
# at 0 degrees, so we need to offset the commanded angle as well.
can_sends.append(create_lta_steer_command(self.packer, apply_angle, apply_steer_req, self.op_params))
else:
apply_angle = 0
can_sends.append(create_steer_command(self.packer, apply_steer, apply_steer_req))
if TSS2_CAR:
can_sends.append(create_lta_steer_command(self.packer, 0, False, self.op_params))
self.last_angle = apply_angle self.last_angle = apply_angle
self.last_steer = apply_steer self.last_steer = apply_steer

Write Preview
Loading…
Cancel
Save