mirror of
https://github.com/sunnypilot/sunnypilot.git
synced 2026-09-18 10:23:43 +08:00
Ford: build C1 error correction four times faster
Raise I from 0.25 to 1.0 with P held at 0.75. Route 151 showed persistent undertracking with unused C1 range and slow integral buildup. Preserve C0, bounds, anti-windup and upstream fallback; identify the trial as v14. Validate with 22 fixed-motion route replays, 5,201,912 CAN round trips, and 468 tests plus 25 subtests. Replay verifies commands, not improved physical tracking or stable release.
This commit is contained in:
@@ -0,0 +1,102 @@
|
||||
# Ford C1 integral trial: I=1.0
|
||||
|
||||
Route 151 had large entry shortfalls before driver input while C1 still had
|
||||
command range available. At the segment-5 right-turn shortfall, the selected
|
||||
request was 125 degrees right and the wheel was at 49 degrees right. The stored
|
||||
correction was only 0.015 rad and was growing at about 0.0245 rad/s. Neither the
|
||||
C1 field bound nor the PSCM reached-limit flag explained that point.
|
||||
|
||||
This trial raises I from 0.25 to **1.0**, keeping P at **0.75**. The fresh-error
|
||||
increment is four times larger for the same curvature error, speed and elapsed
|
||||
measurement time. This also retires existing correction four times faster for
|
||||
the same opposing error, before the existing accumulation/headroom rules apply.
|
||||
It does not introduce a new state, rate limit, threshold or release heuristic.
|
||||
Runtime changes are one gain constant and the diagnostic identifier
|
||||
`model-action-curvature-c0-distance-pi-v14`.
|
||||
|
||||
C0's desired-curvature formula and distance toggle, base C1, +0.40 s low-speed
|
||||
model preview, feedback measurement, driver override, PSCM arbitration, field
|
||||
bounds and 100 Hz sender remain unchanged. Zero error holds I. Driver override
|
||||
clears P and I. Fresh limitReached blocks outward accumulation while allowing
|
||||
retirement. Accumulation cannot charge beyond the combined command's available
|
||||
range. C2/C3 stay zero, and toggle-off still selects upstream Ford control.
|
||||
|
||||
## Why this coefficient
|
||||
|
||||
Compared I=0.25, 0.50, 1.0 and 2.0 with P=0.75 on routes 149 and 151, using
|
||||
the production adapter and Float32/CAN path for every sample. I=1.0 gives a
|
||||
substantial increase in retained correction. I=2.0 adds considerably more
|
||||
opposite-direction correction on reviewed exits and approaches the C1 bound
|
||||
in the route-151 right turn. I=1.0 is a fourfold experimental step, not an
|
||||
offline-fitted optimum or a validated physical calibration.
|
||||
|
||||
Paired fixed-motion examples with I=0.25 / I=1.0:
|
||||
|
||||
| Route / time | Recorded situation | C1 before | C1 candidate |
|
||||
| --- | --- | ---: | ---: |
|
||||
| 151 / 306.891 s | 125-degree right request, 49-degree wheel | +0.3270 | +0.3725 |
|
||||
| 151 / 307.999 s | Wheel remains behind on the same right turn | +0.2675 | +0.3625 |
|
||||
| 151 / 2363.807 s | 137-degree left request, 64-degree wheel | -0.3410 | -0.3785 |
|
||||
| 149 / 319.549 s | Right-turn entry shortfall | +0.4190 | +0.4840 |
|
||||
| 149 / 850.497 s | Right-turn release | +0.0670 | +0.0010 |
|
||||
| 149 / 851.331 s | Near center on that release | +0.0105 | -0.0460 |
|
||||
|
||||
These are same-cycle controller requests, not necessarily the preceding CAN
|
||||
message at that timestamp. Positive C1 requests right steering; positive logged
|
||||
wheel angle means left. Both replays use P=0.75; route 149 originally drove
|
||||
P=0.50, so the old-I replay is not its historical command trace.
|
||||
|
||||
The exit examples show why faster retirement does not guarantee a smoother
|
||||
unwind: the candidate can accumulate more correction in the opposite direction
|
||||
and retain it when error reaches zero. Higher I also affects centering. On
|
||||
route 151's clean requests under 10 degrees, mean absolute C1 rises from about
|
||||
0.0083 to 0.0122 rad on the fixed recording; route 149 rises from 0.0077 to
|
||||
0.0154 rad. The vehicle would generate different errors under the candidate.
|
||||
These are command observations, not predictions of wheel motion, stability,
|
||||
overshoot, or future tracking accuracy.
|
||||
|
||||
## Broader validation
|
||||
|
||||
The paired production replay covers 22 extracts: 112–117, 119, 11a, 120, 124,
|
||||
125, 146, 149, 151, a0, a2, a5, a9, b8, b9, ca and Raptor 02. It processes
|
||||
2,600,956 source cycles and 5,201,912 Float32/CAN round trips. Both settings have
|
||||
identical eligibility, C0, P, base heading, overflow, and driver/PSCM feedback
|
||||
gates on every cycle. Output remains finite and bounded; inactive commands and
|
||||
C2/C3 are zero. The decoder checks fields, mode and counter on every command.
|
||||
|
||||
Scoring excludes driver override, inactive/invalid control, disabled feedback,
|
||||
the following second, and speeds below 3 mph. There are 12,213.62 scored seconds.
|
||||
C1-bound time rises from 22.67 to 25.13 seconds. Route 151 has no C1-bound samples
|
||||
in that cohort under either setting. Its I=0.25 baseline matches the recorded
|
||||
path output to Float32 precision: maximum C0 error 5.8e-8 m, C1 error 1.5e-8 rad.
|
||||
Older routes intentionally retain their original model requests and physical
|
||||
measurements, including any historical tracking errors. Their baseline command
|
||||
traces need not match older controller implementations.
|
||||
|
||||
The existing controlsd-to-publication-to-CAN feedback test was updated before
|
||||
the gain change. It failed on the old default (0.005 rad accumulated versus
|
||||
0.020 rad required over its one-second error interval), then passed with the
|
||||
new default. **468 tests and 25 subtests pass**, covering selection, current
|
||||
references, accumulation/hold/retirement, reversals, duplicate measurements,
|
||||
PSCM limits, driver overrides, downstream checksums and upstream fallback.
|
||||
Ruff and `git diff --check` pass. No device build or physical evaluation is
|
||||
claimed by these offline checks.
|
||||
|
||||
Evidence: [ford_c1_i1_validation.json](ford_c1_i1_validation.json). Local arrays
|
||||
are in `.cache/ford_i1_trial`, with the four-setting comparison in
|
||||
`.cache/ford_i_trial_sweep`. Reproduce a comparison:
|
||||
|
||||
```sh
|
||||
PYTHONPATH=.:opendbc_repo PYTHONDONTWRITEBYTECODE=1 python \
|
||||
tools/ford_pscm_lab/proportional_replay.py \
|
||||
--routes 151=.cache/ford_route151/full 149=.cache/ford_route149/full \
|
||||
--settings .75:.25 .75:1.0 \
|
||||
--output .cache/ford_i1_recheck --workers 2
|
||||
```
|
||||
|
||||
The replay tool's default P=0.50 / P=0.75 comparison with I=0.25 is preserved.
|
||||
Explicit `--settings` accepts P:I pairs; the first is the baseline. Publication
|
||||
timestamps approximate execution time because full process scheduling and
|
||||
SubMaster state are not logged. The trial still needs physical measurement;
|
||||
route 151 also changed the big model from CTMV2 to Tee Time, so its comparison
|
||||
with route 149 cannot isolate the controller's physical effect.
|
||||
File diff suppressed because it is too large
Load Diff
@@ -1,7 +1,7 @@
|
||||
# Ford selected-action drive-test branch
|
||||
|
||||
This v13 trial increases C1's proportional gain from 0.50 to **0.75**, retaining
|
||||
I=0.25, [curvature-derived C0](ford_curvature_c0_v8.md), direct C0/C1 requests,
|
||||
This v14 trial increases C1's integral gain from 0.25 to **1.0**, retaining
|
||||
P=0.75, [curvature-derived C0](ford_curvature_c0_v8.md), direct C0/C1 requests,
|
||||
and [continuous C1 PI feedback](ford_c1_minimal_pi.md).
|
||||
Only integrated tracking error accumulates correction; C0/C1 reflect the current bounded request. C0 defaults to a 7 m circular arc from selected desired curvature. An on-device toggle can instead use max(7 m, speed × 1 second).
|
||||
[Base C1 overflow allocation to C0](ford_c1_overflow.md) remains.
|
||||
@@ -12,9 +12,10 @@ turn-exit behavior and closed-loop stability remain unvalidated.
|
||||
|
||||
Both base commands use selected, upstream-limited desired curvature. The +0.40 s
|
||||
low-speed model preview from `b720e9f1b` remains: full offset at 15 mph and below,
|
||||
tapering to zero at 30 mph. The trial changes only the immediate error correction;
|
||||
PSCM `LimitReached` handling, integral gain, field bounds, and selection are retained.
|
||||
See [P=0.75 replay results](ford_c1_p75_trial.md) for scope, tradeoffs, and reproduction.
|
||||
tapering to zero at 30 mph. The trial multiplies each fresh integral error increment
|
||||
by four, for both accumulation and retirement. P, PSCM `LimitReached` handling,
|
||||
integral arithmetic, field bounds, and selection are retained.
|
||||
See [I=1.0 replay results](ford_c1_i1_trial.md) for scope, tradeoffs, and reproduction.
|
||||
|
||||
## Select and restore
|
||||
|
||||
@@ -28,10 +29,10 @@ See [P=0.75 replay results](ford_c1_p75_trial.md) for scope, tradeoffs, and repr
|
||||
|
||||
The startup event `Ford path controller selected` should report
|
||||
`FordModelActionController`. Periodic `Ford C2-free path tracking` events
|
||||
identify **`hypothesis=model-action-curvature-c0-distance-pi-v13`**. They report desired and measured
|
||||
identify **`hypothesis=model-action-curvature-c0-distance-pi-v14`**. They report desired and measured
|
||||
curvature, base heading, proportional and accumulated correction, applied heading,
|
||||
feedback timing and driver/PSCM gating. `proportional_gain=0.75` and
|
||||
`integral_gain=0.25` identify the trial. `offset_overflow` reports the extra C0
|
||||
`integral_gain=1.0` identify the trial. `offset_overflow` reports the extra C0
|
||||
target in meters before C0 amplitude limits. `calibration_approved=false`
|
||||
remains. The retired request/unwind/reversal diagnostic fields are removed.
|
||||
|
||||
|
||||
@@ -18,7 +18,7 @@ from openpilot.selfdrive.controls.lib.ford_path import FordPath, _model_path
|
||||
OFFSET_STATION_M = 7.0
|
||||
HEADING_TIME_S = 1.0
|
||||
C1_PROPORTIONAL_GAIN = 0.75 # Drive-trial gains, not a learned calibration.
|
||||
C1_INTEGRAL_GAIN = 0.25
|
||||
C1_INTEGRAL_GAIN = 1.0
|
||||
CALIBRATION_APPROVED = False
|
||||
|
||||
|
||||
@@ -131,7 +131,7 @@ class FordModelActionController:
|
||||
"""
|
||||
def __init__(self, proportional_gain=C1_PROPORTIONAL_GAIN, integral_gain=C1_INTEGRAL_GAIN, *, c0_time_based=False):
|
||||
self.core = ModelActionController(proportional_gain=proportional_gain, integral_gain=integral_gain, c0_time_based=c0_time_based)
|
||||
self.hypothesis = 'model-action-curvature-c0-distance-pi-v13'
|
||||
self.hypothesis = 'model-action-curvature-c0-distance-pi-v14'
|
||||
self.reset()
|
||||
|
||||
def set_c0_time_based(self, enabled, *, lateral_engaged):
|
||||
|
||||
@@ -53,7 +53,7 @@ class TestFordControlsLogging(unittest.TestCase):
|
||||
controls = SimpleNamespace(ford_path_controller=controller, desired_curvature=.03, curvature=.015,
|
||||
sm=SimpleNamespace(logMonoTime={'modelV2': 123456789, 'carState': 123450000}))
|
||||
record = self.emit_controls_event('Ford C2-free path tracking', controls)
|
||||
self.assertEqual(record['hypothesis'], 'model-action-curvature-c0-distance-pi-v13')
|
||||
self.assertEqual(record['hypothesis'], 'model-action-curvature-c0-distance-pi-v14')
|
||||
self.assertIs(record['calibration_approved'], False)
|
||||
self.assertEqual(record['command'][2:], [0., 0.])
|
||||
self.assertEqual(record['status'], controller.diagnostics['status'])
|
||||
|
||||
@@ -248,9 +248,9 @@ def test_feedback_through_actual_controlsd_publication_and_100hz_sender(pipeline
|
||||
frame = 0
|
||||
# Every fresh error sample integrates within amplitude headroom.
|
||||
# Matched steering removes P and preserves I.
|
||||
for measured, torque, count, expected in [(sign*.004, 0., 100, 0.), (sign*.003, 0., 100, sign*.005),
|
||||
(sign*.004, 0., 100, sign*.005), (sign*.005, 0., 100, 0.),
|
||||
(sign*.003, 0., 100, sign*.005), (0., 1.0625, 5, 0.)]:
|
||||
for measured, torque, count, expected in [(sign*.004, 0., 100, 0.), (sign*.003, 0., 100, sign*.02),
|
||||
(sign*.004, 0., 100, sign*.02), (sign*.005, 0., 100, 0.),
|
||||
(sign*.003, 0., 100, sign*.02), (0., 1.0625, 5, 0.)]:
|
||||
for _ in range(count):
|
||||
now = 1.+frame*.01
|
||||
controls.curvature, cs.steeringTorque = measured, torque
|
||||
@@ -305,7 +305,7 @@ def test_actual_controlsd_passes_only_valid_pscm_service_to_feedback(pipeline, s
|
||||
assert controller.diagnostics['pscm_limited'] is service_valid
|
||||
assert controller.core.proportional == pytest.approx(.015)
|
||||
# All three fresh samples may integrate unless the valid PSCM limit blocks it.
|
||||
assert controller.core.correction == pytest.approx(0. if service_valid else .00015)
|
||||
assert controller.core.correction == pytest.approx(0. if service_valid else .0006)
|
||||
assert cc.latActive and controls.ford_path.valid
|
||||
|
||||
|
||||
@@ -338,7 +338,7 @@ def test_continuous_pi_reversal_through_selected_limited_request_and_actual_can(
|
||||
'lp': SimpleNamespace(roll=0.), 'clip_curvature': clip_curvature,
|
||||
'time': SimpleNamespace(monotonic=lambda now=now: now)})
|
||||
assert_current_request(core, controls.desired_curvature, cs.vEgo)
|
||||
increment = .25*speed*(controls.desired_curvature-controls.curvature)*.01
|
||||
increment = speed*(controls.desired_curvature-controls.curvature)*.01
|
||||
assert abs(core.correction-before[2]) <= abs(increment)+1e-10
|
||||
msg = custom.CarControlSP.new_message()
|
||||
exec(publication, {'self': controls, 'CC_SP': msg})
|
||||
@@ -355,7 +355,7 @@ def test_continuous_pi_reversal_through_selected_limited_request_and_actual_can(
|
||||
assert wire['LatCtlPath_No_Cs'] == calculate_lat_ctl2_checksum(2, frame % 16, packet[1])
|
||||
if frame == 199:
|
||||
assert sign*core.correction < 0. if same_turn else sign*core.correction > 0.
|
||||
assert controls.ford_path_controller.diagnostics['hypothesis'] == 'model-action-curvature-c0-distance-pi-v13'
|
||||
assert controls.ford_path_controller.diagnostics['hypothesis'] == 'model-action-curvature-c0-distance-pi-v14'
|
||||
if same_turn:
|
||||
assert controls.desired_curvature == pytest.approx(sign*.01)
|
||||
assert sign*controls.ford_path.path_angle >= speed*.01 # No old unwind correction left below the new base.
|
||||
@@ -394,7 +394,7 @@ def test_unwind_and_catchup_through_selected_request_and_actual_can(pipeline, si
|
||||
'time': SimpleNamespace(monotonic=lambda now=now: now)})
|
||||
assert_current_request(core, controls.desired_curvature, cs.vEgo)
|
||||
if frame == 129:
|
||||
assert 0. < sign*core.correction < .02
|
||||
assert 0. < sign*core.correction < .08
|
||||
if frame == 130:
|
||||
# Zero measured error removes P, but does not arbitrarily erase I.
|
||||
assert core.correction == before[2]
|
||||
@@ -413,7 +413,7 @@ def test_unwind_and_catchup_through_selected_request_and_actual_can(pipeline, si
|
||||
packet = next(packet for packet in packets if packet[0] == address)
|
||||
assert wire['LatCtlPath_No_Cs'] == calculate_lat_ctl2_checksum(2, frame % 16, packet[1])
|
||||
assert controls.ford_path.path_angle == pytest.approx(core.correction, abs=.00025)
|
||||
assert 0. < sign*core.correction < .02
|
||||
assert 0. < sign*core.correction < .08
|
||||
assert controls.ford_path.path_offset == pytest.approx(0.)
|
||||
|
||||
|
||||
|
||||
@@ -47,8 +47,8 @@ def test_actual_startup_priority(candidate, observer, fingerprint):
|
||||
if candidate:
|
||||
assert type(selected.ford_path_controller) is FordModelActionController
|
||||
assert selected.ford_path_controller.core.proportional_gain == C1_PROPORTIONAL_GAIN == .75
|
||||
assert selected.ford_path_controller.core.integral_gain == C1_INTEGRAL_GAIN == .25
|
||||
assert selected.ford_path_controller.diagnostics['hypothesis'] == 'model-action-curvature-c0-distance-pi-v13'
|
||||
assert selected.ford_path_controller.core.integral_gain == C1_INTEGRAL_GAIN == 1.
|
||||
assert selected.ford_path_controller.diagnostics['hypothesis'] == 'model-action-curvature-c0-distance-pi-v14'
|
||||
else:
|
||||
assert selected.ford_path_controller is None
|
||||
assert selected.ford_model_action == candidate
|
||||
|
||||
@@ -7,17 +7,19 @@ from openpilot.selfdrive.controls.lib.ford_model_action import ModelActionContro
|
||||
|
||||
|
||||
@pytest.mark.parametrize('sign', [-1., 1.])
|
||||
def test_existing_integral_unwinds_in_current_output(sign):
|
||||
core = ModelActionController(.5, .25)
|
||||
@pytest.mark.parametrize('ki', [.25, 1.])
|
||||
def test_existing_integral_unwinds_in_current_output(sign, ki):
|
||||
core = ModelActionController(.75, ki)
|
||||
core.c1, core.correction = sign*.07, sign*.03
|
||||
core.update(straight(), sign*.002, current_curvature=sign*.004, speed=20., dt=.01)
|
||||
assert core.correction == pytest.approx(sign*.0299)
|
||||
assert core.c1 == pytest.approx(sign*.0499)
|
||||
assert core.correction == pytest.approx(sign*(.03-.0004*ki))
|
||||
assert core.c1 == pytest.approx(sign*(.04-.0004*ki))
|
||||
|
||||
|
||||
@pytest.mark.parametrize('sign', [-1., 1.])
|
||||
def test_unwinding_cannot_charge_opposite_correction_beyond_amplitude_limit(sign):
|
||||
core = ModelActionController(.5, .25)
|
||||
@pytest.mark.parametrize('ki', [.25, 1.])
|
||||
def test_unwinding_cannot_charge_opposite_correction_beyond_amplitude_limit(sign, ki):
|
||||
core = ModelActionController(.75, ki)
|
||||
core.c1, core.correction = sign*.07, sign*.03
|
||||
core.update(straight(), 0., current_curvature=sign*.5, speed=20., dt=.01, feedback_dt=.15)
|
||||
assert core.correction == 0.
|
||||
@@ -35,8 +37,9 @@ def test_integral_gain_scales_fresh_error_only(ki):
|
||||
assert core.correction == before
|
||||
|
||||
|
||||
def test_zero_error_does_not_erase_holding_correction():
|
||||
core = ModelActionController(.5, .25)
|
||||
@pytest.mark.parametrize('ki', [.25, 1.])
|
||||
def test_zero_error_does_not_erase_holding_correction(ki):
|
||||
core = ModelActionController(.75, ki)
|
||||
core.c1, core.correction = .14, .1
|
||||
for _ in range(50):
|
||||
core.update(straight(), .002, current_curvature=.002, speed=20., dt=.01)
|
||||
@@ -69,8 +72,9 @@ def test_centering_cannot_gate_continuous_heading_correction(sign):
|
||||
@pytest.mark.parametrize('sign', [-1., 1.])
|
||||
@pytest.mark.parametrize('limited', [False, True])
|
||||
@pytest.mark.parametrize('gain', [.5, .75])
|
||||
def test_duplicate_measurements_cannot_retire_integral(sign, limited, gain):
|
||||
core = ModelActionController(gain, .25)
|
||||
@pytest.mark.parametrize('ki', [.25, 1.])
|
||||
def test_duplicate_measurements_cannot_retire_integral(sign, limited, gain, ki):
|
||||
core = ModelActionController(gain, ki)
|
||||
core.c1, core.correction = sign*.07, sign*.03
|
||||
core.update(straight(), -sign*.002, current_curvature=sign*.004, speed=20., dt=.01,
|
||||
feedback_dt=0., pscm_limited=limited)
|
||||
|
||||
@@ -1,4 +1,4 @@
|
||||
"""Compare P gains using production adapters and fixed recorded motion.
|
||||
"""Compare explicit PI gains using production adapters and fixed recorded motion.
|
||||
|
||||
This checks command behavior, not counterfactual tracking or stability. Original
|
||||
selected curvature is retained, including each route's original model/delay.
|
||||
@@ -21,6 +21,7 @@ from tools.ford_pscm_lab.model_action_replay import WireCheck, sample, table
|
||||
|
||||
|
||||
GAINS = (.50, .75)
|
||||
DEFAULT_SETTINGS = tuple((kp, .25) for kp in GAINS)
|
||||
DIAGNOSTICS = ('heading_feedforward', 'heading_proportional', 'heading_correction',
|
||||
'feedback_enabled', 'pscm_limited', 'driver_override', 'offset_overflow')
|
||||
|
||||
@@ -31,7 +32,9 @@ def describe(values, mask):
|
||||
'max': float(np.max(values))} if len(values) else None
|
||||
|
||||
|
||||
def replay(route, output):
|
||||
def replay(route, output, settings=DEFAULT_SETTINGS):
|
||||
if len(settings) < 2 or any(len(pair) != 2 or not np.isfinite(pair).all() or min(pair) < 0. for pair in settings):
|
||||
raise ValueError('Provide at least two finite, nonnegative P:I pairs')
|
||||
label, source = route.split('=', 1)
|
||||
directory = Path(source).resolve()
|
||||
destination = output.resolve()/label
|
||||
@@ -58,11 +61,11 @@ def replay(route, output):
|
||||
& cs['can_valid'].astype(bool) & pa['valid'].astype(bool) & exact
|
||||
& r['model']['valid'][mi].astype(bool) & (abs(cc['t']-t) < .005)
|
||||
& (t-pa['t'] >= 0.) & (t-pa['t'] <= .15))
|
||||
controllers = [FordModelActionController(proportional_gain=kp, integral_gain=.25, c0_time_based=False) for kp in GAINS]
|
||||
commands = np.zeros((2, len(t), 4))
|
||||
valid = np.zeros((2, len(t)), bool)
|
||||
diagnostics = np.zeros((2, len(t), len(DIAGNOSTICS)))
|
||||
reasons = [Counter(), Counter()]
|
||||
controllers = [FordModelActionController(proportional_gain=kp, integral_gain=ki, c0_time_based=False) for kp, ki in settings]
|
||||
commands = np.zeros((len(settings), len(t), 4))
|
||||
valid = np.zeros((len(settings), len(t)), bool)
|
||||
diagnostics = np.zeros((len(settings), len(t), len(DIAGNOSTICS)))
|
||||
reasons = [Counter() for _ in settings]
|
||||
wire = WireCheck()
|
||||
for i, now in enumerate(t):
|
||||
status = SimpleNamespace(valid=bool(ps['valid'][i] and ps['status_valid'][i]), canMonoTime=round(ps['stamp'][i]*1e9),
|
||||
@@ -82,11 +85,14 @@ def replay(route, output):
|
||||
assert np.isfinite(commands).all() and np.isfinite(diagnostics).all()
|
||||
assert (abs(commands[:, :, :2]) <= [5.1100001, .5000001]).all()
|
||||
assert (commands[:, :, 2:] == 0.).all() and (commands[~valid] == 0.).all()
|
||||
np.testing.assert_array_equal(valid[0], valid[1])
|
||||
np.testing.assert_array_equal(commands[0, :, 0], commands[1, :, 0])
|
||||
np.testing.assert_array_equal(diagnostics[0, :, [0, 3, 4, 5, 6]], diagnostics[1, :, [0, 3, 4, 5, 6]])
|
||||
np.testing.assert_allclose(diagnostics[1, :, 1], 1.5*diagnostics[0, :, 1], rtol=1e-12, atol=1e-12)
|
||||
assert reasons[0] == reasons[1]
|
||||
for k, (kp, _) in enumerate(settings):
|
||||
np.testing.assert_array_equal(valid[0], valid[k])
|
||||
np.testing.assert_array_equal(commands[0, :, 0], commands[k, :, 0])
|
||||
np.testing.assert_array_equal(diagnostics[0, :, [0, 3, 4, 5, 6]], diagnostics[k, :, [0, 3, 4, 5, 6]])
|
||||
expected_p = kp*np.maximum(7., cs['speed'])*(c['desired']-c['measured'])*diagnostics[k, :, 3]
|
||||
np.testing.assert_allclose(diagnostics[k, :, 1], expected_p, rtol=1e-12, atol=1e-12)
|
||||
assert (diagnostics[k, diagnostics[k, :, 3] == 0., 1:3] == 0.).all()
|
||||
assert reasons[0] == reasons[k]
|
||||
|
||||
driver = (cs['pressed'] > 0.) | (abs(cs['torque']) > 1.) | ((ps['status_valid'] > 0) & (ps['limit'] == 3))
|
||||
bad = driver | ~valid[0] | (diagnostics[0, :, 3] == 0.)
|
||||
@@ -106,18 +112,22 @@ def replay(route, output):
|
||||
cohorts = {}
|
||||
for name, mask in masks.items():
|
||||
cohorts[name] = {'seconds': float(weights[mask].sum()), 'abs_c1_change': describe(abs(change), mask),
|
||||
'settings': [{'kp': kp, 'c1_bound_seconds': float(weights[mask & (abs(commands[k, :, 1]) >= .4995)].sum()),
|
||||
'settings': [{'kp': kp, 'ki': ki,
|
||||
'abs_c1_change_from_baseline': describe(abs(commands[k, :, 1]-commands[0, :, 1]), mask),
|
||||
'c1_bound_seconds': float(weights[mask & (abs(commands[k, :, 1]) >= .4995)].sum()),
|
||||
'abs_c1': describe(abs(commands[k, :, 1]), mask),
|
||||
'abs_integral': describe(abs(diagnostics[k, :, 2]), mask)} for k, kp in enumerate(GAINS)]}
|
||||
'abs_integral': describe(abs(diagnostics[k, :, 2]), mask)} for k, (kp, ki) in enumerate(settings)]}
|
||||
paired = clean[1:] & clean[:-1]
|
||||
step = np.diff(commands[:, :, 1], axis=1)
|
||||
step_metrics = [{'kp': kp, 'abs_c1_per_cycle_change': describe(abs(step[k]), paired)} for k, kp in enumerate(GAINS)]
|
||||
step_metrics = [{'kp': kp, 'ki': ki, 'abs_c1_per_cycle_change': describe(abs(step[k]), paired)} for k, (kp, ki) in enumerate(settings)]
|
||||
rec = sample(r['path'], t, nearest=True)
|
||||
rec_mask = clean & (rec['valid'] > 0) & (abs(rec['t']-t) < .005)
|
||||
baseline_comparison = {name: describe(abs(commands[0, :, idx]-rec[name]), rec_mask) for idx, name in enumerate(('c0', 'c1'))}
|
||||
sources = (directory/'route.npz', directory/'model_paths.npz', directory/'metadata.json',
|
||||
Path(__file__).resolve(), Path(ford_model_action.__file__).resolve())
|
||||
report = {'scope': __doc__, 'route': label, 'cycles': len(t), 'gains': list(GAINS), 'ki': .25,
|
||||
report = {'scope': __doc__, 'route': label, 'cycles': len(t), 'gains': [kp for kp, _ in settings],
|
||||
'settings': [{'kp': kp, 'ki': ki} for kp, ki in settings],
|
||||
'ki': settings[0][1] if len({ki for _, ki in settings}) == 1 else None,
|
||||
'can_round_trips': wire.count, 'validity_and_c0_and_gates_identical': True,
|
||||
'status_counts': dict(reasons[0]), 'cohorts': cohorts, 'per_cycle_changes': step_metrics,
|
||||
'baseline_difference_from_recorded_path': baseline_comparison,
|
||||
@@ -126,6 +136,7 @@ def replay(route, output):
|
||||
destination.mkdir(parents=True, exist_ok=True)
|
||||
(destination/'report.json').write_text(json.dumps(report, indent=2, allow_nan=False)+'\n')
|
||||
np.savez_compressed(destination/'commands.npz', t=t-metadata['t0'], commands=commands, valid=valid,
|
||||
settings=np.array(settings),
|
||||
diagnostics=diagnostics, diagnostic_names=DIAGNOSTICS, clean=clean,
|
||||
driver=driver, speed=cs['speed'], desired=c['desired'], measured=c['measured'])
|
||||
return report
|
||||
@@ -136,12 +147,19 @@ if __name__ == '__main__':
|
||||
parser.add_argument('--routes', nargs='+', required=True, help='label=extract-directory pairs')
|
||||
parser.add_argument('--output', type=Path, required=True)
|
||||
parser.add_argument('--workers', type=int, default=4)
|
||||
parser.add_argument('--settings', nargs='+', help='Explicit P:I pairs; defaults to 0.50:0.25 and 0.75:0.25')
|
||||
args = parser.parse_args()
|
||||
try:
|
||||
settings = tuple(tuple(float(v) for v in pair.split(':')) for pair in args.settings) if args.settings else DEFAULT_SETTINGS
|
||||
if len(settings) < 2 or any(len(pair) != 2 or not np.isfinite(pair).all() or min(pair) < 0. for pair in settings):
|
||||
raise ValueError
|
||||
except ValueError:
|
||||
parser.error('Settings require at least two finite, nonnegative P:I pairs')
|
||||
labels = [route.split('=', 1)[0] for route in args.routes]
|
||||
if len(set(labels)) != len(labels) or any(Path(label).name != label or label in ('.', '..') for label in labels):
|
||||
parser.error('Route labels must be unique directory names')
|
||||
with ProcessPoolExecutor(max_workers=args.workers) as pool:
|
||||
jobs = {pool.submit(replay, route, args.output): route for route in args.routes}
|
||||
jobs = {pool.submit(replay, route, args.output, settings): route for route in args.routes}
|
||||
for job in as_completed(jobs):
|
||||
result = job.result()
|
||||
print(json.dumps({'route': result['route'], 'cycles': result['cycles'], 'can_round_trips': result['can_round_trips'],
|
||||
|
||||
Reference in New Issue
Block a user