mirror of
https://github.com/firestar5683/StarPilot.git
synced 2026-09-30 19:33:45 +08:00
firestar5683
69 lines
3.2 KiB
Python
69 lines
3.2 KiB
Python
import math
|
|
|
|
import numpy as np
|
|
import pytest
|
|
|
|
from opendbc.car import structs
|
|
from opendbc.car.lateral import get_friction
|
|
from openpilot.selfdrive.controls.lib import latcontrol_vehicle_tunes as tunes
|
|
|
|
|
|
def legacy_threshold(speed, accel, jerk):
|
|
def sigmoid(x):
|
|
return 1.0 / (1.0 + math.exp(-x))
|
|
gain = (0.10 * sigmoid((speed - 10.0) / 3.0) * sigmoid((35.0 - speed) / 6.0) *
|
|
sigmoid((0.28 - abs(accel)) / 0.10) * sigmoid((0.35 - abs(jerk)) / 0.10))
|
|
return tunes.get_standard_friction_threshold(speed) * (1.0 + gain)
|
|
|
|
|
|
@pytest.mark.parametrize('speed,scale', [(0, 1), (5, 1), (10, 1), (15, 1.5), (20, 2), (35, 2), (45, 2)])
|
|
@pytest.mark.parametrize('accel,jerk', [(0, 0), (0.1, 0.2), (0.8, 0.3), (1.5, -0.5), (-0.8, -0.3)])
|
|
def test_speed_blend(speed, scale, accel, jerk):
|
|
assert tunes.get_genesis_g70_friction_threshold(speed, accel, jerk) == pytest.approx(
|
|
legacy_threshold(speed, accel, jerk) * scale)
|
|
|
|
|
|
def test_small_error_gain_and_full_compensation():
|
|
params = structs.CarParams.LateralTorqueTuning.new_message(friction=0.08, latAccelFactor=2.96)
|
|
old = legacy_threshold(30, 0.8, 0.2)
|
|
new = tunes.get_genesis_g70_friction_threshold(30, 0.8, 0.2)
|
|
for error in (-0.1, 0.1):
|
|
assert get_friction(error, 0, new, params) == pytest.approx(get_friction(error, 0, old, params) / 2)
|
|
for error in (-2, 2):
|
|
assert get_friction(error, 0, new, params) == pytest.approx(get_friction(error, 0, old, params))
|
|
|
|
|
|
def test_symmetric_and_continuous():
|
|
for speed in np.linspace(0, 45, 100):
|
|
assert tunes.get_genesis_g70_friction_threshold(speed, 0.8, 0.2) == pytest.approx(
|
|
tunes.get_genesis_g70_friction_threshold(speed, -0.8, -0.2))
|
|
for speed in (10, 20):
|
|
assert abs(tunes.get_genesis_g70_friction_threshold(speed + 1e-6) -
|
|
tunes.get_genesis_g70_friction_threshold(speed - 1e-6)) < 1e-6
|
|
|
|
|
|
@pytest.mark.parametrize('speed,accel,jerk,actual', [
|
|
(30, 0.2, 0.8, 0.1), (30, 0.35, 0.8, 0.3), (20, 1.0, 0.8, 0.8),
|
|
(10, 1.0, 0.8, 0.8), (30, 1.0, -0.8, 1.2), (30, 1.0, 0.0, 1.2),
|
|
])
|
|
def test_turn_in_preserves_center_low_speed_and_unwind(monkeypatch, speed, accel, jerk, actual):
|
|
revised = tunes.get_genesis_g70_friction_jerk_deadzone(speed, accel, jerk, actual)
|
|
monkeypatch.setattr(tunes, 'GENESIS_G70_CURVE_TURN_IN_JERK_REDUCTION', 0.0)
|
|
assert revised == tunes.get_genesis_g70_friction_jerk_deadzone(speed, accel, jerk, actual)
|
|
|
|
|
|
@pytest.mark.parametrize('direction', [-1, 1])
|
|
@pytest.mark.parametrize('jerk', [0.1, 0.5, 1.0])
|
|
def test_curve_turn_in_halves_remaining_jerk(monkeypatch, direction, jerk):
|
|
revised = tunes.get_genesis_g70_friction_jerk_deadzone(30, direction, direction * jerk, direction * 0.8)
|
|
monkeypatch.setattr(tunes, 'GENESIS_G70_CURVE_TURN_IN_JERK_REDUCTION', 0.0)
|
|
original = tunes.get_genesis_g70_friction_jerk_deadzone(30, direction, direction * jerk, direction * 0.8)
|
|
assert max(jerk - revised, 0) == pytest.approx(0.5 * max(jerk - original, 0))
|
|
|
|
|
|
@pytest.mark.parametrize('speed,accel', [(20, 0.8), (25, 0.8), (30, 0.35), (30, 0.7)])
|
|
def test_curve_turn_in_blend_continuity(speed, accel):
|
|
low = tunes.get_genesis_g70_friction_jerk_deadzone(speed - 1e-6, accel - 1e-6, 0.8)
|
|
high = tunes.get_genesis_g70_friction_jerk_deadzone(speed + 1e-6, accel + 1e-6, 0.8)
|
|
assert abs(high - low) < 1e-5
|