mirror of
https://github.com/sunnypilot/sunnypilot.git
synced 2026-08-24 04:23:47 +08:00
Keep longitudinal braking progressive through handoffs
This commit is contained in:
rav4kumar
@@ -1,4 +1,3 @@
|
||||
#!/usr/bin/env python3
|
||||
from collections import deque
|
||||
from dataclasses import dataclass, field
|
||||
from enum import IntEnum
|
||||
@@ -424,7 +423,7 @@ class AccelController:
|
||||
separation = path.robust_departure_separation(lead_index)
|
||||
if math.isfinite(separation) and path.departure_references[lead_index] is None:
|
||||
path.departure_references[lead_index] = separation
|
||||
raw_departure = ((has_lead and envelope.departure_lead_speed > STOP_HOLD_CREEP_SPEED
|
||||
raw_departure = ((has_lead and min(envelope.selected_lead_speed, envelope.departure_lead_speed) > STOP_HOLD_CREEP_SPEED
|
||||
and envelope.departure_cap > STOP_HOLD_CREEP_SPEED)
|
||||
or (not envelope.lead_status and path.lead_loss_frames >= self.lead_loss_hold_frames))
|
||||
departed = self._creep_departure(path, envelope) or raw_departure
|
||||
|
||||
@@ -44,6 +44,7 @@ MATCHED_PACE_DECEL_RATE = 0.50
|
||||
BRAKING_ACCEL_LIMIT_THRESHOLD = -0.11
|
||||
MPC_DECEL_JERK_COST_MULTIPLIER = 1.05
|
||||
MPC_DECEL_JERK_MAX_REQUIRED_DECEL = 0.80
|
||||
MPC_DECEL_JERK_MAX_REQUIRED_DECEL_RATE = 0.35
|
||||
MPC_DECEL_JERK_MAX_TARGET_REDUCTION = 9.0
|
||||
|
||||
STOP_HOLD_EGO_SPEED = 0.30
|
||||
|
||||
@@ -526,6 +526,26 @@ class TestPaceAndLifecycle:
|
||||
assert results[launch_index].departure_launching
|
||||
assert results[launch_index].effective_accel_max == pytest.approx(results[launch_index].positive_accel_max)
|
||||
|
||||
def test_stopped_governing_lead_rejects_route_51d_radar_speed_pulse_without_delaying_departure(self):
|
||||
controller = make_controller()
|
||||
enter_stop_hold(controller, v_ego=0.0)
|
||||
speed_pulse = (0.1361, 0.1731, 0.2146, 0.2253, 0.2137, 0.1877)
|
||||
distances = (6.0, 6.0, 6.0, 5.96, 6.04, 6.04)
|
||||
|
||||
for distance, speed in zip(distances, speed_pulse, strict=True):
|
||||
radar = make_radar(make_lead(status=True, d_rel=distance, v_lead_k=speed, radar_track_id=4887),
|
||||
make_lead(status=True, d_rel=6.08, v_lead_k=0.0, radar_track_id=4905))
|
||||
held = update(controller, radar, base_speed=8.0, v_ego=0.0)
|
||||
assert held.state == AccelControllerState.stopHold
|
||||
assert held.target_speed == 0.0 and not held.launching
|
||||
|
||||
departing = make_radar(make_lead(status=True, d_rel=6.4, v_lead_k=2.0, radar_track_id=4887),
|
||||
make_lead(status=True, d_rel=6.48, v_lead_k=2.0, radar_track_id=4905))
|
||||
results = [update(controller, departing, base_speed=8.0, v_ego=0.0) for _ in range(STOP_HOLD_EXIT_FRAMES)]
|
||||
|
||||
assert all(result.state == AccelControllerState.stopHold for result in results[:-1])
|
||||
assert results[-1].launching and results[-1].departure_launching
|
||||
|
||||
def test_reused_radar_does_not_pulse_stop_hold_or_departure_target(self):
|
||||
controller = make_controller()
|
||||
enter_stop_hold(controller)
|
||||
|
||||
+51
-1
@@ -1,3 +1,4 @@
|
||||
from collections import deque
|
||||
import inspect
|
||||
import math
|
||||
from types import SimpleNamespace
|
||||
@@ -7,6 +8,7 @@ import pytest
|
||||
|
||||
from cereal import custom, log, messaging
|
||||
from opendbc.car.interfaces import ACCEL_MAX, ACCEL_MIN
|
||||
from openpilot.common.realtime import DT_MDL
|
||||
from openpilot.selfdrive.controls.lib.longitudinal_mpc_lib.long_mpc import N, LongitudinalMpc
|
||||
from openpilot.selfdrive.controls.lib.longitudinal_mpc_lib.long_mpc import LongitudinalPlanSource as MpcLongitudinalPlanSource
|
||||
from openpilot.sunnypilot.selfdrive.controls.lib.accel_personality import AccelControllerState, AccelProfile
|
||||
@@ -40,6 +42,9 @@ def planner_for_mpc_test(*, target_speed=15.0, active=True, is_e2e=False, mpc_ac
|
||||
is_e2e_calls = []
|
||||
planner.is_e2e = lambda _sm: is_e2e_calls.append(True) or is_e2e
|
||||
planner._accel_jerk_smoothing_blocked = False
|
||||
planner._accel_required_decel_samples = deque(maxlen=4)
|
||||
planner._accel_required_decel_lead = -1
|
||||
planner._dt = DT_MDL
|
||||
planner.mpc = SimpleNamespace(source=mpc_source, last_solution_status=0)
|
||||
planner.update_accel_controller = lambda *_args, **_kwargs: setattr(
|
||||
planner, "accel_controller_result",
|
||||
@@ -272,6 +277,50 @@ def test_ineligible_required_decel_blocks_smoothing_only_until_the_restriction_e
|
||||
assert rearmed_calls[0][1] == {"jerk_cost_multiplier": MPC_DECEL_JERK_COST_MULTIPLIER}
|
||||
|
||||
|
||||
def test_consistently_tightening_lead_releases_smoothing_until_the_restriction_ends():
|
||||
planner, _ = planner_for_mpc_test(
|
||||
state=AccelControllerState.restrict, selected_lead=0, required_decel=0.18,
|
||||
)
|
||||
_, calls = run_controller_mpc(planner)
|
||||
routine_result = planner.accel_controller_result
|
||||
multipliers = [calls[0][1]["jerk_cost_multiplier"]]
|
||||
for required_decel in (0.20, 0.23, 0.25):
|
||||
result = SimpleNamespace(**(vars(routine_result) | {"required_decel": required_decel}))
|
||||
planner.update_accel_controller = lambda *_args, result=result, **_kwargs: setattr(planner, "accel_controller_result", result)
|
||||
_, calls = run_controller_mpc(planner)
|
||||
multipliers.append(calls[0][1]["jerk_cost_multiplier"])
|
||||
|
||||
assert multipliers == [MPC_DECEL_JERK_COST_MULTIPLIER] * 3 + [1.0]
|
||||
|
||||
easing_result = SimpleNamespace(**(vars(routine_result) | {"required_decel": 0.20}))
|
||||
planner.update_accel_controller = lambda *_args, **_kwargs: setattr(planner, "accel_controller_result", easing_result)
|
||||
_, calls = run_controller_mpc(planner)
|
||||
assert calls[0][1] == {"jerk_cost_multiplier": 1.0}
|
||||
|
||||
free_result = SimpleNamespace(**(vars(routine_result) | {"state": AccelControllerState.free, "target_speed": 20.0}))
|
||||
planner.update_accel_controller = lambda *_args, **_kwargs: setattr(planner, "accel_controller_result", free_result)
|
||||
run_controller_mpc(planner)
|
||||
planner.update_accel_controller = lambda *_args, **_kwargs: setattr(planner, "accel_controller_result", routine_result)
|
||||
_, calls = run_controller_mpc(planner)
|
||||
assert calls[0][1] == {"jerk_cost_multiplier": MPC_DECEL_JERK_COST_MULTIPLIER}
|
||||
|
||||
|
||||
def test_one_frame_required_decel_noise_does_not_disable_routine_smoothing():
|
||||
planner, _ = planner_for_mpc_test(
|
||||
state=AccelControllerState.restrict, selected_lead=0, required_decel=0.18,
|
||||
)
|
||||
_, calls = run_controller_mpc(planner)
|
||||
routine_result = planner.accel_controller_result
|
||||
multipliers = [calls[0][1]["jerk_cost_multiplier"]]
|
||||
for required_decel in (0.24, 0.19, 0.22):
|
||||
result = SimpleNamespace(**(vars(routine_result) | {"required_decel": required_decel}))
|
||||
planner.update_accel_controller = lambda *_args, result=result, **_kwargs: setattr(planner, "accel_controller_result", result)
|
||||
_, calls = run_controller_mpc(planner)
|
||||
multipliers.append(calls[0][1]["jerk_cost_multiplier"])
|
||||
|
||||
assert multipliers == [MPC_DECEL_JERK_COST_MULTIPLIER] * 4
|
||||
|
||||
|
||||
@pytest.mark.parametrize(
|
||||
("state", "selected_lead", "launching", "required_decel", "target_speed", "mpc_source"),
|
||||
[
|
||||
@@ -335,10 +384,11 @@ def test_radar_freshness_is_computed_once_and_shared_with_dec_and_controller():
|
||||
planner._read_accel_controller_params = lambda: None
|
||||
planner.events_sp = SimpleNamespace(clear=lambda: None)
|
||||
dec_freshness = []
|
||||
planner.dec = SimpleNamespace(update=lambda _sm, *, radar_fresh: dec_freshness.append(radar_fresh))
|
||||
planner.dec = SimpleNamespace(update=lambda _sm, *, radar_fresh, planner_accel: dec_freshness.append(radar_fresh))
|
||||
planner.e2e_alerts_helper = SimpleNamespace(update=lambda *_args: None)
|
||||
planner.accel_personality = int(AccelProfile.normal)
|
||||
planner.accel_personality_enabled = True
|
||||
planner.output_a_target = 0.0
|
||||
planner.a_desired = 0.0
|
||||
planner.v_desired_filter = SimpleNamespace(x=10.0)
|
||||
planner.mpc = SimpleNamespace(source=log.LongitudinalPlan.LongitudinalPlanSource.cruise)
|
||||
|
||||
@@ -29,6 +29,12 @@ class WMACConstants:
|
||||
|
||||
MODEL_DECEL_START = -0.5
|
||||
MODEL_DECEL_RANGE = 2.0
|
||||
MODEL_DECEL_TREND_FRAMES = 4
|
||||
MODEL_DECEL_TREND_ACCEL = -0.075
|
||||
MODEL_DECEL_TREND_RATE = 0.35
|
||||
MODEL_DECEL_TREND_MAX_MPC_ACCEL = 0.075
|
||||
MODEL_DECEL_TREND_MAX_COMMAND_STEP = 0.15
|
||||
MODEL_DECEL_TREND_RELEASE_ACCEL = -0.02
|
||||
ENDPOINT_URGENCY_GAIN = 1.3
|
||||
CRITICAL_ENDPOINT_FACTOR = 0.3
|
||||
CRITICAL_URGENCY_GAIN = 1.5
|
||||
|
||||
@@ -6,12 +6,15 @@ See the LICENSE.md file in the root directory for more details.
|
||||
"""
|
||||
# Version = 2025-6-30
|
||||
|
||||
from collections import deque
|
||||
import math
|
||||
from typing import Literal
|
||||
|
||||
from cereal import messaging
|
||||
from numpy import interp
|
||||
from opendbc.car import structs
|
||||
from openpilot.common.params import Params
|
||||
from openpilot.common.realtime import DT_MDL
|
||||
from openpilot.sunnypilot.selfdrive.controls.lib.dec.constants import WMACConstants
|
||||
|
||||
ModeType = Literal['acc', 'blended']
|
||||
@@ -168,6 +171,10 @@ class DynamicExperimentalController:
|
||||
self._expected_distance = 0.0
|
||||
self._trajectory_valid = False
|
||||
self._raw_urgency = 0.0
|
||||
self._model_accel_samples = deque(maxlen=WMACConstants.MODEL_DECEL_TREND_FRAMES)
|
||||
self._model_decel_trending = False
|
||||
self._model_decel_latched = False
|
||||
self._planner_accel = math.nan
|
||||
|
||||
def _read_params(self) -> None:
|
||||
if self._frame % WMACConstants.PARAM_READ_FRAMES == 0:
|
||||
@@ -234,6 +241,7 @@ class DynamicExperimentalController:
|
||||
self._expected_distance = 0.0
|
||||
self._trajectory_valid = False
|
||||
|
||||
self._update_model_decel_trend(md)
|
||||
urgency = self._model_action_urgency(md)
|
||||
position_valid = len(md.position.x) == WMACConstants.TRAJECTORY_SIZE
|
||||
|
||||
@@ -247,6 +255,31 @@ class DynamicExperimentalController:
|
||||
self._has_slow_down = self._slow_down_tracker.update(self._raw_urgency)
|
||||
self._urgency = self._slow_down_tracker.value
|
||||
|
||||
def _update_model_decel_trend(self, md) -> None:
|
||||
try:
|
||||
desired_accel = float(md.action.desiredAcceleration)
|
||||
except (AttributeError, OverflowError, TypeError, ValueError):
|
||||
desired_accel = math.nan
|
||||
if not math.isfinite(desired_accel):
|
||||
self._reset_model_decel_trend()
|
||||
else:
|
||||
self._model_accel_samples.append(desired_accel)
|
||||
history = tuple(self._model_accel_samples)
|
||||
self._model_decel_trending = (len(history) == self._model_accel_samples.maxlen
|
||||
and history[-1] <= WMACConstants.MODEL_DECEL_TREND_ACCEL
|
||||
and (history[0] - history[-1]) / (DT_MDL * (len(history) - 1)) > WMACConstants.MODEL_DECEL_TREND_RATE
|
||||
and all(after <= before for before, after in zip(history[:-1], history[1:], strict=True))
|
||||
and sum(after < before for before, after in zip(history[:-1], history[1:], strict=True)) >= 2)
|
||||
if len(history) == self._model_accel_samples.maxlen and all(
|
||||
accel >= WMACConstants.MODEL_DECEL_TREND_RELEASE_ACCEL for accel in history
|
||||
):
|
||||
self._model_decel_latched = False
|
||||
|
||||
def _reset_model_decel_trend(self) -> None:
|
||||
self._model_accel_samples.clear()
|
||||
self._model_decel_trending = False
|
||||
self._model_decel_latched = False
|
||||
|
||||
def _radar_acc_lead_score(self, lead_one) -> float:
|
||||
radar_track_id = int(getattr(lead_one, 'radarTrackId', -1))
|
||||
return float(lead_one.status and (bool(getattr(lead_one, 'radar', False)) or radar_track_id >= 0))
|
||||
@@ -290,11 +323,21 @@ class DynamicExperimentalController:
|
||||
|
||||
return urgency
|
||||
|
||||
def _model_decel_handoff_ready(self) -> bool:
|
||||
try:
|
||||
mpc_accel = float(self._mpc.a_solution[1])
|
||||
return (math.isfinite(mpc_accel) and mpc_accel <= WMACConstants.MODEL_DECEL_TREND_MAX_MPC_ACCEL
|
||||
and math.isfinite(self._planner_accel) and self._planner_accel <= WMACConstants.MODEL_DECEL_TREND_MAX_MPC_ACCEL
|
||||
and self._planner_accel - self._model_accel_samples[-1] <= WMACConstants.MODEL_DECEL_TREND_MAX_COMMAND_STEP)
|
||||
except (AttributeError, IndexError, OverflowError, TypeError, ValueError):
|
||||
return False
|
||||
|
||||
def _desired_mode(self) -> tuple[ModeType, bool]:
|
||||
standstill = self._standstill_count > WMACConstants.STANDSTILL_FRAMES
|
||||
urgent_slow_down = self._has_slow_down and self._raw_urgency > WMACConstants.URGENT_SLOW_DOWN_PROB
|
||||
|
||||
if not self._CP.radarUnavailable and self._has_current_radar_acc_lead:
|
||||
self._reset_model_decel_trend()
|
||||
return 'acc', True
|
||||
|
||||
radar_stale = not self._radar_fresh if self._has_mpc_fcw else self._radar_stale_frames > 1
|
||||
@@ -304,8 +347,14 @@ class DynamicExperimentalController:
|
||||
return 'blended', True
|
||||
|
||||
if not self._CP.radarUnavailable and self._has_radar_acc_lead:
|
||||
self._reset_model_decel_trend()
|
||||
return 'acc', True
|
||||
|
||||
entering_model_slowdown = self._model_decel_trending and self._model_decel_handoff_ready() and not self._model_decel_latched
|
||||
self._model_decel_latched |= entering_model_slowdown
|
||||
if self._model_decel_latched:
|
||||
return 'blended', entering_model_slowdown
|
||||
|
||||
if self._has_mpc_fcw:
|
||||
return 'blended', True
|
||||
|
||||
@@ -319,15 +368,24 @@ class DynamicExperimentalController:
|
||||
|
||||
return 'acc', False
|
||||
|
||||
def update(self, sm: messaging.SubMaster, *, radar_fresh: bool = True) -> None:
|
||||
def update(self, sm: messaging.SubMaster, *, radar_fresh: bool = True, planner_accel: float | None = None) -> None:
|
||||
self._read_params()
|
||||
self.set_mpc_fcw_crash_cnt()
|
||||
try:
|
||||
self._planner_accel = float(planner_accel)
|
||||
except (OverflowError, TypeError, ValueError):
|
||||
self._planner_accel = math.nan
|
||||
self._update_calculations(sm, radar_fresh)
|
||||
self._active = sm['selfdriveState'].experimentalMode and self._enabled
|
||||
if not self._active:
|
||||
model_decel_latched = self._model_decel_latched
|
||||
self._reset_model_decel_trend()
|
||||
if model_decel_latched:
|
||||
self._mode_manager.request_mode('acc', immediate=True)
|
||||
|
||||
mode, immediate = self._desired_mode()
|
||||
self._mode_manager.request_mode(mode, immediate=immediate, hold_frames=WMACConstants.EMERGENCY_HOLD_FRAMES,
|
||||
cancel_hold=not self._CP.radarUnavailable and self._has_radar_acc_lead)
|
||||
self._mode_manager.update()
|
||||
|
||||
self._active = sm['selfdriveState'].experimentalMode and self._enabled
|
||||
self._frame += 1
|
||||
|
||||
@@ -77,6 +77,7 @@ def mock_cp():
|
||||
def mock_mpc():
|
||||
class MPC:
|
||||
crash_cnt = 0
|
||||
a_solution = [0.0, 0.0]
|
||||
return MPC()
|
||||
|
||||
|
||||
@@ -159,6 +160,159 @@ def test_model_should_stop_triggers_blended_without_valid_trajectory(mock_cp, mo
|
||||
assert controller.mode() == "blended"
|
||||
|
||||
|
||||
def test_confirmed_model_decel_trend_enters_blended_before_a_large_command(mock_cp, mock_mpc, default_sm):
|
||||
controller = DynamicExperimentalController(mock_cp, mock_mpc, params=MockParams())
|
||||
default_sm['radarState'] = MockRadarState(status=0.0)
|
||||
|
||||
for desired_acceleration in (-0.02, -0.05, -0.08):
|
||||
default_sm['modelV2'] = MockModelData(valid=False, desired_acceleration=desired_acceleration)
|
||||
controller.update(default_sm, planner_accel=0.0)
|
||||
assert controller.mode() == "acc"
|
||||
|
||||
default_sm['modelV2'] = MockModelData(valid=False, desired_acceleration=-0.12)
|
||||
controller.update(default_sm, planner_accel=0.0)
|
||||
|
||||
assert controller._model_decel_trending
|
||||
assert not controller._has_slow_down
|
||||
assert controller.mode() == "blended"
|
||||
|
||||
|
||||
def test_confirmed_model_decel_handoff_stays_latched_through_a_plateau(mock_cp, mock_mpc, default_sm):
|
||||
controller = DynamicExperimentalController(mock_cp, mock_mpc, params=MockParams())
|
||||
default_sm['radarState'] = MockRadarState(status=0.0)
|
||||
|
||||
for desired_acceleration in (-0.02, -0.05, -0.08, -0.12):
|
||||
default_sm['modelV2'] = MockModelData(valid=False, desired_acceleration=desired_acceleration)
|
||||
controller.update(default_sm, planner_accel=0.0)
|
||||
|
||||
for _ in range(WMACConstants.EMERGENCY_HOLD_FRAMES + WMACConstants.EXIT_BLENDED_FRAMES + 1):
|
||||
default_sm['modelV2'] = MockModelData(valid=False, desired_acceleration=-0.12)
|
||||
controller.update(default_sm, planner_accel=0.0)
|
||||
|
||||
assert not controller._model_decel_trending
|
||||
assert controller._model_decel_latched
|
||||
assert controller.mode() == "blended"
|
||||
|
||||
for _ in range(WMACConstants.MODEL_DECEL_TREND_FRAMES + WMACConstants.EXIT_BLENDED_FRAMES):
|
||||
default_sm['modelV2'] = MockModelData(valid=False, desired_acceleration=0.0)
|
||||
controller.update(default_sm, planner_accel=0.0)
|
||||
|
||||
assert not controller._model_decel_latched
|
||||
assert controller.mode() == "acc"
|
||||
|
||||
|
||||
def test_model_decel_trend_never_overrides_a_radar_lead(mock_cp, mock_mpc, default_sm):
|
||||
controller = DynamicExperimentalController(mock_cp, mock_mpc, params=MockParams())
|
||||
|
||||
for desired_acceleration in (-0.02, -0.05, -0.08, -0.12):
|
||||
default_sm['modelV2'] = MockModelData(valid=False, desired_acceleration=desired_acceleration)
|
||||
controller.update(default_sm)
|
||||
|
||||
assert not controller._model_accel_samples
|
||||
assert not controller._model_decel_latched
|
||||
assert controller._has_radar_acc_lead
|
||||
assert controller.mode() == "acc"
|
||||
|
||||
|
||||
def test_radar_acquisition_clears_a_latched_model_decel_handoff(mock_cp, mock_mpc, default_sm):
|
||||
controller = DynamicExperimentalController(mock_cp, mock_mpc, params=MockParams())
|
||||
default_sm['radarState'] = MockRadarState(status=0.0)
|
||||
for desired_acceleration in (-0.02, -0.05, -0.08, -0.12):
|
||||
default_sm['modelV2'] = MockModelData(valid=False, desired_acceleration=desired_acceleration)
|
||||
controller.update(default_sm, planner_accel=0.0)
|
||||
assert controller._model_decel_latched
|
||||
|
||||
default_sm['radarState'] = MockRadarState(status=1.0, radar=True, radarTrackId=7)
|
||||
controller.update(default_sm, planner_accel=0.0)
|
||||
|
||||
assert not controller._model_accel_samples
|
||||
assert not controller._model_decel_latched
|
||||
assert controller.mode() == "acc"
|
||||
|
||||
|
||||
def test_model_decel_trend_does_not_accumulate_while_dec_is_inactive(mock_cp, mock_mpc, default_sm):
|
||||
controller = DynamicExperimentalController(mock_cp, mock_mpc, params=MockParams())
|
||||
default_sm['radarState'] = MockRadarState(status=0.0)
|
||||
default_sm['selfdriveState'].experimentalMode = False
|
||||
for desired_acceleration in (-0.02, -0.05, -0.08, -0.12):
|
||||
default_sm['modelV2'] = MockModelData(valid=False, desired_acceleration=desired_acceleration)
|
||||
controller.update(default_sm, planner_accel=0.0)
|
||||
|
||||
assert not controller._model_accel_samples
|
||||
assert not controller._model_decel_latched
|
||||
|
||||
default_sm['selfdriveState'].experimentalMode = True
|
||||
controller.update(default_sm, planner_accel=0.0)
|
||||
assert not controller._model_decel_trending
|
||||
assert controller.mode() == "acc"
|
||||
|
||||
|
||||
def test_disabling_dec_clears_a_latched_model_decel_mode(mock_cp, mock_mpc, default_sm):
|
||||
controller = DynamicExperimentalController(mock_cp, mock_mpc, params=MockParams())
|
||||
default_sm['radarState'] = MockRadarState(status=0.0)
|
||||
for desired_acceleration in (-0.02, -0.05, -0.08, -0.12):
|
||||
default_sm['modelV2'] = MockModelData(valid=False, desired_acceleration=desired_acceleration)
|
||||
controller.update(default_sm, planner_accel=0.0)
|
||||
assert controller._model_decel_latched
|
||||
assert controller.mode() == "blended"
|
||||
|
||||
default_sm['selfdriveState'].experimentalMode = False
|
||||
default_sm['modelV2'] = MockModelData(valid=False, desired_acceleration=0.0)
|
||||
controller.update(default_sm, planner_accel=0.0)
|
||||
|
||||
assert not controller._model_decel_latched
|
||||
assert controller.mode() == "acc"
|
||||
|
||||
|
||||
def test_model_decel_trend_waits_while_mpc_is_accelerating(mock_cp, mock_mpc, default_sm):
|
||||
controller = DynamicExperimentalController(mock_cp, mock_mpc, params=MockParams())
|
||||
default_sm['radarState'] = MockRadarState(status=0.0)
|
||||
mock_mpc.a_solution[1] = 0.5
|
||||
|
||||
for desired_acceleration in (-0.02, -0.05, -0.08, -0.12):
|
||||
default_sm['modelV2'] = MockModelData(valid=False, desired_acceleration=desired_acceleration)
|
||||
controller.update(default_sm, planner_accel=0.0)
|
||||
|
||||
assert controller._model_decel_trending
|
||||
assert controller.mode() == "acc"
|
||||
|
||||
|
||||
def test_steep_model_decel_trend_defers_to_the_existing_urgent_path(mock_cp, mock_mpc, default_sm):
|
||||
controller = DynamicExperimentalController(mock_cp, mock_mpc, params=MockParams())
|
||||
default_sm['radarState'] = MockRadarState(status=0.0)
|
||||
|
||||
for desired_acceleration in (0.0, -0.2, -0.4, -0.6):
|
||||
default_sm['modelV2'] = MockModelData(valid=False, desired_acceleration=desired_acceleration)
|
||||
controller.update(default_sm, planner_accel=0.05)
|
||||
|
||||
assert controller._model_decel_trending
|
||||
assert controller.mode() == "acc"
|
||||
|
||||
|
||||
def test_model_decel_trend_waits_while_the_planner_is_accelerating(mock_cp, mock_mpc, default_sm):
|
||||
controller = DynamicExperimentalController(mock_cp, mock_mpc, params=MockParams())
|
||||
default_sm['radarState'] = MockRadarState(status=0.0)
|
||||
|
||||
for desired_acceleration in (-0.02, -0.05, -0.08, -0.12):
|
||||
default_sm['modelV2'] = MockModelData(valid=False, desired_acceleration=desired_acceleration)
|
||||
controller.update(default_sm, planner_accel=0.2)
|
||||
|
||||
assert controller._model_decel_trending
|
||||
assert controller.mode() == "acc"
|
||||
|
||||
|
||||
def test_alternating_model_accel_noise_does_not_trigger_an_early_handoff(mock_cp, mock_mpc, default_sm):
|
||||
controller = DynamicExperimentalController(mock_cp, mock_mpc, params=MockParams())
|
||||
default_sm['radarState'] = MockRadarState(status=0.0)
|
||||
|
||||
for desired_acceleration in (0.0, -0.2, 0.0, -0.2):
|
||||
default_sm['modelV2'] = MockModelData(valid=False, desired_acceleration=desired_acceleration)
|
||||
controller.update(default_sm)
|
||||
|
||||
assert not controller._model_decel_trending
|
||||
assert controller.mode() == "acc"
|
||||
|
||||
|
||||
def test_radar_lead_keeps_acc_over_model_slowdown(mock_cp, mock_mpc, default_sm):
|
||||
controller = DynamicExperimentalController(mock_cp, mock_mpc, params=MockParams())
|
||||
default_sm['radarState'] = MockRadarState(status=1.0, radar=True, radarTrackId=7)
|
||||
|
||||
@@ -5,6 +5,9 @@ This file is part of sunnypilot and is licensed under the MIT License.
|
||||
See the LICENSE.md file in the root directory for more details.
|
||||
"""
|
||||
|
||||
from collections import deque
|
||||
import math
|
||||
|
||||
from cereal import messaging, custom
|
||||
from opendbc.car import structs
|
||||
from openpilot.common.constants import CV
|
||||
@@ -14,7 +17,8 @@ from openpilot.selfdrive.car.cruise import V_CRUISE_MAX
|
||||
from openpilot.sunnypilot import get_sanitize_int_param
|
||||
from openpilot.sunnypilot.selfdrive.controls.lib.accel_personality import AccelController, AccelControllerState, AccelProfile
|
||||
from openpilot.sunnypilot.selfdrive.controls.lib.accel_personality.constants import (
|
||||
MPC_DECEL_JERK_COST_MULTIPLIER, MPC_DECEL_JERK_MAX_REQUIRED_DECEL, MPC_DECEL_JERK_MAX_TARGET_REDUCTION,
|
||||
MPC_DECEL_JERK_COST_MULTIPLIER, MPC_DECEL_JERK_MAX_REQUIRED_DECEL, MPC_DECEL_JERK_MAX_REQUIRED_DECEL_RATE,
|
||||
MPC_DECEL_JERK_MAX_TARGET_REDUCTION,
|
||||
)
|
||||
from openpilot.sunnypilot.selfdrive.controls.lib.dec.dec import DynamicExperimentalController
|
||||
from openpilot.sunnypilot.selfdrive.controls.lib.e2e_alerts_helper import E2EAlertsHelper
|
||||
@@ -43,6 +47,9 @@ class LongitudinalPlannerSP:
|
||||
self.accel_controller = AccelController(CP, dt=dt)
|
||||
self.accel_controller_result = None
|
||||
self._accel_jerk_smoothing_blocked = False
|
||||
self._accel_required_decel_samples = deque(maxlen=4)
|
||||
self._accel_required_decel_lead = -1
|
||||
self._dt = dt
|
||||
self._radar_log_mono_time = None
|
||||
self._radar_fresh_this_cycle = True
|
||||
|
||||
@@ -158,8 +165,18 @@ class LongitudinalPlannerSP:
|
||||
actuating and prev_accel_constraint and result.state == AccelControllerState.restrict and result.selected_lead >= 0
|
||||
and not result.launching and target_reduction > 1e-6
|
||||
)
|
||||
if not lead_restriction or result.selected_lead != self._accel_required_decel_lead or not math.isfinite(result.required_decel):
|
||||
self._accel_required_decel_samples.clear()
|
||||
if lead_restriction and math.isfinite(result.required_decel):
|
||||
self._accel_required_decel_samples.append(result.required_decel)
|
||||
self._accel_required_decel_lead = result.selected_lead if lead_restriction else -1
|
||||
required_decel_history = tuple(self._accel_required_decel_samples)
|
||||
tightening_lead = (len(required_decel_history) == self._accel_required_decel_samples.maxlen
|
||||
and (required_decel_history[-1] - required_decel_history[0]) /
|
||||
(self._dt * (len(required_decel_history) - 1)) > MPC_DECEL_JERK_MAX_REQUIRED_DECEL_RATE
|
||||
and sum(after > before for before, after in zip(required_decel_history[:-1], required_decel_history[1:], strict=True)) >= 2)
|
||||
smoothing_eligible = (lead_restriction and target_reduction < MPC_DECEL_JERK_MAX_TARGET_REDUCTION
|
||||
and 0.0 < result.required_decel < MPC_DECEL_JERK_MAX_REQUIRED_DECEL)
|
||||
and 0.0 < result.required_decel < MPC_DECEL_JERK_MAX_REQUIRED_DECEL and not tightening_lead)
|
||||
smoothing_blocked = getattr(self, '_accel_jerk_smoothing_blocked', False)
|
||||
if previous_mpc_failed:
|
||||
smoothing_blocked = True
|
||||
@@ -185,7 +202,7 @@ class LongitudinalPlannerSP:
|
||||
self._radar_fresh_this_cycle = self._update_radar_freshness(sm)
|
||||
self._read_accel_controller_params()
|
||||
self.events_sp.clear()
|
||||
self.dec.update(sm, radar_fresh=self._radar_fresh_this_cycle)
|
||||
self.dec.update(sm, radar_fresh=self._radar_fresh_this_cycle, planner_accel=self.output_a_target)
|
||||
self.e2e_alerts_helper.update(sm, self.events_sp)
|
||||
|
||||
def publish_longitudinal_plan_sp(self, sm: messaging.SubMaster, pm: messaging.PubMaster) -> None:
|
||||
|
||||
@@ -13,7 +13,8 @@ from openpilot.selfdrive.test.longitudinal_maneuvers.plant import PRIUS_TSS2_ROU
|
||||
from openpilot.sunnypilot.selfdrive.controls.lib import longitudinal_planner as longitudinal_planner_sp
|
||||
from openpilot.sunnypilot.selfdrive.controls.lib.accel_personality import AccelControllerState, AccelProfile
|
||||
from openpilot.sunnypilot.selfdrive.controls.lib.accel_personality.constants import (
|
||||
MATCHED_PACE_DECEL_RATE, MPC_DECEL_JERK_COST_MULTIPLIER, PACE_TARGET_RESERVE,
|
||||
MATCHED_PACE_DECEL_RATE, MPC_DECEL_JERK_COST_MULTIPLIER, MPC_DECEL_JERK_MAX_REQUIRED_DECEL_RATE, PACE_TARGET_RESERVE,
|
||||
STOP_HOLD_EXIT_FRAMES,
|
||||
)
|
||||
|
||||
ACTUATOR_DYNAMICS = (
|
||||
@@ -387,6 +388,35 @@ def test_dec_retains_acc_through_route_like_radar_marker_dropout():
|
||||
assert trace.solver_failures == 0
|
||||
|
||||
|
||||
def test_dec_uses_confirmed_model_slowdown_while_the_handoff_is_still_gentle():
|
||||
def model_action(current_time: float, _v_ego: float, _a_ego: float) -> tuple[float, bool]:
|
||||
if current_time < 1.0:
|
||||
return 0.0, False
|
||||
if current_time < 1.75:
|
||||
return -0.5 * (current_time - 1.0), False
|
||||
if current_time < 3.25:
|
||||
return -0.375, False
|
||||
return -0.375 - 0.5 * (current_time - 3.25), False
|
||||
|
||||
trace = _run(
|
||||
duration=4.5, controller_enabled=True, dec_enabled=True, e2e=True, lead_relevancy=False, speed=22.0,
|
||||
v_cruise=22.0, model_action_fn=model_action, actuator_delay=0.15, actuator_lag=0.20,
|
||||
)
|
||||
mode_changes = np.flatnonzero(np.asarray(trace.dec_mode)[1:] != np.asarray(trace.dec_mode)[:-1]) + 1
|
||||
response = trace.time >= 0.5
|
||||
|
||||
assert len(mode_changes) == 1
|
||||
assert trace.dec_mode[mode_changes[0]] == "blended"
|
||||
assert trace.time[mode_changes[0]] <= 1.20 + 1e-9
|
||||
assert -0.10 < trace.a_target[mode_changes[0]] < 0.0
|
||||
assert np.all(np.asarray(trace.dec_mode)[(trace.time >= 1.75) & (trace.time < 3.25)] == "blended")
|
||||
assert np.max(np.abs(_command_jerk(trace)[response[1:]])) < 3.0
|
||||
assert not trace.fcw.any()
|
||||
assert trace.raw_radar_passthrough.all()
|
||||
assert np.all(trace.mpc_calls == 1)
|
||||
assert trace.solver_failures == 0
|
||||
|
||||
|
||||
def test_clear_road_launch_is_prompt_and_profiles_separate_above_launch_speed():
|
||||
traces = [
|
||||
_run(
|
||||
@@ -467,6 +497,43 @@ def test_lead_bound_routine_decel_uses_smoothing_without_delaying_initial_brakin
|
||||
assert smoothed.solver_failures == 0
|
||||
|
||||
|
||||
def test_tightening_lead_releases_smoothing_before_late_catchup(monkeypatch):
|
||||
event_time = 3.0
|
||||
lead_jerk = 1.02
|
||||
max_lead_decel = 2.22
|
||||
ramp_time = max_lead_decel / lead_jerk
|
||||
|
||||
def lead_speed(current_time: float) -> float:
|
||||
braking_time = max(current_time - event_time, 0.0)
|
||||
ramp = min(braking_time, ramp_time)
|
||||
return 16.9 - 0.5 * lead_jerk * ramp**2 - max_lead_decel * max(braking_time - ramp_time, 0.0)
|
||||
|
||||
common = dict(
|
||||
duration=7.0, profile=AccelProfile.eco, lead_relevancy=True, speed=15.9, distance_lead=35.7,
|
||||
v_lead=lead_speed, v_cruise=17.4, actuator_delay=0.15, actuator_lag=0.20,
|
||||
)
|
||||
stock = _run(controller_enabled=False, **common)
|
||||
monkeypatch.setattr(longitudinal_planner_sp, "MPC_DECEL_JERK_MAX_REQUIRED_DECEL_RATE", np.inf)
|
||||
always_smoothed = _run(controller_enabled=True, **common)
|
||||
monkeypatch.setattr(longitudinal_planner_sp, "MPC_DECEL_JERK_MAX_REQUIRED_DECEL_RATE", MPC_DECEL_JERK_MAX_REQUIRED_DECEL_RATE)
|
||||
trace = _run(controller_enabled=True, **common)
|
||||
response = trace.time >= event_time
|
||||
response_jerk = trace.time[1:] >= event_time
|
||||
required_decel_rate = (trace.required_decel[3:] - trace.required_decel[:-3]) / (3 * DT_MDL)
|
||||
gap = trace.distance_lead - trace.distance
|
||||
always_smoothed_gap = always_smoothed.distance_lead - always_smoothed.distance
|
||||
|
||||
assert np.max(required_decel_rate[trace.required_decel[3:] >= 0.15]) > MPC_DECEL_JERK_MAX_REQUIRED_DECEL_RATE
|
||||
assert _first_time_below(trace, -0.5) <= _first_time_below(stock, -0.5) + 1e-9
|
||||
assert _first_time_below(trace, -0.5) <= _first_time_below(always_smoothed, -0.5) - DT_MDL + 1e-9
|
||||
assert np.max(np.abs(np.diff(trace.a_target)[response_jerk] / DT_MDL)) < 3.0
|
||||
assert not _has_brake_coast_brake(trace.a_target[response])
|
||||
assert not _has_propulsion_brake_cycle(trace.a_target[response])
|
||||
assert np.min(gap) >= np.min(always_smoothed_gap) - 1e-6
|
||||
assert not stock.fcw.any() and not always_smoothed.fcw.any() and not trace.fcw.any()
|
||||
assert stock.solver_failures == always_smoothed.solver_failures == trace.solver_failures == 0
|
||||
|
||||
|
||||
def test_prius_route_model_launches_without_a_dead_pedal():
|
||||
trace = _run(
|
||||
duration=3.0, controller_enabled=True, profile=1, lead_relevancy=False, speed=0.0,
|
||||
@@ -496,6 +563,40 @@ def test_stop_hold_survives_short_full_field_dropout():
|
||||
_assert_no_new_solver_failures(trace, baseline)
|
||||
|
||||
|
||||
def test_route_51d_duplicate_lead_speed_pulse_cannot_release_stop_hold():
|
||||
pulse_start = 1.0
|
||||
departure_time = 2.0
|
||||
pulse_speeds = (0.1361, 0.1731, 0.2146, 0.2253, 0.2137, 0.1877)
|
||||
pulse_distances = (6.0, 6.0, 6.0, 5.96, 6.04, 6.04)
|
||||
|
||||
def lead_speed(current_time: float) -> float:
|
||||
return 0.0 if current_time < departure_time else 2.0
|
||||
|
||||
def observe(current_time: float, lead_name: str, truth: LeadObservation) -> LeadObservation:
|
||||
result = truth | {"radar": True, "radarTrackId": 4887 if lead_name == "leadOne" else 4905}
|
||||
pulse_frame = round((current_time - pulse_start) / DT_MDL)
|
||||
if 0 <= pulse_frame < len(pulse_speeds):
|
||||
speed = pulse_speeds[pulse_frame] if lead_name == "leadOne" else 0.0
|
||||
distance = pulse_distances[pulse_frame] if lead_name == "leadOne" else 6.08
|
||||
result |= {"dRel": distance, "vLead": speed, "vLeadK": speed, "vRel": speed, "aLeadK": 0.0}
|
||||
return result
|
||||
|
||||
trace = _run(
|
||||
duration=3.0, controller_enabled=True, lead_relevancy=True, speed=0.0, distance_lead=6.0,
|
||||
v_lead=lead_speed, v_cruise=8.0, lead_observation_fn=observe, actuator_delay=0.15, actuator_lag=0.20,
|
||||
)
|
||||
pulse = (trace.time >= pulse_start) & (trace.time < pulse_start + len(pulse_speeds) * DT_MDL)
|
||||
launched = np.flatnonzero((trace.time >= departure_time) & trace.launching)
|
||||
|
||||
assert np.all(trace.state[pulse] == int(AccelControllerState.stopHold))
|
||||
assert np.all(trace.target_speed[pulse] == 0.0)
|
||||
assert np.max(trace.speed[pulse]) < 0.01
|
||||
assert len(launched) and trace.time[launched[0]] <= departure_time + STOP_HOLD_EXIT_FRAMES * DT_MDL + 1e-9
|
||||
assert not _has_propulsion_brake_cycle(trace.a_target[trace.time >= departure_time])
|
||||
assert not trace.fcw.any()
|
||||
assert trace.solver_failures == 0
|
||||
|
||||
|
||||
@pytest.mark.parametrize(("actuator_delay", "actuator_lag"), ACTUATOR_DYNAMICS, ids=ACTUATOR_IDS)
|
||||
def test_stopped_lead_requires_four_departure_frames_and_launches_within_one_second(actuator_delay, actuator_lag):
|
||||
departure_time = 1.0
|
||||
|
||||
Reference in New Issue
Block a user