mirror of
https://github.com/dragonpilot/dragonpilot.git
synced 2026-06-28 01:12:05 +08:00
removing ipas steering control option from toyota (#1193)
* removing ipas steering control from toyota
This commit is contained in:
rbiasini
@@ -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):
|
||||
|
||||
@@ -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),
|
||||
]
|
||||
|
||||
@@ -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
|
||||
|
||||
@@ -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."""
|
||||
|
||||
|
||||
@@ -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
|
||||
|
||||
Reference in New Issue
Block a user