Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Jump to bottom

Toyota: filter pitch #1482

Draft
wants to merge 1 commit into
base: master
Choose a base branch
from
Draft

Toyota: filter pitch #1482

Changes from all commits
Commits
Show all changes
1 commit
Select commit
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
18 changes: 12 additions & 6 deletions opendbc/car/toyota/carcontroller.py
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -19,7 +19,7 @@
# The up limit allows the brakes/gas to unwind quickly leaving a stop,
# the down limit matches the rate of ACCEL_NET, reducing PCM compensation windup
ACCEL_WINDUP_LIMIT = 0.5 # m/s^2 / frame
ACCEL_WINDDOWN_LIMIT = -5.0 * DT_CTRL * 3 # m/s^2 / frame
ACCEL_WINDDOWN_LIMIT = -4.0 * DT_CTRL * 3 # m/s^2 / frame

# LKA limits
# EPS faults if you apply torque while the steering rate is above 100 deg/s for too long
Expand Down Expand Up @@ -47,6 +47,7 @@ def __init__(self, dbc_name, CP):
self.steer_rate_counter = 0
self.distance_button = 0

self.pitch = FirstOrderFilter(0, 2, DT_CTRL)
self.pcm_accel_compensation = FirstOrderFilter(0, 0.5, DT_CTRL * 3)
self.permit_braking = True

Expand All @@ -66,6 +67,10 @@ def update(self, CC, CS, now_nanos):
pcm_cancel_cmd = CC.cruiseControl.cancel
lat_active = CC.latActive and abs(CS.out.steeringTorque) < MAX_USER_TORQUE

# update pitch within sane bounds
if abs(CS.out.aEgo) < 0.5 and not CS.out.standstill and len(CC.orientationNED) == 3:
self.pitch.update(CC.orientationNED[1])

# *** control msgs ***
can_sends = []

Expand Down Expand Up @@ -177,10 +182,10 @@ def update(self, CC, CS, now_nanos):
# internal PCM gas command can get stuck unwinding from negative accel so we apply a generous rate limit
pcm_accel_cmd = actuators.accel
if CC.longActive:
pcm_accel_cmd = rate_limit(pcm_accel_cmd, self.prev_accel, ACCEL_WINDDOWN_LIMIT, ACCEL_WINDUP_LIMIT)
pcm_accel_cmd = rate_limit(pcm_accel_cmd, self.prev_accel, ACCEL_WINDDOWN_LIMIT, ACCEL_WINDUP_LIMIT / 3)

# calculate amount of acceleration PCM should apply to reach target, given pitch
accel_due_to_pitch = math.sin(CC.orientationNED[1]) * ACCELERATION_DUE_TO_GRAVITY if len(CC.orientationNED) == 3 else 0.0
accel_due_to_pitch = math.sin(self.pitch.x) * ACCELERATION_DUE_TO_GRAVITY
net_acceleration_request = pcm_accel_cmd + accel_due_to_pitch

# For cars where we allow a higher max acceleration of 2.0 m/s^2, compensate for PCM request overshoot and imprecise braking
Expand All @@ -201,10 +206,11 @@ def update(self, CC, CS, now_nanos):
self.permit_braking = True

# Along with rate limiting positive jerk above, this greatly improves gas response time
# Consider the net acceleration request that the PCM should be applying (pitch included)
if net_acceleration_request < 0.1 or stopping or not CC.longActive:
# Consider the net acceleration request that the PCM should be applying before rate limiting (pitch included)
net_acceleration_request_raw = actuators.accel + accel_due_to_pitch
if net_acceleration_request_raw < 0.1 or stopping or not CC.longActive:
self.permit_braking = True
elif net_acceleration_request > 0.2:
elif net_acceleration_request_raw > 0.2:
self.permit_braking = False

pcm_accel_cmd = clip(pcm_accel_cmd, self.params.ACCEL_MIN, self.params.ACCEL_MAX)
Expand Down
Loading