From ef720213255fe9dc986180032151d0e39a3599de Mon Sep 17 00:00:00 2001 From: jjackbrandt Date: Wed, 22 Jul 2026 10:35:52 -0400 Subject: [PATCH 1/4] fix(tesla): keep grade control in net acceleration domain --- .../ci/tesla_preap_longitudinal_mutations.py | 97 ++++++ .github/ci/test_tests_workflow.py | 4 + .github/workflows/tests.yml | 1 + opendbc/car/tesla/preap/carcontroller.py | 28 +- .../tesla/preap/tests/test_accel_limits.py | 3 +- .../tesla/preap/tests/test_pedal_authority.py | 13 +- .../preap/tests/test_vdas_grade_control.py | 302 ++++++++++++++++++ .../car/tesla/preap/tests/test_virtual_das.py | 94 +++++- opendbc/car/tesla/preap/virtual_das.py | 76 +++-- 9 files changed, 569 insertions(+), 49 deletions(-) create mode 100644 opendbc/car/tesla/preap/tests/test_vdas_grade_control.py diff --git a/.github/ci/tesla_preap_longitudinal_mutations.py b/.github/ci/tesla_preap_longitudinal_mutations.py index eb9d343f4b3..b35165c98b5 100644 --- a/.github/ci/tesla_preap_longitudinal_mutations.py +++ b/.github/ci/tesla_preap_longitudinal_mutations.py @@ -137,6 +137,103 @@ class HistoricalMutation: "test_inner_feedback_holds_cruise_against_sustained_road_load" ), ), + HistoricalMutation( + name="steady-grade-removed-from-effort", + source_path="opendbc/car/tesla/preap/virtual_das.py", + original=( + b" a_limited + steady_grade_compensation + transient_pitch_compensation,\n" + ), + replacement=b" a_limited + transient_pitch_compensation,\n", + test_node=( + "opendbc/car/tesla/preap/tests/test_vdas_grade_control.py::" + + "test_preserved_grade_handoff_holds_net_acceleration_for_1p5_seconds[0.5-15.0]" + ), + ), + HistoricalMutation( + name="steady-grade-subtracted-from-net-feedback", + source_path="opendbc/car/tesla/preap/virtual_das.py", + original=( + b" a_ego_filtered = self.a_ego_filter.update(a_ego)\n" + + b" self.a_ego_initialized = True\n" + ), + replacement=( + b" a_ego_corrected = a_ego - steady_grade_compensation\n" + + b" a_ego_filtered = self.a_ego_filter.update(a_ego_corrected)\n" + + b" self.a_ego_initialized = True\n" + ), + test_node=( + "opendbc/car/tesla/preap/tests/test_vdas_grade_control.py::" + + "test_steady_grade_hold_does_not_double_compensate[0.5-15.0]" + ), + ), + HistoricalMutation( + name="engage-grade-bypasses-effort-envelope", + source_path="opendbc/car/tesla/preap/virtual_das.py", + original=b" effort_min, effort_max = accel_effort_limits or (REGEN_MAX, ACCEL_MAX)\n", + replacement=b" effort_min, effort_max = REGEN_MAX, ACCEL_MAX\n", + test_node=( + "opendbc/car/tesla/preap/tests/test_virtual_das.py::TestVDASDomainBoundaries::" + + "test_engage_effort_limits_include_grade_compensation" + ), + ), + HistoricalMutation( + name="engage-grade-bypasses-pedal-slew-envelope", + source_path="opendbc/car/tesla/preap/virtual_das.py", + original=( + b" pedal_di = self._rate_limit(pedal_di_bounded, prev_pedal_di, pedal_ramp_rate_up)\n" + ), + replacement=b" pedal_di = self._rate_limit(pedal_di_bounded, prev_pedal_di)\n", + test_node=( + "opendbc/car/tesla/preap/tests/test_virtual_das.py::TestVDASDomainBoundaries::" + + "test_engage_pedal_ramp_limit_applies_after_feedforward" + ), + ), + HistoricalMutation( + name="engage-slew-stops-before-pedal-catches-up", + source_path="opendbc/car/tesla/preap/carcontroller.py", + original=( + b" pedal_ramp_rate_up = (\n" + + b" ENGAGE_GRACE_PEDAL_RAMP_RATE_UP\n" + + b" if self.preap_long_handoff_slew_active\n" + + b" else PEDAL_RAMP_RATE_UP\n" + + b" )\n" + ), + replacement=b" pedal_ramp_rate_up = PEDAL_RAMP_RATE_UP\n", + test_node=( + "opendbc/car/tesla/preap/tests/test_pedal_authority.py::" + + "test_non_timeout_gas_override_release_has_no_launch_for_1p5_seconds" + ), + ), + HistoricalMutation( + name="full-strength-transient-grade-overshoot", + source_path="opendbc/car/tesla/preap/virtual_das.py", + original=b"TRANSIENT_GRADE_GAIN = 0.4\n", + replacement=b"TRANSIENT_GRADE_GAIN = 1.0\n", + test_node=( + "opendbc/car/tesla/preap/tests/test_vdas_grade_control.py::" + + "test_flat_to_grade_step_improves_on_no_pitch_baseline_at_1p5_seconds[0.4]" + ), + ), + HistoricalMutation( + name="steady-grade-pitch-outlier-unbounded", + source_path="opendbc/car/tesla/preap/virtual_das.py", + original=b" pitch = float(clip(orientation_ned[1], -maximum_pitch, maximum_pitch))\n", + replacement=b" pitch = orientation_ned[1]\n", + test_node=( + "opendbc/car/tesla/preap/tests/test_virtual_das.py::TestGradeEstimator::" + + "test_sustained_pitch_outlier_cannot_exceed_steady_grade_limit" + ), + ), + HistoricalMutation( + name="orientation-dropout-zeroes-grade-effort", + source_path="opendbc/car/tesla/preap/virtual_das.py", + original=b" return self._steady_grade_compensation(), 0.0\n", + replacement=b" return 0.0, 0.0\n", + test_node=( + "opendbc/car/tesla/preap/tests/test_virtual_das.py::TestGradeEstimator::" + + "test_orientation_dropout_holds_filtered_steady_grade" + ), + ), HistoricalMutation( name="focused-job-skip-bypass", source_path=".github/workflows/tests.yml", diff --git a/.github/ci/test_tests_workflow.py b/.github/ci/test_tests_workflow.py index f61121f21ce..aac901554a5 100644 --- a/.github/ci/test_tests_workflow.py +++ b/.github/ci/test_tests_workflow.py @@ -51,8 +51,12 @@ def test_focused_longitudinal_tests_are_pinned(self): focused_job = indented_block(self.workflow, " tesla_preap_longitudinal_regression:") required_test_paths = ( + "opendbc/car/tesla/preap/tests/test_pedal_authority.py", "opendbc/car/tesla/preap/tests/test_longitudinal_tuning.py", "opendbc/car/tesla/preap/tests/test_virtual_das.py", + "opendbc/car/tesla/preap/tests/test_vdas_grade_control.py", + "opendbc/car/tesla/preap/tests/test_accel_limits.py", + "opendbc/car/tesla/preap/tests/test_engage_grace.py", ) for test_path in required_test_paths: self.assertIn(test_path, normalized_lines(focused_job)) diff --git a/.github/workflows/tests.yml b/.github/workflows/tests.yml index d09a4f0834a..b0e7b552887 100644 --- a/.github/workflows/tests.yml +++ b/.github/workflows/tests.yml @@ -45,6 +45,7 @@ jobs: pytest -q -n 0 \ opendbc/car/tesla/preap/tests/test_pedal_authority.py \ opendbc/car/tesla/preap/tests/test_virtual_das.py \ + opendbc/car/tesla/preap/tests/test_vdas_grade_control.py \ opendbc/car/tesla/preap/tests/test_accel_limits.py \ opendbc/car/tesla/preap/tests/test_engage_grace.py \ opendbc/car/tesla/preap/tests/test_longitudinal_tuning.py diff --git a/opendbc/car/tesla/preap/carcontroller.py b/opendbc/car/tesla/preap/carcontroller.py index 217eccaa69a..5f7b2bc4adc 100644 --- a/opendbc/car/tesla/preap/carcontroller.py +++ b/opendbc/car/tesla/preap/carcontroller.py @@ -6,7 +6,7 @@ from opendbc.car import Bus from opendbc.car.tesla.preap.nap_conf import nap_conf, PEDAL_DI_MIN from opendbc.car.tesla.preap.interface import get_preap_accel_limits -from opendbc.car.tesla.pedal.controller import get_zero_torque +from opendbc.car.tesla.pedal.controller import get_zero_torque, PEDAL_RAMP_RATE_UP from opendbc.car.tesla.preap.virtual_das import VirtualDAS from opendbc.car.tesla.preap.teslacan import TeslaCANPreAP from opendbc.car.tesla.values import CANBUS, CruiseButtons @@ -24,6 +24,7 @@ def init_preap_can(dbc_names, packers): # Prevents both regen spike (negative) and pedal stab (MPC requesting high # positive accel on frame 1). Inspired by Tinkla's proportional ramp. ENGAGE_GRACE_FRAMES = 50 # 0.5s at 100Hz +ENGAGE_GRACE_PEDAL_RAMP_RATE_UP = 0.9 # DI/update at 50Hz # The pedal controller updates at 50 Hz. Only prompt after the regen rail has # been near its physical limit while measured deceleration trails the shaped @@ -174,6 +175,7 @@ def __init__(self): # Snapshot of max-accel-at-engage-speed; used as the deterministic # ceiling for the grace-period ramp. Set fresh on each engage rising edge. self.engage_a_max = 0.0 + self.preap_long_handoff_slew_active = False self.vdas = VirtualDAS(dt=0.02) self.pedal_authority = PedalAuthority() self.regen_decel_monitor = RegenDecelMonitor() @@ -245,6 +247,7 @@ def update(self, CC, CS, frame, tesla_can, can_bus_party, now_nanos=0): if pedal_action == PedalCommandAction.ACQUIRE: self.preap_long_engage_frame = frame + self.preap_long_handoff_slew_active = True zero_torque_di = get_zero_torque().get(CS.out.vEgo) self.prev_pedal_di = max(CS.pedal_interceptor_value, zero_torque_di) self.vdas.reset( @@ -269,11 +272,13 @@ def update(self, CC, CS, frame, tesla_can, can_bus_party, now_nanos=0): if pedal_action == PedalCommandAction.RESET: self._append_pedal_command(can_sends, CS, tesla_can.create_pedal_command(0, enable=0)) + self.preap_long_handoff_slew_active = False self.regen_decel_monitor.reset() elif pedal_action == PedalCommandAction.RELEASE: self._append_pedal_command(can_sends, CS, tesla_can.create_pedal_command(0, enable=0)) self.prev_pedal_di = 0.0 + self.preap_long_handoff_slew_active = False self.regen_decel_monitor.reset() elif pedal_action in (PedalCommandAction.ACQUIRE, PedalCommandAction.ENABLE): @@ -281,6 +286,12 @@ def update(self, CC, CS, frame, tesla_can, can_bus_party, now_nanos=0): engage_elapsed_frames = frame - self.preap_long_engage_frame in_engage_grace = engage_elapsed_frames < ENGAGE_GRACE_FRAMES accel_request = float(actuators.accel) + accel_effort_limits = None + pedal_ramp_rate_up = ( + ENGAGE_GRACE_PEDAL_RAMP_RATE_UP + if self.preap_long_handoff_slew_active + else PEDAL_RAMP_RATE_UP + ) if in_engage_grace: # Cap at grace_progress * engage_a_max so the ceiling is the # tuned accel-profile envelope, not the live MPC request. @@ -288,11 +299,22 @@ def update(self, CC, CS, frame, tesla_can, can_bus_party, now_nanos=0): grace_progress = engage_elapsed_frames / ENGAGE_GRACE_FRAMES accel_cap = grace_progress * self.engage_a_max accel_request = max(0.0, min(accel_request, accel_cap)) + accel_effort_limits = (0.0, accel_cap) + pedal_ramp_rate_up = ENGAGE_GRACE_PEDAL_RAMP_RATE_UP self.prev_pedal_di = self.vdas.update( accel_request, CS.out.vEgo, self.prev_pedal_di, a_ego=CS.out.aEgo, freeze_integrator=in_engage_grace, - orientation_ned=list(CC.orientationNED)) + orientation_ned=list(CC.orientationNED), + accel_effort_limits=accel_effort_limits, + pedal_ramp_rate_up=pedal_ramp_rate_up) + handoff_slew_complete = ( + self.preap_long_handoff_slew_active + and not in_engage_grace + and not self.vdas.pedal_ramp_limited_up + ) + if handoff_slew_complete: + self.preap_long_handoff_slew_active = False pedal_cmd = nap_conf.di_to_pedal(self.prev_pedal_di) command = tesla_can.create_pedal_command(pedal_cmd, enable=1) self._append_pedal_command(can_sends, CS, command) @@ -313,12 +335,14 @@ def update(self, CC, CS, frame, tesla_can, can_bus_party, now_nanos=0): self._handle_pedal_unavailable(CS) self._append_pedal_command(can_sends, CS, tesla_can.create_pedal_command(0, enable=0)) self.prev_pedal_di = 0.0 + self.preap_long_handoff_slew_active = False self.regen_decel_monitor.reset() pedal_action = PedalCommandAction.FAILURE elif pedal_action == PedalCommandAction.FAILURE: carlog.error("Pre-AP pedal authority acquisition failed") self._handle_pedal_unavailable(CS) + self.preap_long_handoff_slew_active = False self.regen_decel_monitor.reset() else: diff --git a/opendbc/car/tesla/preap/tests/test_accel_limits.py b/opendbc/car/tesla/preap/tests/test_accel_limits.py index deef81d6215..2e959b50e2d 100644 --- a/opendbc/car/tesla/preap/tests/test_accel_limits.py +++ b/opendbc/car/tesla/preap/tests/test_accel_limits.py @@ -77,7 +77,8 @@ def test_highway_speed_step_bounds_pedal_command_rise(monkeypatch): ) pedal_commands.append(pedal_di) - assert max(pedal_commands) - steady_pedal_di <= 7.0 + pedal_rise_di = max(pedal_commands) - steady_pedal_di + assert 1.0 <= pedal_rise_di <= 7.0 @pytest.mark.parametrize( diff --git a/opendbc/car/tesla/preap/tests/test_pedal_authority.py b/opendbc/car/tesla/preap/tests/test_pedal_authority.py index b9189ed3bb5..81542e4de14 100644 --- a/opendbc/car/tesla/preap/tests/test_pedal_authority.py +++ b/opendbc/car/tesla/preap/tests/test_pedal_authority.py @@ -570,7 +570,8 @@ def test_engage_grace_starts_on_actual_long_active_rising(controller_env): @pytest.mark.parametrize("engage_a_max", (0.8, 0.9, 1.0)) -def test_non_timeout_gas_override_release_has_no_launch(controller_env, monkeypatch, engage_a_max): +def test_non_timeout_gas_override_release_has_no_launch_for_1p5_seconds( + controller_env, monkeypatch, engage_a_max): controller, cc, cs, tesla_can = controller_env monkeypatch.setattr( 'opendbc.car.tesla.preap.carcontroller.get_preap_accel_limits', @@ -585,9 +586,11 @@ def test_non_timeout_gas_override_release_has_no_launch(controller_env, monkeypa cc.actuators.accel = 0.67 release_commands = [] - for frame in range(460, 510, 2): + limited_acceleration_by_frame = {} + for frame in range(460, 612, 2): commands = controller.update(cc, cs, frame=frame, tesla_can=tesla_can, can_bus_party=0) release_commands.extend(_decode_pedal_command(command) for command in commands) + limited_acceleration_by_frame[frame] = controller.vdas.jerk_limiter.a_limited assert release_commands assert all(command.enabled for command in release_commands) @@ -596,8 +599,10 @@ def test_non_timeout_gas_override_release_has_no_launch(controller_env, monkeypa for previous, current in zip(release_commands, release_commands[1:], strict=False) ] assert release_commands[0].command < 0.1 - assert max(command_steps) < 1.0 - assert controller.vdas.jerk_limiter.a_limited < 0.4 + assert max(command_steps) < 1.0, command_steps + assert limited_acceleration_by_frame[508] < 0.4 + assert limited_acceleration_by_frame[610] == pytest.approx(cc.actuators.accel) + assert not controller.preap_long_handoff_slew_active def test_gas_override_timeout_rearm_has_no_launch(controller_env): diff --git a/opendbc/car/tesla/preap/tests/test_vdas_grade_control.py b/opendbc/car/tesla/preap/tests/test_vdas_grade_control.py new file mode 100644 index 00000000000..4a460bbf235 --- /dev/null +++ b/opendbc/car/tesla/preap/tests/test_vdas_grade_control.py @@ -0,0 +1,302 @@ +"""Closed-loop grade contracts for the Pre-AP acceleration controller.""" + +import math +from dataclasses import dataclass, replace +from types import SimpleNamespace + +import pytest + +from opendbc.car.tesla.preap import virtual_das +from opendbc.car.tesla.preap.virtual_das import GRAVITY, VirtualDAS + + +CONTROL_DT_S = 0.02 +PLANT_DELAY_S = 0.40 +PLANT_TAU_S = 0.25 +GRADE_ESTIMATOR_SETTLING_S = 6.0 +SHORT_HORIZON_S = 1.5 +STEADY_HORIZON_S = 15.0 +STEADY_WINDOW_S = 2.0 +GRADE_ACCEL_MPS2 = 0.50 +NET_ACCEL_TOLERANCE_MPS2 = 0.12 +SHORT_HORIZON_SPEED_DRIFT_MPS = 0.10 +EFFORT_TOLERANCE_MPS2 = 0.12 +COAST_PEDAL_DI = 3.0 +STEP_GRADE_ACCEL_MPS2 = 0.40 +MATRIX_GRADE_ACCEL_MPS2 = 0.30 +STEP_RESPONSE_TOLERANCE_MPS2 = 0.30 +STEP_RESPONSE_MIN_DRIFT_IMPROVEMENT_MPS = 0.07 +STEP_RESPONSE_MIN_PEDAL_DELTA_DI = 2.0 +STEADY_MEAN_TOLERANCE_MPS2 = 0.12 +STEADY_PEAK_TOLERANCE_MPS2 = 0.18 +STEADY_SPEED_DRIFT_MPS = 2.5 +PHYSICAL_RAIL_MARGIN_DI = 0.25 + + +@dataclass(frozen=True) +class GradePlantSample: + net_acceleration_mps2: float + speed_mps: float + acceleration_effort_mps2: float + integral_trim_mps2: float + + +@dataclass(frozen=True) +class DelayedPedalPlantCase: + delay_s: float + tau_s: float + acceleration_per_di_mps2: float + grade_sensor_scale: float + + +NOMINAL_PLANT = DelayedPedalPlantCase(0.40, 0.25, 0.063, 1.0) +UNCERTAIN_PLANTS = ( + NOMINAL_PLANT, + DelayedPedalPlantCase(0.50, 0.35, 0.0567, 0.90), + DelayedPedalPlantCase(0.30, 0.20, 0.0693, 1.10), +) + + +@dataclass(frozen=True) +class PedalPlantSample: + elapsed_s: float + net_acceleration_mps2: float + speed_delta_mps: float + pedal_di: float + + +def run_grade_hold(*, speed_mps: float, grade_acceleration_mps2: float, + duration_s: float, monkeypatch) -> list[GradePlantSample]: + monkeypatch.setattr( + virtual_das, + "nap_conf", + SimpleNamespace(get_pedal_profile_values=lambda: [50.0] * len(virtual_das.PEDAL_BP)), + ) + + controller = VirtualDAS(dt=CONTROL_DT_S) + acceleration_effort_mps2 = grade_acceleration_mps2 + monkeypatch.setattr( + controller, + "_feedforward", + lambda requested_effort_mps2, _speed_mps: requested_effort_mps2, + ) + + orientation_ned = [0.0, math.asin(grade_acceleration_mps2 / GRAVITY), 0.0] + for _ in range(round(GRADE_ESTIMATOR_SETTLING_S / CONTROL_DT_S)): + controller.observe(a_ego=0.0, orientation_ned=orientation_ned) + + controller.reset( + measured_accel=0.0, + commanded_accel=0.0, + pedal_di_init=acceleration_effort_mps2, + preserve_grade=True, + ) + + delayed_efforts_mps2 = [acceleration_effort_mps2] * round(PLANT_DELAY_S / CONTROL_DT_S) + plant_alpha = CONTROL_DT_S / (PLANT_TAU_S + CONTROL_DT_S) + net_acceleration_mps2 = 0.0 + current_speed_mps = speed_mps + samples = [] + + for _ in range(round(duration_s / CONTROL_DT_S)): + acceleration_effort_mps2 = controller.update( + 0.0, + v_ego=current_speed_mps, + prev_pedal_di=acceleration_effort_mps2, + a_ego=net_acceleration_mps2, + freeze_integrator=False, + orientation_ned=orientation_ned, + ) + applied_effort_mps2 = delayed_efforts_mps2.pop(0) + delayed_efforts_mps2.append(acceleration_effort_mps2) + plant_target_acceleration_mps2 = applied_effort_mps2 - grade_acceleration_mps2 + net_acceleration_mps2 += plant_alpha * ( + plant_target_acceleration_mps2 - net_acceleration_mps2 + ) + current_speed_mps += net_acceleration_mps2 * CONTROL_DT_S + samples.append(GradePlantSample( + net_acceleration_mps2, + current_speed_mps, + acceleration_effort_mps2, + controller.inner_pid.i, + )) + + return samples + + +@pytest.mark.parametrize("speed_mps", [0.0, 5.0, 15.0, 30.0]) +@pytest.mark.parametrize("grade_acceleration_mps2", [GRADE_ACCEL_MPS2, -GRADE_ACCEL_MPS2]) +def test_preserved_grade_handoff_holds_net_acceleration_for_1p5_seconds( + monkeypatch, speed_mps, grade_acceleration_mps2): + samples = run_grade_hold( + speed_mps=speed_mps, + grade_acceleration_mps2=grade_acceleration_mps2, + duration_s=SHORT_HORIZON_S, + monkeypatch=monkeypatch, + ) + + maximum_net_acceleration_mps2 = max(abs(sample.net_acceleration_mps2) for sample in samples) + speed_drift_mps = samples[-1].speed_mps - speed_mps + final_effort_mps2 = samples[-1].acceleration_effort_mps2 + + assert maximum_net_acceleration_mps2 <= NET_ACCEL_TOLERANCE_MPS2 + assert abs(speed_drift_mps) <= SHORT_HORIZON_SPEED_DRIFT_MPS + assert final_effort_mps2 == pytest.approx( + grade_acceleration_mps2, + abs=EFFORT_TOLERANCE_MPS2, + ) + + +@pytest.mark.parametrize("speed_mps", [0.0, 5.0, 15.0, 30.0]) +@pytest.mark.parametrize("grade_acceleration_mps2", [GRADE_ACCEL_MPS2, -GRADE_ACCEL_MPS2]) +def test_steady_grade_hold_does_not_double_compensate( + monkeypatch, speed_mps, grade_acceleration_mps2): + samples = run_grade_hold( + speed_mps=speed_mps, + grade_acceleration_mps2=grade_acceleration_mps2, + duration_s=STEADY_HORIZON_S, + monkeypatch=monkeypatch, + ) + steady_sample_count = round(STEADY_WINDOW_S / CONTROL_DT_S) + steady_samples = samples[-steady_sample_count:] + + assert max(abs(sample.net_acceleration_mps2) for sample in steady_samples) <= NET_ACCEL_TOLERANCE_MPS2 + assert max( + abs(sample.acceleration_effort_mps2 - grade_acceleration_mps2) + for sample in steady_samples + ) <= EFFORT_TOLERANCE_MPS2 + assert max(abs(sample.integral_trim_mps2) for sample in steady_samples) <= 0.02 + + +def run_grade_step(*, speed_mps: float, uphill_load_mps2: float, + duration_s: float, plant: DelayedPedalPlantCase, + monkeypatch) -> list[PedalPlantSample]: + monkeypatch.setattr( + virtual_das, + "nap_conf", + SimpleNamespace(get_pedal_profile_values=lambda: [50.0] * len(virtual_das.PEDAL_BP)), + ) + monkeypatch.setattr( + virtual_das, + "get_zero_torque", + lambda: SimpleNamespace(get=lambda _speed_mps: COAST_PEDAL_DI), + ) + + controller = VirtualDAS(dt=CONTROL_DT_S) + controller.reset( + measured_accel=0.0, + commanded_accel=0.0, + pedal_di_init=COAST_PEDAL_DI, + ) + pedal_di = COAST_PEDAL_DI + net_acceleration_mps2 = 0.0 + + for _ in range(round(2.0 / CONTROL_DT_S)): + pedal_di = controller.update( + 0.0, + v_ego=speed_mps, + prev_pedal_di=pedal_di, + a_ego=net_acceleration_mps2, + freeze_integrator=False, + orientation_ned=[0.0, 0.0, 0.0], + ) + + delay_steps = round(plant.delay_s / CONTROL_DT_S) + delayed_pedals_di = [pedal_di] * delay_steps + plant_alpha = CONTROL_DT_S / (plant.tau_s + CONTROL_DT_S) + sensed_grade_mps2 = uphill_load_mps2 * plant.grade_sensor_scale + orientation_ned = [0.0, math.asin(sensed_grade_mps2 / GRAVITY), 0.0] + current_speed_mps = speed_mps + speed_delta_mps = 0.0 + samples = [] + + for step in range(round(duration_s / CONTROL_DT_S)): + pedal_di = controller.update( + 0.0, + v_ego=max(current_speed_mps, 0.0), + prev_pedal_di=pedal_di, + a_ego=net_acceleration_mps2, + freeze_integrator=False, + orientation_ned=orientation_ned, + ) + applied_pedal_di = delayed_pedals_di.pop(0) + delayed_pedals_di.append(pedal_di) + plant_target_acceleration_mps2 = ( + (applied_pedal_di - COAST_PEDAL_DI) * plant.acceleration_per_di_mps2 + - uphill_load_mps2 + ) + net_acceleration_mps2 += plant_alpha * ( + plant_target_acceleration_mps2 - net_acceleration_mps2 + ) + speed_delta_mps += net_acceleration_mps2 * CONTROL_DT_S + current_speed_mps = speed_mps + speed_delta_mps + samples.append(PedalPlantSample( + elapsed_s=(step + 1) * CONTROL_DT_S, + net_acceleration_mps2=net_acceleration_mps2, + speed_delta_mps=speed_delta_mps, + pedal_di=pedal_di, + )) + + return samples + + +@pytest.mark.parametrize("uphill_load_mps2", [STEP_GRADE_ACCEL_MPS2, -STEP_GRADE_ACCEL_MPS2]) +def test_flat_to_grade_step_improves_on_no_pitch_baseline_at_1p5_seconds( + monkeypatch, uphill_load_mps2): + samples = run_grade_step( + speed_mps=15.0, + uphill_load_mps2=uphill_load_mps2, + duration_s=SHORT_HORIZON_S, + plant=NOMINAL_PLANT, + monkeypatch=monkeypatch, + ) + no_pitch_samples = run_grade_step( + speed_mps=15.0, + uphill_load_mps2=uphill_load_mps2, + duration_s=SHORT_HORIZON_S, + plant=replace(NOMINAL_PLANT, grade_sensor_scale=0.0), + monkeypatch=monkeypatch, + ) + checkpoint_samples = [sample for sample in samples if sample.elapsed_s >= 1.4] + mean_net_acceleration_mps2 = sum( + sample.net_acceleration_mps2 for sample in checkpoint_samples + ) / len(checkpoint_samples) + + assert abs(mean_net_acceleration_mps2) <= STEP_RESPONSE_TOLERANCE_MPS2 + assert ( + abs(samples[-1].speed_delta_mps) + STEP_RESPONSE_MIN_DRIFT_IMPROVEMENT_MPS + <= abs(no_pitch_samples[-1].speed_delta_mps) + ) + assert abs(samples[-1].pedal_di - COAST_PEDAL_DI) >= STEP_RESPONSE_MIN_PEDAL_DELTA_DI + assert math.copysign(1.0, samples[-1].pedal_di - COAST_PEDAL_DI) == math.copysign( + 1.0, + uphill_load_mps2, + ) + + +@pytest.mark.parametrize("speed_mps", [0.0, 5.0, 15.0, 30.0]) +@pytest.mark.parametrize("uphill_load_mps2", [MATRIX_GRADE_ACCEL_MPS2, -MATRIX_GRADE_ACCEL_MPS2]) +@pytest.mark.parametrize("plant", UNCERTAIN_PLANTS) +def test_grade_hold_survives_speed_and_plant_uncertainty( + monkeypatch, speed_mps, uphill_load_mps2, plant): + samples = run_grade_step( + speed_mps=speed_mps, + uphill_load_mps2=uphill_load_mps2, + duration_s=STEADY_HORIZON_S, + plant=plant, + monkeypatch=monkeypatch, + ) + steady_samples = samples[-round(STEADY_WINDOW_S / CONTROL_DT_S):] + mean_net_acceleration_mps2 = sum( + sample.net_acceleration_mps2 for sample in steady_samples + ) / len(steady_samples) + + assert abs(mean_net_acceleration_mps2) <= STEADY_MEAN_TOLERANCE_MPS2 + assert max(abs(sample.net_acceleration_mps2) for sample in steady_samples) <= STEADY_PEAK_TOLERANCE_MPS2 + assert abs(samples[-1].speed_delta_mps) <= STEADY_SPEED_DRIFT_MPS + assert min(sample.pedal_di for sample in steady_samples) >= virtual_das.PEDAL_DI_MIN + PHYSICAL_RAIL_MARGIN_DI + assert max(sample.pedal_di for sample in steady_samples) <= 50.0 - PHYSICAL_RAIL_MARGIN_DI + assert math.copysign(1.0, steady_samples[-1].pedal_di - COAST_PEDAL_DI) == math.copysign( + 1.0, + uphill_load_mps2, + ) diff --git a/opendbc/car/tesla/preap/tests/test_virtual_das.py b/opendbc/car/tesla/preap/tests/test_virtual_das.py index 73e747c7bc1..85494901e44 100644 --- a/opendbc/car/tesla/preap/tests/test_virtual_das.py +++ b/opendbc/car/tesla/preap/tests/test_virtual_das.py @@ -967,22 +967,43 @@ def test_pitch_compensation_clamped(self): _, pitch_comp = ge.update([0.0, math.radians(20.0), 0.0]) assert abs(pitch_comp) <= MAX_PITCH_COMPENSATION + 0.01 - def test_grade_subtracted_from_aego(self, mock_nap_conf, mock_zero_torque): - """On a downhill, grade compensation should reduce the effective a_ego - so the PID doesn't think the car is over-accelerating.""" + def test_sustained_pitch_outlier_cannot_exceed_steady_grade_limit(self): + import math + from opendbc.car.tesla.preap.virtual_das import GradeEstimator, MAX_STEADY_GRADE_COMPENSATION + ge = GradeEstimator(dt=0.02) + + for _ in range(500): + grade, _ = ge.update([0.0, math.radians(20.0), 0.0]) + + assert abs(grade) <= MAX_STEADY_GRADE_COMPENSATION + for _ in range(50): + recovered_grade, _ = ge.update([0.0, 0.0, 0.0]) + assert abs(recovered_grade) < 0.3 + + def test_orientation_dropout_holds_filtered_steady_grade(self): + import math + from opendbc.car.tesla.preap.virtual_das import GradeEstimator + ge = GradeEstimator(dt=0.02) + + for _ in range(200): + steady_grade, _ = ge.update([0.0, math.radians(3.0), 0.0]) + + dropout_grade, dropout_transient = ge.update([]) + + assert dropout_grade == pytest.approx(steady_grade) + assert dropout_transient == 0.0 + + def test_grade_does_not_change_net_acceleration_feedback(self, mock_nap_conf, mock_zero_torque): + """Wheel-speed acceleration and planner targets remain in the net domain.""" import math vdas = VirtualDAS(dt=0.02) pitch = math.radians(-3.0) # downhill - # Run with grade: the PID should see less error than without for _ in range(100): vdas.update(0.0, v_ego=15.0, prev_pedal_di=vdas.prev_pedal_di, a_ego=0.5, orientation_ned=[0.0, pitch, 0.0]) - # The a_ego_filter should reflect corrected value (a_ego - grade) - # grade is negative on downhill, so corrected = 0.5 - (-0.51) = ~1.01 - # Without grade: filter would settle near 0.5 - assert vdas.a_ego_filter.x > 0.8 # corrected is higher than raw + assert vdas.a_ego_filter.x == pytest.approx(0.5, abs=0.01) def test_reset_clears_grade(self): import math @@ -1035,6 +1056,54 @@ def record_feedforward(acceleration_effort_mps2, _v_ego): ) assert pedal_outputs_di == [feedforward_sentinel_di] * 100 + def test_engage_effort_limits_include_grade_compensation(self, monkeypatch): + import math + + feedforward_inputs_mps2 = [] + vdas = VirtualDAS(dt=0.02) + orientation_ned = [0.0, math.radians(3.0), 0.0] + for _ in range(200): + vdas.observe(0.0, orientation_ned) + vdas.reset( + measured_accel=0.0, + commanded_accel=0.0, + pedal_di_init=0.0, + preserve_grade=True, + ) + + def record_feedforward(acceleration_effort_mps2, _v_ego): + feedforward_inputs_mps2.append(acceleration_effort_mps2) + return 0.0 + + monkeypatch.setattr(vdas, '_feedforward', record_feedforward) + pedal_di = vdas.update( + 0.0, + v_ego=15.0, + prev_pedal_di=0.0, + a_ego=0.0, + freeze_integrator=True, + orientation_ned=orientation_ned, + accel_effort_limits=(0.0, 0.0), + ) + + assert feedforward_inputs_mps2 == [0.0] + assert pedal_di == pytest.approx(0.0) + + def test_engage_pedal_ramp_limit_applies_after_feedforward(self, monkeypatch): + vdas = VirtualDAS(dt=0.02) + monkeypatch.setattr(vdas, '_feedforward', lambda _acceleration_effort_mps2, _v_ego: 20.0) + + pedal_di = vdas.update( + 0.0, + v_ego=15.0, + prev_pedal_di=0.0, + a_ego=0.0, + freeze_integrator=True, + pedal_ramp_rate_up=0.9, + ) + + assert pedal_di == pytest.approx(0.9) + def test_pid_starts_with_acceleration_domain_limits(self): vdas = VirtualDAS(dt=0.02) @@ -1283,7 +1352,7 @@ def test_engage_reset_starts_estimator_from_measured_acceleration(self, monkeypa assert controller.vdas.prev_a_ego_filtered == pytest.approx(measured_acceleration) assert controller.vdas.jerk_limiter.a_limited == pytest.approx(0.0) - def test_preserved_grade_reset_keeps_acceleration_filter_in_corrected_domain(self): + def test_preserved_grade_reset_keeps_acceleration_filter_in_net_domain(self): import math vdas = VirtualDAS(dt=0.02) @@ -1292,11 +1361,10 @@ def test_preserved_grade_reset_keeps_acceleration_filter_in_corrected_domain(sel for _ in range(300): vdas.observe(measured_acceleration, orientation_ned) - corrected_acceleration = vdas.a_ego_filter.x - assert corrected_acceleration < -0.7 + assert vdas.a_ego_filter.x == pytest.approx(measured_acceleration, abs=0.01) vdas.reset(measured_accel=measured_acceleration, commanded_accel=0.0, preserve_grade=True) - assert vdas.a_ego_filter.x == pytest.approx(corrected_acceleration) - assert vdas.prev_a_ego_filtered == pytest.approx(corrected_acceleration) + assert vdas.a_ego_filter.x == pytest.approx(measured_acceleration, abs=0.01) + assert vdas.prev_a_ego_filtered == pytest.approx(measured_acceleration, abs=0.01) assert vdas.jerk_limiter.a_limited == pytest.approx(0.0) diff --git a/opendbc/car/tesla/preap/virtual_das.py b/opendbc/car/tesla/preap/virtual_das.py index e181ab356a4..57d2c7395fa 100644 --- a/opendbc/car/tesla/preap/virtual_das.py +++ b/opendbc/car/tesla/preap/virtual_das.py @@ -51,15 +51,17 @@ PITCH_HP_RC1 = 0.1 # high-pass inner RC for transient grade detection PITCH_HP_RC2 = 1.0 # high-pass outer RC MAX_PITCH_COMPENSATION = 1.5 # m/s² — clamp transient compensation +MAX_STEADY_GRADE_COMPENSATION = 1.5 # m/s² — reject implausible sustained pitch +TRANSIENT_GRADE_GAIN = 0.4 class GradeEstimator: """Estimates road grade from IMU pitch and compensates the controller. Uses a low-pass filter on pitch for the steady-state grade component - (subtracted from a_ego so the inner PID doesn't fight gravity) and - a high-pass filter for transient grade changes (added to feedforward - so the controller anticipates crests and dips). + and a high-pass filter for transient grade changes. Both components are + added to actuator effort so planner targets and measured acceleration stay + in the same net-acceleration domain. Follows the same pattern as Toyota's carcontroller.py lines 68-69, 204-235. """ @@ -78,23 +80,31 @@ def update(self, orientation_ned: list) -> tuple: Returns: (grade_accel, pitch_compensation): grade_accel: steady-state gravitational component along road (m/s²). - Positive = downhill (gravity accelerates the car). + Positive = uphill (gravity resists the car). pitch_compensation: transient feedforward bump for grade changes (m/s²). """ if len(orientation_ned) < 2: - return 0.0, 0.0 + return self._steady_grade_compensation(), 0.0 - pitch = orientation_ned[1] + maximum_pitch = math.asin(MAX_STEADY_GRADE_COMPENSATION / GRAVITY) + pitch = float(clip(orientation_ned[1], -maximum_pitch, maximum_pitch)) self.pitch_lp.update(pitch) self.pitch_hp.update(pitch) - grade_accel = math.sin(self.pitch_lp.x) * GRAVITY + grade_accel = self._steady_grade_compensation() pitch_compensation = float(clip( - math.sin(self.pitch_hp.x) * GRAVITY, + math.sin(self.pitch_hp.x) * GRAVITY * TRANSIENT_GRADE_GAIN, -MAX_PITCH_COMPENSATION, MAX_PITCH_COMPENSATION)) return grade_accel, pitch_compensation + def _steady_grade_compensation(self) -> float: + return float(clip( + math.sin(self.pitch_lp.x) * GRAVITY, + -MAX_STEADY_GRADE_COMPENSATION, + MAX_STEADY_GRADE_COMPENSATION, + )) + def reset(self): self.pitch_lp.x = 0.0 self.pitch_hp.x = 0.0 @@ -397,10 +407,13 @@ def __init__(self, dt: float = 0.02): self.a_ego_filter = FirstOrderFilter(0.0, VDAS_AEGO_FILTER_RC, dt) self.prev_a_ego_filtered = 0.0 self.a_ego_initialized = False + self.pedal_ramp_limited_up = False def update(self, a_cmd: float, v_ego: float, prev_pedal_di: float, a_ego: float = 0.0, freeze_integrator: bool = False, - orientation_ned: list | None = None) -> float: + orientation_ned: list | None = None, + accel_effort_limits: tuple[float, float] | None = None, + pedal_ramp_rate_up: float = PEDAL_RAMP_RATE_UP) -> float: """Compute pedal DI from acceleration command. Args: @@ -410,25 +423,29 @@ def update(self, a_cmd: float, v_ego: float, prev_pedal_di: float, a_ego: measured longitudinal acceleration in m/s² freeze_integrator: True during engage grace period orientation_ned: [roll, pitch, yaw] from CC.orientationNED, or None + accel_effort_limits: optional lower and upper acceleration-effort bounds + pedal_ramp_rate_up: maximum positive pedal DI change for this update Returns: pedal_di: output in DI units (caller converts to voltage via di_to_pedal) """ a_limited = self.jerk_limiter.update(a_cmd) - grade_accel, pitch_compensation = self.grade_estimator.update( + steady_grade_compensation, transient_pitch_compensation = self.grade_estimator.update( orientation_ned if orientation_ned is not None else []) + effort_min, effort_max = accel_effort_limits or (REGEN_MAX, ACCEL_MAX) + if not REGEN_MAX <= effort_min <= effort_max <= ACCEL_MAX: + raise ValueError("acceleration-effort limits exceed the physical control range") + if not 0.0 <= pedal_ramp_rate_up <= PEDAL_RAMP_RATE_UP: + raise ValueError("pedal ramp limit exceeds the physical control range") base_accel_effort = float(clip( - a_limited + pitch_compensation, - REGEN_MAX, - ACCEL_MAX, + a_limited + steady_grade_compensation + transient_pitch_compensation, + effort_min, + effort_max, )) - # Subtract grade from a_ego so the PID doesn't fight gravity - a_ego_corrected = a_ego - grade_accel - - a_ego_filtered = self.a_ego_filter.update(a_ego_corrected) + a_ego_filtered = self.a_ego_filter.update(a_ego) self.a_ego_initialized = True j_ego = float(clip( (a_ego_filtered - self.prev_a_ego_filtered) / self.dt, @@ -444,9 +461,9 @@ def update(self, a_cmd: float, v_ego: float, prev_pedal_di: float, error = 0.0 # Keep residual control in acceleration space. The PID can use only the - # authority left after the desired acceleration and transient grade term. - self.inner_pid.neg_limit = REGEN_MAX - base_accel_effort - self.inner_pid.pos_limit = ACCEL_MAX - base_accel_effort + # authority left after desired acceleration and grade compensation. + self.inner_pid.neg_limit = effort_min - base_accel_effort + self.inner_pid.pos_limit = effort_max - base_accel_effort self.inner_pid.i = float(clip( self.inner_pid.i, self.inner_pid.neg_limit, @@ -457,8 +474,8 @@ def update(self, a_cmd: float, v_ego: float, prev_pedal_di: float, error, speed=v_ego, freeze_integrator=freeze_integrator)) accel_effort = float(clip( base_accel_effort + accel_trim, - REGEN_MAX, - ACCEL_MAX, + effort_min, + effort_max, )) pedal_di_unclipped = self._feedforward(accel_effort, v_ego) @@ -467,7 +484,8 @@ def update(self, a_cmd: float, v_ego: float, prev_pedal_di: float, max_pedal_value = float(interp(v_ego, PEDAL_BP, pedal_profile)) pedal_di_bounded = float(clip(pedal_di_unclipped, PEDAL_DI_MIN, max_pedal_value)) - pedal_di = self._rate_limit(pedal_di_bounded, prev_pedal_di) + pedal_di = self._rate_limit(pedal_di_bounded, prev_pedal_di, pedal_ramp_rate_up) + self.pedal_ramp_limited_up = pedal_di < pedal_di_bounded physical_bound_blocks_error = ( (pedal_di_bounded < pedal_di_unclipped and error > 0.0) or (pedal_di_bounded > pedal_di_unclipped and error < 0.0) @@ -484,10 +502,8 @@ def update(self, a_cmd: float, v_ego: float, prev_pedal_di: float, def observe(self, a_ego: float, orientation_ned: list | None = None): """Keep measured acceleration and grade state current without authority.""" - grade_accel, _ = self.grade_estimator.update( - orientation_ned if orientation_ned is not None else []) - a_ego_corrected = a_ego - grade_accel - a_ego_filtered = self.a_ego_filter.update(a_ego_corrected) + self.grade_estimator.update(orientation_ned if orientation_ned is not None else []) + a_ego_filtered = self.a_ego_filter.update(a_ego) self.prev_a_ego_filtered = a_ego_filtered self.a_ego_initialized = True self.inner_pid.reset() @@ -509,16 +525,18 @@ def reset(self, measured_accel: float = 0.0, commanded_accel: float = 0.0, self.prev_a_ego_filtered = measured_accel self.a_ego_initialized = True self.prev_pedal_di = pedal_di_init + self.pedal_ramp_limited_up = False def _feedforward(self, a_cmd: float, v_ego: float) -> float: """Map acceleration to raw pedal DI via the finite 2D lookup table.""" zero_torque_di = get_zero_torque().get(v_ego) return self.ff_model.get(a_cmd, v_ego, zero_torque_di) - def _rate_limit(self, pedal_di: float, prev_pedal_di: float) -> float: + def _rate_limit(self, pedal_di: float, prev_pedal_di: float, + ramp_rate_up: float = PEDAL_RAMP_RATE_UP) -> float: """Safety backstop: asymmetric DI rate limit.""" return float(clip( pedal_di, prev_pedal_di - PEDAL_RAMP_RATE_DOWN, - prev_pedal_di + PEDAL_RAMP_RATE_UP, + prev_pedal_di + ramp_rate_up, )) From 091567c2ee3895f6a0c497769c9740dbacf24e8f Mon Sep 17 00:00:00 2001 From: jjackbrandt Date: Wed, 22 Jul 2026 14:34:30 -0400 Subject: [PATCH 2/4] fix(tesla): bound grade estimate dropout --- .../ci/tesla_preap_longitudinal_mutations.py | 35 ++++++++++-- .../car/tesla/preap/tests/test_virtual_das.py | 55 ++++++++++++++++++- opendbc/car/tesla/preap/virtual_das.py | 32 ++++++++++- 3 files changed, 116 insertions(+), 6 deletions(-) diff --git a/.github/ci/tesla_preap_longitudinal_mutations.py b/.github/ci/tesla_preap_longitudinal_mutations.py index b35165c98b5..934aaad466c 100644 --- a/.github/ci/tesla_preap_longitudinal_mutations.py +++ b/.github/ci/tesla_preap_longitudinal_mutations.py @@ -225,13 +225,40 @@ class HistoricalMutation: ), ), HistoricalMutation( - name="orientation-dropout-zeroes-grade-effort", + name="orientation-dropout-skips-short-hold", source_path="opendbc/car/tesla/preap/virtual_das.py", - original=b" return self._steady_grade_compensation(), 0.0\n", - replacement=b" return 0.0, 0.0\n", + original=( + b" dropout_decay_elapsed_s = self.missing_orientation_elapsed_s - " + + b"ORIENTATION_DROPOUT_HOLD_S\n" + ), + replacement=b" dropout_decay_elapsed_s = self.missing_orientation_elapsed_s\n", + test_node=( + "opendbc/car/tesla/preap/tests/test_virtual_das.py::TestGradeEstimator::" + + "test_orientation_dropout_holds_then_decays_steady_grade[0.5-1.0]" + ), + ), + HistoricalMutation( + name="orientation-dropout-disables-bounded-decay", + source_path="opendbc/car/tesla/preap/virtual_das.py", + original=b" self.pitch_lp.x = self.pitch_before_dropout_rad * dropout_grade_scale\n", + replacement=b" self.pitch_lp.x = self.pitch_before_dropout_rad\n", + test_node=( + "opendbc/car/tesla/preap/tests/test_virtual_das.py::TestGradeEstimator::" + + "test_orientation_dropout_holds_then_decays_steady_grade[4.64-0.0]" + ), + ), + HistoricalMutation( + name="orientation-dropout-reset-retains-stale-state", + source_path="opendbc/car/tesla/preap/virtual_das.py", + original=( + b" self._clear_high_pass_state()\n" + + b" self.missing_orientation_elapsed_s = 0.0\n" + + b" self.pitch_before_dropout_rad = 0.0\n" + ), + replacement=b"", test_node=( "opendbc/car/tesla/preap/tests/test_virtual_das.py::TestGradeEstimator::" + - "test_orientation_dropout_holds_filtered_steady_grade" + "test_reset_clears_grade" ), ), HistoricalMutation( diff --git a/opendbc/car/tesla/preap/tests/test_virtual_das.py b/opendbc/car/tesla/preap/tests/test_virtual_das.py index 85494901e44..fe46ed06ef3 100644 --- a/opendbc/car/tesla/preap/tests/test_virtual_das.py +++ b/opendbc/car/tesla/preap/tests/test_virtual_das.py @@ -993,6 +993,55 @@ def test_orientation_dropout_holds_filtered_steady_grade(self): assert dropout_grade == pytest.approx(steady_grade) assert dropout_transient == 0.0 + @pytest.mark.parametrize(("dropout_duration_s", "expected_grade_scale"), [ + (0.10, 1.0), + (0.50, 1.0), + (1.25, 0.5), + (2.00, 0.0), + (4.64, 0.0), + ]) + def test_orientation_dropout_holds_then_decays_steady_grade( + self, dropout_duration_s, expected_grade_scale, + ): + import math + from opendbc.car.tesla.preap.virtual_das import GradeEstimator + dt = 0.02 + settled_pitch = math.radians(4.45) + ge = GradeEstimator(dt=dt) + + for _ in range(500): + ge.update([0.0, settled_pitch, 0.0]) + + for _ in range(round(dropout_duration_s / dt)): + dropout_grade, dropout_transient = ge.update([]) + assert dropout_transient == 0.0 + + expected_grade = math.sin(settled_pitch) * 9.81 * expected_grade_scale + assert dropout_grade == pytest.approx(expected_grade, abs=0.02) + + def test_crest_reacquisition_after_long_dropout_uses_new_grade_sign(self): + import math + from opendbc.car.tesla.preap.virtual_das import GradeEstimator + dt = 0.02 + ge = GradeEstimator(dt=dt) + + for _ in range(500): + ge.update([0.0, math.radians(4.45), 0.0]) + for _ in range(round(4.64 / dt)): + dropout_grade, dropout_transient = ge.update([]) + + assert abs(dropout_grade + dropout_transient) < 0.02 + + grade, transient = ge.update([0.0, math.radians(-3.0), 0.0]) + first_reacquired_compensation = grade + transient + assert -0.25 <= first_reacquired_compensation < 0.0 + + for _ in range(round(1.50 / dt) - 1): + grade, transient = ge.update([0.0, math.radians(-3.0), 0.0]) + + assert grade == pytest.approx(math.sin(math.radians(-3.0)) * 9.81, abs=0.05) + assert abs(transient) < 0.15 + def test_grade_does_not_change_net_acceleration_feedback(self, mock_nap_conf, mock_zero_torque): """Wheel-speed acceleration and planner targets remain in the net domain.""" import math @@ -1012,8 +1061,12 @@ def test_reset_clears_grade(self): for _ in range(100): ge.update([0.0, math.radians(5.0), 0.0]) assert abs(ge.pitch_lp.x) > 0.01 + ge.update([]) + ge.reset() - assert ge.pitch_lp.x == 0.0 + + assert ge.update([]) == (0.0, 0.0) + assert ge.update([0.0, 0.0, 0.0]) == (0.0, 0.0) class TestVDASDomainBoundaries: diff --git a/opendbc/car/tesla/preap/virtual_das.py b/opendbc/car/tesla/preap/virtual_das.py index 57d2c7395fa..211927be92b 100644 --- a/opendbc/car/tesla/preap/virtual_das.py +++ b/opendbc/car/tesla/preap/virtual_das.py @@ -53,6 +53,8 @@ MAX_PITCH_COMPENSATION = 1.5 # m/s² — clamp transient compensation MAX_STEADY_GRADE_COMPENSATION = 1.5 # m/s² — reject implausible sustained pitch TRANSIENT_GRADE_GAIN = 0.4 +ORIENTATION_DROPOUT_HOLD_S = 0.50 +ORIENTATION_DROPOUT_DECAY_S = 1.50 class GradeEstimator: @@ -67,8 +69,11 @@ class GradeEstimator: """ def __init__(self, dt: float = 0.02): + self.dt = dt self.pitch_lp = FirstOrderFilter(0.0, PITCH_LP_RC, dt) self.pitch_hp = HighPassFilter(0.0, PITCH_HP_RC1, PITCH_HP_RC2, dt) + self.missing_orientation_elapsed_s = 0.0 + self.pitch_before_dropout_rad = 0.0 def update(self, orientation_ned: list) -> tuple: """Update filters with current pitch. @@ -84,7 +89,9 @@ def update(self, orientation_ned: list) -> tuple: pitch_compensation: transient feedforward bump for grade changes (m/s²). """ if len(orientation_ned) < 2: - return self._steady_grade_compensation(), 0.0 + return self._update_for_missing_orientation() + + self.missing_orientation_elapsed_s = 0.0 maximum_pitch = math.asin(MAX_STEADY_GRADE_COMPENSATION / GRAVITY) pitch = float(clip(orientation_ned[1], -maximum_pitch, maximum_pitch)) @@ -98,6 +105,24 @@ def update(self, orientation_ned: list) -> tuple: return grade_accel, pitch_compensation + def _update_for_missing_orientation(self) -> tuple[float, float]: + if self.missing_orientation_elapsed_s == 0.0: + self.pitch_before_dropout_rad = self.pitch_lp.x + + self.missing_orientation_elapsed_s += self.dt + dropout_decay_elapsed_s = self.missing_orientation_elapsed_s - ORIENTATION_DROPOUT_HOLD_S + dropout_grade_scale = float(clip( + 1.0 - dropout_decay_elapsed_s / ORIENTATION_DROPOUT_DECAY_S, + 0.0, + 1.0, + )) + self.pitch_lp.x = self.pitch_before_dropout_rad * dropout_grade_scale + + if dropout_grade_scale == 0.0: + self._clear_high_pass_state() + + return self._steady_grade_compensation(), 0.0 + def _steady_grade_compensation(self) -> float: return float(clip( math.sin(self.pitch_lp.x) * GRAVITY, @@ -107,6 +132,11 @@ def _steady_grade_compensation(self) -> float: def reset(self): self.pitch_lp.x = 0.0 + self._clear_high_pass_state() + self.missing_orientation_elapsed_s = 0.0 + self.pitch_before_dropout_rad = 0.0 + + def _clear_high_pass_state(self): self.pitch_hp.x = 0.0 self.pitch_hp._f1.x = 0.0 self.pitch_hp._f2.x = 0.0 From 97c7c043c5a03a21d84310d43bf7a62505192a89 Mon Sep 17 00:00:00 2001 From: jjackbrandt Date: Wed, 22 Jul 2026 16:29:51 -0400 Subject: [PATCH 3/4] fix(tesla): calibrate positive VDAS fallback --- .../ci/tesla_preap_longitudinal_mutations.py | 188 ++++- opendbc/car/tesla/preap/ff_table_default.py | 24 +- .../tesla/preap/tests/test_accel_limits.py | 6 +- .../preap/tests/test_vdas_grade_control.py | 669 +++++++++++++++++- .../car/tesla/preap/tests/test_virtual_das.py | 109 ++- opendbc/car/tesla/preap/virtual_das.py | 140 +++- 6 files changed, 1098 insertions(+), 38 deletions(-) diff --git a/.github/ci/tesla_preap_longitudinal_mutations.py b/.github/ci/tesla_preap_longitudinal_mutations.py index 934aaad466c..bdbe2437fbb 100644 --- a/.github/ci/tesla_preap_longitudinal_mutations.py +++ b/.github/ci/tesla_preap_longitudinal_mutations.py @@ -137,6 +137,102 @@ class HistoricalMutation: "test_inner_feedback_holds_cruise_against_sustained_road_load" ), ), + HistoricalMutation( + name="hard-inner-error-deadband-restored", + source_path="opendbc/car/tesla/preap/virtual_das.py", + original=b" error = self._gate_pid_error_noise(error, freeze_integrator)\n", + replacement=( + b" if abs(error) < PID_ERROR_DEADBAND:\n" + + b" error = 0.0\n" + ), + test_node=( + "opendbc/car/tesla/preap/tests/test_virtual_das.py::TestInnerPID::" + + "test_persistent_sub_deadband_error_earns_residual_authority[0.0196]" + ), + ), + HistoricalMutation( + name="inner-error-noise-gate-call-bypassed", + source_path="opendbc/car/tesla/preap/virtual_das.py", + original=b" error = self._gate_pid_error_noise(error, freeze_integrator)\n", + replacement=b" error = error\n", + test_node=( + "opendbc/car/tesla/preap/tests/test_virtual_das.py::TestInnerPID::" + + "test_sub_deadband_sign_changing_noise_does_not_accumulate_residual_authority" + ), + ), + HistoricalMutation( + name="persistent-error-dwell-removed", + source_path="opendbc/car/tesla/preap/virtual_das.py", + original=b" if self.persistent_error_elapsed_s < PID_PERSISTENT_ERROR_DWELL_S:\n", + replacement=b" if False:\n", + test_node=( + "opendbc/car/tesla/preap/tests/test_virtual_das.py::TestInnerPID::" + + "test_sign_changing_sub_deadband_error_never_completes_dwell" + ), + ), + HistoricalMutation( + name="persistent-error-sign-reset-removed", + source_path="opendbc/car/tesla/preap/virtual_das.py", + original=( + b" if error_sign != self.persistent_error_sign:\n" + + b" self.persistent_error_sign = error_sign\n" + + b" self.persistent_error_elapsed_s = self.dt\n" + ), + replacement=( + b" if error_sign != self.persistent_error_sign:\n" + + b" self.persistent_error_sign = error_sign\n" + ), + test_node=( + "opendbc/car/tesla/preap/tests/test_virtual_das.py::TestInnerPID::" + + "test_sign_changing_sub_deadband_error_never_completes_dwell" + ), + ), + HistoricalMutation( + name="persistent-error-freeze-reset-removed", + source_path="opendbc/car/tesla/preap/virtual_das.py", + original=b" if freeze_integrator or error == 0.0:\n", + replacement=b" if error == 0.0:\n", + test_node=( + "opendbc/car/tesla/preap/tests/test_virtual_das.py::TestInnerPID::" + + "test_persistent_error_dwell_restarts_after_freeze_observe_and_reset" + ), + ), + HistoricalMutation( + name="persistent-error-observe-reset-removed", + source_path="opendbc/car/tesla/preap/virtual_das.py", + original=( + b" self.inner_pid.reset()\n" + + b" self._reset_negative_handoff()\n" + + b" self._reset_persistent_error()\n\n" + + b" def reset(" + ), + replacement=( + b" self.inner_pid.reset()\n" + + b" self._reset_negative_handoff()\n\n" + + b" def reset(" + ), + test_node=( + "opendbc/car/tesla/preap/tests/test_virtual_das.py::TestInnerPID::" + + "test_persistent_error_dwell_restarts_after_freeze_observe_and_reset" + ), + ), + HistoricalMutation( + name="persistent-error-command-reset-removed", + source_path="opendbc/car/tesla/preap/virtual_das.py", + original=( + b" self._reset_negative_handoff()\n" + + b" self._reset_persistent_error()\n\n" + + b" def _gate_pid_error_noise(" + ), + replacement=( + b" self._reset_negative_handoff()\n\n" + + b" def _gate_pid_error_noise(" + ), + test_node=( + "opendbc/car/tesla/preap/tests/test_virtual_das.py::TestInnerPID::" + + "test_persistent_error_dwell_restarts_after_freeze_observe_and_reset" + ), + ), HistoricalMutation( name="steady-grade-removed-from-effort", source_path="opendbc/car/tesla/preap/virtual_das.py", @@ -180,9 +276,21 @@ class HistoricalMutation: name="engage-grade-bypasses-pedal-slew-envelope", source_path="opendbc/car/tesla/preap/virtual_das.py", original=( - b" pedal_di = self._rate_limit(pedal_di_bounded, prev_pedal_di, pedal_ramp_rate_up)\n" + b" pedal_di = self._rate_limit(\n" + + b" pedal_di_bounded,\n" + + b" prev_pedal_di,\n" + + b" pedal_ramp_rate_up,\n" + + b" pedal_ramp_rate_down,\n" + + b" )\n" + ), + replacement=( + b" pedal_di = self._rate_limit(\n" + + b" pedal_di_bounded,\n" + + b" prev_pedal_di,\n" + + b" PEDAL_RAMP_RATE_UP,\n" + + b" pedal_ramp_rate_down,\n" + + b" )\n" ), - replacement=b" pedal_di = self._rate_limit(pedal_di_bounded, prev_pedal_di)\n", test_node=( "opendbc/car/tesla/preap/tests/test_virtual_das.py::TestVDASDomainBoundaries::" + "test_engage_pedal_ramp_limit_applies_after_feedforward" @@ -261,6 +369,32 @@ class HistoricalMutation: "test_reset_clears_grade" ), ), + HistoricalMutation( + name="legacy-positive-fallback-actuator-gain", + source_path="opendbc/car/tesla/preap/ff_table_default.py", + original=( + b" [-5.00, -3.33, -1.67, 0.00, 9.62, 19.24, 28.86, 38.48, 48.10], # 20 m/s\n" + + b" [-5.00, -3.33, -1.67, 0.00, 10.66, 21.32, 31.98, 42.64, 53.30], # 30 m/s\n" + ), + replacement=( + b" [-5.00, -3.33, -1.67, 0.00, 14.80, 29.60, 44.40, 59.20, 74.00], # 20 m/s\n" + + b" [-5.00, -3.33, -1.67, 0.00, 16.40, 32.80, 49.20, 65.60, 82.00], # 30 m/s\n" + ), + test_node=( + "opendbc/car/tesla/preap/tests/test_vdas_grade_control.py::" + + "test_route_shaped_positive_transition_bounds_delivered_acceleration" + ), + ), + HistoricalMutation( + name="legacy-5mps-positive-fallback-transition", + source_path="opendbc/car/tesla/preap/ff_table_default.py", + original=b" [-5.00, -3.33, -1.67, 0.00, 7.54, 15.08, 22.62, 30.16, 37.70], # 5 m/s\n", + replacement=b" [-5.00, -3.33, -1.67, 0.00, 11.60, 23.20, 34.80, 46.40, 58.00], # 5 m/s\n", + test_node=( + "opendbc/car/tesla/preap/tests/test_vdas_grade_control.py::" + + "test_speed_ramp_through_fallback_transition_is_smooth" + ), + ), HistoricalMutation( name="focused-job-skip-bypass", source_path=".github/workflows/tests.yml", @@ -323,6 +457,56 @@ class HistoricalMutation: "test_retained_integral_is_clipped_to_remaining_acceleration_authority" ), ), + HistoricalMutation( + name="negative-handoff-integral-shaping-removed", + source_path="opendbc/car/tesla/preap/virtual_das.py", + original=b"NEGATIVE_HANDOFF_INTEGRAL_SLEW = 0.25 # m/s\xc2\xb3\n", + replacement=b"NEGATIVE_HANDOFF_INTEGRAL_SLEW = 100.0 # m/s\xc2\xb3\n", + test_node=( + "opendbc/car/tesla/preap/tests/test_vdas_grade_control.py::" + + "test_negative_command_handoff_keeps_grade_effort_separate" + ), + ), + HistoricalMutation( + name="negative-handoff-braking-effort-budget-removed", + source_path="opendbc/car/tesla/preap/virtual_das.py", + original=( + b" shared_effort_min = max(\n" + + b" effort_min,\n" + + b" previous_accel_effort - VDAS_DECEL_JERK_MAX * self.dt,\n" + + b" )\n" + ), + replacement=b" shared_effort_min = effort_min\n", + test_node=( + "opendbc/car/tesla/preap/tests/test_vdas_grade_control.py::" + + "test_handoff_shares_command_jerk_budget_in_both_directions" + ), + ), + HistoricalMutation( + name="negative-handoff-recovery-effort-budget-removed", + source_path="opendbc/car/tesla/preap/virtual_das.py", + original=( + b" shared_effort_max = min(\n" + + b" effort_max,\n" + + b" previous_accel_effort + VDAS_ACCEL_JERK_MAX * self.dt,\n" + + b" )\n" + ), + replacement=b" shared_effort_max = effort_max\n", + test_node=( + "opendbc/car/tesla/preap/tests/test_vdas_grade_control.py::" + + "test_handoff_shares_command_jerk_budget_in_both_directions" + ), + ), + HistoricalMutation( + name="negative-handoff-comfort-pedal-step-removed", + source_path="opendbc/car/tesla/preap/virtual_das.py", + original=b"NEGATIVE_HANDOFF_PEDAL_STEP = 0.50 # DI/update\n", + replacement=b"NEGATIVE_HANDOFF_PEDAL_STEP = PEDAL_RAMP_RATE_DOWN # DI/update\n", + test_node=( + "opendbc/car/tesla/preap/tests/test_vdas_grade_control.py::" + + "test_handoff_full_brake_and_recovery_stay_inside_comfort_pedal_step" + ), + ), HistoricalMutation( name="hidden-physical-profile-clipping", source_path="opendbc/car/tesla/preap/virtual_das.py", diff --git a/opendbc/car/tesla/preap/ff_table_default.py b/opendbc/car/tesla/preap/ff_table_default.py index 5a2e67231d7..389a6f72bde 100644 --- a/opendbc/car/tesla/preap/ff_table_default.py +++ b/opendbc/car/tesla/preap/ff_table_default.py @@ -1,9 +1,10 @@ """Default feedforward lookup table for VirtualDAS. Generated from the existing 3-breakpoint linear interpolation at a grid of -(speed, accel) points. This is the fallback when no data-driven table is -available. The generate_ff_table.py script produces a refined version from -real drive logs. +(speed, accel) points, with a conservative field correction on the positive +branch. This is the fallback when no data-driven table is available. +The generate_ff_table.py script produces a refined version from real drive +logs. Table format: SPEED_BP × ACCEL_BP → pedal_di Zero-torque offset is applied at runtime (not baked into the table). @@ -16,7 +17,12 @@ ACCEL_BP = [-1.5, -1.0, -0.5, 0.0, 0.5, 1.0, 1.5, 2.0, 2.5] # pedal_di values: DEFAULT_TABLE[speed_idx][accel_idx] -# Computed from: interp(accel, [REGEN_MAX, 0, ACCEL_MAX], [DI_MIN, 0, max_pedal]) +# Negative and zero values, plus the 0 m/s row, are computed from: +# interp(accel, [REGEN_MAX, 0, ACCEL_MAX], [DI_MIN, 0, max_pedal]). +# Positive values at 5-40 m/s apply a conservative 0.65 field correction to +# the legacy-derived fallback. The 5 m/s correction preserves monotonic +# positive effort and prevents a 5-to-12 m/s drop. External calibrated tables +# remain authoritative. # where max_pedal = interp(speed, PEDAL_BP, PEDAL_MAX_VALUES) # and zero_torque_di = 0 (applied as offset at runtime) # @@ -24,9 +30,9 @@ # -1.5 -1.0 -0.5 0.0 0.5 1.0 1.5 2.0 2.5 DEFAULT_TABLE = [ [-5.00, -3.33, -1.67, 0.00, 10.00, 20.00, 30.00, 40.00, 50.00], # 0 m/s - [-5.00, -3.33, -1.67, 0.00, 11.60, 23.20, 34.80, 46.40, 58.00], # 5 m/s - [-5.00, -3.33, -1.67, 0.00, 13.20, 26.40, 39.60, 52.80, 66.00], # 12 m/s - [-5.00, -3.33, -1.67, 0.00, 14.80, 29.60, 44.40, 59.20, 74.00], # 20 m/s - [-5.00, -3.33, -1.67, 0.00, 16.40, 32.80, 49.20, 65.60, 82.00], # 30 m/s - [-5.00, -3.33, -1.67, 0.00, 18.00, 36.00, 54.00, 72.00, 90.00], # 40 m/s + [-5.00, -3.33, -1.67, 0.00, 7.54, 15.08, 22.62, 30.16, 37.70], # 5 m/s + [-5.00, -3.33, -1.67, 0.00, 8.58, 17.16, 25.74, 34.32, 42.90], # 12 m/s + [-5.00, -3.33, -1.67, 0.00, 9.62, 19.24, 28.86, 38.48, 48.10], # 20 m/s + [-5.00, -3.33, -1.67, 0.00, 10.66, 21.32, 31.98, 42.64, 53.30], # 30 m/s + [-5.00, -3.33, -1.67, 0.00, 11.70, 23.40, 35.10, 46.80, 58.50], # 40 m/s ] diff --git a/opendbc/car/tesla/preap/tests/test_accel_limits.py b/opendbc/car/tesla/preap/tests/test_accel_limits.py index 2e959b50e2d..41f38b74dfa 100644 --- a/opendbc/car/tesla/preap/tests/test_accel_limits.py +++ b/opendbc/car/tesla/preap/tests/test_accel_limits.py @@ -15,6 +15,7 @@ # Solving the deployed feedforward mapping at the values above gives 4.997 DI. # This is inferred controller zero-torque state, not pedal calibration voltage. CAPTURE_INFERRED_ZERO_TORQUE_DI = 5.0 +HIGH_SPEED_FALLBACK_FIELD_SCALE = 0.65 class CapturedZeroTorque: @@ -64,7 +65,10 @@ def test_highway_speed_step_bounds_pedal_command_rise(monkeypatch): freeze_integrator=True, ) steady_pedal_di = pedal_di - assert steady_pedal_di == pytest.approx(CAPTURE_STEADY_PEDAL_DI, abs=0.1) + calibrated_capture_bound_di = CAPTURE_INFERRED_ZERO_TORQUE_DI + HIGH_SPEED_FALLBACK_FIELD_SCALE * ( + CAPTURE_STEADY_PEDAL_DI - CAPTURE_INFERRED_ZERO_TORQUE_DI + ) + assert CAPTURE_INFERRED_ZERO_TORQUE_DI < steady_pedal_di <= calibrated_capture_bound_di pedal_commands = [] for _ in range(50): diff --git a/opendbc/car/tesla/preap/tests/test_vdas_grade_control.py b/opendbc/car/tesla/preap/tests/test_vdas_grade_control.py index 4a460bbf235..82555f55e0a 100644 --- a/opendbc/car/tesla/preap/tests/test_vdas_grade_control.py +++ b/opendbc/car/tesla/preap/tests/test_vdas_grade_control.py @@ -5,9 +5,11 @@ from types import SimpleNamespace import pytest +from numpy import interp from opendbc.car.tesla.preap import virtual_das -from opendbc.car.tesla.preap.virtual_das import GRAVITY, VirtualDAS +from opendbc.car.tesla.preap.constants import VDAS_ACCEL_JERK_MAX, VDAS_DECEL_JERK_MAX +from opendbc.car.tesla.preap.virtual_das import FeedforwardModel, GRAVITY, VirtualDAS CONTROL_DT_S = 0.02 @@ -25,12 +27,59 @@ STEP_GRADE_ACCEL_MPS2 = 0.40 MATRIX_GRADE_ACCEL_MPS2 = 0.30 STEP_RESPONSE_TOLERANCE_MPS2 = 0.30 +STEP_RESPONSE_UPHILL_OVERSHOOT_LIMIT_MPS2 = 0.10 STEP_RESPONSE_MIN_DRIFT_IMPROVEMENT_MPS = 0.07 STEP_RESPONSE_MIN_PEDAL_DELTA_DI = 2.0 STEADY_MEAN_TOLERANCE_MPS2 = 0.12 STEADY_PEAK_TOLERANCE_MPS2 = 0.18 STEADY_SPEED_DRIFT_MPS = 2.5 PHYSICAL_RAIL_MARGIN_DI = 0.25 +ROUTE_PLANT_PEDAL_DI_BP = [-5.0, -2.0, 0.0, 3.0, 8.0, 15.0, 22.0, 27.0, 35.0, 50.0] +ROUTE_PLANT_NET_ACCEL_BP = [-1.05, -0.62, -0.50, -0.36, 0.0, 0.50, 1.15, 1.65, 2.10, 2.45] +ROUTE_INITIAL_TARGET_MPS2 = -0.57 +ROUTE_PEAK_TARGET_MPS2 = 0.75 +ROUTE_TARGET_RAMP_UP_S = 6.25 +ROUTE_TARGET_HOLD_S = 2.50 +ROUTE_TARGET_RAMP_DOWN_S = 2.25 +ROUTE_INITIAL_PEDAL_DI = -1.85 +ROUTE_SPEED_MPS = 20.0 +ROUTE_PEAK_ACCEL_LIMIT_MPS2 = 1.00 +ROUTE_UNCERTAIN_PEAK_ACCEL_LIMIT_MPS2 = 1.10 +ROUTE_POSITIVE_EXCESS_LIMIT_MPS = 0.75 +ROUTE_POSITIVE_JERK_LIMIT_MPS3 = 1.00 +ROUTE_FINAL_TRACKING_ERROR_LIMIT_MPS2 = 0.15 +RESIDUAL_BUILD_LOAD_MPS2 = 0.443 +NEGATIVE_HANDOFF_LOAD_MPS2 = 0.20 +NEGATIVE_HANDOFF_TARGET_MPS2 = -0.20 +RESIDUAL_BUILD_RAMP_S = 0.50 +RESIDUAL_BUILD_HOLD_S = 12.0 +NEGATIVE_HANDOFF_RAMP_S = 0.80 +NEGATIVE_HANDOFF_HOLD_S = 1.0 +NEGATIVE_HANDOFF_SETTLED_WINDOW_S = 1.0 +NEGATIVE_HANDOFF_PEDAL_MARGIN_DI = 0.05 +PEDAL_PLANT_ACCEL_PER_DI_MPS2 = 0.063 +NEGATIVE_HANDOFF_MAX_PEDAL_STEP_DI = 0.50 +NEGATIVE_HANDOFF_MAX_EFFORT_JERK_MPS3 = 0.75 +NEGATIVE_HANDOFF_MAX_UNDERSHOOT_MPS2 = 0.12 +NEGATIVE_HANDOFF_GRADE_SEPARATION_TOLERANCE_MPS2 = 5e-5 +NEAR_ZERO_TARGET_MPS2 = 0.02 +NEAR_ZERO_HALF_CYCLE_S = 0.20 +NEAR_ZERO_CYCLE_COUNT = 6 +NEAR_ZERO_MAX_TRIM_LOSS_MPS2 = 0.05 +FULL_BRAKE_TARGET_MPS2 = -0.30 +STILL_NEGATIVE_RECOVERY_TARGET_MPS2 = -0.04 +FULL_BRAKE_HOLD_S = 0.60 +STILL_NEGATIVE_RECOVERY_HOLD_S = 2.0 +SPEED_RAMP_TARGET_ACCELERATIONS_MPS2 = (0.50, 0.75) +SPEED_RAMP_WARMUP_S = 10.0 +SPEED_RAMP_TIMEOUT_S = 45.0 +SPEED_RAMP_INITIAL_MPS = 5.0 +SPEED_RAMP_FINAL_MPS = 20.0 +SPEED_RAMP_ROLLING_WINDOW_S = 0.75 +SPEED_RAMP_DROOP_LIMIT_MPS2 = 0.075 +SPEED_RAMP_TRACKING_ERROR_LIMIT_MPS = 0.75 +SPEED_RAMP_JERK_LIMIT_MPS3 = 1.00 +SPEED_RAMP_PEAK_ACCEL_LIMIT_MPS2 = 1.00 @dataclass(frozen=True) @@ -65,6 +114,622 @@ class PedalPlantSample: pedal_di: float +@dataclass(frozen=True) +class RoutePlantCase: + delay_s: float + tau_s: float + output_scale: float + + +@dataclass(frozen=True) +class RoutePlantSample: + elapsed_s: float + target_acceleration_mps2: float + net_acceleration_mps2: float + pedal_di: float + + +@dataclass(frozen=True) +class SpeedRampPlantSample: + speed_mps: float + net_acceleration_mps2: float + pedal_di: float + + +@dataclass(frozen=True) +class ResidualHandoffSample: + target_acceleration_mps2: float + net_acceleration_mps2: float + output: float + + +NOMINAL_ROUTE_PLANT = RoutePlantCase(0.40, 0.25, 1.0) +UNCERTAIN_ROUTE_PLANTS = ( + RoutePlantCase(0.30, 0.20, 0.9), + NOMINAL_ROUTE_PLANT, + RoutePlantCase(0.50, 0.35, 1.1), +) + + +def route_target_acceleration(elapsed_s: float) -> float: + if elapsed_s <= ROUTE_TARGET_RAMP_UP_S: + ramp_fraction = elapsed_s / ROUTE_TARGET_RAMP_UP_S + return ROUTE_INITIAL_TARGET_MPS2 + ramp_fraction * ( + ROUTE_PEAK_TARGET_MPS2 - ROUTE_INITIAL_TARGET_MPS2 + ) + + hold_end_s = ROUTE_TARGET_RAMP_UP_S + ROUTE_TARGET_HOLD_S + if elapsed_s <= hold_end_s: + return ROUTE_PEAK_TARGET_MPS2 + + ramp_down_fraction = (elapsed_s - hold_end_s) / ROUTE_TARGET_RAMP_DOWN_S + return ROUTE_PEAK_TARGET_MPS2 * (1.0 - ramp_down_fraction) + + +def run_route_shaped_acceleration( + plant: RoutePlantCase, + monkeypatch, +) -> list[RoutePlantSample]: + """Exercise the fallback controller against fixed, field-observed plant anchors.""" + monkeypatch.setattr( + virtual_das, + "nap_conf", + SimpleNamespace(get_pedal_profile_values=lambda: [50.0] * len(virtual_das.PEDAL_BP)), + ) + monkeypatch.setattr( + virtual_das, + "get_zero_torque", + lambda: SimpleNamespace(get=lambda _speed_mps: 0.0), + ) + + controller = VirtualDAS(dt=CONTROL_DT_S) + controller.ff_model = FeedforwardModel(table_path="/nonexistent") + controller.reset( + measured_accel=ROUTE_INITIAL_TARGET_MPS2, + commanded_accel=ROUTE_INITIAL_TARGET_MPS2, + pedal_di_init=ROUTE_INITIAL_PEDAL_DI, + ) + + pedal_di = ROUTE_INITIAL_PEDAL_DI + net_acceleration_mps2 = ROUTE_INITIAL_TARGET_MPS2 + delayed_pedals_di = [pedal_di] * round(plant.delay_s / CONTROL_DT_S) + plant_alpha = CONTROL_DT_S / (plant.tau_s + CONTROL_DT_S) + duration_s = ROUTE_TARGET_RAMP_UP_S + ROUTE_TARGET_HOLD_S + ROUTE_TARGET_RAMP_DOWN_S + samples = [] + + for step in range(round(duration_s / CONTROL_DT_S)): + elapsed_s = (step + 1) * CONTROL_DT_S + target_acceleration_mps2 = route_target_acceleration(elapsed_s) + pedal_di = controller.update( + target_acceleration_mps2, + v_ego=ROUTE_SPEED_MPS, + prev_pedal_di=pedal_di, + a_ego=net_acceleration_mps2, + freeze_integrator=False, + orientation_ned=[0.0, 0.0, 0.0], + ) + applied_pedal_di = delayed_pedals_di.pop(0) + delayed_pedals_di.append(pedal_di) + plant_target_acceleration_mps2 = float(interp( + applied_pedal_di, + ROUTE_PLANT_PEDAL_DI_BP, + ROUTE_PLANT_NET_ACCEL_BP, + )) * plant.output_scale + net_acceleration_mps2 += plant_alpha * ( + plant_target_acceleration_mps2 - net_acceleration_mps2 + ) + samples.append(RoutePlantSample( + elapsed_s=elapsed_s, + target_acceleration_mps2=target_acceleration_mps2, + net_acceleration_mps2=net_acceleration_mps2, + pedal_di=pedal_di, + )) + + return samples + + +def peak_delivered_acceleration(samples: list[RoutePlantSample]) -> float: + return max(sample.net_acceleration_mps2 for sample in samples) + + +def run_residual_trim_handoff( + *, + grade_acceleration_mps2: float, + identity_feedforward: bool, + monkeypatch, +) -> list[ResidualHandoffSample]: + """Build residual road-load trim, then cross into a finite-jerk decel request.""" + monkeypatch.setattr( + virtual_das, + "nap_conf", + SimpleNamespace(get_pedal_profile_values=lambda: [50.0] * len(virtual_das.PEDAL_BP)), + ) + monkeypatch.setattr( + virtual_das, + "get_zero_torque", + lambda: SimpleNamespace(get=lambda _speed_mps: COAST_PEDAL_DI), + ) + + controller = VirtualDAS(dt=CONTROL_DT_S) + orientation_ned = [0.0, math.asin(grade_acceleration_mps2 / GRAVITY), 0.0] + for _ in range(round(GRADE_ESTIMATOR_SETTLING_S / CONTROL_DT_S)): + controller.observe(a_ego=0.0, orientation_ned=orientation_ned) + + initial_output = grade_acceleration_mps2 if identity_feedforward else COAST_PEDAL_DI + controller.reset( + measured_accel=0.0, + commanded_accel=0.0, + pedal_di_init=initial_output, + preserve_grade=True, + ) + if identity_feedforward: + monkeypatch.setattr( + controller, + "_feedforward", + lambda acceleration_effort_mps2, _speed_mps: acceleration_effort_mps2, + ) + + output = initial_output + net_acceleration_mps2 = 0.0 + delayed_outputs = [output] * round(PLANT_DELAY_S / CONTROL_DT_S) + plant_alpha = CONTROL_DT_S / (PLANT_TAU_S + CONTROL_DT_S) + phases = ( + (RESIDUAL_BUILD_RAMP_S, 0.0, RESIDUAL_BUILD_LOAD_MPS2), + (RESIDUAL_BUILD_HOLD_S, 0.0, RESIDUAL_BUILD_LOAD_MPS2), + (NEGATIVE_HANDOFF_RAMP_S, NEGATIVE_HANDOFF_TARGET_MPS2, NEGATIVE_HANDOFF_LOAD_MPS2), + (NEGATIVE_HANDOFF_HOLD_S, NEGATIVE_HANDOFF_TARGET_MPS2, NEGATIVE_HANDOFF_LOAD_MPS2), + ) + target_acceleration_mps2 = 0.0 + residual_road_load_mps2 = 0.0 + samples = [] + + for duration_s, target_end_mps2, load_end_mps2 in phases: + frame_count = round(duration_s / CONTROL_DT_S) + target_start_mps2 = target_acceleration_mps2 + load_start_mps2 = residual_road_load_mps2 + for frame_index in range(1, frame_count + 1): + phase_fraction = frame_index / frame_count + target_acceleration_mps2 = target_start_mps2 + ( + target_end_mps2 - target_start_mps2 + ) * phase_fraction + residual_road_load_mps2 = load_start_mps2 + ( + load_end_mps2 - load_start_mps2 + ) * phase_fraction + output = controller.update( + target_acceleration_mps2, + v_ego=25.0, + prev_pedal_di=output, + a_ego=net_acceleration_mps2, + freeze_integrator=False, + orientation_ned=orientation_ned, + ) + applied_output = delayed_outputs.pop(0) + delayed_outputs.append(output) + if identity_feedforward: + plant_target_acceleration_mps2 = ( + applied_output - grade_acceleration_mps2 - residual_road_load_mps2 + ) + else: + plant_target_acceleration_mps2 = ( + (applied_output - COAST_PEDAL_DI) * PEDAL_PLANT_ACCEL_PER_DI_MPS2 + - grade_acceleration_mps2 + - residual_road_load_mps2 + ) + net_acceleration_mps2 += plant_alpha * ( + plant_target_acceleration_mps2 - net_acceleration_mps2 + ) + samples.append(ResidualHandoffSample( + target_acceleration_mps2=target_acceleration_mps2, + net_acceleration_mps2=net_acceleration_mps2, + output=output, + )) + + return samples + + +def test_residual_trim_handoff_tracks_negative_command_without_harsh_pedal_step(monkeypatch): + samples = run_residual_trim_handoff( + grade_acceleration_mps2=0.0, + identity_feedforward=False, + monkeypatch=monkeypatch, + ) + settled_sample_count = round(NEGATIVE_HANDOFF_SETTLED_WINDOW_S / CONTROL_DT_S) + settled_samples = samples[-settled_sample_count:] + pedal_steps_di = [ + current.output - previous.output + for previous, current in zip(samples, samples[1:], strict=False) + ] + + assert all(sample.target_acceleration_mps2 <= -0.15 for sample in settled_samples) + assert max(abs(step_di) for step_di in pedal_steps_di) <= NEGATIVE_HANDOFF_MAX_PEDAL_STEP_DI + # This fixture ends with road load equal to the negative target magnitude, + # making coast its equilibrium. The regen-side margin is not a universal + # rule for negative commands under other road loads. + assert max(sample.output for sample in settled_samples) <= ( + COAST_PEDAL_DI - NEGATIVE_HANDOFF_PEDAL_MARGIN_DI + ) + assert min( + sample.net_acceleration_mps2 - sample.target_acceleration_mps2 + for sample in settled_samples + ) >= -NEGATIVE_HANDOFF_MAX_UNDERSHOOT_MPS2 + + +def test_negative_command_handoff_keeps_grade_effort_separate(monkeypatch): + flat_samples = run_residual_trim_handoff( + grade_acceleration_mps2=0.0, + identity_feedforward=True, + monkeypatch=monkeypatch, + ) + grade_acceleration_mps2 = 0.35 + uphill_samples = run_residual_trim_handoff( + grade_acceleration_mps2=grade_acceleration_mps2, + identity_feedforward=True, + monkeypatch=monkeypatch, + ) + settled_sample_count = round(NEGATIVE_HANDOFF_SETTLED_WINDOW_S / CONTROL_DT_S) + flat_settled_outputs = [sample.output for sample in flat_samples[-settled_sample_count:]] + uphill_settled_outputs = [sample.output for sample in uphill_samples[-settled_sample_count:]] + flat_effort_jerks_mps3 = [ + (current.output - previous.output) / CONTROL_DT_S + for previous, current in zip(flat_samples, flat_samples[1:], strict=False) + ] + + assert max(abs(jerk_mps3) for jerk_mps3 in flat_effort_jerks_mps3) <= NEGATIVE_HANDOFF_MAX_EFFORT_JERK_MPS3 + assert uphill_settled_outputs == pytest.approx([ + flat_output + grade_acceleration_mps2 + for flat_output in flat_settled_outputs + ], abs=NEGATIVE_HANDOFF_GRADE_SEPARATION_TOLERANCE_MPS2) + + +def test_repeated_near_zero_crossings_preserve_learned_disturbance_trim(monkeypatch): + monkeypatch.setattr( + virtual_das, + "nap_conf", + SimpleNamespace(get_pedal_profile_values=lambda: [50.0] * len(virtual_das.PEDAL_BP)), + ) + controller = VirtualDAS(dt=CONTROL_DT_S) + monkeypatch.setattr( + controller, + "_feedforward", + lambda acceleration_effort_mps2, _speed_mps: acceleration_effort_mps2, + ) + controller.reset(measured_accel=0.0, commanded_accel=0.0, pedal_di_init=0.0) + output = 0.0 + net_acceleration_mps2 = 0.0 + plant_alpha = CONTROL_DT_S / (PLANT_TAU_S + CONTROL_DT_S) + delayed_outputs = [output] * round(PLANT_DELAY_S / CONTROL_DT_S) + + for _ in range(round(RESIDUAL_BUILD_HOLD_S / CONTROL_DT_S)): + output = controller.update( + 0.0, + v_ego=25.0, + prev_pedal_di=output, + a_ego=net_acceleration_mps2, + freeze_integrator=False, + orientation_ned=[0.0, 0.0, 0.0], + ) + applied_output = delayed_outputs.pop(0) + delayed_outputs.append(output) + plant_target_acceleration_mps2 = applied_output - RESIDUAL_BUILD_LOAD_MPS2 + net_acceleration_mps2 += plant_alpha * ( + plant_target_acceleration_mps2 - net_acceleration_mps2 + ) + + learned_trim_output = output + crossing_outputs = [] + target_acceleration_mps2 = 0.0 + half_cycle_steps = round(NEAR_ZERO_HALF_CYCLE_S / CONTROL_DT_S) + for half_cycle_index in range(NEAR_ZERO_CYCLE_COUNT * 2): + target_end_mps2 = NEAR_ZERO_TARGET_MPS2 * (-1.0 if half_cycle_index % 2 == 0 else 1.0) + target_start_mps2 = target_acceleration_mps2 + for step_index in range(1, half_cycle_steps + 1): + target_acceleration_mps2 = target_start_mps2 + ( + target_end_mps2 - target_start_mps2 + ) * step_index / half_cycle_steps + output = controller.update( + target_acceleration_mps2, + v_ego=25.0, + prev_pedal_di=output, + a_ego=net_acceleration_mps2, + freeze_integrator=False, + orientation_ned=[0.0, 0.0, 0.0], + ) + applied_output = delayed_outputs.pop(0) + delayed_outputs.append(output) + plant_target_acceleration_mps2 = applied_output - RESIDUAL_BUILD_LOAD_MPS2 + net_acceleration_mps2 += plant_alpha * ( + plant_target_acceleration_mps2 - net_acceleration_mps2 + ) + crossing_outputs.append(output) + + assert min(crossing_outputs) >= learned_trim_output - NEAR_ZERO_MAX_TRIM_LOSS_MPS2 + assert max( + abs(current - previous) + for previous, current in zip(crossing_outputs, crossing_outputs[1:], strict=False) + ) <= NEGATIVE_HANDOFF_MAX_EFFORT_JERK_MPS3 * CONTROL_DT_S + + +def run_full_brake_and_still_negative_recovery( + *, + identity_feedforward: bool, + monkeypatch, +) -> tuple[float, list[float], list[float]]: + monkeypatch.setattr( + virtual_das, + "nap_conf", + SimpleNamespace(get_pedal_profile_values=lambda: [50.0] * len(virtual_das.PEDAL_BP)), + ) + monkeypatch.setattr( + virtual_das, + "get_zero_torque", + lambda: SimpleNamespace(get=lambda _speed_mps: COAST_PEDAL_DI), + ) + controller = VirtualDAS(dt=CONTROL_DT_S) + output = 0.0 if identity_feedforward else COAST_PEDAL_DI + if identity_feedforward: + monkeypatch.setattr( + controller, + "_feedforward", + lambda acceleration_effort_mps2, _speed_mps: acceleration_effort_mps2, + ) + controller.reset(measured_accel=0.0, commanded_accel=0.0, pedal_di_init=output) + + net_acceleration_mps2 = 0.0 + plant_alpha = CONTROL_DT_S / (PLANT_TAU_S + CONTROL_DT_S) + delayed_outputs = [output] * round(PLANT_DELAY_S / CONTROL_DT_S) + for _ in range(round(RESIDUAL_BUILD_HOLD_S / CONTROL_DT_S)): + output = controller.update( + 0.0, + v_ego=25.0, + prev_pedal_di=output, + a_ego=net_acceleration_mps2, + freeze_integrator=False, + orientation_ned=[0.0, 0.0, 0.0], + ) + applied_output = delayed_outputs.pop(0) + delayed_outputs.append(output) + acceleration_effort_mps2 = ( + applied_output + if identity_feedforward + else (applied_output - COAST_PEDAL_DI) * PEDAL_PLANT_ACCEL_PER_DI_MPS2 + ) + plant_target_acceleration_mps2 = acceleration_effort_mps2 - RESIDUAL_BUILD_LOAD_MPS2 + net_acceleration_mps2 += plant_alpha * ( + plant_target_acceleration_mps2 - net_acceleration_mps2 + ) + + learned_trim_output = output + braking_outputs = [] + for _ in range(round(FULL_BRAKE_HOLD_S / CONTROL_DT_S)): + output = controller.update( + FULL_BRAKE_TARGET_MPS2, + v_ego=25.0, + prev_pedal_di=output, + a_ego=0.0, + freeze_integrator=False, + orientation_ned=[0.0, 0.0, 0.0], + ) + braking_outputs.append(output) + + recovery_outputs = [] + for _ in range(round(STILL_NEGATIVE_RECOVERY_HOLD_S / CONTROL_DT_S)): + output = controller.update( + STILL_NEGATIVE_RECOVERY_TARGET_MPS2, + v_ego=25.0, + prev_pedal_di=output, + a_ego=0.0, + freeze_integrator=False, + orientation_ned=[0.0, 0.0, 0.0], + ) + recovery_outputs.append(output) + + return learned_trim_output, braking_outputs, recovery_outputs + + +def test_handoff_shares_command_jerk_budget_in_both_directions(monkeypatch): + learned_trim_effort, braking_efforts, recovery_efforts = run_full_brake_and_still_negative_recovery( + identity_feedforward=True, + monkeypatch=monkeypatch, + ) + braking_jerks_mps3 = [ + (current - previous) / CONTROL_DT_S + for previous, current in zip([learned_trim_effort] + braking_efforts[:-1], braking_efforts, strict=True) + ] + recovery_jerks_mps3 = [ + (current - previous) / CONTROL_DT_S + for previous, current in zip([braking_efforts[-1]] + recovery_efforts[:-1], recovery_efforts, strict=True) + ] + + minimum_braking_jerk_mps3 = min(braking_jerks_mps3) + maximum_recovery_jerk_mps3 = max(recovery_jerks_mps3) + assert minimum_braking_jerk_mps3 >= -VDAS_DECEL_JERK_MAX or math.isclose( + minimum_braking_jerk_mps3, + -VDAS_DECEL_JERK_MAX, + abs_tol=1e-12, + ) + assert maximum_recovery_jerk_mps3 <= VDAS_ACCEL_JERK_MAX or math.isclose( + maximum_recovery_jerk_mps3, + VDAS_ACCEL_JERK_MAX, + abs_tol=1e-12, + ) + + +def test_handoff_full_brake_and_recovery_stay_inside_comfort_pedal_step(monkeypatch): + learned_trim_pedal_di, braking_pedals_di, recovery_pedals_di = run_full_brake_and_still_negative_recovery( + identity_feedforward=False, + monkeypatch=monkeypatch, + ) + braking_steps_di = [ + current - previous + for previous, current in zip([learned_trim_pedal_di] + braking_pedals_di[:-1], braking_pedals_di, strict=True) + ] + recovery_steps_di = [ + current - previous + for previous, current in zip([braking_pedals_di[-1]] + recovery_pedals_di[:-1], recovery_pedals_di, strict=True) + ] + + assert min(braking_steps_di) >= -NEGATIVE_HANDOFF_MAX_PEDAL_STEP_DI + assert max(recovery_steps_di) <= NEGATIVE_HANDOFF_MAX_PEDAL_STEP_DI + + +def run_speed_ramp_acceleration( + target_acceleration_mps2: float, + monkeypatch, +) -> tuple[float, list[SpeedRampPlantSample]]: + """Integrate speed through the fallback transition using observed plant anchors.""" + monkeypatch.setattr( + virtual_das, + "nap_conf", + SimpleNamespace(get_pedal_profile_values=lambda: [50.0] * len(virtual_das.PEDAL_BP)), + ) + monkeypatch.setattr( + virtual_das, + "get_zero_torque", + lambda: SimpleNamespace(get=lambda _speed_mps: 0.0), + ) + + controller = VirtualDAS(dt=CONTROL_DT_S) + controller.ff_model = FeedforwardModel(table_path="/nonexistent") + pedal_di = float(interp( + target_acceleration_mps2, + ROUTE_PLANT_NET_ACCEL_BP, + ROUTE_PLANT_PEDAL_DI_BP, + )) + net_acceleration_mps2 = target_acceleration_mps2 + speed_mps = SPEED_RAMP_INITIAL_MPS + controller.reset( + measured_accel=net_acceleration_mps2, + commanded_accel=target_acceleration_mps2, + pedal_di_init=pedal_di, + ) + + delayed_pedals_di = [pedal_di] * round(NOMINAL_ROUTE_PLANT.delay_s / CONTROL_DT_S) + plant_alpha = CONTROL_DT_S / (NOMINAL_ROUTE_PLANT.tau_s + CONTROL_DT_S) + warmup_steps = round(SPEED_RAMP_WARMUP_S / CONTROL_DT_S) + timeout_steps = round(SPEED_RAMP_TIMEOUT_S / CONTROL_DT_S) + warmup_accelerations_mps2 = [] + samples = [] + + for step in range(warmup_steps + timeout_steps): + pedal_di = controller.update( + target_acceleration_mps2, + v_ego=speed_mps, + prev_pedal_di=pedal_di, + a_ego=net_acceleration_mps2, + freeze_integrator=False, + orientation_ned=[0.0, 0.0, 0.0], + ) + applied_pedal_di = delayed_pedals_di.pop(0) + delayed_pedals_di.append(pedal_di) + plant_target_acceleration_mps2 = float(interp( + applied_pedal_di, + ROUTE_PLANT_PEDAL_DI_BP, + ROUTE_PLANT_NET_ACCEL_BP, + )) + net_acceleration_mps2 += plant_alpha * ( + plant_target_acceleration_mps2 - net_acceleration_mps2 + ) + + if step < warmup_steps: + warmup_accelerations_mps2.append(net_acceleration_mps2) + else: + speed_mps += net_acceleration_mps2 * CONTROL_DT_S + samples.append(SpeedRampPlantSample( + speed_mps=speed_mps, + net_acceleration_mps2=net_acceleration_mps2, + pedal_di=pedal_di, + )) + if speed_mps >= SPEED_RAMP_FINAL_MPS: + break + + rolling_window_steps = round(SPEED_RAMP_ROLLING_WINDOW_S / CONTROL_DT_S) + warmup_mean_acceleration_mps2 = sum( + warmup_accelerations_mps2[-rolling_window_steps:] + ) / rolling_window_steps + return warmup_mean_acceleration_mps2, samples + + +def assert_speed_ramp_transition_is_smooth( + target_acceleration_mps2: float, + monkeypatch, +) -> None: + warmup_mean_acceleration_mps2, samples = run_speed_ramp_acceleration( + target_acceleration_mps2, + monkeypatch, + ) + jerks_mps3 = [ + (current.net_acceleration_mps2 - previous.net_acceleration_mps2) / CONTROL_DT_S + for previous, current in zip(samples, samples[1:], strict=False) + ] + rolling_window_steps = round(SPEED_RAMP_ROLLING_WINDOW_S / CONTROL_DT_S) + rolling_mean_accelerations_mps2 = [ + sum(sample.net_acceleration_mps2 for sample in samples[start:start + rolling_window_steps]) / rolling_window_steps + for start in range(len(samples) - rolling_window_steps + 1) + ] + rolling_mean_droop_mps2 = warmup_mean_acceleration_mps2 - min(rolling_mean_accelerations_mps2) + accumulated_tracking_error_mps = sum( + max(target_acceleration_mps2 - sample.net_acceleration_mps2, 0.0) * CONTROL_DT_S + for sample in samples + ) + peak_absolute_jerk_mps3 = max(abs(jerk_mps3) for jerk_mps3 in jerks_mps3) + peak_acceleration_mps2 = max(sample.net_acceleration_mps2 for sample in samples) + + assert samples[-1].speed_mps >= SPEED_RAMP_FINAL_MPS + assert rolling_mean_droop_mps2 <= SPEED_RAMP_DROOP_LIMIT_MPS2, ( + f"{SPEED_RAMP_ROLLING_WINDOW_S:.2f} s mean drooped {rolling_mean_droop_mps2:.3f} m/s²; " + + f"warmup mean {warmup_mean_acceleration_mps2:.3f} m/s²; " + + f"accumulated tracking error {accumulated_tracking_error_mps:.3f} m/s" + ) + assert accumulated_tracking_error_mps <= SPEED_RAMP_TRACKING_ERROR_LIMIT_MPS + assert peak_absolute_jerk_mps3 <= SPEED_RAMP_JERK_LIMIT_MPS3 + assert peak_acceleration_mps2 <= SPEED_RAMP_PEAK_ACCEL_LIMIT_MPS2 + assert min(sample.pedal_di for sample in samples) > virtual_das.PEDAL_DI_MIN + assert max(sample.pedal_di for sample in samples) < 50.0 + + +def test_speed_ramp_through_fallback_transition_is_smooth(monkeypatch): + assert_speed_ramp_transition_is_smooth(SPEED_RAMP_TARGET_ACCELERATIONS_MPS2[0], monkeypatch) + + +def test_speed_ramp_at_route_peak_remains_bounded(monkeypatch): + assert_speed_ramp_transition_is_smooth(SPEED_RAMP_TARGET_ACCELERATIONS_MPS2[1], monkeypatch) + + +def test_route_shaped_positive_transition_bounds_delivered_acceleration(monkeypatch): + samples = run_route_shaped_acceleration(NOMINAL_ROUTE_PLANT, monkeypatch) + positive_tracking_excess_mps = sum( + max(sample.net_acceleration_mps2 - sample.target_acceleration_mps2, 0.0) * CONTROL_DT_S + for sample in samples + ) + peak_positive_jerk_mps3 = max( + (current.net_acceleration_mps2 - previous.net_acceleration_mps2) / CONTROL_DT_S + for previous, current in zip(samples, samples[1:], strict=False) + ) + final_samples = samples[-round(1.0 / CONTROL_DT_S):] + final_mean_tracking_error_mps2 = sum( + sample.net_acceleration_mps2 - sample.target_acceleration_mps2 + for sample in final_samples + ) / len(final_samples) + + peak_acceleration_mps2 = peak_delivered_acceleration(samples) + assert peak_acceleration_mps2 <= ROUTE_PEAK_ACCEL_LIMIT_MPS2, ( + f"delivered peak {peak_acceleration_mps2:.3f} m/s²; " + + f"positive tracking excess {positive_tracking_excess_mps:.3f} m/s" + ) + assert positive_tracking_excess_mps <= ROUTE_POSITIVE_EXCESS_LIMIT_MPS + assert peak_positive_jerk_mps3 <= ROUTE_POSITIVE_JERK_LIMIT_MPS3 + assert abs(final_mean_tracking_error_mps2) <= ROUTE_FINAL_TRACKING_ERROR_LIMIT_MPS2 + assert min(sample.pedal_di for sample in samples) > virtual_das.PEDAL_DI_MIN + assert max(sample.pedal_di for sample in samples) < 50.0 + + +@pytest.mark.parametrize("plant", UNCERTAIN_ROUTE_PLANTS) +def test_route_shaped_positive_transition_survives_plant_uncertainty(monkeypatch, plant): + samples = run_route_shaped_acceleration(plant, monkeypatch) + + assert peak_delivered_acceleration(samples) <= ROUTE_UNCERTAIN_PEAK_ACCEL_LIMIT_MPS2 + + def run_grade_hold(*, speed_mps: float, grade_acceleration_mps2: float, duration_s: float, monkeypatch) -> list[GradePlantSample]: monkeypatch.setattr( @@ -263,6 +928,8 @@ def test_flat_to_grade_step_improves_on_no_pitch_baseline_at_1p5_seconds( ) / len(checkpoint_samples) assert abs(mean_net_acceleration_mps2) <= STEP_RESPONSE_TOLERANCE_MPS2 + if uphill_load_mps2 > 0.0: + assert mean_net_acceleration_mps2 <= STEP_RESPONSE_UPHILL_OVERSHOOT_LIMIT_MPS2 assert ( abs(samples[-1].speed_delta_mps) + STEP_RESPONSE_MIN_DRIFT_IMPROVEMENT_MPS <= abs(no_pitch_samples[-1].speed_delta_mps) diff --git a/opendbc/car/tesla/preap/tests/test_virtual_das.py b/opendbc/car/tesla/preap/tests/test_virtual_das.py index fe46ed06ef3..64eea430312 100644 --- a/opendbc/car/tesla/preap/tests/test_virtual_das.py +++ b/opendbc/car/tesla/preap/tests/test_virtual_das.py @@ -24,6 +24,7 @@ ) COMFORT_SNAP_MAX = 4.0 # m/s^4 +HIGH_SPEED_FALLBACK_FIELD_SCALE = 0.65 # --- Phase 1: JerkLimiter --- @@ -290,10 +291,11 @@ def test_steady_state_zero_accel(self): def test_steady_state_max_accel(self): vdas = VirtualDAS(dt=0.02) - expected_max = float(np.interp(15.0, PEDAL_BP, PEDAL_MAX_VALUES)) + physical_max = float(np.interp(15.0, PEDAL_BP, PEDAL_MAX_VALUES)) for _ in range(500): di = vdas.update(ACCEL_MAX, v_ego=15.0, prev_pedal_di=vdas.prev_pedal_di) - assert abs(di - expected_max) < 0.5 + assert di == pytest.approx(physical_max * HIGH_SPEED_FALLBACK_FIELD_SCALE, abs=0.01) + assert di < physical_max def test_steady_state_max_regen(self): vdas = VirtualDAS(dt=0.02) @@ -440,7 +442,7 @@ def test_negative_accel_produces_regen(self): di = vdas.update(-1.0, v_ego=15.0, prev_pedal_di=vdas.prev_pedal_di) assert di < PEDAL_DI_ZERO - def test_speed_dependent_max(self): + def test_field_correction_is_continuous_from_five_to_thirty_mps(self): vdas_slow = VirtualDAS(dt=0.02) vdas_fast = VirtualDAS(dt=0.02) @@ -448,7 +450,16 @@ def test_speed_dependent_max(self): di_slow = vdas_slow.update(ACCEL_MAX, v_ego=5.0, prev_pedal_di=vdas_slow.prev_pedal_di) di_fast = vdas_fast.update(ACCEL_MAX, v_ego=30.0, prev_pedal_di=vdas_fast.prev_pedal_di) - assert di_fast > di_slow + low_speed_physical_max = float(np.interp(5.0, PEDAL_BP, PEDAL_MAX_VALUES)) + high_speed_physical_max = float(np.interp(30.0, PEDAL_BP, PEDAL_MAX_VALUES)) + assert di_slow == pytest.approx( + low_speed_physical_max * HIGH_SPEED_FALLBACK_FIELD_SCALE, + abs=0.01, + ) + assert di_fast == pytest.approx( + high_speed_physical_max * HIGH_SPEED_FALLBACK_FIELD_SCALE, + abs=0.01, + ) # --- Phase 2: Inner PID + delay compensation --- @@ -517,8 +528,8 @@ def test_inner_feedback_holds_cruise_against_sustained_road_load(self, monkeypat coast_pedal_di = 3.0 accel_per_di_mps2 = 0.063 road_load_accel_mps2 = -0.40 - steady_error_bound_mps2 = 0.10 - settling_time_bound_s = 6.0 + steady_error_bound_mps2 = 0.105 + settling_time_bound_s = 7.0 simulation_dt_s = 0.02 simulation_steps = round(30.0 / simulation_dt_s) steady_window_steps = round(2.0 / simulation_dt_s) @@ -616,6 +627,82 @@ def test_integrator_accumulates_after_grace(self): a_ego=0.0, freeze_integrator=False) assert abs(vdas.inner_pid.i) > 0.01 + @pytest.mark.parametrize("persistent_error_mps2", [-0.09, -0.0196, 0.0196, 0.09]) + def test_persistent_sub_deadband_error_earns_residual_authority(self, persistent_error_mps2): + vdas = VirtualDAS(dt=0.02) + + for _ in range(round(5.0 / vdas.dt)): + vdas.update( + persistent_error_mps2, + v_ego=25.0, + prev_pedal_di=vdas.prev_pedal_di, + a_ego=0.0, + freeze_integrator=False, + ) + + assert abs(vdas.inner_pid.i) > 0.005 + assert vdas.inner_pid.i * persistent_error_mps2 > 0.0 + + def test_sub_deadband_sign_changing_noise_does_not_accumulate_residual_authority(self): + vdas = VirtualDAS(dt=0.02) + maximum_integral_mps2 = 0.0 + + for step in range(round(10.0 / vdas.dt)): + noisy_command_mps2 = 0.09 if (step // 25) % 2 == 0 else -0.09 + vdas.update( + noisy_command_mps2, + v_ego=25.0, + prev_pedal_di=vdas.prev_pedal_di, + a_ego=0.0, + freeze_integrator=False, + ) + maximum_integral_mps2 = max(maximum_integral_mps2, abs(vdas.inner_pid.i)) + + assert maximum_integral_mps2 < 1e-9 + + def test_sign_changing_sub_deadband_error_never_completes_dwell(self): + vdas = VirtualDAS(dt=0.02) + + selected_errors_mps2 = [ + vdas._gate_pid_error_noise( + 0.09 if (step // 5) % 2 == 0 else -0.09, + freeze_integrator=False, + ) + for step in range(round(10.0 / vdas.dt)) + ] + + assert selected_errors_mps2 == pytest.approx(np.zeros(len(selected_errors_mps2))) + + def test_persistent_error_dwell_restarts_after_freeze_observe_and_reset(self): + vdas = VirtualDAS(dt=0.02) + + def hold_small_error(duration_s, freeze_integrator=False): + for _ in range(round(duration_s / vdas.dt)): + vdas.update( + 0.09, + v_ego=25.0, + prev_pedal_di=vdas.prev_pedal_di, + a_ego=0.0, + freeze_integrator=freeze_integrator, + ) + + hold_small_error(0.8) + hold_small_error(vdas.dt, freeze_integrator=True) + hold_small_error(0.4) + assert vdas.inner_pid.i == 0.0 + + hold_small_error(1.0) + assert vdas.inner_pid.i > 0.0 + vdas.observe(a_ego=0.0) + hold_small_error(0.4) + assert vdas.inner_pid.i == 0.0 + + hold_small_error(1.0) + assert vdas.inner_pid.i > 0.0 + vdas.reset(measured_accel=0.0, commanded_accel=0.09) + hold_small_error(0.4) + assert vdas.inner_pid.i == 0.0 + def test_anti_windup(self): """Integrator should be bounded by acceleration-map authority.""" vdas = VirtualDAS(dt=0.02) @@ -698,8 +785,8 @@ class TestFeedforwardModel: def _fixtures(self, mock_nap_conf, mock_zero_torque): pass - def test_default_table_matches_legacy_at_grid_points(self): - """Default FF table should match the old 3-breakpoint interp at grid points.""" + def test_default_table_only_scales_positive_branch_above_zero_speed(self): + """The field correction must not alter regen or the 0 m/s fallback.""" from opendbc.car.tesla.preap.virtual_das import FeedforwardModel from opendbc.car.tesla.preap.ff_table_default import SPEED_BP, ACCEL_BP @@ -711,10 +798,12 @@ def test_default_table_matches_legacy_at_grid_points(self): expected = float(np.interp(accel, [REGEN_MAX, 0.0, ACCEL_MAX], [PEDAL_DI_MIN, 0.0, max_pedal])) - # FF model with zero_torque_di=0 should match legacy interp + if speed >= 5.0 and accel > 0.0: + expected *= HIGH_SPEED_FALLBACK_FIELD_SCALE got = ff.get(accel, speed, zero_torque_di=0.0) - assert abs(got - expected) < 0.5, \ + assert got == pytest.approx(expected, abs=0.01), ( f"Mismatch at speed={speed}, accel={accel}: got={got:.2f}, expected={expected:.2f}" + ) def test_zero_torque_shift_positive_accel(self): """Positive accel zt offset fades: full at accel=0, zero at ACCEL_MAX.""" diff --git a/opendbc/car/tesla/preap/virtual_das.py b/opendbc/car/tesla/preap/virtual_das.py index 211927be92b..e3786136ef2 100644 --- a/opendbc/car/tesla/preap/virtual_das.py +++ b/opendbc/car/tesla/preap/virtual_das.py @@ -40,11 +40,18 @@ FF_TABLE_PATH = "/data/vdas_ff_table.json" -# Inner PID error deadband: errors below this threshold are zeroed before -# entering the PID. Prevents integral accumulation from sensor noise and -# MPC jitter near steady-state. Applied to the error input, not the output, -# so there's no discontinuity in the correction signal. +# Inner PID error deadband: brief or sign-changing errors below this threshold +# are zeroed before entering the PID. A coherent same-sign residual earns +# integral authority after a dwell, so persistent road-load bias still closes. +# Applied to the error input, not the output, so the correction stays continuous. PID_ERROR_DEADBAND = 0.1 # m/s² +PID_PERSISTENT_ERROR_DWELL_S = 1.0 +# The negative-command handoff stays dormant around zero, then uses a smaller +# directional error band and a bounded slew to unwind only opposing trim. +NEGATIVE_HANDOFF_ACTIVATION_ACCEL = -0.05 # m/s² +NEGATIVE_HANDOFF_ERROR_DEADBAND = 0.02 # m/s² +NEGATIVE_HANDOFF_INTEGRAL_SLEW = 0.25 # m/s³ +NEGATIVE_HANDOFF_PEDAL_STEP = 0.50 # DI/update GRAVITY = 9.81 # m/s² PITCH_LP_RC = 0.5 # low-pass filter RC for steady-state grade (seconds) @@ -438,6 +445,12 @@ def __init__(self, dt: float = 0.02): self.prev_a_ego_filtered = 0.0 self.a_ego_initialized = False self.pedal_ramp_limited_up = False + self.negative_handoff_pending = False + self.negative_handoff_active = False + self.negative_handoff_integral_at_crossing = 0.0 + self.prev_accel_effort = 0.0 + self.persistent_error_sign = 0 + self.persistent_error_elapsed_s = 0.0 def update(self, a_cmd: float, v_ego: float, prev_pedal_di: float, a_ego: float = 0.0, freeze_integrator: bool = False, @@ -459,7 +472,9 @@ def update(self, a_cmd: float, v_ego: float, prev_pedal_di: float, Returns: pedal_di: output in DI units (caller converts to voltage via di_to_pedal) """ + previous_a_limited = self.jerk_limiter.a_limited a_limited = self.jerk_limiter.update(a_cmd) + self._update_negative_handoff_state(previous_a_limited, a_limited) steady_grade_compensation, transient_pitch_compensation = self.grade_estimator.update( orientation_ned if orientation_ned is not None else []) @@ -487,11 +502,24 @@ def update(self, a_cmd: float, v_ego: float, prev_pedal_di: float, a_ego_future = a_ego_filtered + j_ego * future_t error = a_limited - a_ego_future - if abs(error) < PID_ERROR_DEADBAND: - error = 0.0 - - # Keep residual control in acceleration space. The PID can use only the - # authority left after desired acceleration and grade compensation. + self._shape_integral_for_negative_handoff(a_limited, error) + error = self._gate_pid_error_noise(error, freeze_integrator) + + # Keep residual control in acceleration space. Command and residual trim + # share one effort-jerk envelope during the negative handoff. + negative_handoff_in_progress = self.negative_handoff_pending or self.negative_handoff_active + if negative_handoff_in_progress: + previous_accel_effort = float(clip(self.prev_accel_effort, effort_min, effort_max)) + shared_effort_min = max( + effort_min, + previous_accel_effort - VDAS_DECEL_JERK_MAX * self.dt, + ) + shared_effort_max = min( + effort_max, + previous_accel_effort + VDAS_ACCEL_JERK_MAX * self.dt, + ) + else: + shared_effort_min, shared_effort_max = effort_min, effort_max self.inner_pid.neg_limit = effort_min - base_accel_effort self.inner_pid.pos_limit = effort_max - base_accel_effort self.inner_pid.i = float(clip( @@ -502,11 +530,15 @@ def update(self, a_cmd: float, v_ego: float, prev_pedal_di: float, integral_before_update = self.inner_pid.i accel_trim = float(self.inner_pid.update( error, speed=v_ego, freeze_integrator=freeze_integrator)) + requested_accel_effort = base_accel_effort + accel_trim accel_effort = float(clip( - base_accel_effort + accel_trim, - effort_min, - effort_max, + requested_accel_effort, + shared_effort_min, + shared_effort_max, )) + if negative_handoff_in_progress: + self.inner_pid.i += accel_effort - requested_accel_effort + self.prev_accel_effort = accel_effort pedal_di_unclipped = self._feedforward(accel_effort, v_ego) @@ -514,7 +546,17 @@ def update(self, a_cmd: float, v_ego: float, prev_pedal_di: float, max_pedal_value = float(interp(v_ego, PEDAL_BP, pedal_profile)) pedal_di_bounded = float(clip(pedal_di_unclipped, PEDAL_DI_MIN, max_pedal_value)) - pedal_di = self._rate_limit(pedal_di_bounded, prev_pedal_di, pedal_ramp_rate_up) + if negative_handoff_in_progress: + pedal_ramp_rate_up = min(pedal_ramp_rate_up, NEGATIVE_HANDOFF_PEDAL_STEP) + pedal_ramp_rate_down = NEGATIVE_HANDOFF_PEDAL_STEP + else: + pedal_ramp_rate_down = PEDAL_RAMP_RATE_DOWN + pedal_di = self._rate_limit( + pedal_di_bounded, + prev_pedal_di, + pedal_ramp_rate_up, + pedal_ramp_rate_down, + ) self.pedal_ramp_limited_up = pedal_di < pedal_di_bounded physical_bound_blocks_error = ( (pedal_di_bounded < pedal_di_unclipped and error > 0.0) @@ -537,6 +579,8 @@ def observe(self, a_ego: float, orientation_ned: list | None = None): self.prev_a_ego_filtered = a_ego_filtered self.a_ego_initialized = True self.inner_pid.reset() + self._reset_negative_handoff() + self._reset_persistent_error() def reset(self, measured_accel: float = 0.0, commanded_accel: float = 0.0, pedal_di_init: float = 0.0, preserve_grade: bool = False): @@ -556,6 +600,71 @@ def reset(self, measured_accel: float = 0.0, commanded_accel: float = 0.0, self.a_ego_initialized = True self.prev_pedal_di = pedal_di_init self.pedal_ramp_limited_up = False + preserved_grade_effort = ( + self.grade_estimator._steady_grade_compensation() + if preserve_grade + else 0.0 + ) + self.prev_accel_effort = float(clip( + commanded_accel + preserved_grade_effort, + REGEN_MAX, + ACCEL_MAX, + )) + self._reset_negative_handoff() + self._reset_persistent_error() + + def _gate_pid_error_noise(self, error: float, freeze_integrator: bool) -> float: + if abs(error) >= PID_ERROR_DEADBAND: + self._reset_persistent_error() + return error + + if freeze_integrator or error == 0.0: + self._reset_persistent_error() + return 0.0 + + error_sign = 1 if error > 0.0 else -1 + if error_sign != self.persistent_error_sign: + self.persistent_error_sign = error_sign + self.persistent_error_elapsed_s = self.dt + else: + self.persistent_error_elapsed_s += self.dt + + if self.persistent_error_elapsed_s < PID_PERSISTENT_ERROR_DWELL_S: + return 0.0 + return error + + def _reset_persistent_error(self): + self.persistent_error_sign = 0 + self.persistent_error_elapsed_s = 0.0 + + def _update_negative_handoff_state(self, previous_a_limited: float, a_limited: float): + if a_limited >= 0.0: + self._reset_negative_handoff() + return + + if previous_a_limited >= 0.0: + self.negative_handoff_pending = True + self.negative_handoff_integral_at_crossing = self.inner_pid.i + + if self.negative_handoff_pending and a_limited <= NEGATIVE_HANDOFF_ACTIVATION_ACCEL: + self.negative_handoff_pending = False + self.negative_handoff_active = True + + def _shape_integral_for_negative_handoff(self, a_limited: float, tracking_error: float): + needs_more_deceleration = tracking_error < -NEGATIVE_HANDOFF_ERROR_DEADBAND + if not self.negative_handoff_active or not needs_more_deceleration or self.inner_pid.i <= 0.0: + return + + # Rebase residual authority by the finite-jerk command change, but slew + # toward it so learned road-load trim cannot disappear in one update. + rebased_integral = max(0.0, self.negative_handoff_integral_at_crossing + a_limited) + maximum_integral_step = NEGATIVE_HANDOFF_INTEGRAL_SLEW * self.dt + self.inner_pid.i = max(rebased_integral, self.inner_pid.i - maximum_integral_step) + + def _reset_negative_handoff(self): + self.negative_handoff_pending = False + self.negative_handoff_active = False + self.negative_handoff_integral_at_crossing = 0.0 def _feedforward(self, a_cmd: float, v_ego: float) -> float: """Map acceleration to raw pedal DI via the finite 2D lookup table.""" @@ -563,10 +672,11 @@ def _feedforward(self, a_cmd: float, v_ego: float) -> float: return self.ff_model.get(a_cmd, v_ego, zero_torque_di) def _rate_limit(self, pedal_di: float, prev_pedal_di: float, - ramp_rate_up: float = PEDAL_RAMP_RATE_UP) -> float: + ramp_rate_up: float = PEDAL_RAMP_RATE_UP, + ramp_rate_down: float = PEDAL_RAMP_RATE_DOWN) -> float: """Safety backstop: asymmetric DI rate limit.""" return float(clip( pedal_di, - prev_pedal_di - PEDAL_RAMP_RATE_DOWN, + prev_pedal_di - ramp_rate_down, prev_pedal_di + ramp_rate_up, )) From 2282f562ec73b0513661db0f622251b5083098ad Mon Sep 17 00:00:00 2001 From: jjackbrandt Date: Wed, 22 Jul 2026 22:13:34 -0400 Subject: [PATCH 4/4] ci: make NAP branch checks authoritative Route nap and naponsp pushes and pull requests through a dedicated build, lint, car, and safety gate while preserving the upstream master matrix. Modernize the upstream model job, fix the MG test adapter's torque units, and enforce the single-feedback-owner test contract. --- .github/ci/test_tests_workflow.py | 74 +++++++++++++++++++++++++++ .github/workflows/tests.yml | 58 ++++++++++++++++----- opendbc/car/tesla/fingerprints.py | 2 +- opendbc/car/tesla/preap/interface.py | 2 +- opendbc/car/tesla/test_pedal_regen.py | 10 ++-- opendbc/safety/tests/test_mg.py | 2 +- 6 files changed, 126 insertions(+), 22 deletions(-) diff --git a/.github/ci/test_tests_workflow.py b/.github/ci/test_tests_workflow.py index aac901554a5..b64142c1920 100644 --- a/.github/ci/test_tests_workflow.py +++ b/.github/ci/test_tests_workflow.py @@ -4,6 +4,14 @@ WORKFLOW_PATH = Path(__file__).resolve().parents[1] / "workflows" / "tests.yml" +NAP_JOB_CONDITION = "".join(( + "if: ${{ startsWith(github.ref_name, 'nap-') || startsWith(github.base_ref, 'nap-') || ", + "startsWith(github.ref_name, 'naponsp-') || startsWith(github.base_ref, 'naponsp-') }}", +)) +UPSTREAM_JOB_CONDITION = "".join(( + "if: ${{ !startsWith(github.ref_name, 'nap-') && !startsWith(github.base_ref, 'nap-') && ", + "!startsWith(github.ref_name, 'naponsp-') && !startsWith(github.base_ref, 'naponsp-') }}", +)) def indented_block(document: str, header: str) -> str: @@ -33,6 +41,45 @@ def test_nap_branches_run_on_push(self): push_branches = indented_block(push_config, " branches:") self.assertRegex(push_branches, re.compile(r"^\s*-\s+['\"]?nap-\*['\"]?\s*$", re.MULTILINE)) + self.assertRegex(push_branches, re.compile(r"^\s*-\s+['\"]?naponsp-\*['\"]?\s*$", re.MULTILINE)) + + def test_nap_gate_routes_pushes_and_pull_requests(self): + nap_job = indented_block(self.workflow, " nap_tests:") + nap_lines = normalized_lines(nap_job) + + self.assertIn("name: NAP build and safety", nap_lines) + self.assertIn(NAP_JOB_CONDITION, nap_lines) + + def test_upstream_jobs_are_isolated_from_nap_branches(self): + for job_header in (" tests:", " safety_tests:", " mutation:", " test_models:"): + with self.subTest(job=job_header): + job = indented_block(self.workflow, job_header) + self.assertEqual(re.findall(r"^ if: .+$", job, re.MULTILINE), [f" {UPSTREAM_JOB_CONDITION}"]) + + def test_nap_gate_pins_build_lint_and_safety_suites(self): + nap_job = indented_block(self.workflow, " nap_tests:") + required_steps = ( + (" - name: Build NAP opendbc", ("scons -j$(nproc)",)), + (" - name: Lint NAP implementation and gates", ("ruff check",)), + (" - name: Run NAP car and safety suites", ( + "opendbc/car/tesla/preap/tests/", + "opendbc/safety/tests/test_tesla_preap.py", + "opendbc/safety/tests/test_mg.py", + )), + ) + + for step_header, required_commands in required_steps: + with self.subTest(step=step_header): + step = indented_block(nap_job, step_header) + self.assertNotRegex(step, r"^\s+(?:if|continue-on-error):", msg=f"{step_header} must be unconditional") + for command in required_commands: + self.assertIn(command, step) + + def test_nap_gate_cannot_be_skipped_or_soft_failed(self): + nap_job = indented_block(self.workflow, " nap_tests:") + + self.assertEqual(re.findall(r"^ if: .+$", nap_job, re.MULTILINE), [f" {NAP_JOB_CONDITION}"]) + self.assertIsNone(re.search(r"^\s+continue-on-error:", nap_job, re.MULTILINE)) def test_focused_longitudinal_job_is_present(self): focused_job = indented_block(self.workflow, " tesla_preap_longitudinal_regression:") @@ -66,6 +113,33 @@ def test_focused_longitudinal_job_cannot_be_skipped_or_soft_failed(self): self.assertIsNone(re.search(r"^\s*(?:if|continue-on-error)\s*:", focused_job, re.MULTILINE)) + def test_model_job_uses_supported_openpilot_setup(self): + model_job = indented_block(self.workflow, " test_models:") + model_lines = normalized_lines(model_job) + + self.assertIn("- run: ./tools/op.sh setup", model_lines) + self.assertNotIn("uses: ./.github/workflows/setup-with-retry", model_lines) + self.assertNotIn("setup-step.outputs.duration", model_job) + self.assertLess(model_job.index("repository: 'commaai/openpilot'"), model_job.index("- run: ./tools/op.sh setup")) + self.assertLess(model_job.index("- run: ./tools/op.sh setup"), model_job.index("- run: rm -rf opendbc_repo/")) + + def test_model_job_uses_current_openpilot_layout(self): + model_job = indented_block(self.workflow, " test_models:") + model_lines = normalized_lines(model_job) + model_test_command = " ".join(( + "run: MAX_EXAMPLES=1 pytest --continue-on-collection-errors --durations=0 --durations-min=5 -n logical", + "openpilot/selfdrive/car/tests/test_models.py", + )) + + self.assertIn("CI: 1", model_lines) + self.assertIn( + "run: scons -j$(nproc) openpilot/common/ openpilot/cereal/ openpilot/selfdrive/pandad/ msgq_repo/ opendbc_repo", + model_lines, + ) + self.assertIn(model_test_command, model_lines) + for obsolete_variable in ("BASE_IMAGE:", "BUILD:", "RUN:", "PYTEST:"): + self.assertNotIn(obsolete_variable, model_job) + if __name__ == "__main__": unittest.main() diff --git a/.github/workflows/tests.yml b/.github/workflows/tests.yml index b0e7b552887..2d29c839106 100644 --- a/.github/workflows/tests.yml +++ b/.github/workflows/tests.yml @@ -5,11 +5,46 @@ on: branches: - master - 'nap-*' + - 'naponsp-*' pull_request: jobs: + nap_tests: + name: NAP build and safety + if: ${{ startsWith(github.ref_name, 'nap-') || startsWith(github.base_ref, 'nap-') || startsWith(github.ref_name, 'naponsp-') || startsWith(github.base_ref, 'naponsp-') }} + runs-on: ubuntu-24.04 + timeout-minutes: 30 + steps: + - uses: actions/checkout@v4 + - uses: ./.github/workflows/cache + - name: Build NAP opendbc + run: | + source setup.sh + scons -j$(nproc) + - name: Lint NAP implementation and gates + run: | + source .venv/bin/activate + ruff check \ + .github/ci/tesla_preap_longitudinal_mutations.py \ + .github/ci/test_tests_workflow.py \ + opendbc/car/tesla/fingerprints.py \ + opendbc/car/tesla/pedal/controller.py \ + opendbc/car/tesla/preap \ + opendbc/car/tesla/test_pedal_regen.py \ + opendbc/safety/tests/test_mg.py \ + opendbc/safety/tests/test_tesla_preap.py + - name: Run NAP car and safety suites + run: | + source .venv/bin/activate + pytest -q -n 0 \ + opendbc/car/tesla/preap/tests/ \ + opendbc/car/tesla/test_pedal_regen.py \ + opendbc/safety/tests/test_mg.py \ + opendbc/safety/tests/test_tesla_preap.py + tests: name: ./test.sh + if: ${{ !startsWith(github.ref_name, 'nap-') && !startsWith(github.base_ref, 'nap-') && !startsWith(github.ref_name, 'naponsp-') && !startsWith(github.base_ref, 'naponsp-') }} runs-on: ${{ matrix.os }} strategy: fail-fast: false @@ -56,6 +91,7 @@ jobs: safety_tests: name: safety + if: ${{ !startsWith(github.ref_name, 'nap-') && !startsWith(github.base_ref, 'nap-') && !startsWith(github.ref_name, 'naponsp-') && !startsWith(github.base_ref, 'naponsp-') }} runs-on: ${{ github.repository == 'commaai/opendbc' && 'namespace-profile-amd64-8x16' || 'ubuntu-latest' }} strategy: fail-fast: false @@ -72,6 +108,7 @@ jobs: mutation: name: Safety mutation tests + if: ${{ !startsWith(github.ref_name, 'nap-') && !startsWith(github.base_ref, 'nap-') && !startsWith(github.ref_name, 'naponsp-') && !startsWith(github.base_ref, 'naponsp-') }} runs-on: ${{ github.repository == 'commaai/opendbc' && 'namespace-profile-amd64-8x16' || 'ubuntu-latest' }} timeout-minutes: 45 env: @@ -90,41 +127,36 @@ jobs: # TODO: this needs to move to opendbc test_models: name: test models + if: ${{ !startsWith(github.ref_name, 'nap-') && !startsWith(github.base_ref, 'nap-') && !startsWith(github.ref_name, 'naponsp-') && !startsWith(github.base_ref, 'naponsp-') }} runs-on: ${{ github.repository == 'commaai/opendbc' && 'namespace-profile-amd64-8x16' || 'ubuntu-latest' }} strategy: fail-fast: false matrix: job: [0, 1, 2, 3] env: - BASE_IMAGE: openpilot-base - BUILD: selfdrive/test/docker_build.sh base - RUN: docker run --shm-size 2G -v $PWD:/tmp/openpilot -w /tmp/openpilot -e CI=1 -e PYTHONWARNINGS=error -e FILEREADER_CACHE=1 -e PYTHONPATH=/tmp/openpilot -e NUM_JOBS -e JOB_ID -e GITHUB_ACTION -e GITHUB_REF -e GITHUB_HEAD_REF -e GITHUB_SHA -e GITHUB_REPOSITORY -e GITHUB_RUN_ID -v $GITHUB_WORKSPACE/.ci_cache/scons_cache:/tmp/scons_cache -v $GITHUB_WORKSPACE/.ci_cache/comma_download_cache:/tmp/comma_download_cache -v $GITHUB_WORKSPACE/.ci_cache/openpilot_cache:/tmp/openpilot_cache $BASE_IMAGE /bin/bash -c - PYTEST: pytest --continue-on-collection-errors --durations=0 --durations-min=5 -n logical + CI: 1 steps: - uses: actions/checkout@v4 with: repository: 'commaai/openpilot' ref: 'master' submodules: true + - run: ./tools/op.sh setup - run: rm -rf opendbc_repo/ - uses: actions/checkout@v4 with: path: opendbc_repo - - uses: ./.github/workflows/setup-with-retry - id: setup-step - name: Cache test routes id: routes-cache uses: actions/cache@v4 with: - path: .ci_cache/comma_download_cache - key: car_models-${{ hashFiles('selfdrive/car/tests/test_models.py', 'opendbc/car/tests/routes.py') }}-${{ matrix.job }} + path: /tmp/comma_download_cache + key: car_models-${{ hashFiles('openpilot/selfdrive/car/tests/test_models.py', 'opendbc/car/tests/routes.py') }}-${{ matrix.job }} - name: Build openpilot - run: ${{ env.RUN }} "scons -j$(nproc) common/ cereal/ selfdrive/pandad/ msgq_repo/ opendbc_repo" + run: scons -j$(nproc) openpilot/common/ openpilot/cereal/ openpilot/selfdrive/pandad/ msgq_repo/ opendbc_repo - name: Test car models - timeout-minutes: ${{ contains(runner.name, 'nsc') && (steps.routes-cache.outputs.cache-hit == 'true') && ((steps.setup-step.outputs.duration < 18) && 1 || 2) || 6 }} - run: | - ${{ env.RUN }} "MAX_EXAMPLES=1 $PYTEST selfdrive/car/tests/test_models.py && \ - chmod -R 777 /tmp/comma_download_cache" + timeout-minutes: ${{ contains(runner.name, 'nsc') && (steps.routes-cache.outputs.cache-hit == 'true') && 2 || 6 }} + run: MAX_EXAMPLES=1 pytest --continue-on-collection-errors --durations=0 --durations-min=5 -n logical openpilot/selfdrive/car/tests/test_models.py env: NUM_JOBS: 4 JOB_ID: ${{ matrix.job }} diff --git a/opendbc/car/tesla/fingerprints.py b/opendbc/car/tesla/fingerprints.py index 4dec4b3427f..d99adbf9fba 100644 --- a/opendbc/car/tesla/fingerprints.py +++ b/opendbc/car/tesla/fingerprints.py @@ -89,7 +89,7 @@ FINGERPRINTS = { CAR.TESLA_MODEL_S_PREAP: [ { - 1: 8, 3: 8, 14: 8, 21: 4, 69: 8, 109: 4, 257: 3, 264: 8, 277: 6, 280: 6, 293: 4, 296: 4, 309: 5, 325: 8, 336: 8, 341: 8, 360: 7, 373: 8, 389: 8, 415: 8, 513: 5, 516: 8, 520: 4, 522: 8, 524: 8, 527: 8, 536: 8, 551: 4, 552: 2, 556: 8, 568: 8, 582: 5, 638: 8, 643: 8, 693: 8, 696: 8, 712: 8, 728: 8, 744: 8, 760: 8, 771: 2, 772: 8, 775: 8, 776: 8, 778: 8, 780: 2, 783: 8, 785: 8, 787: 8, 788: 8, 791: 8, 792: 8, 796: 2, 799: 8, 804: 8, 805: 8, 807: 8, 808: 1, 812: 8, 815: 8, 820: 8, 823: 8, 824: 8, 831: 8, 836: 8, 840: 8, 856: 4, 863: 8, 872: 8, 880: 8, 888: 8, 896: 8, 901: 6, 904: 3, 920: 8, 936: 8, 949: 8, 952: 8, 953: 6, 968: 8, 984: 8, 1000: 8, 1006: 8, 1026: 8, 1028: 8, 1029: 8, 1030: 8, 1032: 1, 1034: 8, 1048: 1, 1064: 8, 1080: 8, 1281: 8, 1285: 8, 1332: 8, 1335: 8, 1362: 6, 1368: 8, 1412: 8, 1436: 8, 1456: 8, 1463: 8, 1476: 8, 1524: 8, 1527: 8, 1601: 8, 1605: 8, 1617: 8, 1621: 8, 1800: 4, 1804: 8, 1812: 8, 1815: 8, 1816: 8, 1828: 8, 1831: 8, 1832: 8, 1840: 8, 1848: 8, 1864: 8, 1880: 8, 1892: 8, 1896: 8, 1912: 8, 1960: 8, 1992: 8, 2008: 3, 2043: 5 + 1: 8, 3: 8, 14: 8, 21: 4, 69: 8, 109: 4, 257: 3, 264: 8, 277: 6, 280: 6, 293: 4, 296: 4, 309: 5, 325: 8, 336: 8, 341: 8, 360: 7, 373: 8, 389: 8, 415: 8, 513: 5, 516: 8, 520: 4, 522: 8, 524: 8, 527: 8, 536: 8, 551: 4, 552: 2, 556: 8, 568: 8, 582: 5, 638: 8, 643: 8, 693: 8, 696: 8, 712: 8, 728: 8, 744: 8, 760: 8, 771: 2, 772: 8, 775: 8, 776: 8, 778: 8, 780: 2, 783: 8, 785: 8, 787: 8, 788: 8, 791: 8, 792: 8, 796: 2, 799: 8, 804: 8, 805: 8, 807: 8, 808: 1, 812: 8, 815: 8, 820: 8, 823: 8, 824: 8, 831: 8, 836: 8, 840: 8, 856: 4, 863: 8, 872: 8, 880: 8, 888: 8, 896: 8, 901: 6, 904: 3, 920: 8, 936: 8, 949: 8, 952: 8, 953: 6, 968: 8, 984: 8, 1000: 8, 1006: 8, 1026: 8, 1028: 8, 1029: 8, 1030: 8, 1032: 1, 1034: 8, 1048: 1, 1064: 8, 1080: 8, 1281: 8, 1285: 8, 1332: 8, 1335: 8, 1362: 6, 1368: 8, 1412: 8, 1436: 8, 1456: 8, 1463: 8, 1476: 8, 1524: 8, 1527: 8, 1601: 8, 1605: 8, 1617: 8, 1621: 8, 1800: 4, 1804: 8, 1812: 8, 1815: 8, 1816: 8, 1828: 8, 1831: 8, 1832: 8, 1840: 8, 1848: 8, 1864: 8, 1880: 8, 1892: 8, 1896: 8, 1912: 8, 1960: 8, 1992: 8, 2008: 3, 2043: 5 # noqa: E501 } ], } diff --git a/opendbc/car/tesla/preap/interface.py b/opendbc/car/tesla/preap/interface.py index 10ab63777e3..dc080dce613 100644 --- a/opendbc/car/tesla/preap/interface.py +++ b/opendbc/car/tesla/preap/interface.py @@ -1,6 +1,6 @@ import numpy as np -from opendbc.car import get_safety_config, structs, STD_CARGO_KG +from opendbc.car import get_safety_config, structs from opendbc.car.carlog import carlog from opendbc.car.tesla.preap.nap_conf import nap_conf diff --git a/opendbc/car/tesla/test_pedal_regen.py b/opendbc/car/tesla/test_pedal_regen.py index b62dc643d1e..e672c9fc79c 100644 --- a/opendbc/car/tesla/test_pedal_regen.py +++ b/opendbc/car/tesla/test_pedal_regen.py @@ -37,7 +37,6 @@ from opendbc.car.tesla.pedal.controller import ( compute_pedal_command, PEDAL_RAMP_RATE_UP, PEDAL_RAMP_RATE_DOWN, ) -from opendbc.car.tesla.carcontroller import CarController from opendbc.car.tesla.preap.nap_conf import nap_conf, PEDAL_DI_MIN as TC_PEDAL_DI_MIN @@ -49,11 +48,10 @@ def test_kp_is_zero(self): for kp in PEDAL_LONG_KP_V: self.assertAlmostEqual(kp, 0.0) - def test_ki_values(self): - """ki should be low (0.05-0.15) for slow integral trim with kf=1.0.""" - expected = [0.05, 0.08, 0.10, 0.15] - for got, exp in zip(PEDAL_LONG_KI_V, expected): - self.assertAlmostEqual(got, exp) + def test_outer_integral_is_disabled(self): + """VDAS owns acceleration feedback; the framework loop must not integrate it again.""" + self.assertEqual(len(PEDAL_LONG_KI_V), len(PEDAL_LONG_KP_V)) + self.assertTrue(all(ki == 0.0 for ki in PEDAL_LONG_KI_V)) def test_ki_monotonically_increasing(self): """ki should increase with speed (more correction at highway).""" diff --git a/opendbc/safety/tests/test_mg.py b/opendbc/safety/tests/test_mg.py index d9dd86aa5a7..4dddd648bc3 100755 --- a/opendbc/safety/tests/test_mg.py +++ b/opendbc/safety/tests/test_mg.py @@ -36,7 +36,7 @@ def _speed_msg(self, speed): return self.packer.make_can_msg_safety("SCS_HSC2_FrP15", 0, values) def _torque_driver_msg(self, torque): - values = {"DrvrStrgDlvrdToqHSC2": torque} + values = {"DrvrStrgDlvrdToqHSC2": torque * 0.01} return self.packer.make_can_msg_safety("EPS_HSC2_FrP03", 0, values) def _user_brake_msg(self, brake):