sadmen
/
dragonpilot
mirror of https://github.com/dragonpilot-community/dragonpilot.git
1
0
Fork 0
Code Issues 67 Packages Projects Releases Wiki Activity

removing ipas steering control option from toyota (#1193)

Browse Source 
* removing ipas steering control from toyota
pull/214/head
rbiasini 6 years ago committed by GitHub
parent 4695b2daa9
commit a984da1f9c
No known key found for this signature in database
GPG Key ID: 4AEE18F83AFDEB23
5 changed files with 3 additions and 109 deletions
Whitespace
Show all changes Ignore whitespace when comparing lines Ignore changes in amount of whitespace Ignore changes in whitespace at EOL
Split View
Diff Options
Show Stats Download Patch File Download Diff File
  1. 74
      selfdrive/car/toyota/carcontroller.py
  2. 2
      selfdrive/car/toyota/carstate.py
  3. 2
      selfdrive/car/toyota/interface.py
  4. 24
      selfdrive/car/toyota/toyotacan.py
  5. 10
      selfdrive/car/toyota/values.py

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

@ -1,9 +1,8 @@
from cereal import car
from common.numpy_fast import clip, interp
from common.numpy_fast import clip
from selfdrive.car import apply_toyota_steer_torque_limits, create_gas_command, make_can_msg
from selfdrive.car.toyota.toyotacan import create_steer_command, create_ui_command, \
create_ipas_steer_command, create_accel_command, \
create_acc_cancel_command, create_fcw_command
create_accel_command, create_acc_cancel_command, create_fcw_command
from selfdrive.car.toyota.values import Ecu, CAR, STATIC_MSGS, SteerLimitParams
from opendbc.can.packer import CANPacker
@ -15,14 +14,6 @@ ACCEL_MAX = 1.5 # 1.5 m/s2
ACCEL_MIN = -3.0 # 3 m/s2
ACCEL_SCALE = max(ACCEL_MAX, -ACCEL_MIN)
# Steer angle limits (tested at the Crows Landing track and considered ok)
ANGLE_MAX_BP = [0., 5.]
ANGLE_MAX_V = [510., 300.]
ANGLE_DELTA_BP = [0., 5., 15.]
ANGLE_DELTA_V = [5., .8, .15] # windup limit
ANGLE_DELTA_VU = [5., 3.5, 0.4] # unwind limit
def accel_hysteresis(accel, accel_steady, enabled):
# for small accel oscillations within ACCEL_HYST_GAP, don't change the accel command
@ -51,51 +42,22 @@ def process_hud_alert(hud_alert):
return steer, fcw
def ipas_state_transition(steer_angle_enabled, enabled, ipas_active, ipas_reset_counter):
if enabled and not steer_angle_enabled:
#ipas_reset_counter = max(0, ipas_reset_counter - 1)
#if ipas_reset_counter == 0:
# steer_angle_enabled = True
#else:
# steer_angle_enabled = False
#return steer_angle_enabled, ipas_reset_counter
return True, 0
elif enabled and steer_angle_enabled:
if steer_angle_enabled and not ipas_active:
ipas_reset_counter += 1
else:
ipas_reset_counter = 0
if ipas_reset_counter > 10: # try every 0.1s
steer_angle_enabled = False
return steer_angle_enabled, ipas_reset_counter
else:
return False, 0
class CarController():
def __init__(self, dbc_name, CP, VM):
self.braking = False
self.last_steer = 0
self.last_angle = 0
self.accel_steady = 0.
self.car_fingerprint = CP.carFingerprint
self.alert_active = False
self.last_standstill = False
self.standstill_req = False
self.angle_control = False
self.steer_angle_enabled = False
self.ipas_reset_counter = 0
self.last_fault_frame = -200
self.steer_rate_limited = False
self.fake_ecus = set()
if CP.enableCamera: self.fake_ecus.add(Ecu.fwdCamera)
if CP.enableDsu: self.fake_ecus.add(Ecu.dsu)
if CP.enableApgs: self.fake_ecus.add(Ecu.apgs)
self.packer = CANPacker(dbc_name)
@ -134,26 +96,6 @@ class CarController():
else:
apply_steer_req = 1
self.steer_angle_enabled, self.ipas_reset_counter = \
ipas_state_transition(self.steer_angle_enabled, enabled, CS.ipas_active, self.ipas_reset_counter)
#print("{0} {1} {2}".format(self.steer_angle_enabled, self.ipas_reset_counter, CS.ipas_active))
# steer angle
if self.steer_angle_enabled and CS.ipas_active:
apply_angle = actuators.steerAngle
angle_lim = interp(CS.out.vEgo, ANGLE_MAX_BP, ANGLE_MAX_V)
apply_angle = clip(apply_angle, -angle_lim, angle_lim)
# windup slower
if self.last_angle * apply_angle > 0. and abs(apply_angle) > abs(self.last_angle):
angle_rate_lim = interp(CS.out.vEgo, ANGLE_DELTA_BP, ANGLE_DELTA_V)
else:
angle_rate_lim = interp(CS.out.vEgo, ANGLE_DELTA_BP, ANGLE_DELTA_VU)
apply_angle = clip(apply_angle, self.last_angle - angle_rate_lim, self.last_angle + angle_rate_lim)
else:
apply_angle = CS.out.steeringAngle
if not enabled and CS.pcm_acc_status:
# send pcm acc cancel cmd if drive is disabled but pcm is still on, or if the system can't be activated
pcm_cancel_cmd = 1
@ -166,7 +108,6 @@ class CarController():
self.standstill_req = False
self.last_steer = apply_steer
self.last_angle = apply_angle
self.last_accel = apply_accel
self.last_standstill = CS.out.standstill
@ -179,16 +120,7 @@ class CarController():
# sending it at 100Hz seem to allow a higher rate limit, as the rate limit seems imposed
# on consecutive messages
if Ecu.fwdCamera in self.fake_ecus:
if self.angle_control:
can_sends.append(create_steer_command(self.packer, 0., 0, frame))
else:
can_sends.append(create_steer_command(self.packer, apply_steer, apply_steer_req, frame))
if self.angle_control:
can_sends.append(create_ipas_steer_command(self.packer, apply_angle, self.steer_angle_enabled,
Ecu.apgs in self.fake_ecus))
elif Ecu.apgs in self.fake_ecus:
can_sends.append(create_ipas_steer_command(self.packer, 0, 0, True))
can_sends.append(create_steer_command(self.packer, apply_steer, apply_steer_req, frame))
# we can spam can to cancel the system even if we are using lat only control
if (frame % 3 == 0 and CS.CP.openpilotLongitudinalControl) or (pcm_cancel_cmd and Ecu.fwdCamera in self.fake_ecus):

2
selfdrive/car/toyota/carstate.py
Unescape View File

@ -90,7 +90,6 @@ class CarState(CarStateBase):
# 2 is standby, 10 is active. TODO: check that everything else is really a faulty state
self.steer_state = cp.vl["EPS_STATUS"]['LKA_STATE']
self.steer_warning = cp.vl["EPS_STATUS"]['LKA_STATE'] not in [1, 5]
self.ipas_active = cp.vl['EPS_STATUS']['IPAS_STATE'] == 3
return ret
@ -121,7 +120,6 @@ class CarState(CarStateBase):
("STEER_TORQUE_EPS", "STEER_TORQUE_SENSOR", 0),
("TURN_SIGNALS", "STEERING_LEVERS", 3), # 3 is no blinkers
("LKA_STATE", "EPS_STATUS", 0),
("IPAS_STATE", "EPS_STATUS", 1),
("BRAKE_LIGHTS_ACC", "ESP_CONTROL", 0),
("AUTO_HIGH_BEAM", "LIGHT_STALK", 0),
]

