forked from IQ.Lvbs/IQ.Pilot
92 lines
3.0 KiB
Python
92 lines
3.0 KiB
Python
"""
|
|
Copyright © IQ.Lvbs, apart of Project Teal Lvbs, All Rights Reserved, licensed under https://konn3kt.com/tos
|
|
"""
|
|
import time
|
|
|
|
import pytest
|
|
|
|
import iqpilot.cereal.messaging as messaging
|
|
from iqpilot.cereal import car
|
|
from iqpilot.common.params import Params
|
|
from iqpilot.common.steer_delay import (
|
|
SteerDelayPublisher,
|
|
cached_steer_delay,
|
|
fixed_steer_delay,
|
|
lateral_action_delay,
|
|
resolve_steer_delay,
|
|
)
|
|
|
|
ANGLE = car.CarParams.SteerControlType.angle
|
|
TORQUE = car.CarParams.SteerControlType.torque
|
|
|
|
LIVE_DELAY = 0.4387
|
|
RACK_DELAY = 0.10
|
|
OFFSET = 0.05
|
|
|
|
|
|
@pytest.fixture
|
|
def params(tmp_path, monkeypatch):
|
|
monkeypatch.setenv("PARAMS_ROOT", str(tmp_path))
|
|
p = Params()
|
|
p.put("IQSteerDelayCache", LIVE_DELAY)
|
|
p.put("IQSoftwareSteerDelay", OFFSET)
|
|
return p
|
|
|
|
|
|
def _car_params(steer_control_type):
|
|
cp = car.CarParams.new_message()
|
|
cp.steerControlType = steer_control_type
|
|
cp.steerActuatorDelay = RACK_DELAY
|
|
return cp
|
|
|
|
|
|
def _lateral_delay_msg(value):
|
|
msg = messaging.new_message("lateralDelay")
|
|
msg.lateralDelay.lateralDelay = value
|
|
return msg.as_reader()
|
|
|
|
|
|
def test_params_fixture_is_isolated_from_the_real_device(params, tmp_path):
|
|
assert str(tmp_path) in params.get_param_path("")
|
|
|
|
|
|
@pytest.mark.parametrize("live_enabled", [True, False])
|
|
def test_torque_cars_always_use_live_delay(params, live_enabled):
|
|
params.put_bool("IQLiveSteerDelay", live_enabled)
|
|
assert lateral_action_delay(params, _car_params(TORQUE), LIVE_DELAY) == pytest.approx(LIVE_DELAY)
|
|
|
|
|
|
def test_angle_cars_ignore_live_delay_when_self_tuning_is_off(params):
|
|
params.put_bool("IQLiveSteerDelay", False)
|
|
delay = lateral_action_delay(params, _car_params(ANGLE), LIVE_DELAY)
|
|
assert delay == pytest.approx(RACK_DELAY + OFFSET)
|
|
assert delay != pytest.approx(LIVE_DELAY)
|
|
|
|
|
|
def test_angle_cars_use_cached_delay_when_self_tuning_is_on(params):
|
|
params.put_bool("IQLiveSteerDelay", True)
|
|
assert lateral_action_delay(params, _car_params(ANGLE), LIVE_DELAY) == pytest.approx(LIVE_DELAY)
|
|
|
|
|
|
@pytest.mark.parametrize("offset", [0.05, 0.20, 0.50])
|
|
def test_manual_offset_reaches_the_path_and_matches_what_the_ui_reports(params, offset):
|
|
params.put_bool("IQLiveSteerDelay", False)
|
|
params.put("IQSoftwareSteerDelay", offset)
|
|
ui_total = RACK_DELAY + offset
|
|
assert fixed_steer_delay(params, RACK_DELAY) == pytest.approx(ui_total)
|
|
assert lateral_action_delay(params, _car_params(ANGLE), LIVE_DELAY) == pytest.approx(ui_total)
|
|
|
|
|
|
@pytest.mark.parametrize("live_enabled", [False, True])
|
|
def test_publisher_writes_the_value_the_resolver_reads(params, live_enabled):
|
|
params.put_bool("IQLiveSteerDelay", live_enabled)
|
|
params.put("IQSteerDelayCache", -1.0)
|
|
SteerDelayPublisher(_car_params(ANGLE)).update(_lateral_delay_msg(LIVE_DELAY))
|
|
|
|
expected = LIVE_DELAY if live_enabled else RACK_DELAY + OFFSET
|
|
deadline = time.monotonic() + 5.0
|
|
while cached_steer_delay() != pytest.approx(expected) and time.monotonic() < deadline:
|
|
time.sleep(0.01)
|
|
assert cached_steer_delay() == pytest.approx(expected)
|
|
assert resolve_steer_delay(params, RACK_DELAY) == pytest.approx(expected)
|