mirror of
https://github.com/firestar5683/StarPilot.git
synced 2026-11-08 13:33:42 +08:00
firestar5683
79c8945a41
Integrate torque feedback, lane-change smoothing and low-speed Turn Assist through dedicated lateral owners. Co-authored-by: Dom <168790843+firestar5683@users.noreply.github.com> Co-authored-by: pharmacomaniac <blittle65@gmail.com>
75 lines
3.9 KiB
Python
75 lines
3.9 KiB
Python
"""Actual Genesis controller proof for exact mapped Dom leaves."""
|
|
|
|
from collections import deque
|
|
from types import SimpleNamespace
|
|
import math
|
|
import pytest
|
|
from opendbc.car.car_helpers import interfaces
|
|
from opendbc.car.hyundai.values import CAR
|
|
from opendbc.car.vehicle_model import VehicleModel
|
|
from openpilot.cereal import log
|
|
from openpilot.selfdrive.controls.lib.latcontrol_torque import LatControlTorque
|
|
from openpilot.starpilot.lateral.controller_selection import ControllerMode
|
|
from openpilot.starpilot.lateral import genesis_gv70_policy as gv70
|
|
from openpilot.starpilot.lateral import genesis_g70_policy as g70
|
|
|
|
|
|
@pytest.mark.parametrize('direction', [-1.0, 1.0])
|
|
def test_gv70_actual_undertracking_curve_matches_undamped_policy(monkeypatch, direction):
|
|
cp = interfaces[CAR.GENESIS_GV70_ELECTRIFIED_1ST_GEN].get_non_essential_params(CAR.GENESIS_GV70_ELECTRIFIED_1ST_GEN)
|
|
|
|
def run():
|
|
controller = LatControlTorque(cp.as_reader(), interfaces[cp.carFingerprint](cp), 0.01, controller_mode=ControllerMode.STARPILOT)
|
|
vm = VehicleModel(cp)
|
|
cs = SimpleNamespace(vEgo=30.0, steeringAngleDeg=math.degrees(vm.get_steer_from_curvature(-direction * 0.6 / 900.0, 30.0, 0.0)), steeringPressed=False)
|
|
params = log.VehicleParameters.new_message(angleOffsetDeg=0.0, roll=0.0)
|
|
request = direction * 0.8 / 900.0
|
|
policy = controller.starpilot_extension.policy
|
|
policy.curvature_request_buffer = deque([request] * policy.request_buffer_len, maxlen=policy.request_buffer_len)
|
|
rows = []
|
|
for _ in range(100):
|
|
out, _, state = controller.update(True, cs, vm, params, False, request, False, 0.2)
|
|
assert math.isfinite(out) and abs(out) <= controller.steer_max + 1e-6
|
|
rows.append((out, state.desiredLateralAccel, state.p, state.i, state.d, state.f))
|
|
return rows, controller.torque_params.to_dict(), (controller.pid.pos_limit, controller.pid.neg_limit)
|
|
|
|
actual = run()
|
|
for name in ('get_genesis_gv70_unwind_ff_scale', 'get_genesis_gv70_high_speed_error_scale', 'get_genesis_gv70_reversal_output_scale'):
|
|
monkeypatch.setattr(gv70, name, lambda *_: 1.0)
|
|
assert actual == run()
|
|
|
|
|
|
@pytest.mark.parametrize('direction', [-1.0, 1.0])
|
|
@pytest.mark.parametrize('pressed', [False, True])
|
|
def test_g70_actual_friction_receives_traffic_increment_only_without_driver_override(monkeypatch, direction, pressed):
|
|
cp = interfaces[CAR.GENESIS_G70_2020].get_non_essential_params(CAR.GENESIS_G70_2020)
|
|
actual = LatControlTorque(cp.as_reader(), interfaces[cp.carFingerprint](cp), 0.01, controller_mode=ControllerMode.STARPILOT)
|
|
actual.update_torque_parameters(3.0, 0.02, 0.12)
|
|
tune = actual.torque_params.to_dict()
|
|
limits = (actual.pid.pos_limit, actual.pid.neg_limit, actual.steer_max)
|
|
cs = SimpleNamespace(vEgo=5.0, steeringAngleDeg=0.0, steeringPressed=pressed)
|
|
params = log.VehicleParameters.new_message(angleOffsetDeg=0.0, roll=0.0)
|
|
vm = VehicleModel(cp)
|
|
calls = []
|
|
original = g70.get_friction
|
|
|
|
def capture(error, deadzone, threshold, tune):
|
|
calls.append(threshold)
|
|
return original(error, deadzone, threshold, tune)
|
|
|
|
monkeypatch.setattr(g70, 'get_friction', capture)
|
|
for _ in range(100):
|
|
out, _, state = actual.update(True, cs, vm, params, False, direction * 0.0001, False, 0.2)
|
|
assert math.isfinite(out) and abs(out) <= actual.steer_max + 1e-6
|
|
base = g70.get_genesis_g70_friction_threshold(cs.vEgo, state.desiredLateralAccel, state.desiredLateralJerk)
|
|
inc = (
|
|
0.0 if pressed else g70.get_genesis_g70_traffic_friction_increment(cs.vEgo, state.desiredLateralAccel, state.desiredLateralJerk, direction * 0.0001, 0.0)
|
|
)
|
|
assert calls[-1] == pytest.approx(base + inc)
|
|
assert (inc == 0.0) is pressed
|
|
assert actual.torque_params.to_dict() == tune
|
|
assert (actual.pid.pos_limit, actual.pid.neg_limit, actual.steer_max) == limits
|
|
calls.clear()
|
|
out, _, state = actual.update(False, cs, vm, params, False, direction * 0.0001, False, 0.2)
|
|
assert out == 0.0 and not state.active and calls == []
|