diff --git a/selfdrive/car/toyota/interface.py b/selfdrive/car/toyota/interface.py index 53accecfe0..9ca9e4e11b 100644 --- a/selfdrive/car/toyota/interface.py +++ b/selfdrive/car/toyota/interface.py @@ -2,6 +2,7 @@ from cereal import car from common.conversions import Conversions as CV from panda import Panda +from selfdrive.controls.lib.latcontrol_torque import set_torque_tune from selfdrive.car.toyota.tunes import LatTunes, LongTunes, set_long_tune, set_lat_tune from selfdrive.car.toyota.values import Ecu, CAR, ToyotaFlags, TSS2_CAR, RADAR_ACC_CAR, NO_DSU_CAR, MIN_ACC_SPEED, EPS_SCALE, EV_HYBRID_CAR, CarControllerParams from selfdrive.car import STD_CARGO_KG, scale_rot_inertia, scale_tire_stiffness, gen_empty_fingerprint, get_safety_config @@ -32,6 +33,8 @@ class CarInterface(CarInterfaceBase): stop_and_go = False torque_params = CarInterfaceBase.get_torque_params(candidate) + steering_angle_deadzone_deg = 0.0 + set_torque_tune(ret.lateralTuning, torque_params['LAT_ACCEL_FACTOR'], torque_params['FRICTION'], steering_angle_deadzone_deg) if candidate == CAR.PRIUS: stop_and_go = True @@ -39,8 +42,11 @@ class CarInterface(CarInterfaceBase): ret.steerRatio = 15.74 # unknown end-to-end spec tire_stiffness_factor = 0.6371 # hand-tune ret.mass = 3045. * CV.LB_TO_KG + STD_CARGO_KG - ret.maxLateralAccel = 1.7 - set_lat_tune(ret.lateralTuning, LatTunes.TORQUE, MAX_LAT_ACCEL=ret.maxLateralAccel, FRICTION=0.06, steering_angle_deadzone_deg=1.0) + # Only give steer angle deadzone to for bad angle sensor prius + for fw in car_fw: + if fw.ecu == "eps" and not fw.fwVersion == b'8965B47060\x00\x00\x00\x00\x00\x00': + steering_angle_deadzone_deg = 1.0 + set_torque_tune(ret.lateralTuning, torque_params['LAT_ACCEL_FACTOR'], torque_params['FRICTION'], steering_angle_deadzone_deg) elif candidate == CAR.PRIUS_V: stop_and_go = True @@ -48,8 +54,10 @@ class CarInterface(CarInterfaceBase): ret.steerRatio = 17.4 tire_stiffness_factor = 0.5533 ret.mass = 3340. * CV.LB_TO_KG + STD_CARGO_KG + # TODO override until there is enough data ret.maxLateralAccel = 1.8 - set_lat_tune(ret.lateralTuning, LatTunes.TORQUE, MAX_LAT_ACCEL=ret.maxLateralAccel, FRICTION=0.06) + torque_params = CarInterfaceBase.get_torque_params(CAR.PRIUS) + set_torque_tune(ret.lateralTuning, torque_params['LAT_ACCEL_FACTOR'], torque_params['FRICTION'], steering_angle_deadzone_deg) elif candidate in (CAR.RAV4, CAR.RAV4H): stop_and_go = True if (candidate in CAR.RAV4H) else False @@ -57,16 +65,12 @@ class CarInterface(CarInterfaceBase): ret.steerRatio = 16.88 # 14.5 is spec end-to-end tire_stiffness_factor = 0.5533 ret.mass = 3650. * CV.LB_TO_KG + STD_CARGO_KG # mean between normal and hybrid - ret.maxLateralAccel = 1.8 - set_lat_tune(ret.lateralTuning, LatTunes.TORQUE, MAX_LAT_ACCEL=ret.maxLateralAccel, FRICTION=0.06) elif candidate == CAR.COROLLA: ret.wheelbase = 2.70 ret.steerRatio = 18.27 tire_stiffness_factor = 0.444 # not optimized yet ret.mass = 2860. * CV.LB_TO_KG + STD_CARGO_KG # mean between normal and hybrid - ret.maxLateralAccel = 2.8 - set_lat_tune(ret.lateralTuning, LatTunes.TORQUE, MAX_LAT_ACCEL=ret.maxLateralAccel, FRICTION=0.024) elif candidate in (CAR.LEXUS_RX, CAR.LEXUS_RXH, CAR.LEXUS_RX_TSS2, CAR.LEXUS_RXH_TSS2): stop_and_go = True @@ -91,9 +95,7 @@ class CarInterface(CarInterfaceBase): ret.steerRatio = 13.7 tire_stiffness_factor = 0.7933 ret.mass = 3400. * CV.LB_TO_KG + STD_CARGO_KG # mean between normal and hybrid - if candidate in (CAR.CAMRY_TSS2, CAR.CAMRYH_TSS2): - set_lat_tune(ret.lateralTuning, LatTunes.TORQUE, torque_params['LAT_ACCEL_FACTOR'], torque_params['FRICTION']) - else: + if candidate not in (CAR.CAMRY_TSS2, CAR.CAMRYH_TSS2): set_lat_tune(ret.lateralTuning, LatTunes.PID_C) elif candidate in (CAR.HIGHLANDER_TSS2, CAR.HIGHLANDERH_TSS2): @@ -143,8 +145,6 @@ class CarInterface(CarInterfaceBase): ret.steerRatio = 13.9 tire_stiffness_factor = 0.444 # not optimized yet ret.mass = 3060. * CV.LB_TO_KG + STD_CARGO_KG - ret.maxLateralAccel = 2.0 - set_lat_tune(ret.lateralTuning, LatTunes.TORQUE, MAX_LAT_ACCEL=ret.maxLateralAccel, FRICTION=0.07) elif candidate in (CAR.LEXUS_ES_TSS2, CAR.LEXUS_ESH_TSS2, CAR.LEXUS_ESH): stop_and_go = True diff --git a/selfdrive/test/process_replay/ref_commit b/selfdrive/test/process_replay/ref_commit index 613b7f26ae..61bbd5cdd1 100644 --- a/selfdrive/test/process_replay/ref_commit +++ b/selfdrive/test/process_replay/ref_commit @@ -1 +1 @@ -123506cad1877e93bfe5c91ecdce654ef339959b +fe2da24194e3def1823681cc18e7879f24edfc6e