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

Improve Toyota no-DSU steering #680

Closed
wants to merge 1 commit into from
Closed

Improve Toyota no-DSU steering #680

Show file tree
Hide file tree
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.
Jump to
Jump to file
Failed to load files.
Diff view
Diff view
1 change: 1 addition & 0 deletions opendbc/generator/toyota/toyota_chr_2018_pt.dbc
View file
Open in desktop
Expand Up @@ -15,6 +15,7 @@ BO_ 608 STEER_TORQUE_SENSOR: 8 XXX
SG_ STEER_TORQUE_DRIVER : 15|16@0- (1,0) [-32768|32767] "" XXX
SG_ STEER_OVERRIDE : 0|1@0+ (1,0) [0|1] "" XXX
SG_ CHECKSUM : 63|8@0+ (1,0) [0|255] "" XXX
SG_ STEER_ANGLE : 31|16@0- (0.05527,0) [-500|500] "" XXX

BO_ 610 EPS_STATUS: 8 EPS
SG_ IPAS_STATE : 3|4@0+ (1,0) [0|15] "" XXX
Expand Down
1 change: 1 addition & 0 deletions opendbc/generator/toyota/toyota_chr_hybrid_2018_pt.dbc
View file
Open in desktop
Expand Up @@ -20,6 +20,7 @@ BO_ 608 STEER_TORQUE_SENSOR: 8 XXX
SG_ STEER_TORQUE_DRIVER : 15|16@0- (1,0) [-32768|32767] "" XXX
SG_ STEER_OVERRIDE : 0|1@0+ (1,0) [0|1] "" XXX
SG_ CHECKSUM : 63|8@0+ (1,0) [0|255] "" XXX
SG_ STEER_ANGLE : 31|16@0- (0.05527,0) [-500|500] "" XXX

BO_ 610 EPS_STATUS: 8 EPS
SG_ IPAS_STATE : 3|4@0+ (1,0) [0|15] "" XXX
Expand Down
1 change: 1 addition & 0 deletions opendbc/toyota_chr_2018_pt_generated.dbc
View file
Open in desktop
Expand Up @@ -313,6 +313,7 @@ BO_ 608 STEER_TORQUE_SENSOR: 8 XXX
SG_ STEER_TORQUE_DRIVER : 15|16@0- (1,0) [-32768|32767] "" XXX
SG_ STEER_OVERRIDE : 0|1@0+ (1,0) [0|1] "" XXX
SG_ CHECKSUM : 63|8@0+ (1,0) [0|255] "" XXX
SG_ STEER_ANGLE : 31|16@0- (0.05527,0) [-500|500] "" XXX

BO_ 610 EPS_STATUS: 8 EPS
SG_ IPAS_STATE : 3|4@0+ (1,0) [0|15] "" XXX
Expand Down
1 change: 1 addition & 0 deletions opendbc/toyota_chr_hybrid_2018_pt_generated.dbc
View file
Open in desktop
Expand Up @@ -318,6 +318,7 @@ BO_ 608 STEER_TORQUE_SENSOR: 8 XXX
SG_ STEER_TORQUE_DRIVER : 15|16@0- (1,0) [-32768|32767] "" XXX
SG_ STEER_OVERRIDE : 0|1@0+ (1,0) [0|1] "" XXX
SG_ CHECKSUM : 63|8@0+ (1,0) [0|255] "" XXX
SG_ STEER_ANGLE : 31|16@0- (0.05527,0) [-500|500] "" XXX

BO_ 610 EPS_STATUS: 8 EPS
SG_ IPAS_STATE : 3|4@0+ (1,0) [0|15] "" XXX
Expand Down
27 changes: 25 additions & 2 deletions selfdrive/car/toyota/carstate.py
View file
Open in desktop
Expand Up @@ -2,7 +2,7 @@
from common.kalman.simple_kalman import KF1D
from selfdrive.can.parser import CANParser, CANDefine
from selfdrive.config import Conversions as CV
from selfdrive.car.toyota.values import CAR, DBC, STEER_THRESHOLD
from selfdrive.car.toyota.values import CAR, DBC, STEER_THRESHOLD, NO_DSU_CAR

