mirror of
https://github.com/sunnypilot/sunnypilot.git
synced 2026-10-24 04:33:43 +08:00
feat(sunnypilot): add the stopping controller, the eco lead boost and per-feature Params toggles
This commit is contained in:
rav4kumar
@@ -192,10 +192,12 @@ inline static std::unordered_map<std::string, ParamKeyAttributes> keys = {
|
||||
{"ShowAdvancedControls", {PERSISTENT | BACKUP, BOOL, "0"}},
|
||||
{"ShowTurnSignals", {PERSISTENT | BACKUP, BOOL, "0"}},
|
||||
{"StandstillTimer", {PERSISTENT | BACKUP, BOOL, "0"}},
|
||||
{"SunnypilotStoppingController", {PERSISTENT | BACKUP, BOOL, "0"}},
|
||||
{"TrueVEgoUI", {PERSISTENT | BACKUP, BOOL, "0"}},
|
||||
|
||||
// toyota specific params
|
||||
{"ToyotaAutoHold", {PERSISTENT | BACKUP, BOOL, "0"}},
|
||||
{"ToyotaBrakeOnset", {PERSISTENT | BACKUP, BOOL, "0"}},
|
||||
{"ToyotaEnhancedBsm", {PERSISTENT | BACKUP, BOOL, "0"}},
|
||||
{"ToyotaTSS2Long", {PERSISTENT | BACKUP, BOOL, "0"}},
|
||||
{"ToyotaDriveMode", {PERSISTENT | BACKUP, BOOL, "0"}},
|
||||
|
||||
@@ -136,6 +136,7 @@ class Controls(ControlsExt):
|
||||
# accel PID loop
|
||||
pid_accel_limits = self.CI.get_pid_accel_limits(self.CP, self.CP_SP, CS.vEgo, CS.vCruise * CV.KPH_TO_MS)
|
||||
actuators.accel = float(self.LoC.update(CC.longActive, CS, long_plan.aTarget, long_plan.shouldStop, pid_accel_limits))
|
||||
self.long_accel_sp(actuators, CS, long_plan, pid_accel_limits) # sunnypilot hook
|
||||
|
||||
# Steering PID loop and lateral MPC
|
||||
# Reset desired curvature to current to avoid violating the limits on engage
|
||||
|
||||
@@ -130,6 +130,7 @@ def initialize_params(params) -> list[dict[str, Any]]:
|
||||
"ToyotaTSS2Long",
|
||||
"ToyotaEnhancedBsm",
|
||||
"ToyotaAutoHold",
|
||||
"ToyotaBrakeOnset",
|
||||
])
|
||||
|
||||
return [{k: params.get(k, return_default=True)} for k in keys]
|
||||
|
||||
@@ -12,11 +12,13 @@ from openpilot.cereal import log, custom
|
||||
from opendbc.car import structs
|
||||
from openpilot.common.params import Params
|
||||
from openpilot.common.swaglog import cloudlog
|
||||
from openpilot.selfdrive.controls.lib.longcontrol import LongCtrlState
|
||||
from openpilot.sunnypilot import PARAMS_UPDATE_PERIOD
|
||||
from openpilot.sunnypilot.livedelay.helpers import get_lat_delay
|
||||
from openpilot.sunnypilot.modeld_v2.modeld_base import ModelStateBase
|
||||
from openpilot.sunnypilot.selfdrive.controls.lib.blinker_pause_lateral import BlinkerPauseLateral
|
||||
from openpilot.sunnypilot.selfdrive.controls.lib.latcontrol_torque_v0 import LatControlTorque as LatControlTorqueV0
|
||||
from openpilot.sunnypilot.selfdrive.controls.lib.stopping_controller import StoppingController
|
||||
|
||||
|
||||
class ControlsExt(ModelStateBase):
|
||||
@@ -31,6 +33,9 @@ class ControlsExt(ModelStateBase):
|
||||
self.CP_SP = messaging.log_from_bytes(params.get("CarParamsSP", block=True), custom.CarParamsSP)
|
||||
cloudlog.info("controlsd_ext got CarParamsSP")
|
||||
|
||||
self.stopping_controller = StoppingController(self.CP.stopAccel) if params.get_bool("SunnypilotStoppingController") else None
|
||||
self._stopping_prev = (LongCtrlState.off, 0.0)
|
||||
|
||||
self.sm_services_ext = ['radarState', 'selfdriveStateSP']
|
||||
self.pm_services_ext = ['carControlSP']
|
||||
|
||||
@@ -47,6 +52,27 @@ class ControlsExt(ModelStateBase):
|
||||
else:
|
||||
return lac
|
||||
|
||||
def long_accel_sp(self, actuators, CS, long_plan, accel_limits: tuple[float, float]) -> None:
|
||||
"""sunnypilot: terminal-stop policy applied after the stock LongControl.update() (see stopping_controller.py)."""
|
||||
stock_state = self.LoC.long_control_state
|
||||
stock_accel = actuators.accel
|
||||
prev_state, prev_accel = self._stopping_prev
|
||||
self._stopping_prev = (stock_state, stock_accel)
|
||||
|
||||
if self.stopping_controller is None:
|
||||
return
|
||||
|
||||
state, accel = self.stopping_controller.update(
|
||||
prev_state, stock_state, CS, long_plan.aTarget, prev_accel, stock_accel, accel_limits, long_plan.hasLead,
|
||||
pitch=self.calibrated_pose.orientation.pitch if self.calibrated_pose is not None else None,
|
||||
a_long=self.calibrated_pose.acceleration.x if self.calibrated_pose is not None else None)
|
||||
if state != stock_state:
|
||||
self.LoC.reset()
|
||||
self.LoC.long_control_state = state
|
||||
self.LoC.last_output_accel = accel
|
||||
self._stopping_prev = (state, accel)
|
||||
actuators.accel = float(accel)
|
||||
|
||||
def get_params_sp(self, sm: messaging.SubMaster) -> None:
|
||||
if time.monotonic() - self._param_update_time > PARAMS_UPDATE_PERIOD:
|
||||
self.blinker_pause_lateral.get_params()
|
||||
|
||||
@@ -15,12 +15,12 @@ AccelProfile = custom.LongitudinalPlanSP.AccelController.Profile
|
||||
|
||||
MAX_ACCEL_BREAKPOINTS = [0., 3., 12, 24., 36.] # m/s
|
||||
MAX_ACCEL_PROFILES = {
|
||||
AccelProfile.eco: [1.85, 1.55, 0.35, 0.13, 0.10],
|
||||
AccelProfile.eco: [1.75, 1.50, 0.40, 0.15, 0.12],
|
||||
AccelProfile.normal: [1.95, 1.80, 0.70, 0.36, 0.25],
|
||||
AccelProfile.sport: [2.00, 2.00, 1.20, 0.70, 0.50],
|
||||
}
|
||||
ECO_ENGINE_OFF_BP = [0., 3., 12., 16.67, 19.44, 22.22, 24., 36.] # m/s
|
||||
ECO_ENGINE_OFF_MAX_ACCEL = [1.85, 1.55, 0.35, 0.25, 0.16, 0.12, 0.08, 0.06]
|
||||
ECO_ENGINE_OFF_MAX_ACCEL = [1.75, 1.50, 0.35, 0.25, 0.16, 0.12, 0.08, 0.06]
|
||||
|
||||
CRUISE_DECEL_RESPONSE_TIME = {
|
||||
AccelProfile.normal: 3.5,
|
||||
@@ -30,11 +30,19 @@ CRUISE_DECEL_ACCEL = {
|
||||
AccelProfile.normal: -0.50,
|
||||
AccelProfile.sport: -0.65,
|
||||
}
|
||||
ECO_CRUISE_DECEL_BP = [0., 16.67, 19.44, 25.0, 33.3] # m/s
|
||||
ECO_CRUISE_DECEL_BP = [0., 16.67, 19.44, 25.0, 33.3] # m/s
|
||||
ECO_CRUISE_DECEL_V = [-0.40, -0.40, -0.22, -0.30, -0.40] # m/s^2
|
||||
ECO_COAST_BLEND_BP = [16.67, 19.44] # m/s
|
||||
ECO_COAST_MIN, ECO_COAST_MAX = -1.2, -0.05 # m/s^2
|
||||
CRUISE_DECEL_TAPER_TIME = 1.0 # s
|
||||
ECO_BOOST_DV_BP = [0.3, 2.5] # m/s, lead speed minus ego speed
|
||||
ECO_BOOST_V = [0.0, 0.35] # m/s^2 added to the eco limit
|
||||
ECO_BOOST_FADE_BP = [19.44, 22.22] # m/s (70 -> 80 km/h): full -> none
|
||||
ECO_BOOST_D_MAX = 80.0 # m, only a lead this close counts
|
||||
ECO_BOOST_PROB_MIN = 0.5
|
||||
ECO_BOOST_RISE = 0.25 # m/s^3, how fast the boost builds
|
||||
ECO_BOOST_FALL = 0.5 # m/s^3, how fast it fades
|
||||
ECO_BOOST_DT = 0.05 # s, planner step
|
||||
CRUISE_DECEL_TAPER_TIME = 1.0 # s; inside |decel| * this of the target the decel eases off proportionally (no overshoot)
|
||||
|
||||
|
||||
class AccelController:
|
||||
@@ -42,6 +50,7 @@ class AccelController:
|
||||
self.params = Params()
|
||||
self._cruise_decel: float | None = None
|
||||
self._cruise_decel_target: float | None = None
|
||||
self._boost = 0.0
|
||||
self.update()
|
||||
|
||||
def update(self) -> None:
|
||||
@@ -55,10 +64,26 @@ class AccelController:
|
||||
def is_enabled(self) -> bool:
|
||||
return self._enabled
|
||||
|
||||
def get_max_accel(self, v_ego: float, engine_off: bool = False) -> float:
|
||||
def get_max_accel(self, v_ego: float, engine_off: bool = False, lead=None) -> float:
|
||||
if engine_off and self._profile == AccelProfile.eco:
|
||||
return float(np.interp(max(0.0, v_ego), ECO_ENGINE_OFF_BP, ECO_ENGINE_OFF_MAX_ACCEL))
|
||||
return float(np.interp(max(0.0, v_ego), MAX_ACCEL_BREAKPOINTS, MAX_ACCEL_PROFILES[self._profile]))
|
||||
base = float(np.interp(max(0.0, v_ego), ECO_ENGINE_OFF_BP, ECO_ENGINE_OFF_MAX_ACCEL))
|
||||
else:
|
||||
base = float(np.interp(max(0.0, v_ego), MAX_ACCEL_BREAKPOINTS, MAX_ACCEL_PROFILES[self._profile]))
|
||||
if self._profile != AccelProfile.eco:
|
||||
self._boost = 0.0
|
||||
return base
|
||||
return base + self._eco_lead_boost(v_ego, base, lead)
|
||||
|
||||
def _eco_lead_boost(self, v_ego: float, base: float, lead) -> float:
|
||||
target = 0.0
|
||||
if lead is not None and lead.present and lead.modelProb > ECO_BOOST_PROB_MIN and lead.dRel < ECO_BOOST_D_MAX:
|
||||
target = float(np.interp(lead.vLead - v_ego, ECO_BOOST_DV_BP, ECO_BOOST_V))
|
||||
target *= float(np.interp(v_ego, ECO_BOOST_FADE_BP, [1.0, 0.0]))
|
||||
normal = float(np.interp(max(0.0, v_ego), MAX_ACCEL_BREAKPOINTS, MAX_ACCEL_PROFILES[AccelProfile.normal]))
|
||||
target = float(np.clip(target, 0.0, max(normal - base, 0.0)))
|
||||
step = (ECO_BOOST_RISE if target > self._boost else ECO_BOOST_FALL) * ECO_BOOST_DT
|
||||
self._boost = float(np.clip(target, self._boost - step, self._boost + step))
|
||||
return self._boost
|
||||
|
||||
def get_cruise_target(self, v_ego: float, v_target: float, accel_coast: float | None = None) -> float:
|
||||
if not np.isfinite(v_target) or v_target <= 0.0 or v_target >= v_ego:
|
||||
|
||||
+8
-4
@@ -5,6 +5,8 @@ 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
|
||||
|
||||
import numpy as np
|
||||
|
||||
from opendbc.car.interfaces import ACCEL_MAX
|
||||
@@ -67,13 +69,15 @@ class TestAccelController(OpenpilotTestCase):
|
||||
wheel_power = 1500 * controller.get_max_accel(speed, engine_off=True) * speed + road_load
|
||||
assert wheel_power <= budget_kw * 1e3, kph
|
||||
|
||||
def test_eco_without_lead_reduces_throttle_after_launch(self):
|
||||
def test_lead_boost_is_eco_only_and_absent_without_a_lead(self):
|
||||
# the eco lead pull-away boost: no lead is the plain table lookup, and normal/sport ignore the lead entirely
|
||||
eco = self.set_profile(AccelProfile.eco)
|
||||
assert eco.get_max_accel(1.0, has_lead=False) == eco.get_max_accel(1.0, has_lead=True)
|
||||
assert np.isclose(eco.get_max_accel(20.0, has_lead=False), 0.85 * eco.get_max_accel(20.0, has_lead=True))
|
||||
assert eco.get_max_accel(1.0, lead=None) == eco.get_max_accel(1.0)
|
||||
assert eco.get_max_accel(20.0, lead=None) == eco.get_max_accel(20.0)
|
||||
|
||||
normal = self.set_profile(AccelProfile.normal)
|
||||
assert normal.get_max_accel(20.0, has_lead=False) == normal.get_max_accel(20.0, has_lead=True)
|
||||
lead = SimpleNamespace(present=True, modelProb=0.9, dRel=30.0, vLead=23.0)
|
||||
assert normal.get_max_accel(20.0, lead=lead) == normal.get_max_accel(20.0)
|
||||
|
||||
def test_sport_uses_openpilot_accel_max_at_launch(self):
|
||||
controller = self.set_profile(AccelProfile.sport)
|
||||
|
||||
+129
@@ -0,0 +1,129 @@
|
||||
"""
|
||||
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.params import Params
|
||||
from openpilot.common.test import OpenpilotTestCase
|
||||
from openpilot.sunnypilot.selfdrive.controls.lib.accel_controller.accel_controller import (
|
||||
AccelController, AccelProfile, MAX_ACCEL_BREAKPOINTS, MAX_ACCEL_PROFILES,
|
||||
)
|
||||
|
||||
|
||||
class TestEcoLeadBoost(OpenpilotTestCase):
|
||||
"""Eco lead pull-away boost (driver 2026-10-01 night)."""
|
||||
|
||||
@staticmethod
|
||||
def _ac(profile):
|
||||
from types import SimpleNamespace
|
||||
ac = AccelController.__new__(AccelController)
|
||||
ac._profile, ac._enabled, ac._boost = profile, True, 0.0
|
||||
ac._cruise_decel = ac._cruise_decel_target = None
|
||||
return ac, SimpleNamespace
|
||||
|
||||
def test_no_lead_unchanged(self):
|
||||
ac, _ = self._ac(AccelProfile.eco)
|
||||
v = 15.0
|
||||
base = float(np.interp(v, MAX_ACCEL_BREAKPOINTS, MAX_ACCEL_PROFILES[AccelProfile.eco]))
|
||||
for _ in range(100):
|
||||
assert ac.get_max_accel(v, False, None) == base
|
||||
|
||||
def test_pulling_away_lead_boosts_smoothly_within_normal(self):
|
||||
ac, NS = self._ac(AccelProfile.eco)
|
||||
v = 15.0
|
||||
lead = NS(present=True, modelProb=0.9, dRel=30.0, vLead=v + 3.0)
|
||||
base = float(np.interp(v, MAX_ACCEL_BREAKPOINTS, MAX_ACCEL_PROFILES[AccelProfile.eco]))
|
||||
normal = float(np.interp(v, MAX_ACCEL_BREAKPOINTS, MAX_ACCEL_PROFILES[AccelProfile.normal]))
|
||||
out = [ac.get_max_accel(v, False, lead) for _ in range(60)]
|
||||
steps = np.diff([base] + out)
|
||||
assert max(steps) <= 0.25 * 0.05 + 1e-9 # eased in
|
||||
assert out[-1] > base + 0.2 and out[-1] <= normal + 1e-9
|
||||
lead.present = False
|
||||
out2 = [ac.get_max_accel(v, False, lead) for _ in range(60)]
|
||||
assert min(np.diff([out[-1]] + out2)) >= -0.5 * 0.05 - 1e-9 # eased out
|
||||
assert abs(out2[-1] - base) < 1e-9
|
||||
|
||||
def test_no_boost_at_80_kph_or_for_a_closing_lead(self):
|
||||
ac, NS = self._ac(AccelProfile.eco)
|
||||
v = 80 / 3.6
|
||||
lead = NS(present=True, modelProb=0.9, dRel=30.0, vLead=v + 3.0)
|
||||
base = float(np.interp(v, MAX_ACCEL_BREAKPOINTS, MAX_ACCEL_PROFILES[AccelProfile.eco]))
|
||||
assert abs([ac.get_max_accel(v, False, lead) for _ in range(40)][-1] - base) < 1e-9
|
||||
ac, NS = self._ac(AccelProfile.eco)
|
||||
v = 15.0
|
||||
lead = NS(present=True, modelProb=0.9, dRel=30.0, vLead=v - 1.0)
|
||||
base = float(np.interp(v, MAX_ACCEL_BREAKPOINTS, MAX_ACCEL_PROFILES[AccelProfile.eco]))
|
||||
assert abs([ac.get_max_accel(v, False, lead) for _ in range(40)][-1] - base) < 1e-9
|
||||
|
||||
|
||||
class _Lead:
|
||||
def __init__(self, present=True, modelProb=0.9, dRel=30.0, vLead=18.0):
|
||||
self.present, self.modelProb, self.dRel, self.vLead = present, modelProb, dRel, vLead
|
||||
|
||||
|
||||
class _Radar:
|
||||
def __init__(self, lead):
|
||||
self.leadOne = lead
|
||||
|
||||
|
||||
class _Sink:
|
||||
def clear(self):
|
||||
pass
|
||||
|
||||
def update(self, *args, **kwargs):
|
||||
pass
|
||||
|
||||
|
||||
class TestPlannerLeadWiring(OpenpilotTestCase):
|
||||
"""The boost needs a lead, so the planner has to carry radarState.leadOne into get_max_accel_override."""
|
||||
|
||||
def setUp(self):
|
||||
self.params = Params()
|
||||
self.params.put_bool("AccelPersonalityEnabled", True, block=True)
|
||||
self.params.put("AccelPersonality", AccelProfile.eco, block=True)
|
||||
|
||||
@staticmethod
|
||||
def _planner():
|
||||
from openpilot.sunnypilot.selfdrive.controls.lib.longitudinal_planner import LongitudinalPlannerSP
|
||||
|
||||
planner = LongitudinalPlannerSP.__new__(LongitudinalPlannerSP)
|
||||
planner.accel_controller = AccelController()
|
||||
planner.accel_controller_active = False
|
||||
planner.lead_one = None
|
||||
return planner
|
||||
|
||||
@staticmethod
|
||||
def _settle(planner, v_ego, lead=None, n=60):
|
||||
return [planner.get_max_accel_override(v_ego, lead=lead) for _ in range(n)][-1]
|
||||
|
||||
def test_update_stores_the_radar_lead(self):
|
||||
planner = self._planner()
|
||||
planner.events_sp = _Sink()
|
||||
planner.e2e_alerts_helper = _Sink()
|
||||
lead = _Lead()
|
||||
planner.update({"radarState": _Radar(lead)})
|
||||
assert planner.lead_one is lead
|
||||
|
||||
def test_stored_lead_feeds_the_boost_and_an_explicit_lead_wins(self):
|
||||
v = 15.0
|
||||
lead = _Lead(vLead=v + 3.0)
|
||||
planner = self._planner()
|
||||
planner.lead_one = lead
|
||||
boosted = self._settle(planner, v)
|
||||
|
||||
planner.accel_controller._boost = 0.0
|
||||
planner.lead_one = _Lead(present=False)
|
||||
base = self._settle(planner, v)
|
||||
assert boosted > base + 0.2
|
||||
|
||||
planner.accel_controller._boost = 0.0
|
||||
explicit = self._settle(planner, v, lead=lead)
|
||||
assert abs(explicit - boosted) < 1e-9
|
||||
|
||||
def test_disabled_accel_controller_returns_no_override(self):
|
||||
self.params.put_bool("AccelPersonalityEnabled", False, block=True)
|
||||
assert self._planner().get_max_accel_override(15.0, lead=_Lead()) is None
|
||||
@@ -31,6 +31,7 @@ class LongitudinalPlannerSP:
|
||||
def __init__(self, CP: structs.CarParams, CP_SP: structs.CarParamsSP, mpc):
|
||||
self.accel_controller = AccelController()
|
||||
self.accel_controller_active = False
|
||||
self.lead_one = None
|
||||
self.events_sp = EventsSP()
|
||||
self.dec = DynamicExperimentalController(CP, mpc)
|
||||
self.scc = SmartCruiseControl()
|
||||
@@ -56,11 +57,11 @@ class LongitudinalPlannerSP:
|
||||
|
||||
return False
|
||||
|
||||
def get_max_accel_override(self, v_ego: float, engine_off: bool = False) -> float | None:
|
||||
def get_max_accel_override(self, v_ego: float, engine_off: bool = False, lead=None) -> float | None:
|
||||
if not self.accel_controller.is_enabled():
|
||||
return None
|
||||
|
||||
return self.accel_controller.get_max_accel(v_ego, engine_off)
|
||||
return self.accel_controller.get_max_accel(v_ego, engine_off, lead if lead is not None else self.lead_one)
|
||||
|
||||
def get_cruise_target_override(self, v_ego: float, v_target: float, force_decel: bool, accel_coast: float | None = None) -> float:
|
||||
if not self.accel_controller.is_enabled() or force_decel or self.source != LongitudinalPlanSource.cruise:
|
||||
@@ -120,6 +121,7 @@ class LongitudinalPlannerSP:
|
||||
|
||||
def update(self, sm: messaging.SubMaster) -> None:
|
||||
self.accel_controller.update()
|
||||
self.lead_one = sm['radarState'].leadOne
|
||||
self.events_sp.clear()
|
||||
self.e2e_alerts_helper.update(sm, self.events_sp)
|
||||
|
||||
|
||||
@@ -0,0 +1,243 @@
|
||||
"""
|
||||
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 math
|
||||
|
||||
import numpy as np
|
||||
|
||||
from openpilot.common.realtime import DT_CTRL
|
||||
from openpilot.selfdrive.controls.lib.longcontrol import LongCtrlState
|
||||
|
||||
|
||||
STOP_LEVEL_V = 0.4 # m/s
|
||||
STOP_LEVEL_TAU = 0.2 # s
|
||||
STOP_CONFIRM_RISE = 0.2 # m/s^2
|
||||
STOP_CONFIRM_DELAY = 0.2 # s
|
||||
|
||||
|
||||
class StopConfirm:
|
||||
def __init__(self):
|
||||
self.reset()
|
||||
|
||||
def reset(self):
|
||||
self.level: float | None = None
|
||||
self.collapsed = False
|
||||
self.confirmed_t = -1.0
|
||||
|
||||
def update(self, standstill: bool, v_ego: float, a_long: float | None) -> bool:
|
||||
"""returns True once the stop is confirmed and STOP_CONFIRM_DELAY has passed"""
|
||||
if a_long is None or not math.isfinite(a_long):
|
||||
self.reset()
|
||||
return False
|
||||
if v_ego > STOP_LEVEL_V and not standstill:
|
||||
alpha = DT_CTRL / (STOP_LEVEL_TAU + DT_CTRL)
|
||||
self.level = a_long if self.level is None else self.level + alpha * (a_long - self.level)
|
||||
self.collapsed = False
|
||||
self.confirmed_t = -1.0
|
||||
return False
|
||||
if self.level is not None and a_long >= self.level + STOP_CONFIRM_RISE:
|
||||
self.collapsed = True
|
||||
if standstill and self.collapsed:
|
||||
self.confirmed_t = 0.0 if self.confirmed_t < 0.0 else self.confirmed_t + DT_CTRL
|
||||
elif not standstill:
|
||||
self.confirmed_t = -1.0
|
||||
return self.confirmed_t >= STOP_CONFIRM_DELAY - 1e-9
|
||||
|
||||
|
||||
class StoppingController:
|
||||
"""Optional terminal-stop policy applied after stock LongControl.update()."""
|
||||
|
||||
STOPPING_DECEL_RATE = 0.3 # m/s^2/s
|
||||
STANDSTILL_HOLD_RATE = 1.0 # m/s^2/s
|
||||
STANDSTILL_HOLD_DELAY_LEAD = 1.0 # s
|
||||
STANDSTILL_HOLD_DELAY_NO_LEAD = 1.0 # s
|
||||
STOPPING_FREEZE_MAX = 2.0 # s
|
||||
STOPPING_EXIT_DEBOUNCE = 0.2 # s
|
||||
ENGAGE_STANDSTILL_GRACE = 0.6 # s
|
||||
STOPPING_FOLLOW_MIN = -0.10 # m/s^2
|
||||
STOPPING_FOLLOW_RATE = 2.0 # m/s^3
|
||||
CREEP_V_MIN = 0.03 # m/s
|
||||
CREEP_RATE_GROWTH = 1.5 # m/s^2/s
|
||||
CREEP_RATE_MAX = 1.0 # m/s^2/s
|
||||
|
||||
BLEND_V = 10.0 / 3.6 # m/s
|
||||
END_V_HI = 5.0 / 3.6 # m/s
|
||||
END_HI = -0.65 # m/s^2
|
||||
END_LO = -0.35 # m/s^2
|
||||
|
||||
END_CURVE_V = [0.0, 1.0 / 3.6, 2.0 / 3.6, 3.0 / 3.6, 4.0 / 3.6, 5.0 / 3.6] # m/s
|
||||
END_CURVE_A = [END_LO, END_LO, -0.56, -0.60, -0.65, END_HI] # m/s^2
|
||||
END_PLAN_MIN = -0.25 # m/s^2
|
||||
END_RATE = 2.0 # m/s^3
|
||||
|
||||
RELEASE_FAST = 4.0 # m/s^3
|
||||
RELEASE_SLOW = 1.5 # m/s^3
|
||||
RELEASE_BAND_LO = -0.6 # m/s^2
|
||||
RELEASE_BAND_HI = 0.4 # m/s^2
|
||||
|
||||
REROLL_ACCEL = -0.6 # m/s^2
|
||||
REROLL_JERK = 6.0 # m/s^3
|
||||
|
||||
FLAT_HOLD_ACCEL = -1.2 # m/s^2
|
||||
FLAT_PITCH_DEG = 1.0 # deg
|
||||
SLOPE_PITCH_DEG = 2.0 # deg
|
||||
|
||||
CF_WINDOW = 10.0 # s
|
||||
CF_V_MAX = 8.0 / 3.6 # m/s
|
||||
CF_TRIGGER = -0.15 # m/s^2
|
||||
CF_END = -0.24 # m/s^2
|
||||
CF_PLAN_SOFT = -0.45 # m/s^2
|
||||
CF_PLAN_FIRM = -0.70 # m/s^2
|
||||
CF_RATE = 0.9 # m/s^3
|
||||
|
||||
CF_SETTLE_ACCEL = -0.6 # m/s^2
|
||||
CF_SETTLE_JERK = 0.72 # m/s^3
|
||||
CF_HOLD_RATE = 0.3 # m/s^2/s
|
||||
|
||||
def __init__(self, stop_accel):
|
||||
self.stop_accel = stop_accel
|
||||
self.stop_confirm = StopConfirm()
|
||||
self.cf_t: float | None = None
|
||||
self.cf_vmax = 0.0
|
||||
self.cf_stopping = False
|
||||
self.cf_stopped = False
|
||||
self.standstill_t = 0.0
|
||||
self.stopping_t = 0.0
|
||||
self.go_t = 0.0
|
||||
self.engaged_t = 0.0
|
||||
self.stopped_once = False
|
||||
self.creep_t = 0.0
|
||||
self.end_active = False
|
||||
self.a_entry = self.END_HI
|
||||
self.releasing = False
|
||||
|
||||
def _blend(self, v_ego):
|
||||
if v_ego < self.END_V_HI:
|
||||
return float(np.interp(max(v_ego, 0.0), self.END_CURVE_V, self.END_CURVE_A))
|
||||
return float(np.interp(v_ego, [self.END_V_HI, self.BLEND_V], [self.END_HI, self.a_entry]))
|
||||
|
||||
def _end_request(self, a_target, prev_accel, v_ego):
|
||||
target = min(a_target, self._blend(v_ego)) # the firmer of the plan and the line
|
||||
step = self.END_RATE * DT_CTRL
|
||||
return float(np.clip(target, prev_accel - step, prev_accel + step))
|
||||
|
||||
def _hold_floor(self, pitch, creeping):
|
||||
if creeping or pitch is None or not math.isfinite(pitch):
|
||||
return self.stop_accel
|
||||
if abs(math.degrees(pitch) - self.FLAT_PITCH_DEG) >= self.SLOPE_PITCH_DEG:
|
||||
return self.stop_accel
|
||||
return max(self.stop_accel, self.FLAT_HOLD_ACCEL)
|
||||
|
||||
def _cf_request(self, a_target, prev_accel):
|
||||
target = float(np.interp(a_target, [self.CF_PLAN_FIRM, self.CF_PLAN_SOFT], [self.CF_PLAN_FIRM, self.CF_END]))
|
||||
target = min(target, a_target) if a_target < self.CF_PLAN_FIRM else target
|
||||
if target < prev_accel:
|
||||
return max(target, min(prev_accel, 0.0) - self.CF_RATE * DT_CTRL)
|
||||
return min(target, prev_accel + self.END_RATE * DT_CTRL)
|
||||
|
||||
def _update_creep_follow(self, state, CS, a_target):
|
||||
if state == LongCtrlState.off:
|
||||
self.cf_t, self.cf_stopping, self.cf_stopped = None, False, False
|
||||
return False
|
||||
if CS.standstill:
|
||||
if self.cf_stopping:
|
||||
self.cf_stopped = True
|
||||
self.cf_t = 0.0 if self.standstill_t > 0.5 else self.cf_t
|
||||
self.cf_vmax, self.cf_stopping = 0.0, False
|
||||
return False
|
||||
self.cf_stopped = False
|
||||
if self.cf_t is None:
|
||||
return False
|
||||
self.cf_t += DT_CTRL
|
||||
self.cf_vmax = max(self.cf_vmax, CS.vEgo)
|
||||
if self.cf_t > self.CF_WINDOW or self.cf_vmax >= self.CF_V_MAX or a_target > 0.0 and self.cf_stopping:
|
||||
self.cf_t, self.cf_stopping = None, False
|
||||
return False
|
||||
if a_target <= self.CF_TRIGGER:
|
||||
self.cf_stopping = True
|
||||
return self.cf_stopping
|
||||
|
||||
def update(self, prev_state, state, CS, a_target, prev_accel, stock_accel, accel_limits, has_lead=False, pitch=None,
|
||||
a_long=None):
|
||||
if prev_state == LongCtrlState.stopping and state == LongCtrlState.pid and CS.standstill:
|
||||
self.go_t += DT_CTRL
|
||||
if self.go_t < self.STOPPING_EXIT_DEBOUNCE:
|
||||
state = LongCtrlState.stopping
|
||||
else:
|
||||
self.go_t = 0.0
|
||||
|
||||
self.standstill_t = self.standstill_t + DT_CTRL if CS.standstill else 0.0
|
||||
self.engaged_t = 0.0 if state == LongCtrlState.off else self.engaged_t + DT_CTRL
|
||||
self.stopping_t = self.stopping_t + DT_CTRL if state == LongCtrlState.stopping else 0.0
|
||||
if state != LongCtrlState.stopping:
|
||||
self.stopped_once = False
|
||||
elif CS.standstill:
|
||||
self.stopped_once = True
|
||||
|
||||
creeping = self.stopped_once and not CS.standstill and CS.vEgo > self.CREEP_V_MIN
|
||||
self.creep_t = self.creep_t + DT_CTRL if creeping else 0.0
|
||||
cf_stop = self._update_creep_follow(state, CS, a_target)
|
||||
stop_confirmed = self.stop_confirm.update(CS.standstill, CS.vEgo, a_long)
|
||||
|
||||
rolling = not CS.standstill and not self.stopped_once
|
||||
if state == LongCtrlState.off or not rolling or CS.vEgo >= self.BLEND_V or a_target > 0.0:
|
||||
self.end_active = False
|
||||
elif not self.end_active and a_target <= self.END_PLAN_MIN:
|
||||
self.end_active = True
|
||||
self.a_entry = min(prev_accel, self.END_HI)
|
||||
|
||||
if state != LongCtrlState.stopping:
|
||||
if cf_stop:
|
||||
return state, float(np.clip(self._cf_request(a_target, prev_accel), accel_limits[0], accel_limits[1]))
|
||||
if self.end_active:
|
||||
return state, float(np.clip(self._end_request(a_target, prev_accel, CS.vEgo), accel_limits[0], accel_limits[1]))
|
||||
if prev_state == LongCtrlState.stopping and state == LongCtrlState.pid and prev_accel < self.RELEASE_BAND_HI:
|
||||
self.releasing = True
|
||||
if self.releasing:
|
||||
if stock_accel <= prev_accel or prev_accel >= self.RELEASE_BAND_HI or state == LongCtrlState.off:
|
||||
self.releasing = False
|
||||
return state, stock_accel
|
||||
rate = self.RELEASE_FAST if prev_accel < self.RELEASE_BAND_LO else self.RELEASE_SLOW
|
||||
return state, float(min(stock_accel, prev_accel + rate * DT_CTRL))
|
||||
return state, stock_accel
|
||||
|
||||
output_accel = prev_accel
|
||||
hold_floor = self._hold_floor(pitch, creeping)
|
||||
if output_accel > hold_floor:
|
||||
output_accel = min(output_accel, 0.0)
|
||||
if not CS.standstill and not self.stopped_once and a_target < self.STOPPING_FOLLOW_MIN and a_target > output_accel:
|
||||
output_accel = min(a_target, output_accel + self.STOPPING_FOLLOW_RATE * DT_CTRL)
|
||||
if cf_stop:
|
||||
output_accel = self._cf_request(a_target, prev_accel)
|
||||
elif self.end_active:
|
||||
output_accel = self._end_request(a_target, prev_accel, CS.vEgo)
|
||||
|
||||
hold_delay = self.STANDSTILL_HOLD_DELAY_LEAD if has_lead else self.STANDSTILL_HOLD_DELAY_NO_LEAD
|
||||
if CS.standstill and not has_lead and self.engaged_t < self.ENGAGE_STANDSTILL_GRACE:
|
||||
rate = 0.0
|
||||
elif self.cf_stopped and CS.standstill and output_accel > self.CF_SETTLE_ACCEL:
|
||||
output_accel = max(self.CF_SETTLE_ACCEL, output_accel - self.CF_SETTLE_JERK * DT_CTRL)
|
||||
rate = 0.0
|
||||
elif self.cf_stopped and CS.standstill:
|
||||
rate = self.CF_HOLD_RATE # H: then the rest of the hold, slowly
|
||||
elif self.standstill_t >= hold_delay or stop_confirmed:
|
||||
rate = self.STANDSTILL_HOLD_RATE
|
||||
elif creeping and output_accel > self.REROLL_ACCEL:
|
||||
output_accel = max(self.REROLL_ACCEL, output_accel - self.REROLL_JERK * DT_CTRL)
|
||||
rate = 0.0
|
||||
elif creeping:
|
||||
rate = min(self.STOPPING_DECEL_RATE + self.CREEP_RATE_GROWTH * self.creep_t, self.CREEP_RATE_MAX)
|
||||
elif self.stopping_t >= self.STOPPING_FREEZE_MAX:
|
||||
rate = self.STOPPING_DECEL_RATE
|
||||
else:
|
||||
rate = 0.0
|
||||
if hold_floor != self.stop_accel:
|
||||
output_accel = max(output_accel - rate * DT_CTRL, min(hold_floor, output_accel))
|
||||
else:
|
||||
output_accel -= rate * DT_CTRL
|
||||
|
||||
return state, float(np.clip(output_accel, accel_limits[0], accel_limits[1]))
|
||||
@@ -0,0 +1,314 @@
|
||||
"""
|
||||
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 math
|
||||
from types import SimpleNamespace
|
||||
|
||||
from openpilot.common.realtime import DT_CTRL
|
||||
from openpilot.common.test import OpenpilotTestCase
|
||||
from openpilot.selfdrive.controls.lib.longcontrol import LongCtrlState
|
||||
from openpilot.sunnypilot.selfdrive.controls.lib.stopping_controller import StoppingController
|
||||
|
||||
LIMITS = (-3.5, 2.0)
|
||||
PID, STOPPING, OFF = LongCtrlState.pid, LongCtrlState.stopping, LongCtrlState.off
|
||||
END_LO, END_HI = StoppingController.END_LO, StoppingController.END_HI
|
||||
|
||||
|
||||
def cs(v_kph, standstill=False):
|
||||
return SimpleNamespace(vEgo=v_kph / 3.6, standstill=standstill)
|
||||
|
||||
|
||||
def run(sc, state, car, a_target, prev, secs, stock=None):
|
||||
"""step the controller for secs with a fixed plan; returns the last output"""
|
||||
out = prev
|
||||
for _ in range(int(secs / DT_CTRL)):
|
||||
_, out = sc.update(state, state, car, a_target, out, a_target if stock is None else stock, LIMITS, has_lead=True)
|
||||
return out
|
||||
|
||||
|
||||
def blend(sc, v_kph):
|
||||
return sc._blend(v_kph / 3.6)
|
||||
|
||||
|
||||
class TestEndOfStop(OpenpilotTestCase):
|
||||
def test_the_line_runs_from_the_entry_request_through_5kph_to_the_rest_value(self):
|
||||
sc = StoppingController(-2.0)
|
||||
out = run(sc, PID, cs(9.9), -1.20, -1.20, 0.2)
|
||||
assert sc.end_active and abs(sc.a_entry - (-1.20)) < 1e-6
|
||||
# 7.5 km/h: halfway between the entry request and END_HI; the planner has eased to -0.30, the line is sent
|
||||
out = run(sc, PID, cs(7.5), -0.30, out, 0.6)
|
||||
assert abs(blend(sc, 7.5) - (-1.20 + (END_HI - -1.20) * 0.5)) < 1e-6
|
||||
assert abs(out - blend(sc, 7.5)) < 1e-6
|
||||
# 5 km/h: END_HI; below it a parabola: steep first, flat toward the rest value; the wheel stop: END_LO
|
||||
out = run(sc, PID, cs(5.0), -0.20, out, 0.4)
|
||||
assert abs(out - END_HI) < 1e-6
|
||||
out = run(sc, PID, cs(2.5), -0.20, out, 0.4)
|
||||
assert abs(out - (-0.48 + -0.60) / 2) < 1e-6
|
||||
assert abs(blend(sc, 4.0) - (-0.65)) < 1e-6 and abs(blend(sc, 1.0) - (-0.30)) < 1e-6
|
||||
assert abs(blend(sc, 0.5) - (-0.30)) < 1e-6 # 1-0 km/h unchanged
|
||||
out = run(sc, PID, cs(0.0), -0.20, out, 0.4)
|
||||
assert abs(out - END_LO) < 1e-6
|
||||
|
||||
def test_the_line_is_a_floor_a_firmer_plan_still_passes(self):
|
||||
sc = StoppingController(-2.0)
|
||||
out = run(sc, PID, cs(9.9), -1.20, -1.20, 0.2)
|
||||
out = run(sc, PID, cs(5.0), -1.10, out, 0.5)
|
||||
assert abs(out - (-1.10)) < 1e-6
|
||||
out = run(sc, PID, cs(2.5), -1.8, out, 0.6)
|
||||
assert abs(out - (-1.8)) < 1e-6
|
||||
|
||||
def test_a_light_plan_at_entry_starts_the_line_at_end_hi(self):
|
||||
sc = StoppingController(-2.0)
|
||||
light = END_HI + 0.1
|
||||
out = run(sc, PID, cs(7.0), light, light, 0.5)
|
||||
assert abs(sc.a_entry - END_HI) < 1e-6
|
||||
assert abs(out - END_HI) < 1e-6
|
||||
# the planner's own curve eases off below 5 km/h: the request follows the line, never the plan
|
||||
out = run(sc, PID, cs(2.5), -0.10, out, 0.5)
|
||||
assert abs(out - blend(sc, 2.5)) < 1e-6
|
||||
|
||||
def test_entry_is_rate_limited(self):
|
||||
sc = StoppingController(-2.0)
|
||||
light = END_HI + 0.2
|
||||
_, out = sc.update(PID, PID, cs(7.0), light, light, light, LIMITS, has_lead=True)
|
||||
assert out > END_HI
|
||||
assert abs(out - (light - StoppingController.END_RATE * DT_CTRL)) < 1e-6
|
||||
|
||||
def test_an_emergency_plan_still_passes(self):
|
||||
sc = StoppingController(-2.0)
|
||||
out = run(sc, PID, cs(2.5), -1.8, -0.40, 0.8)
|
||||
assert abs(out - (-1.8)) < 1e-6
|
||||
|
||||
def test_crawl_follow_is_untouched(self):
|
||||
sc = StoppingController(-2.0)
|
||||
out = run(sc, PID, cs(2.0), -0.10, -0.10, 0.5)
|
||||
assert abs(out - (-0.10)) < 1e-6
|
||||
assert not sc.end_active
|
||||
|
||||
def test_above_blend_v_nothing_changes(self):
|
||||
sc = StoppingController(-2.0)
|
||||
out = run(sc, PID, cs(12.0), -0.40, -0.40, 0.5)
|
||||
assert abs(out - (-0.40)) < 1e-6
|
||||
assert not sc.end_active
|
||||
|
||||
def test_lead_moving_off_releases_the_hold(self):
|
||||
sc = StoppingController(-2.0)
|
||||
run(sc, PID, cs(2.0), -0.60, -0.60, 0.5)
|
||||
assert sc.end_active
|
||||
_, out = sc.update(PID, PID, cs(1.5), 0.50, END_LO, 0.50, LIMITS, has_lead=True)
|
||||
assert not sc.end_active and abs(out - 0.50) < 1e-6
|
||||
|
||||
def test_stopping_state_keeps_the_line_until_the_wheels_stop_then_holds(self):
|
||||
sc = StoppingController(-1.0)
|
||||
run(sc, PID, cs(2.0), END_HI, END_HI, 0.5)
|
||||
# 1 km/h, stopping state, the planner's curve has eased to -0.10: Kumar's lighter-follow would ease the request
|
||||
out = run(sc, STOPPING, cs(1.0), -0.10, blend(sc, 2.0), 0.4)
|
||||
assert abs(out - blend(sc, 1.0)) < 1e-6
|
||||
# wheels stop: the last value of the line is held until the hold delay, then ramps to stopAccel at the hold rate
|
||||
out_at_stop = run(sc, STOPPING, cs(0.0, standstill=True), -0.10, out, 0.9)
|
||||
assert abs(out_at_stop - out) < 1e-6 and END_HI < out_at_stop <= END_LO + 1e-6
|
||||
out_held = run(sc, STOPPING, cs(0.0, standstill=True), -0.10, out_at_stop, 1.0)
|
||||
assert out_held < out_at_stop - 0.3
|
||||
assert out_held >= -1.0 - 1e-6
|
||||
|
||||
|
||||
class TestHoldRelease(OpenpilotTestCase):
|
||||
"""2026-10-04 14:43: leaving the hold is fast while the request is far below zero, then linear and slow through the
|
||||
hand-over band, and the plan is never exceeded."""
|
||||
|
||||
def test_release_is_slow_through_the_handover_band(self):
|
||||
sc = StoppingController(-2.0)
|
||||
out = -2.0
|
||||
t = 0.0
|
||||
t_lo = t_hi = None
|
||||
state = STOPPING # controlsd feeds the returned state back as prev_state (the 0.2 s exit debounce lives there)
|
||||
for _ in range(300):
|
||||
state, out = sc.update(state, PID, cs(0.0, standstill=True), 0.8, out, 0.8, LIMITS, has_lead=True)
|
||||
t += DT_CTRL
|
||||
if t_lo is None and out >= StoppingController.RELEASE_BAND_LO:
|
||||
t_lo = t
|
||||
if t_hi is None and out >= StoppingController.RELEASE_BAND_HI:
|
||||
t_hi = t
|
||||
assert out <= 0.8 + 1e-9
|
||||
assert t_lo is not None and t_lo < 0.6 # 0.2 s debounce, then -2.0 -> -0.6 at the fast rate (0.35 s)
|
||||
assert t_hi is not None and 1.15 < t_hi < 1.3 # then -0.6 -> +0.4 at 1.5 m/s^3 (0.67 s)
|
||||
assert abs(out - 0.8) < 1e-9 and not sc.releasing
|
||||
|
||||
def test_a_plan_that_drops_again_ends_the_release(self):
|
||||
sc = StoppingController(-2.0)
|
||||
state, out = STOPPING, -2.0
|
||||
for _ in range(30): # through the exit debounce
|
||||
state, out = sc.update(state, PID, cs(0.0, standstill=True), 0.5, out, 0.5, LIMITS, has_lead=True)
|
||||
assert sc.releasing and -2.0 < out < 0.5
|
||||
_, out = sc.update(state, PID, cs(0.0, standstill=True), -2.0, out, -2.0, LIMITS, has_lead=True)
|
||||
assert abs(out - (-2.0)) < 1e-9 and not sc.releasing
|
||||
|
||||
|
||||
class TestEngageAtStandstill(OpenpilotTestCase):
|
||||
def _engage(self, has_lead, secs):
|
||||
sc = StoppingController(-2.0)
|
||||
car = cs(0.0, standstill=True)
|
||||
for _ in range(300): # standing in the auto hold, openpilot not engaged
|
||||
sc.update(OFF, OFF, car, 0.0, 0.0, 0.0, LIMITS, has_lead=has_lead)
|
||||
out = 0.0
|
||||
for _ in range(int(secs / DT_CTRL)):
|
||||
_, out = sc.update(STOPPING, STOPPING, car, 0.0, out, 0.0, LIMITS, has_lead=has_lead)
|
||||
return out, sc
|
||||
|
||||
def test_no_brake_pulse_right_after_engaging_without_lead(self):
|
||||
# route 0000010a 23:58:49: the hold ramp reached -0.21 in 0.2 s and the PCM braked 1600 N before the launch
|
||||
out, sc = self._engage(False, 0.5)
|
||||
assert out == 0.0
|
||||
|
||||
def test_the_hold_ramp_follows_after_the_grace(self):
|
||||
out, sc = self._engage(False, StoppingController.ENGAGE_STANDSTILL_GRACE + 0.3)
|
||||
assert out < -0.2
|
||||
|
||||
def test_with_a_lead_the_hold_starts_at_once(self):
|
||||
out, _ = self._engage(True, 0.3)
|
||||
assert out < -0.2
|
||||
|
||||
|
||||
def stand(sc, secs, out=StoppingController.END_LO, pitch=None, a_target=-0.1):
|
||||
state = STOPPING
|
||||
for _ in range(int(secs / DT_CTRL)):
|
||||
state, out = sc.update(state, STOPPING, cs(0.0, standstill=True), a_target, out, a_target, LIMITS, has_lead=True, pitch=pitch)
|
||||
return out
|
||||
|
||||
|
||||
FLAT = math.radians(1.0)
|
||||
SLOPE = math.radians(-4.0)
|
||||
|
||||
|
||||
class TestRerollHold(OpenpilotTestCase):
|
||||
"""route 0000010e 17:38:48: stood at -0.30, rolled at 0.8 km/h for 1.1 s inside the hold delay"""
|
||||
|
||||
def _reroll(self, sc):
|
||||
out = stand(sc, 0.9)
|
||||
state = STOPPING
|
||||
outs = []
|
||||
for _ in range(20):
|
||||
state, out = sc.update(state, STOPPING, cs(0.8), -0.05, out, -0.05, LIMITS, has_lead=True)
|
||||
outs.append(out)
|
||||
return outs
|
||||
|
||||
def test_steps_to_the_reroll_request_within_0_1s(self):
|
||||
outs = self._reroll(StoppingController(-2.0))
|
||||
assert outs[4] <= StoppingController.REROLL_ACCEL + 1e-9 # 0.05 s
|
||||
assert all(b - a >= -StoppingController.REROLL_JERK * DT_CTRL - 1e-9 for a, b in zip(outs, outs[1:], strict=False))
|
||||
|
||||
|
||||
class TestFlatHold(OpenpilotTestCase):
|
||||
def test_flat_hold_stops_at_minus_1_2(self):
|
||||
out = stand(StoppingController(-2.0), 4.0, pitch=FLAT)
|
||||
assert abs(out - StoppingController.FLAT_HOLD_ACCEL) < 1e-9
|
||||
|
||||
def test_slope_or_no_pitch_keeps_stop_accel(self):
|
||||
assert stand(StoppingController(-2.0), 4.0, pitch=SLOPE) <= -2.0 + 1e-6
|
||||
assert stand(StoppingController(-2.0), 4.0, pitch=None) <= -2.0 + 1e-6
|
||||
|
||||
def test_a_creep_on_the_flat_hold_goes_deeper(self):
|
||||
sc = StoppingController(-2.0)
|
||||
out = stand(sc, 4.0, pitch=FLAT)
|
||||
state = STOPPING
|
||||
for _ in range(150):
|
||||
state, out = sc.update(state, STOPPING, cs(0.8), -0.1, out, -0.1, LIMITS, has_lead=True, pitch=FLAT)
|
||||
assert out < StoppingController.FLAT_HOLD_ACCEL - 0.5
|
||||
|
||||
|
||||
class TestCreepFollowStop(OpenpilotTestCase):
|
||||
"""route 0000010d 23:56:59: lead moved ~2 m, the car followed at 2.5 km/h and the brake bit at -0.88"""
|
||||
|
||||
def _creep(self, sc, plan, v_kph=2.0, secs=2.0, out=0.4):
|
||||
stand(sc, 2.0)
|
||||
outs = []
|
||||
for _ in range(int(secs / DT_CTRL)):
|
||||
_, out = sc.update(PID, PID, cs(v_kph), plan, out, plan, LIMITS, has_lead=True)
|
||||
outs.append(out)
|
||||
return outs
|
||||
|
||||
def test_stops_with_minus_0_24_not_the_end_line(self):
|
||||
outs = self._creep(StoppingController(-2.0), -0.40)
|
||||
assert outs[0] <= 0.0 # a positive request is dropped at once
|
||||
assert abs(outs[-1] - StoppingController.CF_END) < 1e-9
|
||||
steps = [a - b for a, b in zip(outs, outs[1:], strict=False)]
|
||||
assert max(steps) <= StoppingController.CF_RATE * DT_CTRL + 1e-9
|
||||
|
||||
def test_a_firm_plan_still_wins(self):
|
||||
outs = self._creep(StoppingController(-2.0), -1.0)
|
||||
assert abs(outs[-1] - (-1.0)) < 1e-9
|
||||
|
||||
def test_light_plan_keeps_minus_0_24_to_the_stop(self):
|
||||
sc = StoppingController(-2.0)
|
||||
outs = self._creep(sc, -0.40, secs=1.0)
|
||||
_, out = sc.update(PID, PID, cs(1.0), -0.10, outs[-1], -0.10, LIMITS, has_lead=True)
|
||||
assert abs(out - StoppingController.CF_END) < 1e-9
|
||||
|
||||
def test_not_after_a_real_launch(self):
|
||||
sc = StoppingController(-2.0)
|
||||
stand(sc, 2.0)
|
||||
out = 0.5
|
||||
for _ in range(300):
|
||||
_, out = sc.update(PID, PID, cs(15.0), 0.3, out, 0.3, LIMITS, has_lead=True)
|
||||
_, out = sc.update(PID, PID, cs(6.0), -0.40, out, -0.40, LIMITS, has_lead=True)
|
||||
assert sc.cf_t is None
|
||||
|
||||
def test_lead_moving_off_ends_it(self):
|
||||
sc = StoppingController(-2.0)
|
||||
outs = self._creep(sc, -0.40, secs=0.5)
|
||||
_, out = sc.update(PID, PID, cs(2.0), 0.5, outs[-1], 0.5, LIMITS, has_lead=True)
|
||||
assert out == 0.5 and not sc.cf_stopping
|
||||
|
||||
def test_after_the_creep_stop_a_little_brake_at_once_then_the_hold_slowly(self):
|
||||
"""route 00000112 23:16:57: -0.24 for the 1.0 s hold delay, then -1.2 within ~1 s felt like a second brake"""
|
||||
sc = StoppingController(-2.0)
|
||||
outs = self._creep(sc, -0.40)
|
||||
out, state, trace = outs[-1], STOPPING, []
|
||||
for _ in range(300):
|
||||
state, out = sc.update(state, STOPPING, cs(0.0, standstill=True), -0.05, out, -0.05, LIMITS, has_lead=True,
|
||||
pitch=FLAT)
|
||||
trace.append(out)
|
||||
assert trace[50] <= StoppingController.CF_SETTLE_ACCEL + 1e-9 # -0.6 within 0.5 s, no hold delay
|
||||
assert trace[10] > StoppingController.CF_SETTLE_ACCEL # but gradually
|
||||
assert abs(trace[150] - (StoppingController.CF_SETTLE_ACCEL - 1.0 * StoppingController.CF_HOLD_RATE)) < 0.02
|
||||
assert abs(trace[-1] - StoppingController.FLAT_HOLD_ACCEL) < 1e-9 # the flat hold after ~2.5 s
|
||||
steps = [a - b for a, b in zip(trace, trace[1:], strict=False)]
|
||||
assert max(steps) <= StoppingController.CF_SETTLE_JERK * DT_CTRL + 1e-9
|
||||
|
||||
def test_an_ordinary_stop_keeps_the_hold_delay(self):
|
||||
sc = StoppingController(-2.0)
|
||||
out = stand(sc, 0.9, pitch=FLAT)
|
||||
assert abs(out - END_LO) < 1e-9
|
||||
|
||||
|
||||
class TestConfirmedStopEarlyHold(OpenpilotTestCase):
|
||||
"""hold 0.2 s after a CONFIRMED stop (standstill flag + the body's deceleration collapsing), else the 1.0 s delay"""
|
||||
|
||||
def _stop(self, sc, collapse):
|
||||
out, state = -0.40, PID
|
||||
for k in range(100): # rolling to the stop at -0.45 m/s^2 (body)
|
||||
state, out = sc.update(state, STOPPING, cs(2.0 - 0.019 * k), -0.3, out, -0.3, LIMITS, has_lead=True, pitch=FLAT, a_long=-0.45)
|
||||
trace = []
|
||||
for k in range(150): # standing: the body pitches back (deceleration collapses) after 0.1 s, or not
|
||||
a = -0.05 if (collapse and k >= 10) else -0.45
|
||||
state, out = sc.update(state, STOPPING, cs(0.0, True), -0.05, out, -0.05, LIMITS, has_lead=True, pitch=FLAT, a_long=a)
|
||||
trace.append(out)
|
||||
return trace
|
||||
|
||||
def test_confirmed_stop_holds_after_0_2s(self):
|
||||
sc = StoppingController(-2.0)
|
||||
tr = self._stop(sc, True)
|
||||
assert tr[25] == tr[0] # 0.1 s to the pitch-back + 0.2 s: nothing yet at 0.25 s
|
||||
assert tr[40] < tr[0] - 0.05 # holding by 0.4 s
|
||||
|
||||
def test_no_confirmation_keeps_the_1s_delay(self):
|
||||
sc = StoppingController(-2.0)
|
||||
tr = self._stop(sc, False)
|
||||
assert tr[90] == tr[0] and tr[110] < tr[0]
|
||||
|
||||
def test_no_pose_keeps_the_1s_delay(self):
|
||||
sc = StoppingController(-2.0)
|
||||
assert not sc.stop_confirm.update(True, 0.0, None)
|
||||
@@ -1652,6 +1652,18 @@
|
||||
"widget": "toggle",
|
||||
"title": "Use Metric System",
|
||||
"description": "Display speed in km/h instead of mph."
|
||||
},
|
||||
{
|
||||
"key": "SunnypilotStoppingController",
|
||||
"widget": "toggle",
|
||||
"needs_onroad_cycle": true,
|
||||
"title": "Stopping Controller",
|
||||
"description": "Take over the end of a stop. The request follows the speed down to the wheel stop, then holds the car at a standstill.",
|
||||
"enablement": [
|
||||
{
|
||||
"type": "offroad_only"
|
||||
}
|
||||
]
|
||||
}
|
||||
]
|
||||
},
|
||||
@@ -2360,6 +2372,17 @@
|
||||
}
|
||||
]
|
||||
},
|
||||
{
|
||||
"key": "ToyotaBrakeOnset",
|
||||
"widget": "toggle",
|
||||
"needs_onroad_cycle": true,
|
||||
"title": "Toyota: soft brake onset, overshoot limit and regen hand-over",
|
||||
"enablement": [
|
||||
{
|
||||
"type": "not_engaged"
|
||||
}
|
||||
]
|
||||
},
|
||||
{
|
||||
"key": "ToyotaEnhancedBsm",
|
||||
"widget": "toggle",
|
||||
|
||||
@@ -32,6 +32,14 @@ sections:
|
||||
widget: toggle
|
||||
title: Use Metric System
|
||||
description: Display speed in km/h instead of mph.
|
||||
- key: SunnypilotStoppingController
|
||||
widget: toggle
|
||||
needs_onroad_cycle: true
|
||||
title: Stopping Controller
|
||||
description: Take over the end of a stop. The request follows the speed down to the wheel stop, then holds the car at a
|
||||
standstill.
|
||||
enablement:
|
||||
- $ref: '#/macros/offroad'
|
||||
- id: recording
|
||||
title: Recording
|
||||
description: Camera and audio recording during drives
|
||||
|
||||
@@ -88,6 +88,12 @@ sections:
|
||||
title: 'Toyota: Auto Brake Hold FOR TSS2 HYBRID CARS'
|
||||
enablement:
|
||||
- $ref: '#/macros/not_engaged'
|
||||
- key: ToyotaBrakeOnset
|
||||
widget: toggle
|
||||
needs_onroad_cycle: true
|
||||
title: 'Toyota: soft brake onset, overshoot limit and regen hand-over'
|
||||
enablement:
|
||||
- $ref: '#/macros/not_engaged'
|
||||
- key: ToyotaEnhancedBsm
|
||||
widget: toggle
|
||||
needs_onroad_cycle: true
|
||||
|
||||
Reference in New Issue
Block a user