diff --git a/selfdrive/car/interfaces.py b/selfdrive/car/interfaces.py index 8e8872a539..820e429cc4 100644 --- a/selfdrive/car/interfaces.py +++ b/selfdrive/car/interfaces.py @@ -11,7 +11,7 @@ from common.kalman.simple_kalman import KF1D from common.numpy_fast import interp from common.realtime import DT_CTRL from selfdrive.car import apply_hysteresis, gen_empty_fingerprint -from selfdrive.controls.lib.drive_helpers import V_CRUISE_MAX, apply_deadzone +from selfdrive.controls.lib.drive_helpers import V_CRUISE_MAX, apply_slack from selfdrive.controls.lib.events import Events from selfdrive.controls.lib.vehicle_model import VehicleModel @@ -109,7 +109,7 @@ class CarInterfaceBase(ABC): def torque_from_lateral_accel_linear(lateral_accel_value, torque_params, lateral_accel_error, lateral_accel_deadzone, friction_compensation): # The default is a linear relationship between torque and lateral acceleration (accounting for road roll and steering friction) friction_interp = interp( - apply_deadzone(lateral_accel_error, lateral_accel_deadzone), + apply_slack(lateral_accel_error, lateral_accel_deadzone), [-FRICTION_THRESHOLD, FRICTION_THRESHOLD], [-torque_params.friction, torque_params.friction] ) diff --git a/selfdrive/car/torque_data/params.yaml b/selfdrive/car/torque_data/params.yaml index a9023b4edc..b4dbaf8564 100644 --- a/selfdrive/car/torque_data/params.yaml +++ b/selfdrive/car/torque_data/params.yaml @@ -77,7 +77,8 @@ TOYOTA HIGHLANDER 2020: [2.022340166827233, 1.6183134804881791, 0.14592306380054 TOYOTA HIGHLANDER HYBRID 2018: [1.752033, 1.6433903296845025, 0.144600] TOYOTA HIGHLANDER HYBRID 2020: [1.901174, 2.104015182965606, 0.14447040132184993] TOYOTA MIRAI 2021: [2.506899832157829, 1.7417213930750164, 0.20182618449440565] -TOYOTA PRIUS 2017: [1.746445, 1.5023147650693636, 0.151515] +TOYOTA PRIUS 2017: [1.60, 1.5023147650693636, 0.151515] +TOYOTA PRIUS 2017 x060: [1.746445, 1.5023147650693636, 0.17] TOYOTA PRIUS TSS2 2021: [1.972600, 1.9104337425537743, 0.170968] TOYOTA RAV4 2017: [2.085695074355425, 2.2142832316984733, 0.13339165270103975] TOYOTA RAV4 2019: [2.331293, 2.0993589721530252, 0.129822] diff --git a/selfdrive/car/toyota/interface.py b/selfdrive/car/toyota/interface.py index 0139c692c1..7ee5ac0d0e 100644 --- a/selfdrive/car/toyota/interface.py +++ b/selfdrive/car/toyota/interface.py @@ -42,7 +42,8 @@ class CarInterface(CarInterfaceBase): # 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 + steering_angle_deadzone_deg = 0.2 + ret.steerActuatorDelay = 0.25 CarInterfaceBase.configure_torque_tune(candidate, ret.lateralTuning, steering_angle_deadzone_deg) elif candidate == CAR.PRIUS_V: @@ -51,7 +52,6 @@ class CarInterface(CarInterfaceBase): ret.steerRatio = 17.4 tire_stiffness_factor = 0.5533 ret.mass = 3340. * CV.LB_TO_KG + STD_CARGO_KG - CarInterfaceBase.configure_torque_tune(candidate, ret.lateralTuning, steering_angle_deadzone_deg) elif candidate in (CAR.RAV4, CAR.RAV4H): stop_and_go = True if (candidate in CAR.RAV4H) else False diff --git a/selfdrive/controls/lib/drive_helpers.py b/selfdrive/controls/lib/drive_helpers.py index f0dc2e9467..acef4bee8e 100644 --- a/selfdrive/controls/lib/drive_helpers.py +++ b/selfdrive/controls/lib/drive_helpers.py @@ -149,6 +149,12 @@ def apply_deadzone(error, deadzone): return error +def apply_slack(error, deadzone): + if (error > - deadzone) and (error < deadzone): + error = 0. + return error + + def rate_limit(new_value, last_value, dw_step, up_step): return clip(new_value, last_value + dw_step, last_value + up_step) diff --git a/selfdrive/test/process_replay/ref_commit b/selfdrive/test/process_replay/ref_commit index d4520d2935..71da0dff44 100644 --- a/selfdrive/test/process_replay/ref_commit +++ b/selfdrive/test/process_replay/ref_commit @@ -1 +1 @@ -08ca448a6dcfc1edc4f9fedb2ea94ad6b2b69aa2 \ No newline at end of file +ddfaab44ae64a0d064b847e81080993b51b6b423 \ No newline at end of file