def parse_gear_shifter(gear, vals):

Expand Down Expand Up @@ -63,6 +63,9 @@ def get_can_parser(CP):
if CP.carFingerprint == CAR.PRIUS:
signals += [("STATE", "AUTOPARK_STATUS", 0)]

if CP.carFingerprint in NO_DSU_CAR:
signals += [("STEER_ANGLE", "STEER_TORQUE_SENSOR", 0)]

# add gas interceptor reading if we are using it
if CP.enableGasInterceptor:
signals.append(("INTERCEPTOR_GAS", "GAS_SENSOR", 0))
Expand Down Expand Up @@ -103,7 +106,10 @@ def __init__(self, CP):
K=[[0.12287673], [0.29666309]])
self.v_ego = 0.0

def update(self, cp, cp_cam):
self.old_steer_prev = 0
self.offset_prev = 0

def update(self, cp, cp_cam, frame):
# copy can_valid
self.can_valid = cp.can_valid
self.cam_can_valid = cp_cam.can_valid
Expand Down Expand Up @@ -143,6 +149,23 @@ def update(self, cp, cp_cam):

self.angle_steers = cp.vl["STEER_ANGLE_SENSOR"]['STEER_ANGLE'] + cp.vl["STEER_ANGLE_SENSOR"]['STEER_FRACTION']
self.angle_steers_rate = cp.vl["STEER_ANGLE_SENSOR"]['STEER_RATE']

# steer angle from torque sensor message and learns offset
if self.car_fingerprint in NO_DSU_CAR:

self.old_steer = self.angle_steers
self.new_steer = cp.vl["STEER_TORQUE_SENSOR"]['STEER_ANGLE']

# learn offset in first two seconds of car start at standstill and old angle sensor has not moved
if (frame < 200) and (self.old_steer == self.old_steer_prev) and v_wheel == 0:
self.offset = self.new_steer - self.old_steer
else:
self.offset = self.offset_prev

self.angle_steers = self.new_steer - self.offset
self.old_steer_prev = self.old_steer
self.offset_prev = self.offset

can_gear = int(cp.vl["GEAR_PACKET"]['GEAR'])
self.gear_shifter = parse_gear_shifter(can_gear, self.shifter_values)
self.main_on = cp.vl["PCM_CRUISE_2"]['MAIN_ON']
Expand Down
10 changes: 5 additions & 5 deletions selfdrive/car/toyota/interface.py
View file
Open in desktop
Expand Up @@ -139,11 +139,11 @@ def get_params(candidate, fingerprint):
stop_and_go = True
ret.safetyParam = 100
ret.wheelbase = 2.82448
ret.steerRatio = 13.7
tire_stiffness_factor = 0.7933
ret.steerRatio = 16.5 # stock 13.7
tire_stiffness_factor = 0.8
ret.mass = 3400 * CV.LB_TO_KG + std_cargo #mean between normal and hybrid
ret.lateralTuning.pid.kpV, ret.lateralTuning.pid.kiV = [[0.6], [0.1]]
ret.lateralTuning.pid.kf = 0.00006
ret.lateralTuning.pid.kpV, ret.lateralTuning.pid.kiV = [[0.3], [0.05]]
ret.lateralTuning.pid.kf = 0.00007

elif candidate in [CAR.HIGHLANDER, CAR.HIGHLANDERH]:
stop_and_go = True
Expand Down Expand Up @@ -262,7 +262,7 @@ def update(self, c):
if self.frame < 1000:
self.cp_cam.update(int(sec_since_boot() * 1e9), False)

self.CS.update(self.cp, self.cp_cam)
self.CS.update(self.cp, self.cp_cam, self.frame)

# create message
ret = car.CarState.new_message()
Expand Down