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: pitch compensate new long tune #1260

Merged
merged 3 commits into from
Sep 20, 2024
Merged

Toyota: pitch compensate new long tune #1260

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
3 commits
Select commit Hold shift + click to select a range
fc1e95e
compensate for pitch (shouldn't apply too much on downhills and too l…
sshane Sep 20, 2024
d8aced6
less defart
sshane Sep 20, 2024
d6e7cb8
, given
sshane Sep 20, 2024
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
9 changes: 8 additions & 1 deletion opendbc/car/toyota/carcontroller.py
View file
Open in desktop
Original file line number Diff line number Diff line change
@@ -1,4 +1,5 @@
import copy
import math
from opendbc.car import apply_meas_steer_torque_limits, apply_std_steer_angle_limits, common_fault_avoidance, make_tester_present_msg, rate_limit, structs
from opendbc.car.can_definitions import CanData
from opendbc.car.common.numpy_fast import clip
Expand All @@ -12,6 +13,8 @@
SteerControlType = structs.CarParams.SteerControlType
VisualAlert = structs.CarControl.HUDControl.VisualAlert

ACCELERATION_DUE_TO_GRAVITY = 9.81

# LKA limits
# EPS faults if you apply torque while the steering rate is above 100 deg/s for too long
MAX_STEER_RATE = 100 # deg/s
Expand Down Expand Up @@ -104,7 +107,11 @@ def update(self, CC, CS, now_nanos):
# For cars where we allow a higher max acceleration of 2.0 m/s^2, compensate for PCM request overshoot
# TODO: validate PCM_CRUISE->ACCEL_NET for braking requests and compensate for imprecise braking as well
if self.CP.flags & ToyotaFlags.RAISED_ACCEL_LIMIT and CC.longActive:
pcm_accel_compensation = 2.0 * (CS.pcm_accel_net - actuators.accel) if actuators.accel > 0 else 0.0
# calculate amount of acceleration PCM should apply to reach target, given pitch
accel_due_to_pitch = math.sin(CS.slope_angle) * ACCELERATION_DUE_TO_GRAVITY
net_acceleration_request = actuators.accel + accel_due_to_pitch

pcm_accel_compensation = 2.0 * (CS.pcm_accel_net - net_acceleration_request) if net_acceleration_request > 0 else 0.0

# prevent compensation windup
if actuators.accel - pcm_accel_compensation > self.params.ACCEL_MAX:
Expand Down
5 changes: 5 additions & 0 deletions opendbc/car/toyota/carstate.py
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -48,6 +48,7 @@ def __init__(self, CP):
self.acc_type = 1
self.lkas_hud = {}
self.pcm_accel_net = 0.0
self.slope_angle = 0.0

def update(self, cp, cp_cam, *_) -> structs.CarState:
ret = structs.CarState()
Expand All @@ -57,6 +58,9 @@ def update(self, cp, cp_cam, *_) -> structs.CarState:
if self.CP.flags & ToyotaFlags.RAISED_ACCEL_LIMIT:
self.pcm_accel_net = cp.vl["CLUTCH"]["ACCEL_NET"] # - cp.vl["PCM_CRUISE"]["ACCEL_NET"]

# filtered pitch estimate from the car, negative is a downward slope
self.slope_angle = cp.vl["VSC1S07"]["ASLP"] * CV.DEG_TO_RAD

ret.doorOpen = any([cp.vl["BODY_CONTROL_STATE"]["DOOR_OPEN_FL"], cp.vl["BODY_CONTROL_STATE"]["DOOR_OPEN_FR"],
cp.vl["BODY_CONTROL_STATE"]["DOOR_OPEN_RL"], cp.vl["BODY_CONTROL_STATE"]["DOOR_OPEN_RR"]])
ret.seatbeltUnlatched = cp.vl["BODY_CONTROL_STATE"]["SEATBELT_DRIVER_UNLATCHED"] != 0
Expand Down Expand Up @@ -193,6 +197,7 @@ def get_can_parser(CP):
("BODY_CONTROL_STATE", 3),
("BODY_CONTROL_STATE_2", 2),
("ESP_CONTROL", 3),
("VSC1S07", 20),
("EPS_STATUS", 25),
("BRAKE_MODULE", 40),
("WHEEL_SPEEDS", 80),
Expand Down
Loading