mirror of
https://github.com/firestar5683/StarPilot.git
synced 2026-10-28 09:03:46 +08:00
firestar5683
200632aabd
Learn bounded feed-forward corrections on eligible torque controllers behind Use StarPilot Auto Learn, preserving installed tunes and native safety limits. Co-authored-by: Danny <dannyirwin31@yahoo.com> Co-authored-by: Dom <168790843+firestar5683@users.noreply.github.com> Co-authored-by: Harald Schäfer <harald.the.engineer@gmail.com> Co-authored-by: Prabhaav Pillai <prabhaav@msu.edu> Co-authored-by: dirwin31 <83434411+dirwin31@users.noreply.github.com> Co-authored-by: firestarsdog <229254897+firestarsdog@users.noreply.github.com> Co-authored-by: Intelli <contact@intelli.software> Co-authored-by: haraschax <6804392+haraschax@users.noreply.github.com> Co-authored-by: pharmacomaniac <blittle65@gmail.com> Co-authored-by: whoisdomi <dcantu83@gmail.com>
60 lines
2.8 KiB
Python
60 lines
2.8 KiB
Python
"""Actual registered controller filter regression; CAN closure is covered separately."""
|
|
import math
|
|
from types import SimpleNamespace
|
|
|
|
import pytest
|
|
|
|
from opendbc.car import structs
|
|
from opendbc.car.car_helpers import interfaces
|
|
from opendbc.car.hyundai.values import CAR
|
|
from openpilot.starpilot.flm.controller_replay import (
|
|
ControllerReplay, ReplayFrame, SelectedControllerFactory, _run, capture_snapshot,
|
|
final_command, frame_input, restore_snapshot,
|
|
)
|
|
from openpilot.starpilot.flm.torque_adapter import Objective
|
|
|
|
|
|
@pytest.mark.parametrize('fingerprint', [CAR.GENESIS_G70_2020, CAR.GENESIS_GV70_ELECTRIFIED_1ST_GEN])
|
|
def test_high_speed_causal_features_preserve_selected_filter_across_actual_checkpoint(fingerprint):
|
|
cp = interfaces[fingerprint].get_non_essential_params(fingerprint)
|
|
factory = SelectedControllerFactory(cp, 'starpilot')
|
|
objective = Objective('feedforward', 'high', 'steady')
|
|
engine = ControllerReplay(factory, objective)
|
|
controller, vm = factory.create()
|
|
params = SimpleNamespace(angleOffsetDeg=0.0, roll=0.0)
|
|
desired = .001
|
|
cs = structs.CarState(vEgo=30.0, vEgoRaw=30.0, canValid=True,
|
|
gearShifter=structs.CarState.GearShifter.drive)
|
|
cs.steeringAngleDeg = math.degrees(vm.get_steer_from_curvature(-desired, 30.0, 0.0))
|
|
for _ in range(240):
|
|
controller.update(True, cs, vm, params, False, desired, False, .1)
|
|
first = 10_000_000_000
|
|
initial = capture_snapshot(controller, vm)
|
|
engine.checkpoint(first, initial)
|
|
for tick in range(120):
|
|
stamp = first + tick * 10_000_000
|
|
if tick == 100:
|
|
engine.checkpoint(stamp, capture_snapshot(controller, vm))
|
|
inputs = frame_input(cs, vm, params, active=True, safety_limited=False,
|
|
desired_curvature=desired, curvature_limited=False, lat_delay=.1)
|
|
raw, _, _ = controller.update(True, cs, vm, params, False, desired, False, .1)
|
|
point = ReplayFrame(stamp, inputs, final_command(raw), (0.0, 0.0),
|
|
frame_sequence=tick + 1, actual_lac_command=raw)
|
|
local = engine.normalize(point, epsilon=.01)
|
|
assert local is not None and local.verified
|
|
local_gain = local.features[0]
|
|
engine.replay_windows = ((first, stamp),)
|
|
assert engine.rebuild_causal_features(epsilon=.01) == 0 # Controller-only case.
|
|
expected = []
|
|
for coefficients in ((.01, 0.0), (-.01, 0.0)):
|
|
branch, branch_vm = factory.create()
|
|
restore_snapshot(branch, branch_vm, initial)
|
|
for point in engine.records:
|
|
value, _ = _run(branch, branch_vm, point.inputs, objective, coefficients, point.torque_values)
|
|
expected.append(final_command(value))
|
|
causal = engine.normalized[stamp]
|
|
assert causal.plus_commands[0] == expected[0]
|
|
assert causal.minus_commands[0] == expected[1]
|
|
assert causal.features[0] == (expected[0] - expected[1]) / .02
|
|
assert abs(causal.features[0]) > 1.5 * abs(local_gain)
|