2
selfdrive/car/toyota/interface.py
Unescape View File

@ -248,13 +248,11 @@ class CarInterface(CarInterfaceBase):
smartDsu = 0x2FF in fingerprint[0]
# In TSS2 cars the camera does long control
ret.enableDsu = is_ecu_disconnected(fingerprint[0], FINGERPRINTS, ECU_FINGERPRINT, candidate, Ecu.dsu) and candidate not in TSS2_CAR
ret.enableApgs = False # is_ecu_disconnected(fingerprint[0], FINGERPRINTS, ECU_FINGERPRINT, candidate, Ecu.apgs)
ret.enableGasInterceptor = 0x201 in fingerprint[0]
# if the smartDSU is detected, openpilot can send ACC_CMD (and the smartDSU will block it from the DSU) or not (the DSU is "connected")
ret.openpilotLongitudinalControl = ret.enableCamera and (smartDsu or ret.enableDsu or candidate in TSS2_CAR)
cloudlog.warning("ECU Camera Simulated: %r", ret.enableCamera)
cloudlog.warning("ECU DSU Simulated: %r", ret.enableDsu)
cloudlog.warning("ECU APGS Simulated: %r", ret.enableApgs)
cloudlog.warning("ECU Gas Interceptor: %r", ret.enableGasInterceptor)
# min speed to enable ACC. if car can do stop and go, then set enabling speed

