diff --git a/selfdrive/car/toyota/carcontroller.py b/selfdrive/car/toyota/carcontroller.py index d9019771d5..6f21fec496 100644 --- a/selfdrive/car/toyota/carcontroller.py +++ b/selfdrive/car/toyota/carcontroller.py @@ -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): diff --git a/selfdrive/car/toyota/carstate.py b/selfdrive/car/toyota/carstate.py index d6bc5d487e..c6c7b6536b 100644 --- a/selfdrive/car/toyota/carstate.py +++ b/selfdrive/car/toyota/carstate.py @@ -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), ] diff --git a/selfdrive/car/toyota/interface.py b/selfdrive/car/toyota/interface.py index 44030b2b63..45f9a356e9 100755 --- a/selfdrive/car/toyota/interface.py +++ b/selfdrive/car/toyota/interface.py @@ -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 diff --git a/selfdrive/car/toyota/toyotacan.py b/selfdrive/car/toyota/toyotacan.py index 48d6229c95..a5d1497291 100644 --- a/selfdrive/car/toyota/toyotacan.py +++ b/selfdrive/car/toyota/toyotacan.py @@ -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.""" diff --git a/selfdrive/car/toyota/values.py b/selfdrive/car/toyota/values.py index d754b11e28..fb6ff15198 100644 --- a/selfdrive/car/toyota/values.py +++ b/selfdrive/car/toyota/values.py @@ -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