This commit is contained in:
rav4kumar
2026-08-19 11:57:03 -07:00
parent 6c95a1f024
commit 8adcfc0472
8 changed files with 396 additions and 408 deletions
@@ -149,6 +149,7 @@ class LongitudinalPlanner(LongitudinalPlannerSP):
output_a_target_mpc = get_accel_from_plan(self.v_desired_trajectory, self.a_desired_trajectory, CONTROL_N_T_IDX,
action_t=action_t)
output_should_stop_mpc = should_stop(v_ego, output_a_target_mpc)
output_should_stop_mpc = self.update_lead_departure(sm, output_a_target_mpc, output_should_stop_mpc, reset_state)
output_a_target_e2e = sm['modelV2'].action.desiredAcceleration
output_should_stop_e2e = sm['modelV2'].action.shouldStop
@@ -156,7 +157,6 @@ class LongitudinalPlanner(LongitudinalPlannerSP):
max_accel_override = self.get_max_accel_override(v_ego)
min_accel_override = self.get_min_accel_override(v_ego, is_e2e, force_decel)
output_a_target_mpc = self.update_mpc_comfort(sm, output_a_target_mpc, self.a_desired_trajectory, CONTROL_N_T_IDX, reset_state, min_accel_override)
self.a_cruise, self.accel_controller_active = get_cruise_accel(is_e2e, v_cruise, v_ego,
self.a_cruise, steer_angle_without_offset, self.CP, self.dt,
accel_coast, self.allow_throttle, max_accel_override, min_accel_override)
@@ -28,11 +28,11 @@ DESCRIPTIONS = {
"your steering wheel distance button."
),
"AccelPersonalityEnabled": tr_noop(
"Sets the acceleration limits and early lead-deceleration response for each profile. Following distance, emergency braking, and final-stop " +
"state remain controlled by the longitudinal MPC."
"Sets your preferred acceleration and cruise-deceleration limits by profile. Lead following, braking, and stopping behavior remain " +
"independent of this setting."
),
"AccelPersonality": tr_noop(
"Select the vehicle acceleration and early lead-deceleration response."
"Select the vehicle acceleration response. Chauffeur braking and stopping behavior remain the same across profiles."
),
"IsLdwEnabled": tr_noop(
"Receive alerts to steer back into the lane when your vehicle drifts over a detected lane line " +
@@ -1,95 +0,0 @@
"""
Copyright (c) 2021-, Haibin Wen, sunnypilot, and a number of other contributors.
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.
"""
import numpy as np
from openpilot.common.realtime import DT_MDL
_BRAKE_JERK = 1.4
_RELEASE_JERK = 0.8
_ACTIVATION_JERK = 1.2
_ACTIVATION_DELTA = 0.15
_LEAD_LOSS_HOLD_TIME = 0.2
_PREVIEW_START_TIME = 0.5
_CONFIRMATION_FRAMES = 2
class MpcComfortController:
def __init__(self, dt: float = DT_MDL):
self.dt = dt
self._a_target: float | None = None
self._last_raw_target: float | None = None
self._confirmation_frames = 0
self._hold_frames = 0
@property
def active(self) -> bool:
return self._a_target is not None
def reset(self) -> None:
self._a_target = None
self._last_raw_target = None
self._confirmation_frames = 0
self._hold_frames = 0
def update(self, a_target: float, a_trajectory, t_idxs, lead_present: bool, a_min: float, reset: bool = False) -> float:
if reset:
self.reset()
return a_target
a_trajectory = np.asarray(a_trajectory, dtype=float)
t_idxs = np.asarray(t_idxs, dtype=float)
if (
len(a_trajectory) != len(t_idxs)
or not np.isfinite(a_target)
or not np.isfinite(a_min)
or not np.all(np.isfinite(a_trajectory))
or not np.all(np.isfinite(t_idxs))
):
self.reset()
return a_target
future_a = a_trajectory[t_idxs >= _PREVIEW_START_TIME]
if len(future_a) == 0:
self.reset()
return a_target
previous_raw_target = self._last_raw_target
self._last_raw_target = a_target
preview_a = max(float(np.min(future_a)), a_min)
preview_requested = lead_present and preview_a < a_target - _ACTIVATION_DELTA
if self._a_target is None:
raw_target_falling = previous_raw_target is not None and a_target - previous_raw_target < -_ACTIVATION_JERK * self.dt
if raw_target_falling:
self._confirmation_frames = 0
return a_target
self._confirmation_frames = self._confirmation_frames + 1 if preview_requested else 0
if self._confirmation_frames < _CONFIRMATION_FRAMES:
return a_target
self._a_target = a_target
# Never delay braking requested by MPC.
if a_target < self._a_target:
self.reset()
return a_target
if preview_requested and preview_a <= self._a_target:
self._hold_frames = round(_LEAD_LOSS_HOLD_TIME / self.dt)
self._a_target = max(preview_a, self._a_target - _BRAKE_JERK * self.dt)
elif self._hold_frames > 0:
self._hold_frames -= 1
else:
release_target = preview_a if preview_requested else a_target
self._a_target = min(release_target, self._a_target + _RELEASE_JERK * self.dt)
if self._a_target >= a_target:
self.reset()
return a_target
return self._a_target
@@ -1,291 +0,0 @@
"""
Copyright (c) 2021-, Haibin Wen, sunnypilot, and a number of other contributors.
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 contextlib import ExitStack
from types import SimpleNamespace
from unittest import mock
import numpy as np
from openpilot.common.realtime import DT_MDL
from openpilot.common.test import OpenpilotTestCase
from openpilot.sunnypilot.selfdrive.controls.lib.accel_controller import mpc_comfort_controller as comfort
from openpilot.sunnypilot.selfdrive.controls.lib.accel_controller.accel_controller import AccelProfile
from openpilot.sunnypilot.selfdrive.controls.lib.accel_controller.mpc_comfort_controller import MpcComfortController
from openpilot.sunnypilot.selfdrive.controls.lib.longitudinal_planner import LongitudinalPlannerSP, MpcPlanSource
from openpilot.sunnypilot.selfdrive.test.longitudinal_maneuvers.plant import PlantSP, PRIUS_TSS2_ROUTE_MODEL
class TestMpcComfortController(OpenpilotTestCase):
def setUp(self):
self.controller = MpcComfortController()
self.times = np.array([0.0, 0.5, 2.5])
self.risk_horizon = np.array([0.0, -0.5, -2.0])
self.a_min = -1.0
def activate(self, a_target: float = 0.0) -> float:
self.assertEqual(self.controller.update(a_target, self.risk_horizon, self.times, True, self.a_min), a_target)
return self.controller.update(a_target, self.risk_horizon, self.times, True, self.a_min)
def test_preview_brake_jerk_and_profile_limit(self):
output = self.activate()
self.assertAlmostEqual(output, -comfort._BRAKE_JERK * DT_MDL)
outputs = [output]
while outputs[-1] > self.a_min:
outputs.append(self.controller.update(0.0, self.risk_horizon, self.times, True, self.a_min))
brake_step = comfort._BRAKE_JERK * DT_MDL
assert all(next_a - a >= -brake_step - 1e-9 for a, next_a in zip(outputs, outputs[1:], strict=False))
self.assertEqual(outputs[-1], self.a_min)
assert self.controller.active
def test_falling_raw_target_blocks_activation(self):
self.assertEqual(self.controller.update(0.0, self.risk_horizon, self.times, True, self.a_min), 0.0)
raw_target = -(comfort._ACTIVATION_JERK + 0.1) * DT_MDL
self.assertEqual(self.controller.update(raw_target, self.risk_horizon, self.times, True, self.a_min), raw_target)
assert not self.controller.active
self.assertEqual(self.controller.update(raw_target, self.risk_horizon, self.times, True, self.a_min), raw_target)
self.assertLess(self.controller.update(raw_target, self.risk_horizon, self.times, True, self.a_min), raw_target)
def test_raw_emergency_wins_and_clears_state(self):
self.activate()
self.assertEqual(self.controller.update(-3.5, self.risk_horizon, self.times, True, self.a_min), -3.5)
assert not self.controller.active
self.assertEqual(self.controller.update(0.2, np.zeros(3), self.times, True, self.a_min), 0.2)
def test_transient_preview_does_not_activate(self):
for horizon in (self.risk_horizon, np.zeros(3)) * 4:
self.assertEqual(self.controller.update(0.0, horizon, self.times, True, self.a_min), 0.0)
assert not self.controller.active
def test_lead_dropout_holds_then_releases_at_bounded_jerk(self):
self.activate()
for _ in range(20):
self.controller.update(0.0, self.risk_horizon, self.times, True, self.a_min)
start = self.controller.update(0.0, np.zeros(3), self.times, False, self.a_min)
hold_frames = round(comfort._LEAD_LOSS_HOLD_TIME / DT_MDL)
held = [start]
for _ in range(hold_frames - 1):
held.append(self.controller.update(0.0, np.zeros(3), self.times, False, self.a_min))
assert all(a == start for a in held)
released = [held[-1]]
while self.controller.active:
released.append(self.controller.update(0.0, np.zeros(3), self.times, False, self.a_min))
release_step = comfort._RELEASE_JERK * DT_MDL
assert all(0.0 <= next_a - a <= release_step + 1e-9 for a, next_a in zip(released, released[1:], strict=False))
self.assertEqual(released[-1], 0.0)
def test_active_preview_releases_at_bounded_jerk(self):
self.activate()
for _ in range(20):
self.controller.update(0.5, self.risk_horizon, self.times, True, self.a_min)
milder_horizon = np.array([0.0, -0.2, -0.2])
previous = self.controller.update(0.5, self.risk_horizon, self.times, True, self.a_min)
for _ in range(round(comfort._LEAD_LOSS_HOLD_TIME / DT_MDL)):
self.assertEqual(self.controller.update(0.5, milder_horizon, self.times, True, self.a_min), previous)
output = self.controller.update(0.5, milder_horizon, self.times, True, self.a_min)
self.assertAlmostEqual(output - previous, comfort._RELEASE_JERK * DT_MDL)
def test_alternating_preview_does_not_reverse_jerk(self):
self.activate(0.5)
outputs = []
for frame in range(20):
horizon = self.risk_horizon if frame % 2 == 0 else np.zeros(3)
outputs.append(self.controller.update(0.5, horizon, self.times, True, self.a_min))
assert np.all(np.diff(outputs) <= 1e-9)
def test_reset_and_invalid_horizon_return_raw(self):
self.activate()
self.assertEqual(self.controller.update(0.4, self.risk_horizon, self.times, True, self.a_min, reset=True), 0.4)
self.assertEqual(self.controller.update(0.2, np.array([0.0, np.nan, np.inf]), self.times, True, self.a_min), 0.2)
assert not self.controller.active
def test_current_state_anchor_is_excluded(self):
horizon = np.array([-2.0, 0.0, 0.0])
self.assertEqual(self.controller.update(0.0, horizon, self.times, True, self.a_min), 0.0)
self.assertEqual(self.controller.update(0.0, horizon, self.times, True, self.a_min), 0.0)
assert not self.controller.active
def test_output_never_weakens_raw_target(self):
for a_target, horizon in ((0.2, self.risk_horizon), (-0.2, np.zeros(3)), (-3.5, self.risk_horizon)):
self.assertLessEqual(self.controller.update(a_target, horizon, self.times, True, self.a_min), a_target)
class TestInheritedMpcComfortHook(OpenpilotTestCase):
def setUp(self):
self.planner = object.__new__(LongitudinalPlannerSP)
self.planner.mpc_comfort_controller = MpcComfortController()
self.planner.mpc = SimpleNamespace(source=MpcPlanSource.lead1)
self.times = np.array([0.0, 0.5, 2.5])
self.horizon = np.array([0.0, -0.5, -2.0])
self.a_min = -1.0
@staticmethod
def make_sm(v_ego: float = 10.0):
return {
'radarState': SimpleNamespace(leadOne=SimpleNamespace(present=False), leadTwo=SimpleNamespace(present=True)),
'carControl': SimpleNamespace(cruiseControl=SimpleNamespace(override=False)),
'carState': SimpleNamespace(standstill=False, vEgo=v_ego),
}
def test_lead_two_uses_inherited_hook(self):
sm = self.make_sm()
self.assertEqual(self.planner.update_mpc_comfort(sm, 0.0, self.horizon, self.times, False, self.a_min), 0.0)
self.assertLess(self.planner.update_mpc_comfort(sm, 0.0, self.horizon, self.times, False, self.a_min), 0.0)
def test_non_lead_source_is_ignored(self):
self.planner.mpc.source = MpcPlanSource.cruise
for _ in range(3):
self.assertEqual(self.planner.update_mpc_comfort(self.make_sm(), 0.0, self.horizon, self.times, False, self.a_min), 0.0)
assert not self.planner.mpc_comfort_controller.active
def test_disabled_controller_is_exact_stock(self):
self.planner.mpc_comfort_controller.update(0.0, self.horizon, self.times, True, self.a_min)
self.planner.mpc_comfort_controller.update(0.0, self.horizon, self.times, True, self.a_min)
self.assertEqual(self.planner.update_mpc_comfort(self.make_sm(), 0.2, self.horizon, self.times, False, None), 0.2)
assert not self.planner.mpc_comfort_controller.active
def test_stop_region_remains_raw(self):
self.assertEqual(self.planner.update_mpc_comfort(self.make_sm(0.29), 0.2, self.horizon, self.times, False, self.a_min), 0.2)
assert not self.planner.mpc_comfort_controller.active
sm = self.make_sm(0.3)
self.assertEqual(self.planner.update_mpc_comfort(sm, 0.2, self.horizon, self.times, False, self.a_min), 0.2)
self.assertLess(self.planner.update_mpc_comfort(sm, 0.2, self.horizon, self.times, False, self.a_min), 0.2)
def _run_closed_loop(comfort_enabled, speed, gap, cruise, duration, lead_speed):
plant = PlantSP(lead_relevancy=True, speed=speed, distance_lead=gap, actuator_model=PRIUS_TSS2_ROUTE_MODEL, run_long_control=True)
plant.v_lead_prev = lead_speed(0.0)
planner = plant.planner
planner.accel_controller._enabled = True
planner.accel_controller._profile = AccelProfile.eco
planner.dec._enabled = False
solver_failures = 0
with ExitStack() as patches:
patches.enter_context(mock.patch.object(planner.accel_controller, "update", return_value=None))
patches.enter_context(mock.patch.object(planner.dec, "_read_params", return_value=None))
if not comfort_enabled:
def bypass_comfort(_sm, a_target, *_args):
planner.mpc_comfort_controller.reset()
return a_target
patches.enter_context(mock.patch.object(planner, "update_mpc_comfort", side_effect=bypass_comfort))
original_reset = planner.mpc.reset
def count_reset(*args, **kwargs):
nonlocal solver_failures
solver_failures += int(planner.mpc.solution_status != 0)
return original_reset(*args, **kwargs)
patches.enter_context(mock.patch.object(planner.mpc, "reset", side_effect=count_reset))
rows = []
for _ in range(round(duration / DT_MDL)):
current_time = plant.current_time
v_lead = lead_speed(current_time)
result = plant.step(v_lead=v_lead, v_cruise=cruise)
current_gap = result["distance_lead"] - result["distance"]
closing_speed = max(result["speed"] - v_lead, 0.0)
rows.append(
(
current_time,
result["a_target"],
result["actuator_command"],
result["realized_acceleration"],
result["speed"],
current_gap,
current_gap / closing_speed if closing_speed > 0.01 else np.inf,
planner.mpc_comfort_controller.active,
result["fcw"],
)
)
data = np.asarray(rows, dtype=float)
return {
"time": data[:, 0],
"target": data[:, 1],
"command": data[:, 2],
"accel": data[:, 3],
"speed": data[:, 4],
"gap": data[:, 5],
"ttc": data[:, 6],
"active": data[:, 7].astype(bool),
"fcw": data[:, 8].astype(bool),
"solver_failures": solver_failures,
}
def _sustained_onset(times, values, threshold=-0.2):
for frame in range(len(values) - 1):
if values[frame] <= threshold and values[frame + 1] <= threshold:
return times[frame]
return None
def _command_switches(command, deadband=0.05):
states = np.where(command < -deadband, -1, np.where(command > deadband, 1, 0))
states = states[states != 0]
return int(np.count_nonzero(states[1:] != states[:-1]))
class TestMpcComfortClosedLoop(OpenpilotTestCase):
def test_closing_lead_brakes_earlier_and_more_smoothly(self):
def lead_speed(t):
return 10.0 - 1.5 * np.clip(t - 0.75, 0.0, 3.0)
stock = _run_closed_loop(False, 12.0, 45.0, 20.0, 7.0, lead_speed)
enabled = _run_closed_loop(True, 12.0, 45.0, 20.0, 7.0, lead_speed)
assert stock["solver_failures"] == enabled["solver_failures"] == 0
assert not stock["fcw"].any() and not enabled["fcw"].any()
assert enabled["active"].any()
stock_target_onset = _sustained_onset(stock["time"], stock["target"])
enabled_target_onset = _sustained_onset(enabled["time"], enabled["target"])
stock_accel_onset = _sustained_onset(stock["time"], stock["accel"])
enabled_accel_onset = _sustained_onset(enabled["time"], enabled["accel"])
assert None not in (stock_target_onset, enabled_target_onset, stock_accel_onset, enabled_accel_onset)
assert enabled_target_onset <= stock_target_onset - 1.5
assert enabled_accel_onset <= stock_accel_onset - 1.5
stock_target_jerk = np.diff(stock["target"]) / DT_MDL
enabled_target_jerk = np.diff(enabled["target"]) / DT_MDL
stock_accel_jerk = np.diff(stock["accel"]) / DT_MDL
enabled_accel_jerk = np.diff(enabled["accel"]) / DT_MDL
assert enabled_target_jerk.min() >= stock_target_jerk.min()
assert enabled_target_jerk.max() <= stock_target_jerk.max()
assert enabled_accel_jerk.min() >= stock_accel_jerk.min()
assert enabled_accel_jerk.max() <= stock_accel_jerk.max()
assert np.percentile(np.abs(enabled_accel_jerk), 95) <= np.percentile(np.abs(stock_accel_jerk), 95)
assert enabled["accel"].min() >= stock["accel"].min() + 0.5
assert enabled["gap"].min() >= stock["gap"].min()
assert enabled["ttc"].min() >= stock["ttc"].min()
assert _command_switches(enabled["command"]) <= _command_switches(stock["command"])
def test_hard_lead_brake_remains_stock(self):
def lead_speed(t):
return 22.0 - 4.0 * np.clip(t - 3.0, 0.0, 1.0)
stock = _run_closed_loop(False, 22.0, 60.0, 27.0, 10.0, lead_speed)
enabled = _run_closed_loop(True, 22.0, 60.0, 27.0, 10.0, lead_speed)
assert stock["solver_failures"] == enabled["solver_failures"] == 0
assert not stock["fcw"].any() and not enabled["fcw"].any()
assert not enabled["active"].any()
for key in ("target", "command", "accel", "speed", "gap"):
np.testing.assert_array_equal(enabled[key], stock[key])
@@ -0,0 +1,115 @@
"""
Copyright (c) 2021-, Haibin Wen, sunnypilot, and a number of other contributors.
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 typing import Any
from openpilot.cereal import log
from openpilot.selfdrive.controls.lib.longcontrol import LongCtrlState
LEAD_DEPARTURE_MIN_SPEED = 0.3
LEAD_DEPARTURE_CONFIRM_FRAMES = 3
LEAD_DEPARTURE_MIN_DISTANCE = 0.03
LEAD_DEPARTURE_MAX_EGO_SPEED = 0.3
MpcPlanSource = log.LongitudinalPlan.LongitudinalPlanSource
class LeadDepartureController:
def __init__(self, enabled: bool):
self.enabled = enabled
self._track_id: int | None = None
self._distances: deque[float] = deque(maxlen=LEAD_DEPARTURE_CONFIRM_FRAMES)
self._active = False
@property
def active(self) -> bool:
return self._active
def reset(self) -> None:
self._track_id = None
self._distances.clear()
self._active = False
@staticmethod
def _selected_lead(radar_state: Any, source: Any) -> Any | None:
if source == MpcPlanSource.lead0:
return radar_state.leadOne
if source == MpcPlanSource.lead1:
return radar_state.leadTwo
return None
@staticmethod
def _radar_has_errors(radar_state: Any) -> bool:
errors = radar_state.radarErrors
return errors.canError or errors.radarFault or errors.wrongConfig or errors.radarUnavailableTemporary
def update(self, sm: Any, source: Any, a_target: float, should_stop: bool, reset: bool, radar_valid: bool) -> bool:
CS = sm['carState']
CC = sm['carControl']
controls_state = sm['controlsState']
radar_state = sm['radarState']
blocked = (
not self.enabled
or reset
or not CC.longActive
or CC.cruiseControl.override
or CS.gasPressed
or CS.brakePressed
or controls_state.forceDecel
or controls_state.longControlState == LongCtrlState.off
or not radar_valid
or self._radar_has_errors(radar_state)
)
if blocked or not math.isfinite(CS.vEgo) or CS.vEgo >= LEAD_DEPARTURE_MAX_EGO_SPEED or not math.isfinite(a_target):
self.reset()
return should_stop
lead = self._selected_lead(radar_state, source)
lead_valid = (
lead is not None
and lead.present
and lead.radar
and lead.radarTrackId >= 0
and all(math.isfinite(value) for value in (lead.dRel, lead.vLeadK, lead.vRel))
and lead.dRel > 0.0
and lead.vLeadK >= LEAD_DEPARTURE_MIN_SPEED
and lead.vRel >= LEAD_DEPARTURE_MIN_SPEED
and a_target >= 0.0
)
if not lead_valid:
self.reset()
return should_stop
track_id = int(lead.radarTrackId)
if self._active:
if track_id != self._track_id:
self.reset()
return should_stop
return False
if not should_stop:
self.reset()
return False
if controls_state.longControlState != LongCtrlState.stopping:
self.reset()
return should_stop
if track_id != self._track_id:
self._track_id = track_id
self._distances.clear()
self._distances.append(float(lead.dRel))
if len(self._distances) == LEAD_DEPARTURE_CONFIRM_FRAMES and self._distances[-1] - self._distances[0] >= LEAD_DEPARTURE_MIN_DISTANCE:
self._active = True
return False
return should_stop
@@ -5,14 +5,14 @@ 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 openpilot.cereal import messaging, custom, log
from openpilot.cereal import messaging, custom
from opendbc.car import structs
from openpilot.common.constants import CV
from openpilot.selfdrive.car.cruise import V_CRUISE_MAX
from openpilot.sunnypilot.selfdrive.controls.lib.accel_controller.accel_controller import AccelController
from openpilot.sunnypilot.selfdrive.controls.lib.accel_controller.mpc_comfort_controller import MpcComfortController
from openpilot.sunnypilot.selfdrive.controls.lib.dec.dec import DynamicExperimentalController
from openpilot.sunnypilot.selfdrive.controls.lib.e2e_alerts_helper import E2EAlertsHelper
from openpilot.sunnypilot.selfdrive.controls.lib.lead_departure_controller import LeadDepartureController
from openpilot.sunnypilot.selfdrive.controls.lib.smart_cruise_control.smart_cruise_control import SmartCruiseControl
from openpilot.sunnypilot.selfdrive.controls.lib.speed_limit.speed_limit_assist import SpeedLimitAssist
from openpilot.sunnypilot.selfdrive.controls.lib.speed_limit.speed_limit_resolver import SpeedLimitResolver
@@ -21,13 +21,12 @@ from openpilot.sunnypilot.models.helpers import get_active_bundle
DecState = custom.LongitudinalPlanSP.DynamicExperimentalControl.DynamicExperimentalControlState
LongitudinalPlanSource = custom.LongitudinalPlanSP.LongitudinalPlanSource
MpcPlanSource = log.LongitudinalPlan.LongitudinalPlanSource
class LongitudinalPlannerSP:
def __init__(self, CP: structs.CarParams, CP_SP: structs.CarParamsSP, mpc):
self.accel_controller = AccelController()
self.mpc_comfort_controller = MpcComfortController(mpc.dt)
self.lead_departure_controller = LeadDepartureController(CP.openpilotLongitudinalControl and CP.autoResumeSng and not CP.notCar)
self.events_sp = EventsSP()
self.dec = DynamicExperimentalController(CP, mpc)
self.scc = SmartCruiseControl()
@@ -57,16 +56,9 @@ class LongitudinalPlannerSP:
return None
return self.accel_controller.get_min_accel(v_ego)
def update_mpc_comfort(self, sm: messaging.SubMaster, a_target: float, a_trajectory, t_idxs,
reset_state: bool, a_min: float | None) -> float:
if a_min is None:
self.mpc_comfort_controller.reset()
return a_target
lead_present = ((self.mpc.source == MpcPlanSource.lead0 and sm['radarState'].leadOne.present) or
(self.mpc.source == MpcPlanSource.lead1 and sm['radarState'].leadTwo.present))
reset = reset_state or sm['carControl'].cruiseControl.override or sm['carState'].standstill or sm['carState'].vEgo < 0.3
return self.mpc_comfort_controller.update(a_target, a_trajectory, t_idxs, lead_present, a_min, reset)
def update_lead_departure(self, sm: messaging.SubMaster, a_target: float, should_stop: bool, reset: bool) -> bool:
radar_valid = sm.valid.get('radarState', False) and getattr(sm, 'alive', {}).get('radarState', False)
return self.lead_departure_controller.update(sm, self.mpc.source, a_target, should_stop, reset, radar_valid)
def update_targets(self, sm: messaging.SubMaster, v_ego: float, a_ego: float, v_cruise: float) -> tuple[float, float]:
CS = sm['carState']
@@ -123,7 +115,7 @@ class LongitudinalPlannerSP:
accel_controller = longitudinalPlanSP.accelController
accel_controller.enabled = self.accel_controller.is_enabled()
accel_controller.active = self.accel_controller_active or self.mpc_comfort_controller.active
accel_controller.active = self.accel_controller_active
accel_controller.profile = self.accel_controller.profile
# Smart Cruise Control
@@ -0,0 +1,266 @@
"""
Copyright (c) 2021-, Haibin Wen, sunnypilot, and a number of other contributors.
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 types import SimpleNamespace
from unittest import mock
from openpilot.cereal import log
from openpilot.common.realtime import DT_MDL
from openpilot.common.test import OpenpilotTestCase
from openpilot.selfdrive.controls.lib.longcontrol import LongCtrlState
from openpilot.sunnypilot.selfdrive.controls.lib.lead_departure_controller import LeadDepartureController
from openpilot.sunnypilot.selfdrive.test.longitudinal_maneuvers.plant import PRIUS_TSS2_ROUTE_MODEL, PlantSP
MpcPlanSource = log.LongitudinalPlan.LongitudinalPlanSource
def make_lead(*, d_rel: float = 4.0, v_lead: float = 0.5, v_rel: float = 0.5, present: bool = True, radar: bool = True, track_id: int = 7):
return SimpleNamespace(dRel=d_rel, vLeadK=v_lead, vRel=v_rel, present=present, radar=radar, radarTrackId=track_id)
def make_sm(
*,
lead_one=None,
lead_two=None,
v_ego: float = 0.0,
long_active: bool = True,
long_state=LongCtrlState.stopping,
gas: bool = False,
brake: bool = False,
override: bool = False,
force_decel: bool = False,
radar_error: str | None = None,
):
errors = SimpleNamespace(canError=False, radarFault=False, wrongConfig=False, radarUnavailableTemporary=False)
if radar_error is not None:
setattr(errors, radar_error, True)
return {
'carState': SimpleNamespace(vEgo=v_ego, gasPressed=gas, brakePressed=brake),
'carControl': SimpleNamespace(longActive=long_active, cruiseControl=SimpleNamespace(override=override)),
'controlsState': SimpleNamespace(longControlState=long_state, forceDecel=force_decel),
'radarState': SimpleNamespace(leadOne=lead_one or make_lead(), leadTwo=lead_two or make_lead(track_id=8), radarErrors=errors),
}
def update(controller, sm, *, source=MpcPlanSource.lead0, a_target: float = 0.05, should_stop: bool = True, reset: bool = False, radar_valid: bool = True):
return controller.update(sm, source, a_target, should_stop, reset, radar_valid)
def activate(controller: LeadDepartureController):
assert update(controller, make_sm(lead_one=make_lead(d_rel=4.00)))
assert update(controller, make_sm(lead_one=make_lead(d_rel=4.01)))
assert not update(controller, make_sm(lead_one=make_lead(d_rel=4.04)))
assert controller.active
def run_closed_loop(controller_enabled: bool, gap: float, lead_speed, duration: float, model_should_stop: bool | None = None):
def observe_lead(_t, _name, truth):
truth.update(radar=True, radarTrackId=7)
return truth
def model_action(_t, _v_ego, _a_ego):
return 0.0, bool(model_should_stop)
plant = PlantSP(
lead_relevancy=True,
speed=0.0,
distance_lead=gap,
lead_observation_fn=observe_lead,
actuator_model=PRIUS_TSS2_ROUTE_MODEL,
run_long_control=True,
e2e=model_should_stop is not None,
model_action_fn=model_action if model_should_stop is not None else None,
)
plant.planner.lead_departure_controller.enabled = controller_enabled
original_update = plant.planner.update
def long_active_update(sm):
sm['carControl'].longActive = True
original_update(sm)
solver_resets = 0
original_reset = plant.planner.mpc.reset
def counted_reset(*args, **kwargs):
nonlocal solver_resets
if plant.planner.mpc.solution_status != 0:
solver_resets += 1
return original_reset(*args, **kwargs)
rows = []
active = []
with (
mock.patch.object(plant.planner, 'get_max_accel_override', return_value=None),
mock.patch.object(plant.planner, 'get_min_accel_override', return_value=None),
mock.patch.object(plant.planner, 'update', side_effect=long_active_update),
mock.patch.object(plant.planner.mpc, 'reset', side_effect=counted_reset),
):
for _ in range(round(duration / DT_MDL)):
t = plant.current_time
result = plant.step(v_lead=lead_speed(t), v_cruise=8.0)
rows.append(
(t, result['speed'], result['distance'], result['distance_lead'] - result['distance'], result['actuator_command'], result['should_stop'], result['fcw'])
)
active.append(plant.planner.lead_departure_controller.active)
return rows, active, solver_resets
def first_delay(rows, cue: float, column: int, predicate):
return next(row[0] - cue for row in rows if row[0] >= cue and predicate(row[column]))
class TestLeadDepartureController(OpenpilotTestCase):
def test_requires_three_coherent_radar_frames(self):
controller = LeadDepartureController(True)
assert update(controller, make_sm(lead_one=make_lead(d_rel=4.00)))
assert update(controller, make_sm(lead_one=make_lead(d_rel=4.01)))
assert not update(controller, make_sm(lead_one=make_lead(d_rel=4.04)))
assert controller.active
def test_distance_confirmation_uses_a_sliding_three_frame_window(self):
controller = LeadDepartureController(True)
for d_rel in (4.00, 4.01, 4.02, 4.03):
assert update(controller, make_sm(lead_one=make_lead(d_rel=d_rel)))
assert not controller.active
assert not update(controller, make_sm(lead_one=make_lead(d_rel=4.06)))
assert controller.active
def test_persistent_false_speed_cue_with_static_range_never_arms(self):
controller = LeadDepartureController(True)
for _ in range(10):
assert update(controller, make_sm(lead_one=make_lead(d_rel=4.0)))
assert not controller.active
def test_same_track_can_move_between_lead_slots(self):
controller = LeadDepartureController(True)
assert update(controller, make_sm(lead_one=make_lead(d_rel=4.00)), source=MpcPlanSource.lead0)
assert update(controller, make_sm(lead_two=make_lead(d_rel=4.01)), source=MpcPlanSource.lead1)
assert not update(controller, make_sm(lead_one=make_lead(d_rel=4.04)), source=MpcPlanSource.lead0)
def test_different_track_restarts_confirmation(self):
controller = LeadDepartureController(True)
assert update(controller, make_sm(lead_one=make_lead(d_rel=4.00, track_id=7)))
assert update(controller, make_sm(lead_one=make_lead(d_rel=4.02, track_id=7)))
assert update(controller, make_sm(lead_one=make_lead(d_rel=4.20, track_id=9)))
assert update(controller, make_sm(lead_one=make_lead(d_rel=4.22, track_id=9)))
assert not update(controller, make_sm(lead_one=make_lead(d_rel=4.24, track_id=9)))
def test_active_release_latches_through_native_threshold_churn(self):
controller = LeadDepartureController(True)
activate(controller)
sm = make_sm(lead_one=make_lead(d_rel=4.10), long_state=LongCtrlState.pid)
assert not update(controller, sm, a_target=0.12, should_stop=False)
assert not update(controller, sm, a_target=0.05, should_stop=True)
assert controller.active
def test_active_release_latches_across_same_track_source_churn(self):
controller = LeadDepartureController(True)
activate(controller)
sm = make_sm(lead_two=make_lead(d_rel=4.10), long_state=LongCtrlState.pid)
assert not update(controller, sm, source=MpcPlanSource.lead1)
assert controller.active
def test_active_release_cancels_on_invalid_state(self):
cases = (
('lead lost', make_sm(lead_one=make_lead(present=False))),
('vision lead', make_sm(lead_one=make_lead(radar=False))),
('track changed', make_sm(lead_one=make_lead(track_id=9))),
('lead too slow', make_sm(lead_one=make_lead(v_lead=0.29))),
('relative speed too low', make_sm(lead_one=make_lead(v_rel=0.29))),
('gas', make_sm(gas=True)),
('brake', make_sm(brake=True)),
('override', make_sm(override=True)),
('force decel', make_sm(force_decel=True)),
('long inactive', make_sm(long_active=False)),
('long control off', make_sm(long_state=LongCtrlState.off)),
('ego rolling', make_sm(v_ego=0.3)),
('radar CAN error', make_sm(radar_error='canError')),
('radar fault', make_sm(radar_error='radarFault')),
('radar config', make_sm(radar_error='wrongConfig')),
('radar unavailable', make_sm(radar_error='radarUnavailableTemporary')),
)
for name, sm in cases:
with self.subTest(name=name):
controller = LeadDepartureController(True)
activate(controller)
assert update(controller, sm)
assert not controller.active
def test_active_release_cancels_on_invalid_update_input(self):
cases = (('negative target', -0.01, False, True), ('reset', 0.05, True, True), ('radar invalid', 0.05, False, False))
for name, a_target, reset, radar_valid in cases:
with self.subTest(name=name):
controller = LeadDepartureController(True)
activate(controller)
assert update(controller, make_sm(), a_target=a_target, reset=reset, radar_valid=radar_valid)
assert not controller.active
def test_inactive_controller_arms_only_from_native_stop_and_stopping_state(self):
controller = LeadDepartureController(True)
for d_rel in (4.00, 4.02, 4.04):
assert not update(controller, make_sm(lead_one=make_lead(d_rel=d_rel)), should_stop=False)
for d_rel in (4.00, 4.02, 4.04):
assert update(controller, make_sm(lead_one=make_lead(d_rel=d_rel), long_state=LongCtrlState.pid))
assert not controller.active
def test_capability_gate_disables_controller(self):
controller = LeadDepartureController(False)
for d_rel in (4.00, 4.02, 4.04):
assert update(controller, make_sm(lead_one=make_lead(d_rel=d_rel)))
assert not controller.active
def test_closed_loop_departure_releases_earlier_without_a_safety_regression(self):
lead_accel = 0.31
cue = 1.0 + 0.4 / lead_accel
def lead_speed(t):
return 0.0 if t < 1.0 else min(5.0, lead_accel * (t - 1.0))
stock, stock_active, stock_resets = run_closed_loop(False, 3.81, lead_speed, 8.0)
controller, controller_active, controller_resets = run_closed_loop(True, 3.81, lead_speed, 8.0)
stock_release = first_delay(stock, cue, 5, lambda should_stop: not should_stop)
controller_release = first_delay(controller, cue, 5, lambda should_stop: not should_stop)
stock_motion = first_delay(stock, cue, 1, lambda speed: speed > 0.01)
controller_motion = first_delay(controller, cue, 1, lambda speed: speed > 0.01)
stock_v01 = first_delay(stock, cue, 1, lambda speed: speed > 0.1)
controller_v01 = first_delay(controller, cue, 1, lambda speed: speed > 0.1)
assert any(controller_active) and not any(stock_active)
assert stock_resets == controller_resets == 0
assert not any(row[6] for row in stock + controller)
assert controller_release <= stock_release - 1.0
assert controller_motion <= stock_motion - 0.1
assert controller_v01 <= stock_v01 - 0.1
assert min(row[3] for row in controller) >= min(row[3] for row in stock)
assert max(abs(right[4] - left[4]) for left, right in zip(controller, controller[1:], strict=False)) <= max(
abs(right[4] - left[4]) for left, right in zip(stock, stock[1:], strict=False)
)
def test_model_stop_remains_authoritative(self):
def lead_speed(t):
return 0.0 if t < 1.0 else min(5.0, 0.8 * (t - 1.0))
rows, active, solver_resets = run_closed_loop(True, 4.0, lead_speed, 6.0, model_should_stop=True)
assert any(active)
assert solver_resets == 0
assert all(row[5] for row in rows)
assert all(row[1] == 0.0 and row[2] == 0.0 for row in rows)
assert not any(row[6] for row in rows)
def test_stationary_lead_remains_stock_identical(self):
stock, stock_active, stock_resets = run_closed_loop(False, 8.0, lambda _t: 0.0, 12.0)
controller, controller_active, controller_resets = run_closed_loop(True, 8.0, lambda _t: 0.0, 12.0)
assert stock == controller
assert not any(stock_active) and not any(controller_active)
assert stock_resets == controller_resets == 0
@@ -46,8 +46,8 @@ sections:
- key: AccelPersonalityEnabled
widget: toggle
title: Enable Accel Controller
description: Sets the acceleration limits and early lead-deceleration response for each profile. Following distance,
emergency braking, and final-stop state remain controlled by the longitudinal MPC.
description: Sets your preferred acceleration and cruise-deceleration limits by profile. Lead following, braking,
and stopping behavior remain independent of this setting.
visibility:
- $ref: '#/macros/longitudinal'
enablement:
@@ -55,7 +55,8 @@ sections:
- key: AccelPersonality
widget: multiple_button
title: Acceleration Profile
description: Select the vehicle acceleration and early lead-deceleration response.
description: Select the vehicle acceleration response. Chauffeur braking and stopping behavior remain the same across
profiles.
options:
- value: 0
label: Eco