24
selfdrive/car/toyota/toyotacan.py
Unescape View File

@ -1,27 +1,3 @@
def create_ipas_steer_command(packer, steer, enabled, apgs_enabled):
"""Creates a CAN message for the Toyota Steer Command."""
if steer < 0:
direction = 3
elif steer > 0:
direction = 1
else:
direction = 2
mode = 3 if enabled else 1
values = {
"STATE": mode,
"DIRECTION_CMD": direction,
"ANGLE": steer,
"SET_ME_X10": 0x10,
"SET_ME_X40": 0x40
}
if apgs_enabled:
return packer.make_can_msg("STEERING_IPAS", 0, values)
else:
return packer.make_can_msg("STEERING_IPAS_COMMA", 0, values)
def create_steer_command(packer, steer, steer_req, raw_cnt):
"""Creates a CAN message for the Toyota Steer Command."""

10
selfdrive/car/toyota/values.py
Unescape View File

@ -57,20 +57,11 @@ STATIC_MSGS = [
(0x470, Ecu.dsu, (CAR.PRIUS, CAR.LEXUS_RXH), 1, 100, b'\x00\x00\x02\x7a'),
(0x470, Ecu.dsu, (CAR.HIGHLANDER, CAR.HIGHLANDERH, CAR.RAV4H, CAR.SIENNA, CAR.LEXUS_CTH), 1, 100, b'\x00\x00\x01\x79'),
(0x4CB, Ecu.dsu, (CAR.PRIUS, CAR.RAV4H, CAR.LEXUS_RXH, CAR.LEXUS_NXH, CAR.RAV4, CAR.COROLLA, CAR.HIGHLANDERH, CAR.HIGHLANDER, CAR.AVALON, CAR.SIENNA, CAR.LEXUS_CTH, CAR.LEXUS_RX), 0, 100, b'\x0c\x00\x00\x00\x00\x00\x00\x00'),
(0x292, Ecu.apgs, (CAR.PRIUS), 0, 3, b'\x00\x00\x00\x00\x00\x00\x00\x9e'),
(0x32E, Ecu.apgs, (CAR.PRIUS), 0, 20, b'\x00\x00\x00\x00\x00\x00\x00\x00'),
(0x396, Ecu.apgs, (CAR.PRIUS), 0, 100, b'\xBD\x00\x00\x00\x60\x0F\x02\x00'),
(0x43A, Ecu.apgs, (CAR.PRIUS), 0, 100, b'\x84\x00\x00\x00\x00\x00\x00\x00'),
(0x43B, Ecu.apgs, (CAR.PRIUS), 0, 100, b'\x00\x00\x00\x00\x00\x00\x00\x00'),
(0x497, Ecu.apgs, (CAR.PRIUS), 0, 100, b'\x00\x00\x00\x00\x00\x00\x00\x00'),
(0x4CC, Ecu.apgs, (CAR.PRIUS), 0, 100, b'\x0D\x00\x00\x00\x00\x00\x00\x00'),
]
ECU_FINGERPRINT = {
Ecu.fwdCamera: [0x2e4], # steer torque cmd
Ecu.dsu: [0x283], # accel cmd
Ecu.apgs: [0x835], # angle cmd
}
@ -86,7 +77,6 @@ FINGERPRINTS = {
36: 8, 37: 8, 170: 8, 180: 8, 186: 4, 355: 5, 426: 6, 452: 8, 464: 8, 466: 8, 467: 8, 512: 6, 513: 6, 547: 8, 548: 8, 552: 4, 562: 4, 608: 8, 610: 5, 643: 7, 705: 8, 725: 2, 740: 5, 742: 8, 743: 8, 767:4, 800: 8, 830: 7, 835: 8, 836: 8, 849: 4, 869: 7, 870: 7, 871: 2, 896: 8, 897: 8, 900: 6, 902: 6, 905: 8, 911: 8, 916: 3, 921: 8, 922: 8, 933: 8, 944: 8, 945: 8, 951: 8, 955: 8, 956: 8, 979: 2, 998: 5, 999: 7, 1000: 8, 1001: 8, 1008: 2, 1017: 8, 1041: 8, 1042: 8, 1043: 8, 1044: 8, 1056: 8, 1059: 1, 1114: 8, 1161: 8, 1162: 8, 1163: 8, 1176: 8, 1177: 8, 1178: 8, 1179: 8, 1180: 8, 1181: 8, 1190: 8, 1191: 8, 1192: 8, 1196: 8, 1207: 8, 1227: 8, 1235: 8, 1263: 8, 1279: 8, 1552: 8, 1553: 8, 1554: 8, 1555: 8, 1556: 8, 1557: 8, 1561: 8, 1562: 8, 1568: 8, 1569: 8, 1570: 8, 1571: 8, 1572: 8, 1584: 8, 1589: 8, 1592: 8, 1593: 8, 1595: 8, 1596: 8, 1597: 8, 1600: 8, 1664: 8, 1728: 8, 1745: 8, 1779: 8
}],
CAR.PRIUS: [{
# with ipas
36: 8, 37: 8, 166: 8, 170: 8, 180: 8, 295: 8, 296: 8, 426: 6, 452: 8, 466: 8, 467: 8, 512: 6, 513: 6, 550: 8, 552: 4, 560: 7, 562: 6, 581: 5, 608: 8, 610: 8, 614: 8, 643: 7, 658: 8, 713: 8, 740: 5, 742: 8, 743: 8, 767:4, 800: 8, 810: 2, 814: 8, 824: 2, 829: 2, 830: 7, 835: 8, 836: 8, 845: 5, 863: 8, 869: 7, 870: 7, 871: 2,898: 8, 900: 6, 902: 6, 905: 8, 913: 8, 918: 8, 921: 8, 933: 8, 944: 8, 945: 8, 950: 8, 951: 8, 953: 8, 955: 8, 956: 8, 971: 7, 974: 8, 975: 5, 993: 8, 998: 5, 999: 7, 1000: 8, 1001: 8, 1005: 2, 1014: 8, 1017: 8, 1020: 8, 1041: 8, 1042: 8, 1044: 8, 1056: 8, 1057: 8, 1059: 1, 1071: 8, 1076: 8, 1077: 8, 1082: 8, 1083: 8, 1084: 8, 1085: 8, 1086: 8, 1114: 8, 1132: 8, 1161: 8, 1162: 8, 1163: 8, 1164: 8, 1165: 8, 1166: 8, 1167: 8, 1175: 8, 1227: 8, 1228: 8, 1235: 8, 1237: 8, 1264: 8, 1279: 8, 1552: 8, 1553: 8, 1556: 8, 1557: 8, 1568: 8, 1570: 8, 1571: 8, 1572: 8, 1595: 8, 1777: 8, 1779: 8, 1904: 8, 1912: 8, 1990: 8, 1998: 8
},
#2019 LE

Write Preview
Loading…
Cancel
Save