mirror of
https://github.com/sunnypilot/sunnypilot.git
synced 2026-08-21 04:53:47 +08:00
feat(long): smooth lead following with acceleration profiles
This commit is contained in:
rav4kumar
@@ -203,6 +203,7 @@ struct LongitudinalPlanSP @0xf35cc4560bbf6ec2 {
|
||||
aTarget @5 :Float32;
|
||||
events @6 :List(OnroadEventSP.Event);
|
||||
e2eAlerts @7 :E2eAlerts;
|
||||
accelController @8 :AccelController;
|
||||
|
||||
struct DynamicExperimentalControl {
|
||||
state @0 :DynamicExperimentalControlState;
|
||||
@@ -305,6 +306,35 @@ struct LongitudinalPlanSP @0xf35cc4560bbf6ec2 {
|
||||
greenLightAlert @0 :Bool;
|
||||
leadDepartAlert @1 :Bool;
|
||||
}
|
||||
|
||||
struct AccelController {
|
||||
enabled @0 :Bool;
|
||||
active @1 :Bool;
|
||||
shadowOnlyDEPRECATED @2 :Bool;
|
||||
profile @3 :Profile;
|
||||
state @4 :State;
|
||||
|
||||
enum Profile {
|
||||
eco @0;
|
||||
normal @1;
|
||||
sport @2;
|
||||
}
|
||||
|
||||
enum State {
|
||||
inactive @0;
|
||||
free @1;
|
||||
restrict @2;
|
||||
hold @3;
|
||||
release @4;
|
||||
stopHold @5;
|
||||
}
|
||||
}
|
||||
|
||||
enum AccelerationPersonality {
|
||||
eco @0;
|
||||
normal @1;
|
||||
sport @2;
|
||||
}
|
||||
}
|
||||
|
||||
struct OnroadEventSP @0xda96579883444c35 {
|
||||
|
||||
@@ -231,6 +231,10 @@ inline static std::unordered_map<std::string, ParamKeyAttributes> keys = {
|
||||
{"DynamicExperimentalControl", {PERSISTENT | BACKUP, BOOL, "0"}},
|
||||
{"BlindSpot", {PERSISTENT | BACKUP, BOOL, "0"}},
|
||||
|
||||
// Accel Controller profiles (Eco / Normal / Sport)
|
||||
{"AccelPersonalityEnabled", {PERSISTENT | BACKUP, BOOL, "0"}},
|
||||
{"AccelPersonality", {PERSISTENT | BACKUP, INT, "1"}},
|
||||
|
||||
// sunnypilot model params
|
||||
{"CameraOffset", {PERSISTENT | BACKUP, FLOAT, "0.0"}},
|
||||
{"LagdToggle", {PERSISTENT | BACKUP, BOOL, "1"}},
|
||||
|
||||
@@ -117,12 +117,16 @@ class TestParams(OpenpilotTestCase):
|
||||
def test_params_default_value(self):
|
||||
self.params.remove("LanguageSetting")
|
||||
self.params.remove("LongitudinalPersonality")
|
||||
self.params.remove("AccelPersonalityEnabled")
|
||||
self.params.remove("AccelPersonality")
|
||||
self.params.remove("LiveParametersV2")
|
||||
|
||||
assert self.params.get("LanguageSetting") is None
|
||||
assert self.params.get("LanguageSetting", return_default=False) is None
|
||||
assert isinstance(self.params.get("LanguageSetting", return_default=True), str)
|
||||
assert isinstance(self.params.get("LongitudinalPersonality", return_default=True), int)
|
||||
assert self.params.get("AccelPersonalityEnabled", return_default=True) is False
|
||||
assert self.params.get("AccelPersonality", return_default=True) == 1
|
||||
assert self.params.get("LiveParametersV2") is None
|
||||
assert self.params.get("LiveParametersV2", return_default=True) is None
|
||||
|
||||
|
||||
@@ -151,6 +151,7 @@ class LongitudinalPlanner(LongitudinalPlannerSP):
|
||||
if is_e2e:
|
||||
candidates.append((output_a_target_e2e, LongitudinalPlanSource.e2e, output_should_stop_e2e))
|
||||
|
||||
candidates = self.update_accel_controller(sm, candidates)
|
||||
output_a_target, self.mpc.source, _ = min(candidates, key=lambda c: c[0])
|
||||
self.output_should_stop = any(should_stop for _, _, should_stop in candidates)
|
||||
self.output_a_target = np.clip(output_a_target, ACCEL_MIN, ACCEL_MAX)
|
||||
|
||||
@@ -11,6 +11,14 @@ from openpilot.selfdrive.controls.lib.longitudinal_planner import LongitudinalPl
|
||||
from openpilot.selfdrive.controls.radard import _LEAD_ACCEL_TAU
|
||||
|
||||
|
||||
class PlannerSM(dict):
|
||||
def __init__(self, radar_frame: int, services: dict):
|
||||
super().__init__(services)
|
||||
self.logMonoTime = {"radarState": radar_frame}
|
||||
self.valid = {"radarState": True}
|
||||
self.alive = {"radarState": True}
|
||||
|
||||
|
||||
class Plant:
|
||||
messaging_initialized = False
|
||||
|
||||
@@ -132,7 +140,7 @@ class Plant:
|
||||
car_control.carControl.orientationNED = [0., float(pitch), 0.]
|
||||
|
||||
# ******** get controlsState messages for plotting ***
|
||||
sm = {'radarState': radar.radarState,
|
||||
sm = PlannerSM(self.rk.frame, {'radarState': radar.radarState,
|
||||
'carState': car_state.carState,
|
||||
'carControl': car_control.carControl,
|
||||
'controlsState': control.controlsState,
|
||||
@@ -141,7 +149,7 @@ class Plant:
|
||||
'modelV2': model.modelV2,
|
||||
'carStateSP': car_state_sp.carStateSP,
|
||||
'liveMapDataSP': live_map_data_sp.liveMapDataSP,
|
||||
'gpsLocation': gps_data.gpsLocation}
|
||||
'gpsLocation': gps_data.gpsLocation})
|
||||
self.planner.update(sm)
|
||||
self.acceleration = self.planner.output_a_target
|
||||
if self.planner.output_should_stop:
|
||||
|
||||
@@ -27,6 +27,12 @@ DESCRIPTIONS = {
|
||||
"In relaxed mode sunnypilot will stay further away from lead cars. On supported cars, you can cycle through these personalities with " +
|
||||
"your steering wheel distance button."
|
||||
),
|
||||
"AccelPersonalityEnabled": tr_noop(
|
||||
"Begin slowing early and smoothly behind lead vehicles. Stock longitudinal control retains braking and stopping authority."
|
||||
),
|
||||
"AccelPersonality": tr_noop(
|
||||
"Eco slows earliest and recovers gently, Normal balances comfort and response, and Sport reacts and recovers more quickly."
|
||||
),
|
||||
"IsLdwEnabled": tr_noop(
|
||||
"Receive alerts to steer back into the lane when your vehicle drifts over a detected lane line " +
|
||||
"without a turn signal activated while driving over 31 mph (50 km/h)."
|
||||
@@ -106,6 +112,24 @@ class TogglesLayout(Widget):
|
||||
icon="speed_limit.png"
|
||||
)
|
||||
|
||||
self._accel_personality_enabled = toggle_item(
|
||||
lambda: tr("Enable Accel Controller"),
|
||||
lambda: tr(DESCRIPTIONS["AccelPersonalityEnabled"]),
|
||||
self._params.get_bool("AccelPersonalityEnabled"),
|
||||
callback=self._set_accel_personality_enabled,
|
||||
icon="speed_limit.png",
|
||||
)
|
||||
|
||||
self._accel_personality_setting = multiple_button_item(
|
||||
lambda: tr("Acceleration Profile"),
|
||||
lambda: tr(DESCRIPTIONS["AccelPersonality"]),
|
||||
buttons=[lambda: tr("Eco"), lambda: tr("Normal"), lambda: tr("Sport")],
|
||||
button_width=300,
|
||||
callback=self._set_accel_personality,
|
||||
selected_index=self._params.get("AccelPersonality", return_default=True),
|
||||
icon="speed_limit.png"
|
||||
)
|
||||
|
||||
self._toggles = {}
|
||||
self._locked_toggles = set()
|
||||
for param, (title, desc, icon, needs_restart) in self._toggle_defs.items():
|
||||
@@ -135,9 +159,11 @@ class TogglesLayout(Widget):
|
||||
|
||||
self._toggles[param] = toggle
|
||||
|
||||
# insert longitudinal personality after NDOG toggle
|
||||
# insert longitudinal personality and Accel Controller settings after NDOG toggle
|
||||
if param == "DisengageOnAccelerator":
|
||||
self._toggles["LongitudinalPersonality"] = self._long_personality_setting
|
||||
self._toggles["AccelPersonalityEnabled"] = self._accel_personality_enabled
|
||||
self._toggles["AccelPersonality"] = self._accel_personality_setting
|
||||
|
||||
self._update_experimental_mode_icon()
|
||||
self._scroller = Scroller(list(self._toggles.values()), line_separator=True, spacing=0)
|
||||
@@ -158,6 +184,7 @@ class TogglesLayout(Widget):
|
||||
|
||||
def _update_toggles(self):
|
||||
ui_state.update_params()
|
||||
accel_personality_enabled = self._params.get_bool("AccelPersonalityEnabled")
|
||||
|
||||
e2e_description = tr(
|
||||
"sunnypilot defaults to driving in chill mode. Experimental mode enables alpha-level features that aren't ready for chill mode. " +
|
||||
@@ -176,11 +203,15 @@ class TogglesLayout(Widget):
|
||||
self._toggles["ExperimentalMode"].action_item.set_enabled(True)
|
||||
self._toggles["ExperimentalMode"].set_description(e2e_description)
|
||||
self._long_personality_setting.action_item.set_enabled(True)
|
||||
self._accel_personality_enabled.action_item.set_enabled(True)
|
||||
self._accel_personality_setting.action_item.set_enabled(accel_personality_enabled)
|
||||
else:
|
||||
# no long for now
|
||||
self._toggles["ExperimentalMode"].action_item.set_enabled(False)
|
||||
self._toggles["ExperimentalMode"].action_item.set_state(False)
|
||||
self._long_personality_setting.action_item.set_enabled(False)
|
||||
self._accel_personality_enabled.action_item.set_enabled(False)
|
||||
self._accel_personality_setting.action_item.set_enabled(False)
|
||||
self._params.remove("ExperimentalMode")
|
||||
|
||||
unavailable = tr("Experimental mode is currently unavailable on this car since the car's stock ACC is used for longitudinal control.")
|
||||
@@ -203,6 +234,10 @@ class TogglesLayout(Widget):
|
||||
# refresh toggles from params to mirror external changes
|
||||
for param in self._toggle_defs:
|
||||
self._toggles[param].action_item.set_state(self._params.get_bool(param))
|
||||
self._accel_personality_enabled.action_item.set_state(accel_personality_enabled)
|
||||
self._accel_personality_setting.action_item.set_selected_button(
|
||||
self._params.get("AccelPersonality", return_default=True)
|
||||
)
|
||||
|
||||
# these toggles need restart, block while engaged
|
||||
for toggle_def in self._toggle_defs:
|
||||
@@ -247,3 +282,10 @@ class TogglesLayout(Widget):
|
||||
|
||||
def _set_longitudinal_personality(self, button_index: int):
|
||||
self._params.put("LongitudinalPersonality", button_index, block=True)
|
||||
|
||||
def _set_accel_personality(self, button_index: int):
|
||||
self._params.put("AccelPersonality", button_index, block=True)
|
||||
|
||||
def _set_accel_personality_enabled(self, state: bool):
|
||||
self._params.put_bool("AccelPersonalityEnabled", state, block=True)
|
||||
self._accel_personality_setting.action_item.set_enabled(state and ui_state.has_longitudinal_control)
|
||||
|
||||
@@ -42,6 +42,8 @@ class TogglesLayoutMici(NavScroller):
|
||||
super().__init__()
|
||||
|
||||
self._personality_toggle = BigMultiParamToggle("driving personality", "LongitudinalPersonality", ["aggressive", "standard", "relaxed"])
|
||||
self._accel_personality_enabled = BigParamControl("enable accel controller", "AccelPersonalityEnabled")
|
||||
self._accel_personality_toggle = BigMultiParamToggle("acceleration profile", "AccelPersonality", ["eco", "normal", "sport"])
|
||||
self._experimental_btn = BigToggle("experimental mode", initial_state=ui_state.params.get_bool("ExperimentalMode"),
|
||||
toggle_callback=self._on_experimental_mode)
|
||||
is_metric_toggle = BigParamControl("use metric units", "IsMetric")
|
||||
@@ -53,6 +55,8 @@ class TogglesLayoutMici(NavScroller):
|
||||
|
||||
self._scroller.add_widgets([
|
||||
self._personality_toggle,
|
||||
self._accel_personality_enabled,
|
||||
self._accel_personality_toggle,
|
||||
self._experimental_btn,
|
||||
is_metric_toggle,
|
||||
ldw_toggle,
|
||||
@@ -65,6 +69,7 @@ class TogglesLayoutMici(NavScroller):
|
||||
# Toggle lists
|
||||
self._refresh_toggles = (
|
||||
("ExperimentalMode", self._experimental_btn),
|
||||
("AccelPersonalityEnabled", self._accel_personality_enabled),
|
||||
("IsMetric", is_metric_toggle),
|
||||
("IsLdwEnabled", ldw_toggle),
|
||||
("AlwaysOnDM", always_on_dm_toggle),
|
||||
@@ -74,6 +79,9 @@ class TogglesLayoutMici(NavScroller):
|
||||
)
|
||||
|
||||
enable_openpilot.set_enabled(lambda: not ui_state.engaged)
|
||||
self._accel_personality_toggle.set_enabled(
|
||||
lambda: ui_state.has_longitudinal_control and ui_state.params.get_bool("AccelPersonalityEnabled")
|
||||
)
|
||||
record_front.set_enabled(False if ui_state.params.get_bool("RecordFrontLock") else (lambda: not ui_state.engaged))
|
||||
record_mic.set_enabled(lambda: not ui_state.engaged)
|
||||
|
||||
@@ -104,17 +112,23 @@ class TogglesLayoutMici(NavScroller):
|
||||
if ui_state.has_longitudinal_control:
|
||||
self._experimental_btn.set_visible(True)
|
||||
self._personality_toggle.set_visible(True)
|
||||
self._accel_personality_enabled.set_visible(True)
|
||||
self._accel_personality_toggle.set_visible(True)
|
||||
else:
|
||||
# no long for now
|
||||
self._experimental_btn.set_visible(False)
|
||||
self._experimental_btn.set_checked(False)
|
||||
self._personality_toggle.set_visible(False)
|
||||
self._accel_personality_enabled.set_visible(False)
|
||||
self._accel_personality_toggle.set_visible(False)
|
||||
ui_state.params.remove("ExperimentalMode")
|
||||
|
||||
# Refresh toggles from params to mirror external changes
|
||||
for key, item in self._refresh_toggles:
|
||||
item.set_checked(ui_state.params.get_bool(key))
|
||||
|
||||
self._accel_personality_toggle.refresh()
|
||||
|
||||
def _on_experimental_mode(self, state: bool):
|
||||
if state and not ui_state.params.get_bool("ExperimentalModeConfirmed"):
|
||||
# Don't show enabled state until confirm
|
||||
|
||||
@@ -385,13 +385,18 @@ class BigMultiParamToggle(BigMultiToggle):
|
||||
self._load_value()
|
||||
|
||||
def _load_value(self):
|
||||
self.set_value(self._options[self._params.get(self._param) or 0])
|
||||
value = self._params.get(self._param, return_default=True)
|
||||
index = value if isinstance(value, int) else 0
|
||||
self.set_value(self._options[max(0, min(index, len(self._options) - 1))])
|
||||
|
||||
def _handle_mouse_release(self, mouse_pos: MousePos):
|
||||
super()._handle_mouse_release(mouse_pos)
|
||||
new_idx = self._options.index(self.value)
|
||||
self._params.put(self._param, new_idx)
|
||||
|
||||
def refresh(self):
|
||||
self._load_value()
|
||||
|
||||
|
||||
class BigParamControl(BigToggle):
|
||||
def __init__(self, text: str, param: str, toggle_callback: Callable | None = None):
|
||||
|
||||
@@ -0,0 +1,143 @@
|
||||
import math
|
||||
from dataclasses import dataclass
|
||||
|
||||
from openpilot.cereal import custom
|
||||
from openpilot.common.params import Params
|
||||
from openpilot.common.realtime import DT_MDL
|
||||
from openpilot.sunnypilot import get_sanitize_int_param
|
||||
from openpilot.sunnypilot.selfdrive.controls.lib.accel_controller.constants import (
|
||||
COMFORT_DECEL, EARLY_DECEL_EPSILON, EARLY_DECEL_RELEASE_RATE, EARLY_DECEL_RESPONSE_TIME,
|
||||
EARLY_DECEL_SPEED_DEADBAND, EARLY_DECEL_TIGHTEN_RATE, PARAM_READ_INTERVAL, VEGO_NOISE_TOLERANCE,
|
||||
AccelProfile, profile_accel_max, sanitize_profile,
|
||||
)
|
||||
from openpilot.sunnypilot.selfdrive.controls.lib.accel_controller.lead import LeadPlan, calculate_lead_plan
|
||||
|
||||
|
||||
AccelControllerState = custom.LongitudinalPlanSP.AccelController.State
|
||||
|
||||
|
||||
@dataclass(frozen=True)
|
||||
class AccelDecision:
|
||||
cruise_accel_max: float | None = None
|
||||
early_decel: float | None = None
|
||||
active: bool = False
|
||||
|
||||
|
||||
class AccelController:
|
||||
def __init__(self, CP, dt: float = DT_MDL):
|
||||
if not math.isfinite(dt) or dt <= 0.0:
|
||||
raise ValueError("dt must be finite and positive")
|
||||
|
||||
self.dt = float(dt)
|
||||
self.delay = float(CP.longitudinalActuatorDelay) + DT_MDL
|
||||
self.params = Params()
|
||||
self.available = bool(CP.openpilotLongitudinalControl)
|
||||
self.enabled = False
|
||||
self.profile = AccelProfile.normal
|
||||
self._param_read_frames = max(1, int(round(PARAM_READ_INTERVAL / self.dt)))
|
||||
self._param_frame = 0
|
||||
|
||||
self._early_decel: float | None = None
|
||||
self.is_active = False
|
||||
self.cruise_accel_max: float | None = None
|
||||
self.early_decel: float | None = None
|
||||
self.state = AccelControllerState.inactive
|
||||
self.selected_lead = -1
|
||||
self.selected_lead_track_id = -1
|
||||
self.required_decel = 0.0
|
||||
|
||||
@property
|
||||
def is_enabled(self) -> bool:
|
||||
return self.available and self.enabled
|
||||
|
||||
def update_params(self) -> None:
|
||||
if self._param_frame % self._param_read_frames == 0:
|
||||
self.enabled = self.params.get_bool("AccelPersonalityEnabled")
|
||||
self.profile = get_sanitize_int_param("AccelPersonality", AccelProfile.eco, AccelProfile.sport, self.params)
|
||||
self._param_frame += 1
|
||||
|
||||
def reset(self) -> None:
|
||||
self._early_decel = None
|
||||
self.is_active = False
|
||||
self.cruise_accel_max = None
|
||||
self.early_decel = None
|
||||
self.state = AccelControllerState.inactive
|
||||
self.selected_lead = -1
|
||||
self.selected_lead_track_id = -1
|
||||
self.required_decel = 0.0
|
||||
|
||||
def _valid_context(self, *, v_ego: float, a_ego: float, v_cruise: float, stock_accel_max: float,
|
||||
engaged: bool, cruise_initialized: bool) -> bool:
|
||||
values = (v_ego, a_ego, v_cruise, stock_accel_max, self.delay)
|
||||
return (engaged and cruise_initialized and v_ego >= -VEGO_NOISE_TOLERANCE and v_cruise >= 0.0
|
||||
and self.delay >= 0.0 and all(math.isfinite(value) for value in values))
|
||||
|
||||
def _raw_early_decel(self, lead_plan: LeadPlan) -> float:
|
||||
speed_error = lead_plan.speed_ceiling - lead_plan.v_ego_projected
|
||||
if (lead_plan.selected_lead < 0 or lead_plan.closing_speed <= 0.0
|
||||
or speed_error >= -EARLY_DECEL_SPEED_DEADBAND):
|
||||
return 0.0
|
||||
comfort_decel = COMFORT_DECEL[self.profile]
|
||||
return max(speed_error / EARLY_DECEL_RESPONSE_TIME, -comfort_decel)
|
||||
|
||||
def _update_early_decel(self, raw_target: float) -> None:
|
||||
raw_target = min(float(raw_target), 0.0)
|
||||
previous = self._early_decel if self._early_decel is not None else 0.0
|
||||
|
||||
if raw_target < previous - EARLY_DECEL_EPSILON:
|
||||
updated = max(raw_target, previous - EARLY_DECEL_TIGHTEN_RATE * self.dt)
|
||||
state = AccelControllerState.restrict
|
||||
elif raw_target > previous + EARLY_DECEL_EPSILON:
|
||||
updated = min(raw_target, previous + EARLY_DECEL_RELEASE_RATE * self.dt)
|
||||
state = AccelControllerState.release
|
||||
else:
|
||||
updated = raw_target
|
||||
state = AccelControllerState.hold if updated < -EARLY_DECEL_EPSILON else AccelControllerState.free
|
||||
|
||||
if updated >= -EARLY_DECEL_EPSILON:
|
||||
self._early_decel = None
|
||||
self.early_decel = None
|
||||
self.state = AccelControllerState.free
|
||||
else:
|
||||
self._early_decel = updated
|
||||
self.early_decel = updated
|
||||
self.state = state
|
||||
|
||||
def update(self, radar_state, *, v_ego: float, a_ego: float, v_cruise: float, follow_personality,
|
||||
engaged: bool, cruise_initialized: bool, acc_selected: bool, stock_accel_max: float,
|
||||
radar_fresh: bool = True, force_decel: bool = False) -> AccelDecision:
|
||||
self.profile = sanitize_profile(self.profile)
|
||||
valid_context = self._valid_context(
|
||||
v_ego=v_ego, a_ego=a_ego, v_cruise=v_cruise, stock_accel_max=stock_accel_max,
|
||||
engaged=engaged, cruise_initialized=cruise_initialized,
|
||||
)
|
||||
if not (self.is_enabled and valid_context and bool(acc_selected) and not force_decel):
|
||||
self.reset()
|
||||
return AccelDecision()
|
||||
|
||||
sanitized_v_ego = max(float(v_ego), 0.0)
|
||||
positive_stock_max = max(float(stock_accel_max), 0.0)
|
||||
self.cruise_accel_max = profile_accel_max(self.profile, sanitized_v_ego, positive_stock_max)
|
||||
|
||||
lead_plan = LeadPlan(v_ego_projected=sanitized_v_ego)
|
||||
if radar_fresh and radar_state is not None:
|
||||
try:
|
||||
lead_plan = calculate_lead_plan(
|
||||
radar_state, sanitized_v_ego, float(a_ego), self.delay, self.profile, follow_personality,
|
||||
)
|
||||
except (AttributeError, TypeError, ValueError):
|
||||
lead_plan = LeadPlan(v_ego_projected=sanitized_v_ego)
|
||||
|
||||
self.selected_lead = lead_plan.selected_lead
|
||||
self.selected_lead_track_id = lead_plan.selected_lead_track_id
|
||||
self.required_decel = lead_plan.required_decel
|
||||
self._update_early_decel(self._raw_early_decel(lead_plan))
|
||||
|
||||
profile_binding = self.cruise_accel_max < positive_stock_max - EARLY_DECEL_EPSILON
|
||||
self.is_active = profile_binding or self.early_decel is not None
|
||||
|
||||
return AccelDecision(
|
||||
cruise_accel_max=self.cruise_accel_max,
|
||||
early_decel=self.early_decel,
|
||||
active=self.is_active,
|
||||
)
|
||||
@@ -0,0 +1,53 @@
|
||||
import math
|
||||
|
||||
import numpy as np
|
||||
|
||||
from openpilot.cereal import custom
|
||||
|
||||
|
||||
AccelProfile = custom.LongitudinalPlanSP.AccelController.Profile
|
||||
ACCEL_PROFILES = tuple(AccelProfile.schema.enumerants.values())
|
||||
|
||||
# Scale the stock cruise candidate; stock turn and throttle limits stay authoritative.
|
||||
ACCEL_SCALE_BP = [0.0, 3.0, 10.0, 25.0, 40.0]
|
||||
ACCEL_SCALE_V = {
|
||||
AccelProfile.eco: [0.78, 0.72, 0.60, 0.50, 0.40],
|
||||
AccelProfile.normal: [0.90, 0.86, 0.80, 0.72, 0.60],
|
||||
AccelProfile.sport: [1.00, 1.00, 1.00, 1.00, 1.00],
|
||||
}
|
||||
|
||||
# Smaller values start slowing earlier; stock MPC still owns lead braking.
|
||||
COMFORT_DECEL = {
|
||||
AccelProfile.eco: 0.25,
|
||||
AccelProfile.normal: 0.30,
|
||||
AccelProfile.sport: 0.35,
|
||||
}
|
||||
|
||||
EARLY_DECEL_RESPONSE_TIME = 1.0
|
||||
EARLY_DECEL_SPEED_DEADBAND = 0.15
|
||||
EARLY_DECEL_TIGHTEN_RATE = 1.0
|
||||
EARLY_DECEL_RELEASE_RATE = 0.35
|
||||
EARLY_DECEL_EPSILON = 1e-6
|
||||
|
||||
STOP_GAP_RESERVE = 0.75
|
||||
MAX_LEAD_ACCEL_TAU = 10.0
|
||||
MIN_LEAD_SPEED = -1.0
|
||||
VEGO_NOISE_TOLERANCE = 0.10
|
||||
PARAM_READ_INTERVAL = 0.25
|
||||
|
||||
|
||||
def sanitize_profile(profile: int) -> int:
|
||||
return profile if profile in ACCEL_PROFILES else AccelProfile.normal
|
||||
|
||||
|
||||
def profile_accel_scale(profile: int, v_ego: float) -> float:
|
||||
if not math.isfinite(v_ego):
|
||||
return math.nan
|
||||
return float(np.interp(max(v_ego, 0.0), ACCEL_SCALE_BP, ACCEL_SCALE_V[sanitize_profile(profile)]))
|
||||
|
||||
|
||||
def profile_accel_max(profile: int, v_ego: float, stock_accel_max: float) -> float:
|
||||
if not math.isfinite(stock_accel_max):
|
||||
return math.nan
|
||||
stock_positive_max = max(float(stock_accel_max), 0.0)
|
||||
return stock_positive_max * profile_accel_scale(profile, v_ego)
|
||||
@@ -0,0 +1,107 @@
|
||||
"""
|
||||
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 typing import NamedTuple
|
||||
|
||||
import numpy as np
|
||||
|
||||
from openpilot.cereal import log
|
||||
from openpilot.selfdrive.controls.lib.longitudinal_mpc_lib.long_mpc import (
|
||||
LongitudinalMpc, STOP_DISTANCE, T_IDXS, get_T_FOLLOW,
|
||||
)
|
||||
from openpilot.selfdrive.controls.radard import _LEAD_ACCEL_TAU
|
||||
from openpilot.sunnypilot.selfdrive.controls.lib.accel_controller.constants import (
|
||||
COMFORT_DECEL, MAX_LEAD_ACCEL_TAU, MIN_LEAD_SPEED, STOP_GAP_RESERVE, sanitize_profile,
|
||||
)
|
||||
|
||||
|
||||
class LeadPlan(NamedTuple):
|
||||
speed_ceiling: float = math.inf
|
||||
selected_lead: int = -1
|
||||
selected_lead_track_id: int = -1
|
||||
closing_speed: float = 0.0
|
||||
required_decel: float = 0.0
|
||||
v_ego_projected: float = 0.0
|
||||
|
||||
|
||||
def _project_ego(v_ego: float, a_ego: float, delay: float) -> tuple[float, float]:
|
||||
if a_ego < 0.0:
|
||||
stop_time = -v_ego / a_ego if v_ego > 0.0 else 0.0
|
||||
if stop_time <= delay:
|
||||
distance = -v_ego**2 / (2.0 * a_ego) if v_ego > 0.0 else 0.0
|
||||
return distance, 0.0
|
||||
return max(v_ego * delay + 0.5 * a_ego * delay**2, 0.0), max(v_ego + a_ego * delay, 0.0)
|
||||
|
||||
|
||||
def _lead_values(lead) -> tuple[float, float, float, float, int] | None:
|
||||
if not lead.present:
|
||||
return None
|
||||
|
||||
d_rel = float(lead.dRel)
|
||||
v_lead = float(lead.vLeadK)
|
||||
if not math.isfinite(d_rel) or d_rel < 0.0 or not math.isfinite(v_lead) or v_lead < MIN_LEAD_SPEED:
|
||||
return None
|
||||
|
||||
a_lead = float(lead.aLeadK)
|
||||
if not math.isfinite(a_lead):
|
||||
a_lead = 0.0
|
||||
a_lead_tau = float(lead.aLeadTau)
|
||||
if not math.isfinite(a_lead_tau) or not 0.0 < a_lead_tau <= MAX_LEAD_ACCEL_TAU:
|
||||
a_lead_tau = _LEAD_ACCEL_TAU
|
||||
track_id = max(int(lead.radarTrackId), -1) if math.isfinite(lead.radarTrackId) else -1
|
||||
return d_rel, max(v_lead, 0.0), float(np.clip(a_lead, -10.0, 5.0)), a_lead_tau, track_id
|
||||
|
||||
|
||||
def calculate_lead_plan(radar_state, v_ego: float, a_ego: float, delay: float, profile: int,
|
||||
follow_personality=log.LongitudinalPersonality.standard) -> LeadPlan:
|
||||
if not all(math.isfinite(value) for value in (v_ego, a_ego, delay)) or v_ego < 0.0 or delay < 0.0:
|
||||
return LeadPlan()
|
||||
|
||||
try:
|
||||
t_follow = float(get_T_FOLLOW(follow_personality))
|
||||
except (NotImplementedError, TypeError, ValueError):
|
||||
return LeadPlan()
|
||||
if not math.isfinite(t_follow) or t_follow < 0.0:
|
||||
return LeadPlan()
|
||||
|
||||
profile = sanitize_profile(profile)
|
||||
x_ego, v_ego_projected = _project_ego(v_ego, a_ego, delay)
|
||||
comfort_decel = COMFORT_DECEL[profile]
|
||||
candidates: list[LeadPlan] = []
|
||||
|
||||
for lead_index, lead in enumerate((radar_state.leadOne, radar_state.leadTwo)):
|
||||
values = _lead_values(lead)
|
||||
if values is None:
|
||||
continue
|
||||
|
||||
d_rel, v_lead, a_lead, a_lead_tau, track_id = values
|
||||
lead_xv = LongitudinalMpc.extrapolate_lead(d_rel, v_lead, a_lead, a_lead_tau)
|
||||
x_lead = float(np.interp(delay, T_IDXS, lead_xv[:, 0]))
|
||||
v_lead_projected = float(np.interp(delay, T_IDXS, lead_xv[:, 1]))
|
||||
# Match the stock MPC gap convention, including ego-speed time headway.
|
||||
safety_gap = max(x_lead - x_ego - STOP_DISTANCE - t_follow * v_ego_projected, 0.0)
|
||||
closing_speed = max(v_ego_projected - v_lead_projected, 0.0)
|
||||
required_decel = 0.0 if closing_speed == 0.0 else math.inf if safety_gap == 0.0 else closing_speed**2 / (2.0 * safety_gap)
|
||||
usable_gap = max(safety_gap - STOP_GAP_RESERVE, 0.0)
|
||||
speed_ceiling = v_lead_projected + math.sqrt(2.0 * comfort_decel * usable_gap)
|
||||
|
||||
finite_values = (x_lead, v_lead_projected, safety_gap, usable_gap, closing_speed, speed_ceiling)
|
||||
if (not all(math.isfinite(value) and value >= 0.0 for value in finite_values) or math.isnan(required_decel)
|
||||
or required_decel < 0.0):
|
||||
continue
|
||||
|
||||
candidates.append(LeadPlan(
|
||||
speed_ceiling=speed_ceiling,
|
||||
selected_lead=lead_index,
|
||||
selected_lead_track_id=track_id,
|
||||
closing_speed=closing_speed,
|
||||
required_decel=required_decel,
|
||||
v_ego_projected=v_ego_projected,
|
||||
))
|
||||
|
||||
return min(candidates, key=lambda candidate: candidate.speed_ceiling) if candidates else LeadPlan(v_ego_projected=v_ego_projected)
|
||||
+191
@@ -0,0 +1,191 @@
|
||||
import math
|
||||
from dataclasses import FrozenInstanceError
|
||||
from types import SimpleNamespace
|
||||
|
||||
from openpilot.cereal import log
|
||||
from openpilot.common.realtime import DT_MDL
|
||||
from openpilot.common.test import OpenpilotTestCase
|
||||
from openpilot.sunnypilot.selfdrive.controls.lib.accel_controller.accel_controller import (
|
||||
AccelController, AccelControllerState, AccelDecision,
|
||||
)
|
||||
from openpilot.sunnypilot.selfdrive.controls.lib.accel_controller.constants import (
|
||||
ACCEL_PROFILES, ACCEL_SCALE_BP, EARLY_DECEL_RELEASE_RATE, EARLY_DECEL_TIGHTEN_RATE,
|
||||
AccelProfile, profile_accel_max, profile_accel_scale, sanitize_profile,
|
||||
)
|
||||
from openpilot.sunnypilot.selfdrive.controls.lib.accel_controller.lead import calculate_lead_plan
|
||||
|
||||
|
||||
def lead(*, present=False, distance=0.0, speed=0.0, accel=0.0, tau=1.5, track_id=-1):
|
||||
return SimpleNamespace(
|
||||
present=present, dRel=distance, vLeadK=speed, aLeadK=accel,
|
||||
aLeadTau=tau, radarTrackId=track_id,
|
||||
)
|
||||
|
||||
|
||||
def radar(lead_one=None, lead_two=None):
|
||||
return SimpleNamespace(leadOne=lead_one or lead(), leadTwo=lead_two or lead())
|
||||
|
||||
|
||||
def controller(*, enabled=True, profile=AccelProfile.normal, dt=DT_MDL):
|
||||
instance = AccelController(SimpleNamespace(longitudinalActuatorDelay=0.10, openpilotLongitudinalControl=True), dt=dt)
|
||||
instance.enabled = enabled
|
||||
instance.profile = profile
|
||||
return instance
|
||||
|
||||
|
||||
def update(instance, radar_state=None, **overrides):
|
||||
arguments = {
|
||||
"v_ego": 10.0,
|
||||
"a_ego": 0.0,
|
||||
"v_cruise": 25.0,
|
||||
"follow_personality": log.LongitudinalPersonality.standard,
|
||||
"engaged": True,
|
||||
"cruise_initialized": True,
|
||||
"acc_selected": True,
|
||||
"stock_accel_max": 1.0,
|
||||
"radar_fresh": True,
|
||||
"force_decel": False,
|
||||
}
|
||||
arguments.update(overrides)
|
||||
return instance.update(radar() if radar_state is None else radar_state, **arguments)
|
||||
|
||||
|
||||
def restrictive_radar():
|
||||
return radar(lead(present=True, distance=25.0, speed=5.0, accel=-0.2, track_id=101))
|
||||
|
||||
|
||||
class TestProfiles(OpenpilotTestCase):
|
||||
def test_profile_order_and_stock_scaling(self):
|
||||
for speed in (*ACCEL_SCALE_BP, 17.0, 50.0):
|
||||
with self.subTest(speed=speed):
|
||||
scales = [profile_accel_scale(profile, speed) for profile in ACCEL_PROFILES]
|
||||
self.assertLess(scales[0], scales[1])
|
||||
self.assertLess(scales[1], scales[2])
|
||||
self.assertEqual(scales[2], 1.0)
|
||||
for profile, scale in zip(ACCEL_PROFILES, scales, strict=True):
|
||||
self.assertAlmostEqual(profile_accel_max(profile, speed, 0.73), 0.73 * scale)
|
||||
|
||||
def test_profiles_never_expand_stock_candidate(self):
|
||||
for profile in ACCEL_PROFILES:
|
||||
for stock_limit in (-0.5, 0.0, 0.4, 1.6):
|
||||
with self.subTest(profile=profile, stock_limit=stock_limit):
|
||||
limit = profile_accel_max(profile, 12.0, stock_limit)
|
||||
self.assertGreaterEqual(limit, 0.0)
|
||||
self.assertLessEqual(limit, max(stock_limit, 0.0))
|
||||
|
||||
def test_invalid_profile_is_normal_and_nonfinite_propagates(self):
|
||||
self.assertEqual(sanitize_profile(999), AccelProfile.normal)
|
||||
self.assertTrue(math.isnan(profile_accel_scale(AccelProfile.normal, math.nan)))
|
||||
self.assertTrue(math.isnan(profile_accel_max(AccelProfile.normal, 10.0, math.inf)))
|
||||
|
||||
|
||||
class TestAccelDecision(OpenpilotTestCase):
|
||||
def test_decision_is_a_small_immutable_contract(self):
|
||||
decision = AccelDecision(cruise_accel_max=0.4, early_decel=-0.2, active=True)
|
||||
self.assertEqual((decision.cruise_accel_max, decision.early_decel, decision.active), (0.4, -0.2, True))
|
||||
field = "active"
|
||||
with self.assertRaises(FrozenInstanceError):
|
||||
setattr(decision, field, False)
|
||||
|
||||
def test_context_gates_reset_without_actuating(self):
|
||||
cases = (
|
||||
{"enabled": False},
|
||||
{"engaged": False},
|
||||
{"cruise_initialized": False},
|
||||
{"acc_selected": False},
|
||||
{"force_decel": True},
|
||||
{"v_ego": math.nan},
|
||||
{"v_cruise": -1.0},
|
||||
{"stock_accel_max": math.inf},
|
||||
)
|
||||
for case in cases:
|
||||
with self.subTest(case=case):
|
||||
arguments = dict(case)
|
||||
enabled = arguments.pop("enabled", True)
|
||||
instance = controller(enabled=enabled)
|
||||
decision = update(instance, restrictive_radar(), **arguments)
|
||||
self.assertEqual(decision, AccelDecision())
|
||||
self.assertFalse(instance.is_active)
|
||||
self.assertEqual(instance.state, AccelControllerState.inactive)
|
||||
|
||||
def test_profile_limit_is_only_active_when_binding(self):
|
||||
for profile in ACCEL_PROFILES:
|
||||
with self.subTest(profile=profile):
|
||||
instance = controller(profile=profile)
|
||||
decision = update(instance, v_ego=10.0, stock_accel_max=1.0)
|
||||
expected = profile_accel_max(profile, 10.0, 1.0)
|
||||
self.assertAlmostEqual(decision.cruise_accel_max, expected)
|
||||
self.assertEqual(decision.active, expected < 1.0)
|
||||
|
||||
|
||||
class TestEarlyDecel(OpenpilotTestCase):
|
||||
def test_early_decel_is_nonpositive_and_tightens_at_bound(self):
|
||||
instance = controller()
|
||||
samples = [0.0]
|
||||
for _ in range(10):
|
||||
decision = update(instance, restrictive_radar())
|
||||
samples.append(decision.early_decel or 0.0)
|
||||
|
||||
self.assertTrue(all(value <= 0.0 for value in samples))
|
||||
self.assertTrue(any(value < 0.0 for value in samples))
|
||||
for before, after in zip(samples[:-1], samples[1:], strict=True):
|
||||
self.assertGreaterEqual(after - before, -EARLY_DECEL_TIGHTEN_RATE * DT_MDL - 1e-9)
|
||||
|
||||
def test_dropout_and_stale_radar_release_at_bound(self):
|
||||
releases = ((radar(), True), (restrictive_radar(), False))
|
||||
for radar_state, radar_fresh in releases:
|
||||
with self.subTest(radar_fresh=radar_fresh):
|
||||
instance = controller()
|
||||
for _ in range(10):
|
||||
update(instance, restrictive_radar())
|
||||
previous = instance.early_decel
|
||||
self.assertIsNotNone(previous)
|
||||
|
||||
for _ in range(30):
|
||||
decision = update(instance, radar_state, radar_fresh=radar_fresh)
|
||||
current = decision.early_decel or 0.0
|
||||
self.assertLessEqual(current, 0.0)
|
||||
self.assertGreaterEqual(current, previous - 1e-9)
|
||||
self.assertLessEqual(current - previous, EARLY_DECEL_RELEASE_RATE * DT_MDL + 1e-9)
|
||||
previous = current
|
||||
if decision.early_decel is None:
|
||||
break
|
||||
self.assertIsNone(instance.early_decel)
|
||||
|
||||
def test_early_decel_is_bounded_by_profile_comfort(self):
|
||||
for profile in ACCEL_PROFILES:
|
||||
with self.subTest(profile=profile):
|
||||
instance = controller(profile=profile)
|
||||
for _ in range(30):
|
||||
decision = update(instance, restrictive_radar())
|
||||
self.assertIsNotNone(decision.early_decel)
|
||||
self.assertLessEqual(decision.early_decel, 0.0)
|
||||
self.assertGreaterEqual(decision.early_decel, -0.35)
|
||||
|
||||
|
||||
class TestLeadPlan(OpenpilotTestCase):
|
||||
def test_more_restrictive_of_two_leads_is_selected(self):
|
||||
radar_state = radar(
|
||||
lead(present=True, distance=80.0, speed=12.0, track_id=10),
|
||||
lead(present=True, distance=25.0, speed=6.0, track_id=20),
|
||||
)
|
||||
plan = calculate_lead_plan(radar_state, 10.0, 0.0, 0.15, AccelProfile.normal)
|
||||
self.assertEqual((plan.selected_lead, plan.selected_lead_track_id), (1, 20))
|
||||
self.assertGreater(plan.closing_speed, 0.0)
|
||||
self.assertGreater(plan.required_decel, 0.0)
|
||||
|
||||
def test_malformed_lead_is_ignored_without_hiding_valid_second_lead(self):
|
||||
malformed = lead(present=True, distance=math.nan, speed=8.0)
|
||||
valid = lead(present=True, distance=30.0, speed=7.0, accel=math.nan, tau=math.inf, track_id=math.nan)
|
||||
plan = calculate_lead_plan(radar(malformed, valid), 10.0, 0.0, 0.15, AccelProfile.normal)
|
||||
self.assertEqual(plan.selected_lead, 1)
|
||||
self.assertEqual(plan.selected_lead_track_id, -1)
|
||||
self.assertTrue(math.isfinite(plan.speed_ceiling))
|
||||
|
||||
def test_malformed_radar_and_scalar_inputs_fail_closed_to_no_extension(self):
|
||||
for radar_state in (None, SimpleNamespace(), radar(lead(present=True, distance=-1.0, speed=5.0))):
|
||||
with self.subTest(radar_state=radar_state):
|
||||
instance = controller(profile=AccelProfile.sport)
|
||||
decision = update(instance, radar_state)
|
||||
self.assertIsNone(decision.early_decel)
|
||||
self.assertFalse(decision.active)
|
||||
+212
@@ -0,0 +1,212 @@
|
||||
from types import SimpleNamespace
|
||||
from typing import Any
|
||||
|
||||
from openpilot.cereal import custom, log
|
||||
from openpilot.common.realtime import DT_MDL
|
||||
from openpilot.common.test import OpenpilotTestCase
|
||||
from openpilot.selfdrive.controls.lib.longcontrol import LongCtrlState
|
||||
from openpilot.selfdrive.controls.lib.longitudinal_mpc_lib.long_mpc import LongitudinalPlanSource as MpcSource
|
||||
from openpilot.selfdrive.controls.lib.longitudinal_planner import get_coast_accel, get_cruise_accel, get_max_accel
|
||||
from openpilot.sunnypilot.selfdrive.controls.lib.accel_controller.accel_controller import AccelController, AccelControllerState
|
||||
from openpilot.sunnypilot.selfdrive.controls.lib.accel_controller.constants import (
|
||||
ACCEL_PROFILES, AccelProfile, profile_accel_scale,
|
||||
)
|
||||
from openpilot.sunnypilot.selfdrive.controls.lib.longitudinal_planner import LongitudinalPlannerSP, LongitudinalPlanSource
|
||||
|
||||
|
||||
def lead(*, present=False, distance=0.0, speed=0.0):
|
||||
return SimpleNamespace(
|
||||
present=present, dRel=distance, vLeadK=speed, aLeadK=0.0,
|
||||
aLeadTau=1.5, radarTrackId=-1,
|
||||
)
|
||||
|
||||
|
||||
def radar(lead_one=None, lead_two=None):
|
||||
return SimpleNamespace(leadOne=lead_one or lead(), leadTwo=lead_two or lead())
|
||||
|
||||
|
||||
class PlannerSM(dict):
|
||||
def __init__(self, *, experimental=False, force_decel=False, radar_state=None, radar_time=100):
|
||||
super().__init__(
|
||||
radarState=radar_state or radar(),
|
||||
carState=SimpleNamespace(vEgo=10.0, aEgo=0.0, vCruise=72.0),
|
||||
selfdriveState=SimpleNamespace(
|
||||
enabled=True, experimentalMode=experimental, personality=log.LongitudinalPersonality.standard,
|
||||
),
|
||||
controlsState=SimpleNamespace(forceDecel=force_decel, longControlState=LongCtrlState.pid),
|
||||
)
|
||||
self.valid = {"radarState": True}
|
||||
self.alive = {"radarState": True}
|
||||
self.logMonoTime = {"radarState": radar_time}
|
||||
|
||||
def all_checks(self, service_list=None):
|
||||
return True
|
||||
|
||||
|
||||
def accel_controller(*, enabled=True, profile=AccelProfile.normal):
|
||||
cp = SimpleNamespace(longitudinalActuatorDelay=0.1, openpilotLongitudinalControl=True)
|
||||
instance = AccelController(cp)
|
||||
instance.enabled = enabled
|
||||
instance.profile = profile
|
||||
return instance
|
||||
|
||||
|
||||
def planner_for_hook(*, enabled=True, profile=AccelProfile.normal):
|
||||
planner = LongitudinalPlannerSP.__new__(LongitudinalPlannerSP)
|
||||
dynamic_planner: Any = planner
|
||||
dynamic_planner.accel_controller = accel_controller(enabled=enabled, profile=profile)
|
||||
dynamic_planner.dec = SimpleNamespace(active=lambda: False)
|
||||
dynamic_planner.output_v_target = 30.0
|
||||
dynamic_planner._radar_fresh_this_cycle = True
|
||||
return planner
|
||||
|
||||
|
||||
class TestPlannerHook(OpenpilotTestCase):
|
||||
def test_disabled_e2e_and_force_decel_preserve_exact_candidate_tuple(self):
|
||||
candidates = (
|
||||
(-0.4, MpcSource.lead0, True),
|
||||
(0.6, MpcSource.cruise, False),
|
||||
(0.2, MpcSource.e2e, False),
|
||||
)
|
||||
cases = (
|
||||
(planner_for_hook(enabled=False), PlannerSM()),
|
||||
(planner_for_hook(profile=AccelProfile.eco), PlannerSM(experimental=True)),
|
||||
(planner_for_hook(profile=AccelProfile.eco), PlannerSM(force_decel=True)),
|
||||
)
|
||||
for planner, sm in cases:
|
||||
with self.subTest(experimental=sm["selfdriveState"].experimentalMode,
|
||||
force_decel=sm["controlsState"].forceDecel,
|
||||
enabled=planner.accel_controller.enabled):
|
||||
result = planner.update_accel_controller(sm, candidates)
|
||||
self.assertIs(result, candidates)
|
||||
self.assertEqual(result, candidates)
|
||||
|
||||
def test_missing_cruise_candidate_is_exact_noop(self):
|
||||
planner = planner_for_hook(profile=AccelProfile.eco)
|
||||
candidates = ((-0.4, MpcSource.lead0, True), (0.2, MpcSource.e2e, False))
|
||||
self.assertIs(planner.update_accel_controller(PlannerSM(), candidates), candidates)
|
||||
|
||||
def test_profiles_scale_final_stock_turn_and_coast_candidates(self):
|
||||
cp = SimpleNamespace(steerRatio=15.0, wheelbase=2.7)
|
||||
scenarios = (
|
||||
(25.0, 9.0, 0.35, 2.0, True),
|
||||
(4.0, 0.0, 0.40, get_coast_accel(0.0), False),
|
||||
)
|
||||
for v_ego, steering_angle, previous, accel_coast, allow_throttle in scenarios:
|
||||
stock = get_cruise_accel(
|
||||
False, 30.0, v_ego, previous, steering_angle, cp, DT_MDL, accel_coast, allow_throttle,
|
||||
)
|
||||
self.assertGreater(stock, 0.0)
|
||||
self.assertLess(stock, get_max_accel(v_ego))
|
||||
for profile in ACCEL_PROFILES:
|
||||
with self.subTest(v_ego=v_ego, profile=profile):
|
||||
planner = planner_for_hook(profile=profile)
|
||||
sm = PlannerSM()
|
||||
sm["carState"].vEgo = v_ego
|
||||
candidates = [(-0.5, MpcSource.lead0, True), (stock, MpcSource.cruise, False)]
|
||||
result = planner.update_accel_controller(sm, candidates)
|
||||
self.assertEqual(result[0], candidates[0])
|
||||
self.assertEqual(result[1][1:], candidates[1][1:])
|
||||
self.assertAlmostEqual(result[1][0], stock * profile_accel_scale(profile, v_ego))
|
||||
|
||||
def test_early_decel_is_an_additive_nonpositive_cruise_candidate(self):
|
||||
planner = planner_for_hook(profile=AccelProfile.sport)
|
||||
sm = PlannerSM(radar_state=radar(lead(present=True, distance=25.0, speed=5.0)))
|
||||
candidates = [(-0.02, MpcSource.lead0, False), (0.6, MpcSource.cruise, False)]
|
||||
result = planner.update_accel_controller(sm, candidates)
|
||||
self.assertEqual(result[:2], candidates)
|
||||
self.assertEqual(len(result), 3)
|
||||
self.assertEqual(result[-1][1:], (MpcSource.cruise, False))
|
||||
self.assertLessEqual(result[-1][0], 0.0)
|
||||
|
||||
def test_radar_freshness_requires_a_healthy_advanced_message(self):
|
||||
planner = planner_for_hook()
|
||||
planner._radar_log_mono_time = None
|
||||
sm = PlannerSM(radar_time=100)
|
||||
self.assertTrue(planner._update_radar_freshness(sm))
|
||||
self.assertFalse(planner._update_radar_freshness(sm))
|
||||
sm.logMonoTime["radarState"] = 101
|
||||
self.assertTrue(planner._update_radar_freshness(sm))
|
||||
sm.valid["radarState"] = False
|
||||
sm.logMonoTime["radarState"] = 102
|
||||
self.assertFalse(planner._update_radar_freshness(sm))
|
||||
|
||||
|
||||
class TestParamsSchemaAndTelemetry(OpenpilotTestCase):
|
||||
def test_params_enable_and_sanitize_profile(self):
|
||||
instance = accel_controller(enabled=False)
|
||||
instance.params.put_bool("AccelPersonalityEnabled", True, block=True)
|
||||
instance.params.put("AccelPersonality", 99, block=True)
|
||||
instance.update_params()
|
||||
self.assertTrue(instance.is_enabled)
|
||||
self.assertEqual(instance.profile, AccelProfile.sport)
|
||||
self.assertEqual(instance.params.get("AccelPersonality"), AccelProfile.sport)
|
||||
|
||||
instance.params.put_bool("AccelPersonalityEnabled", False, block=True)
|
||||
instance._param_frame = 0
|
||||
instance.update_params()
|
||||
self.assertFalse(instance.is_enabled)
|
||||
|
||||
def test_schema_contract_and_round_trip(self):
|
||||
fields = custom.LongitudinalPlanSP.AccelController.schema.fields
|
||||
self.assertEqual(
|
||||
{name: field.proto.ordinal.explicit for name, field in fields.items()},
|
||||
{"enabled": 0, "active": 1, "shadowOnlyDEPRECATED": 2, "profile": 3, "state": 4},
|
||||
)
|
||||
self.assertEqual(
|
||||
custom.LongitudinalPlanSP.AccelController.Profile.schema.enumerants,
|
||||
{"eco": 0, "normal": 1, "sport": 2},
|
||||
)
|
||||
|
||||
message = custom.LongitudinalPlanSP.new_message()
|
||||
message.accelController.enabled = True
|
||||
message.accelController.active = True
|
||||
message.accelController.profile = AccelProfile.sport
|
||||
message.accelController.state = AccelControllerState.release
|
||||
with custom.LongitudinalPlanSP.from_bytes(message.to_bytes()) as reader:
|
||||
self.assertTrue(reader.accelController.enabled)
|
||||
self.assertTrue(reader.accelController.active)
|
||||
self.assertEqual(reader.accelController.profile, AccelProfile.sport)
|
||||
self.assertEqual(reader.accelController.state, AccelControllerState.release)
|
||||
|
||||
def test_minimal_controller_telemetry_is_published(self):
|
||||
planner = LongitudinalPlannerSP.__new__(LongitudinalPlannerSP)
|
||||
dynamic_planner: Any = planner
|
||||
dynamic_planner.source = LongitudinalPlanSource.cruise
|
||||
dynamic_planner.output_v_target = 20.0
|
||||
dynamic_planner.output_a_target = -0.1
|
||||
dynamic_planner.events_sp = SimpleNamespace(to_msg=list)
|
||||
dynamic_planner.dec = SimpleNamespace(mode=lambda: "acc", enabled=lambda: False, active=lambda: False)
|
||||
dynamic_planner.accel_controller = accel_controller(profile=AccelProfile.eco)
|
||||
dynamic_planner.accel_controller.is_active = True
|
||||
dynamic_planner.accel_controller.state = AccelControllerState.restrict
|
||||
dynamic_planner.scc = SimpleNamespace(
|
||||
vision=SimpleNamespace(
|
||||
state=0, output_v_target=20.0, output_a_target=0.0, current_lat_acc=0.0,
|
||||
max_pred_lat_acc=0.0, is_enabled=False, is_active=False,
|
||||
),
|
||||
map=SimpleNamespace(state=0, output_v_target=20.0, output_a_target=0.0, is_enabled=False, is_active=False),
|
||||
)
|
||||
dynamic_planner.resolver = SimpleNamespace(
|
||||
speed_limit=0.0, speed_limit_last=0.0, speed_limit_final=0.0, speed_limit_final_last=0.0,
|
||||
speed_limit_valid=False, speed_limit_last_valid=False, speed_limit_offset=0.0, distance=0.0,
|
||||
source=custom.LongitudinalPlanSP.SpeedLimit.Source.none,
|
||||
)
|
||||
dynamic_planner.sla = SimpleNamespace(
|
||||
state=custom.LongitudinalPlanSP.SpeedLimit.AssistState.disabled, is_enabled=False,
|
||||
is_active=False, output_v_target=20.0, output_a_target=0.0,
|
||||
)
|
||||
dynamic_planner.e2e_alerts_helper = SimpleNamespace(green_light_alert=False, lead_depart_alert=False)
|
||||
sent = {}
|
||||
dynamic_planner.publish_longitudinal_plan_sp(
|
||||
PlannerSM(), SimpleNamespace(send=lambda service, message: sent.update({service: message})),
|
||||
)
|
||||
|
||||
telemetry = sent["longitudinalPlanSP"].longitudinalPlanSP.accelController
|
||||
self.assertTrue(telemetry.enabled)
|
||||
self.assertTrue(telemetry.active)
|
||||
self.assertEqual(telemetry.profile, AccelProfile.eco)
|
||||
self.assertEqual(telemetry.state, AccelControllerState.restrict)
|
||||
self.assertEqual(set(custom.LongitudinalPlanSP.AccelController.schema.fields), {
|
||||
"enabled", "active", "shadowOnlyDEPRECATED", "profile", "state",
|
||||
})
|
||||
@@ -38,6 +38,7 @@ class MockSubMaster(dict):
|
||||
def __init__(self, services: dict):
|
||||
super().__init__(services)
|
||||
self.valid = dict.fromkeys(services, True)
|
||||
self.alive = dict.fromkeys(services, True)
|
||||
self.logMonoTime = dict.fromkeys(services, 0)
|
||||
self.updated = dict.fromkeys(services, True)
|
||||
self.recv_frame = dict.fromkeys(services, 1)
|
||||
|
||||
@@ -8,7 +8,10 @@ See the LICENSE.md file in the root directory for more details.
|
||||
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.selfdrive.car.cruise import V_CRUISE_MAX, V_CRUISE_UNSET
|
||||
from openpilot.selfdrive.controls.lib.longcontrol import LongCtrlState
|
||||
from openpilot.selfdrive.controls.lib.longitudinal_mpc_lib.long_mpc import LongitudinalPlanSource as MpcSource
|
||||
from openpilot.sunnypilot.selfdrive.controls.lib.accel_controller.accel_controller import AccelController
|
||||
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.smart_cruise_control.smart_cruise_control import SmartCruiseControl
|
||||
@@ -23,8 +26,8 @@ LongitudinalPlanSource = custom.LongitudinalPlanSP.LongitudinalPlanSource
|
||||
|
||||
class LongitudinalPlannerSP:
|
||||
def __init__(self, CP: structs.CarParams, CP_SP: structs.CarParamsSP, mpc):
|
||||
self.accel_controller = AccelController(CP, dt=mpc.dt)
|
||||
self.events_sp = EventsSP()
|
||||
self.resolver = SpeedLimitResolver()
|
||||
self.dec = DynamicExperimentalController(CP, mpc)
|
||||
self.scc = SmartCruiseControl()
|
||||
self.resolver = SpeedLimitResolver()
|
||||
@@ -32,6 +35,8 @@ class LongitudinalPlannerSP:
|
||||
self.generation = int(model_bundle.generation) if (model_bundle := get_active_bundle()) else None
|
||||
self.source = LongitudinalPlanSource.cruise
|
||||
self.e2e_alerts_helper = E2EAlertsHelper()
|
||||
self._radar_log_mono_time = None
|
||||
self._radar_fresh_this_cycle = True
|
||||
|
||||
self.output_v_target = 0.
|
||||
self.output_a_target = 0.
|
||||
@@ -43,6 +48,42 @@ class LongitudinalPlannerSP:
|
||||
|
||||
return experimental_mode and self.dec.mode() == "blended"
|
||||
|
||||
def update_accel_controller(self, sm: messaging.SubMaster, candidates):
|
||||
cruise_index = next((i for i, candidate in enumerate(candidates) if candidate[1] == MpcSource.cruise), -1)
|
||||
if cruise_index < 0:
|
||||
return candidates
|
||||
|
||||
CS = sm['carState']
|
||||
long_control_off = sm['controlsState'].longControlState == LongCtrlState.off
|
||||
reset_state = ((long_control_off if self.accel_controller.available else not sm['selfdriveState'].enabled)
|
||||
or CS.vCruise == V_CRUISE_UNSET)
|
||||
cruise_accel = candidates[cruise_index][0]
|
||||
decision = self.accel_controller.update(
|
||||
sm['radarState'], v_ego=CS.vEgo, a_ego=CS.aEgo, v_cruise=self.output_v_target,
|
||||
follow_personality=sm['selfdriveState'].personality, engaged=not reset_state,
|
||||
cruise_initialized=CS.vCruise != V_CRUISE_UNSET, acc_selected=not self.is_e2e(sm),
|
||||
stock_accel_max=max(cruise_accel, 0.0), radar_fresh=self._radar_fresh_this_cycle,
|
||||
force_decel=sm['controlsState'].forceDecel,
|
||||
)
|
||||
if decision.cruise_accel_max is None and decision.early_decel is None:
|
||||
return candidates
|
||||
|
||||
candidates = list(candidates)
|
||||
if decision.cruise_accel_max is not None:
|
||||
_, source, stop = candidates[cruise_index]
|
||||
candidates[cruise_index] = (min(cruise_accel, decision.cruise_accel_max), source, stop)
|
||||
if decision.early_decel is not None:
|
||||
candidates.append((decision.early_decel, MpcSource.cruise, False))
|
||||
return candidates
|
||||
|
||||
def _update_radar_freshness(self, sm: messaging.SubMaster) -> bool:
|
||||
radar_log_mono_time = sm.logMonoTime['radarState']
|
||||
radar_healthy = sm.valid['radarState'] and sm.alive['radarState']
|
||||
radar_advanced = self._radar_log_mono_time is None or radar_log_mono_time > self._radar_log_mono_time
|
||||
if radar_advanced:
|
||||
self._radar_log_mono_time = radar_log_mono_time
|
||||
return radar_healthy and radar_advanced
|
||||
|
||||
def update_targets(self, sm: messaging.SubMaster, v_ego: float, a_ego: float, v_cruise: float) -> tuple[float, float]:
|
||||
CS = sm['carState']
|
||||
v_cruise_cluster_kph = min(CS.vCruiseCluster, V_CRUISE_MAX)
|
||||
@@ -74,6 +115,8 @@ class LongitudinalPlannerSP:
|
||||
return self.output_v_target, self.output_a_target
|
||||
|
||||
def update(self, sm: messaging.SubMaster) -> None:
|
||||
self._radar_fresh_this_cycle = self._update_radar_freshness(sm)
|
||||
self.accel_controller.update_params()
|
||||
self.events_sp.clear()
|
||||
self.dec.update(sm)
|
||||
self.e2e_alerts_helper.update(sm, self.events_sp)
|
||||
@@ -95,6 +138,12 @@ class LongitudinalPlannerSP:
|
||||
dec.enabled = self.dec.enabled()
|
||||
dec.active = self.dec.active()
|
||||
|
||||
accel_controller = longitudinalPlanSP.accelController
|
||||
accel_controller.enabled = self.accel_controller.is_enabled
|
||||
accel_controller.active = self.accel_controller.is_active
|
||||
accel_controller.profile = self.accel_controller.profile
|
||||
accel_controller.state = self.accel_controller.state
|
||||
|
||||
# Smart Cruise Control
|
||||
smartCruiseControl = longitudinalPlanSP.smartCruiseControl
|
||||
# Vision Control
|
||||
|
||||
+94
@@ -0,0 +1,94 @@
|
||||
import inspect
|
||||
from typing import Any
|
||||
|
||||
import numpy as np
|
||||
|
||||
from opendbc.car.interfaces import ACCEL_MAX, ACCEL_MIN
|
||||
from openpilot.common.test import OpenpilotTestCase
|
||||
from openpilot.selfdrive.controls.lib.longitudinal_mpc_lib.long_mpc import N, LongitudinalMpc, LongitudinalPlanSource
|
||||
from openpilot.sunnypilot.selfdrive.controls.lib.accel_controller.constants import AccelProfile
|
||||
from openpilot.sunnypilot.selfdrive.test.longitudinal_maneuvers.plant import PlantSP as Plant
|
||||
|
||||
|
||||
def configure(plant, *, enabled=True, profile=AccelProfile.normal):
|
||||
controller: Any = plant.planner.accel_controller
|
||||
controller.enabled = enabled
|
||||
controller.profile = profile
|
||||
controller.update_params = lambda: None
|
||||
dec: Any = plant.planner.dec
|
||||
dec._enabled = False
|
||||
dec._read_params = lambda: None
|
||||
|
||||
|
||||
def record_candidates(plant):
|
||||
snapshots = []
|
||||
planner: Any = plant.planner
|
||||
original = planner.update_accel_controller
|
||||
|
||||
def wrapper(sm, candidates):
|
||||
before = tuple(candidates)
|
||||
after = original(sm, candidates)
|
||||
snapshots.append((before, tuple(after)))
|
||||
return after
|
||||
|
||||
planner.update_accel_controller = wrapper
|
||||
return snapshots
|
||||
|
||||
|
||||
class TestAccelControllerPlannerIntegration(OpenpilotTestCase):
|
||||
def test_one_stock_mpc_solve_with_unmodified_bounds_and_api(self):
|
||||
plant = Plant(enabled=True, lead_relevancy=True, speed=20.0, distance_lead=70.0)
|
||||
configure(plant, profile=AccelProfile.eco)
|
||||
mpc: Any = plant.planner.mpc
|
||||
original_run = mpc.run
|
||||
run_calls = []
|
||||
params_at_solve = []
|
||||
|
||||
def count_run():
|
||||
run_calls.append(None)
|
||||
params_at_solve.append(mpc.params.copy())
|
||||
return original_run()
|
||||
|
||||
mpc.run = count_run
|
||||
result = plant.step(v_lead=14.0, v_cruise=25.0)
|
||||
|
||||
self.assertEqual(len(run_calls), 1)
|
||||
self.assertTrue(np.isfinite(result["a_target"]))
|
||||
self.assertEqual(LongitudinalMpc.__bases__, (object,))
|
||||
self.assertEqual(tuple(inspect.signature(LongitudinalMpc.update).parameters), ("self", "radarstate", "personality"))
|
||||
self.assertFalse(hasattr(mpc, "set_jerk_cost_multiplier"))
|
||||
self.assertFalse(hasattr(mpc, "cruise_accel_max"))
|
||||
self.assertEqual(params_at_solve[0].shape, (N + 1, 6))
|
||||
np.testing.assert_array_equal(params_at_solve[0][:, 0], ACCEL_MIN)
|
||||
np.testing.assert_array_equal(params_at_solve[0][:, 1], ACCEL_MAX)
|
||||
|
||||
def test_stock_lead_mpc_braking_remains_authoritative(self):
|
||||
plant = Plant(enabled=True, lead_relevancy=True, speed=20.0, distance_lead=30.0)
|
||||
configure(plant, profile=AccelProfile.eco)
|
||||
snapshots = record_candidates(plant)
|
||||
|
||||
stock_mpc_won = False
|
||||
for _ in range(20):
|
||||
result = plant.step(v_lead=5.0, v_cruise=30.0)
|
||||
stock, augmented = snapshots[-1]
|
||||
mpc_candidate = stock[0]
|
||||
selected = min(augmented, key=lambda candidate: candidate[0])
|
||||
self.assertAlmostEqual(result["a_target"], selected[0])
|
||||
if mpc_candidate[0] < 0.0 and selected == mpc_candidate:
|
||||
stock_mpc_won = True
|
||||
assert mpc_candidate[1] in (LongitudinalPlanSource.lead0, LongitudinalPlanSource.lead1)
|
||||
break
|
||||
|
||||
self.assertTrue(stock_mpc_won, "the hook must never mask stock lead braking")
|
||||
|
||||
def test_stock_should_stop_survives_controller_candidates(self):
|
||||
plant = Plant(enabled=True, lead_relevancy=True, speed=0.2, distance_lead=3.0)
|
||||
configure(plant, profile=AccelProfile.normal)
|
||||
snapshots = record_candidates(plant)
|
||||
|
||||
result = plant.step(v_lead=0.0, v_cruise=8.0)
|
||||
stock, augmented = snapshots[-1]
|
||||
self.assertTrue(any(candidate[2] for candidate in stock))
|
||||
self.assertTrue(any(candidate[2] for candidate in augmented))
|
||||
self.assertTrue(result["should_stop"])
|
||||
self.assertAlmostEqual(result["a_target"], min(augmented, key=lambda candidate: candidate[0])[0])
|
||||
@@ -0,0 +1,397 @@
|
||||
"""
|
||||
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
|
||||
from collections.abc import Callable
|
||||
from dataclasses import dataclass
|
||||
import math
|
||||
import time
|
||||
from typing import Any
|
||||
|
||||
import numpy as np
|
||||
|
||||
from openpilot.cereal import log, messaging
|
||||
from opendbc.car.interfaces import ACCEL_MAX, ACCEL_MIN
|
||||
from openpilot.common.realtime import DT_CTRL, DT_MDL, Ratekeeper
|
||||
from openpilot.selfdrive.modeld.constants import ModelConstants
|
||||
from openpilot.selfdrive.controls.lib.longcontrol import LongControl, LongCtrlState
|
||||
from openpilot.selfdrive.controls.lib.longitudinal_planner import LongitudinalPlanner
|
||||
from openpilot.selfdrive.controls.radard import _LEAD_ACCEL_TAU
|
||||
from openpilot.selfdrive.test.longitudinal_maneuvers.plant import Plant, PlannerSM
|
||||
|
||||
|
||||
LeadObservation = dict[str, Any]
|
||||
LeadObservationFn = Callable[[float, str, LeadObservation], LeadObservation | None]
|
||||
ModelActionFn = Callable[[float, float, float], tuple[float, bool]]
|
||||
EgoObservationFn = Callable[[float, float, float], tuple[float, float]]
|
||||
|
||||
|
||||
@dataclass(frozen=True)
|
||||
class ActuatorModel:
|
||||
planner_delay: float
|
||||
transport_delay: float
|
||||
actuator_lag: float
|
||||
command_rate_limit: float
|
||||
stopping_acceleration: float
|
||||
standstill_breakaway_acceleration: float
|
||||
standstill_breakaway_time: float
|
||||
|
||||
def __post_init__(self):
|
||||
nonnegative_fields = {
|
||||
"planner_delay": self.planner_delay,
|
||||
"transport_delay": self.transport_delay,
|
||||
"actuator_lag": self.actuator_lag,
|
||||
"standstill_breakaway_acceleration": self.standstill_breakaway_acceleration,
|
||||
"standstill_breakaway_time": self.standstill_breakaway_time,
|
||||
}
|
||||
if any(not math.isfinite(value) or value < 0.0 for value in nonnegative_fields.values()):
|
||||
raise ValueError(f"ActuatorModel fields must be finite and non-negative: {nonnegative_fields}")
|
||||
if not math.isfinite(self.command_rate_limit) or self.command_rate_limit <= 0.0:
|
||||
raise ValueError("command_rate_limit must be finite and positive")
|
||||
if not math.isfinite(self.stopping_acceleration) or self.stopping_acceleration > 0.0:
|
||||
raise ValueError("stopping_acceleration must be finite and non-positive")
|
||||
|
||||
|
||||
# Conservative Prius TSS2 actuator model.
|
||||
PRIUS_TSS2_ROUTE_MODEL = ActuatorModel(
|
||||
planner_delay=0.05,
|
||||
transport_delay=0.0,
|
||||
actuator_lag=0.20,
|
||||
command_rate_limit=4.0,
|
||||
stopping_acceleration=-2.0,
|
||||
standstill_breakaway_acceleration=1.0,
|
||||
standstill_breakaway_time=0.05,
|
||||
)
|
||||
|
||||
|
||||
class PlantSP(Plant):
|
||||
"""Closed-loop plant with configurable observations and actuator response."""
|
||||
|
||||
def __init__(
|
||||
self,
|
||||
lead_relevancy=False,
|
||||
speed=0.0,
|
||||
distance_lead=2.0,
|
||||
enabled=True,
|
||||
only_lead2=False,
|
||||
only_radar=False,
|
||||
e2e=False,
|
||||
personality=0,
|
||||
force_decel=False,
|
||||
lead_observation_fn: LeadObservationFn | None = None,
|
||||
model_action_fn: ModelActionFn | None = None,
|
||||
ego_observation_fn: EgoObservationFn | None = None,
|
||||
actuator_delay: float | None = None,
|
||||
actuator_lag: float = 0.0,
|
||||
actuator_model: ActuatorModel | None = None,
|
||||
run_long_control: bool = False,
|
||||
):
|
||||
if actuator_delay is not None and (not math.isfinite(actuator_delay) or actuator_delay < 0.0):
|
||||
raise ValueError("actuator_delay must be finite and non-negative")
|
||||
if not math.isfinite(actuator_lag) or actuator_lag < 0.0:
|
||||
raise ValueError("actuator_lag must be finite and non-negative")
|
||||
|
||||
self.rate = 1.0 / DT_MDL
|
||||
|
||||
if not Plant.messaging_initialized:
|
||||
Plant.radar = messaging.pub_sock('radarState')
|
||||
Plant.controls_state = messaging.pub_sock('controlsState')
|
||||
Plant.selfdrive_state = messaging.pub_sock('selfdriveState')
|
||||
Plant.car_state = messaging.pub_sock('carState')
|
||||
Plant.plan = messaging.sub_sock('longitudinalPlan')
|
||||
Plant.messaging_initialized = True
|
||||
|
||||
self.v_lead_prev = 0.0
|
||||
|
||||
self.distance = 0.0
|
||||
self.speed = speed
|
||||
self.should_stop = False
|
||||
self.acceleration = 0.0
|
||||
self.a_target = 0.0
|
||||
self.actuator_command = 0.0
|
||||
self.applied_actuator_command = 0.0
|
||||
self.breakaway_confirmed = False
|
||||
self._breakaway_timer = 0.0
|
||||
|
||||
# lead car
|
||||
self.lead_relevancy = lead_relevancy
|
||||
self.distance_lead = distance_lead
|
||||
self.enabled = enabled
|
||||
self.only_lead2 = only_lead2
|
||||
self.only_radar = only_radar
|
||||
self.e2e = e2e
|
||||
self.personality = personality
|
||||
self.force_decel = force_decel
|
||||
self.lead_observation_fn = lead_observation_fn
|
||||
self.model_action_fn = model_action_fn
|
||||
self.ego_observation_fn = ego_observation_fn
|
||||
self.actuator_model = actuator_model
|
||||
self.actuator_delay = actuator_model.planner_delay if actuator_model is not None else actuator_delay
|
||||
self.transport_delay = actuator_model.transport_delay if actuator_model is not None else actuator_delay
|
||||
self.actuator_lag = actuator_model.actuator_lag if actuator_model is not None else actuator_lag
|
||||
self.publish_realized_a_ego = any((lead_observation_fn is not None, model_action_fn is not None, ego_observation_fn is not None,
|
||||
actuator_delay is not None, actuator_lag > 0.0, actuator_model is not None, run_long_control))
|
||||
|
||||
self.rk = Ratekeeper(self.rate, print_delay_threshold=100.0)
|
||||
self.ts = 1.0 / self.rate
|
||||
time.sleep(0.1)
|
||||
self.sm = messaging.SubMaster(['longitudinalPlan'])
|
||||
|
||||
from opendbc.car.honda.values import CAR
|
||||
from opendbc.car.honda.interface import CarInterface
|
||||
|
||||
CP = CarInterface.get_non_essential_params(CAR.HONDA_CIVIC)
|
||||
if self.actuator_delay is not None:
|
||||
CP.longitudinalActuatorDelay = self.actuator_delay
|
||||
CP_SP = CarInterface.get_non_essential_params_sp(CP, CAR.HONDA_CIVIC)
|
||||
self.planner = LongitudinalPlanner(CP, CP_SP, init_v=self.speed)
|
||||
self.long_control = LongControl(CP, CP_SP) if run_long_control else None
|
||||
|
||||
if self.actuator_model is not None and self.speed >= 0.01:
|
||||
self.breakaway_confirmed = True
|
||||
self.integration_dt = DT_CTRL if run_long_control else self.ts
|
||||
delay_steps = 0 if self.transport_delay is None else round(self.transport_delay / self.integration_dt)
|
||||
self._actuator_delay_queue = deque([self.acceleration] * delay_steps)
|
||||
|
||||
@staticmethod
|
||||
def _lead_message(observation: LeadObservation):
|
||||
lead = log.RadarState.LeadData.new_message()
|
||||
for field, value in observation.items():
|
||||
setattr(lead, field, value)
|
||||
return lead
|
||||
|
||||
def _observe_lead(self, lead_name: str, truth: LeadObservation, present_by_default: bool) -> LeadObservation | None:
|
||||
if self.lead_observation_fn is None:
|
||||
return dict(truth) if present_by_default else None
|
||||
|
||||
observed = self.lead_observation_fn(self.current_time, lead_name, dict(truth))
|
||||
if observed is None:
|
||||
return None
|
||||
|
||||
complete_observation = dict(truth)
|
||||
complete_observation.update(observed)
|
||||
return complete_observation
|
||||
|
||||
def _update_actuator(self, command: float) -> tuple[float, float]:
|
||||
if self._actuator_delay_queue:
|
||||
self._actuator_delay_queue.append(command)
|
||||
delayed_command = self._actuator_delay_queue.popleft()
|
||||
else:
|
||||
delayed_command = command
|
||||
|
||||
if self.actuator_model is not None:
|
||||
max_command_delta = self.actuator_model.command_rate_limit * self.integration_dt
|
||||
self.applied_actuator_command = float(np.clip(delayed_command,
|
||||
self.applied_actuator_command - max_command_delta,
|
||||
self.applied_actuator_command + max_command_delta))
|
||||
|
||||
if self.speed < 0.01:
|
||||
if self.applied_actuator_command <= 0.0:
|
||||
self.breakaway_confirmed = False
|
||||
self._breakaway_timer = 0.0
|
||||
elif not self.breakaway_confirmed:
|
||||
breakaway_ready = self.applied_actuator_command + 1e-9 >= self.actuator_model.standstill_breakaway_acceleration
|
||||
if breakaway_ready:
|
||||
self._breakaway_timer += self.integration_dt
|
||||
else:
|
||||
self._breakaway_timer = 0.0
|
||||
|
||||
self.breakaway_confirmed = breakaway_ready and self._breakaway_timer + 1e-9 >= self.actuator_model.standstill_breakaway_time
|
||||
if not self.breakaway_confirmed:
|
||||
self.acceleration = 0.0
|
||||
return delayed_command, self.acceleration
|
||||
else:
|
||||
self.breakaway_confirmed = True
|
||||
|
||||
response_command = self.applied_actuator_command
|
||||
else:
|
||||
self.applied_actuator_command = delayed_command
|
||||
response_command = delayed_command
|
||||
|
||||
if self.actuator_lag > 0.0:
|
||||
alpha = 1.0 - math.exp(-self.integration_dt / self.actuator_lag)
|
||||
self.acceleration += alpha * (response_command - self.acceleration)
|
||||
else:
|
||||
self.acceleration = response_command
|
||||
return delayed_command, self.acceleration
|
||||
|
||||
def _integrate_ego(self, dt: float, stop_at_standstill: bool = False) -> None:
|
||||
self.speed += self.acceleration * dt
|
||||
if self.speed <= 0.0 or stop_at_standstill and self.speed < 0.01 and self.actuator_command <= 0.0:
|
||||
self.speed = self.acceleration = 0.0
|
||||
self.distance += self.speed * dt
|
||||
|
||||
def step(self, v_lead=0.0, prob_lead=1.0, v_cruise=50.0, pitch=0.0, prob_throttle=1.0):
|
||||
# ******** publish a fake model going straight and fake calibration ********
|
||||
# note that this is worst case for MPC, since model will delay long mpc by one time step
|
||||
radar = messaging.new_message('radarState')
|
||||
control = messaging.new_message('controlsState')
|
||||
ss = messaging.new_message('selfdriveState')
|
||||
car_state = messaging.new_message('carState')
|
||||
vehicle_parameters = messaging.new_message('vehicleParameters')
|
||||
car_control = messaging.new_message('carControl')
|
||||
model = messaging.new_message('modelV2')
|
||||
car_state_sp = messaging.new_message('carStateSP')
|
||||
live_map_data_sp = messaging.new_message('liveMapDataSP')
|
||||
gps_data = messaging.new_message('gpsLocation')
|
||||
a_lead = (v_lead - self.v_lead_prev) / self.ts
|
||||
self.v_lead_prev = v_lead
|
||||
|
||||
if self.lead_relevancy:
|
||||
d_rel = np.maximum(0.0, self.distance_lead - self.distance)
|
||||
v_rel = v_lead - self.speed
|
||||
if self.only_radar:
|
||||
status = True
|
||||
elif prob_lead > 0.5:
|
||||
status = True
|
||||
else:
|
||||
status = False
|
||||
else:
|
||||
d_rel = 200.0
|
||||
v_rel = 0.0
|
||||
prob_lead = 0.0
|
||||
status = False
|
||||
|
||||
truth_lead: LeadObservation = {
|
||||
"dRel": float(d_rel),
|
||||
"yRel": 0.0,
|
||||
"vRel": float(v_rel),
|
||||
"vLead": float(v_lead),
|
||||
"vLeadK": float(v_lead),
|
||||
"aLeadK": float(a_lead),
|
||||
"present": bool(status),
|
||||
# TODO use real radard logic for this
|
||||
"aLeadTau": float(_LEAD_ACCEL_TAU),
|
||||
"modelProb": float(prob_lead),
|
||||
"radar": bool(self.only_radar),
|
||||
"radarTrackId": -1,
|
||||
}
|
||||
lead_one_observation = self._observe_lead("leadOne", truth_lead, not self.only_lead2)
|
||||
lead_two_observation = self._observe_lead("leadTwo", truth_lead, True)
|
||||
if lead_one_observation is not None:
|
||||
radar.radarState.leadOne = self._lead_message(lead_one_observation)
|
||||
if lead_two_observation is not None:
|
||||
radar.radarState.leadTwo = self._lead_message(lead_two_observation)
|
||||
|
||||
# Simulate model predicting slightly faster speed
|
||||
# this is to ensure lead policy is effective when model
|
||||
# does not predict slowdown in e2e mode
|
||||
position = log.XYZTData.new_message()
|
||||
position.x = [float(x) for x in (self.speed + 0.5) * np.array(ModelConstants.T_IDXS)]
|
||||
model.modelV2.position = position
|
||||
if self.model_action_fn is None:
|
||||
model_acceleration, model_should_stop = self.acceleration + 0.5, False
|
||||
else:
|
||||
model_acceleration, model_should_stop = self.model_action_fn(self.current_time, self.speed, self.acceleration)
|
||||
model.modelV2.action.desiredAcceleration = float(model_acceleration)
|
||||
model.modelV2.action.shouldStop = bool(model_should_stop)
|
||||
velocity = log.XYZTData.new_message()
|
||||
velocity.x = [float(x) for x in (self.speed + 0.5) * np.ones_like(ModelConstants.T_IDXS)]
|
||||
velocity.x[0] = float(self.speed) # always start at current speed
|
||||
model.modelV2.velocity = velocity
|
||||
acceleration = log.XYZTData.new_message()
|
||||
acceleration.x = [float(x) for x in np.zeros_like(ModelConstants.T_IDXS)]
|
||||
model.modelV2.acceleration = acceleration
|
||||
model.modelV2.meta.disengagePredictions.gasPressProbs = [float(prob_throttle) for _ in range(6)]
|
||||
|
||||
control.controlsState.longControlState = self.long_control.long_control_state if self.long_control is not None else (
|
||||
LongCtrlState.pid if self.enabled else LongCtrlState.off)
|
||||
ss.selfdriveState.experimentalMode = self.e2e
|
||||
ss.selfdriveState.personality = self.personality
|
||||
control.controlsState.forceDecel = self.force_decel
|
||||
true_v_ego = self.speed
|
||||
true_a_ego = self.acceleration
|
||||
published_v_ego = true_v_ego
|
||||
published_a_ego = true_a_ego if self.publish_realized_a_ego else 0.0
|
||||
if self.ego_observation_fn is not None:
|
||||
published_v_ego, published_a_ego = self.ego_observation_fn(self.current_time, true_v_ego, true_a_ego)
|
||||
car_state.carState.vEgo = float(published_v_ego)
|
||||
car_state.carState.aEgo = float(published_a_ego)
|
||||
car_state.carState.standstill = bool(self.speed < 0.01)
|
||||
car_state.carState.vCruise = float(v_cruise * 3.6)
|
||||
car_control.carControl.orientationNED = [0.0, float(pitch), 0.0]
|
||||
|
||||
# ******** get controlsState messages for plotting ***
|
||||
sm = PlannerSM(self.rk.frame, {
|
||||
'radarState': radar.radarState,
|
||||
'carState': car_state.carState,
|
||||
'carControl': car_control.carControl,
|
||||
'controlsState': control.controlsState,
|
||||
'selfdriveState': ss.selfdriveState,
|
||||
'vehicleParameters': vehicle_parameters.vehicleParameters,
|
||||
'modelV2': model.modelV2,
|
||||
'carStateSP': car_state_sp.carStateSP,
|
||||
'liveMapDataSP': live_map_data_sp.liveMapDataSP,
|
||||
'gpsLocation': gps_data.gpsLocation,
|
||||
})
|
||||
self.planner.update(sm)
|
||||
self.a_target = self.planner.output_a_target
|
||||
if self.long_control is None:
|
||||
self.actuator_command = self.a_target
|
||||
if self.planner.output_should_stop:
|
||||
stopping_acceleration = -0.5 if self.actuator_model is None else self.actuator_model.stopping_acceleration
|
||||
self.actuator_command = min(stopping_acceleration, self.actuator_command)
|
||||
self._update_actuator(self.actuator_command)
|
||||
self._integrate_ego(self.ts)
|
||||
else:
|
||||
for _ in range(round(self.ts / DT_CTRL)):
|
||||
car_state.carState.vEgo = self.speed
|
||||
car_state.carState.aEgo = self.acceleration
|
||||
car_state.carState.standstill = self.speed < 0.01
|
||||
self.actuator_command = self.long_control.update(
|
||||
self.enabled, car_state.carState, self.a_target, self.planner.output_should_stop, (ACCEL_MIN, ACCEL_MAX),
|
||||
)
|
||||
self._update_actuator(self.actuator_command)
|
||||
self._integrate_ego(DT_CTRL, stop_at_standstill=True)
|
||||
self.should_stop = self.planner.output_should_stop
|
||||
fcw = self.planner.fcw
|
||||
self.distance_lead = self.distance_lead + v_lead * self.ts
|
||||
|
||||
# *** radar model ***
|
||||
if self.lead_relevancy:
|
||||
d_rel = np.maximum(0.0, self.distance_lead - self.distance)
|
||||
v_rel = v_lead - self.speed
|
||||
else:
|
||||
d_rel = 200.0
|
||||
v_rel = 0.0
|
||||
|
||||
# print at 5hz
|
||||
# if (self.rk.frame % (self.rate // 5)) == 0:
|
||||
# print("%2.2f sec %6.2f m %6.2f m/s %6.2f m/s2 lead_rel: %6.2f m %6.2f m/s"
|
||||
# % (self.current_time, self.distance, self.speed, self.acceleration, d_rel, v_rel))
|
||||
|
||||
# ******** update prevs ********
|
||||
self.rk.monitor_time()
|
||||
|
||||
accel_controller = self.planner.accel_controller
|
||||
return {
|
||||
"distance": self.distance,
|
||||
"speed": self.speed,
|
||||
"acceleration": self.acceleration,
|
||||
"realized_acceleration": self.acceleration,
|
||||
"a_target": self.a_target,
|
||||
"actuator_command": self.actuator_command,
|
||||
"published_a_ego": published_a_ego,
|
||||
"published_v_ego": published_v_ego,
|
||||
"should_stop": self.should_stop,
|
||||
"long_control_state": (int(self.long_control.long_control_state) if self.long_control is not None
|
||||
else control.controlsState.longControlState.raw),
|
||||
"distance_lead": self.distance_lead,
|
||||
"fcw": fcw,
|
||||
"mpc_source": self.planner.mpc.source,
|
||||
"dec_mode": self.planner.dec.mode(),
|
||||
"controller_active": accel_controller.is_active,
|
||||
"controller_accel_max": accel_controller.cruise_accel_max,
|
||||
"controller_early_decel": accel_controller.early_decel,
|
||||
"model_action": {
|
||||
"desiredAcceleration": float(model_acceleration),
|
||||
"shouldStop": bool(model_should_stop),
|
||||
},
|
||||
"truth_lead": dict(truth_lead),
|
||||
"lead_one_observation": None if lead_one_observation is None else dict(lead_one_observation),
|
||||
"lead_two_observation": None if lead_two_observation is None else dict(lead_two_observation),
|
||||
}
|
||||
@@ -0,0 +1,166 @@
|
||||
from collections.abc import Callable
|
||||
import math
|
||||
from typing import cast
|
||||
|
||||
from openpilot.common.parameterized import parameterized
|
||||
from openpilot.common.realtime import DT_MDL
|
||||
from openpilot.common.test import OpenpilotTestCase
|
||||
from openpilot.selfdrive.test.longitudinal_maneuvers.plant import Plant
|
||||
from openpilot.sunnypilot.selfdrive.test.longitudinal_maneuvers.plant import PlantSP
|
||||
|
||||
STOCK_STEP_KEYS = ("distance", "speed", "acceleration", "should_stop", "distance_lead", "fcw")
|
||||
|
||||
|
||||
def departing_lead(current_time: float) -> float:
|
||||
return 0.0 if current_time < 1.0 else min(2.0, 2.0 * (current_time - 1.0))
|
||||
|
||||
|
||||
def stopped_lead(_current_time: float) -> float:
|
||||
return 0.0
|
||||
|
||||
|
||||
PARITY_SCENARIOS = {
|
||||
"approach_stopped_lead": {"lead_relevancy": True, "speed": 15.0, "distance_lead": 60.0, "v_cruise": 20.0, "v_lead": stopped_lead, "steps": 80},
|
||||
"stop_then_depart": {"lead_relevancy": True, "speed": 0.0, "distance_lead": 6.0, "v_cruise": 8.0, "v_lead": departing_lead, "steps": 120},
|
||||
}
|
||||
|
||||
|
||||
def _drive(cls, *, v_cruise: float, v_lead: Callable[[float], float], steps: int, **kwargs):
|
||||
plant = cls(**kwargs)
|
||||
plant.v_lead_prev = v_lead(0.0)
|
||||
solver_failures = 0
|
||||
original_reset = plant.planner.mpc.reset
|
||||
|
||||
def counting_reset(*args, **kw):
|
||||
nonlocal solver_failures
|
||||
if plant.planner.mpc.solution_status != 0:
|
||||
solver_failures += 1
|
||||
return original_reset(*args, **kw)
|
||||
|
||||
plant.planner.mpc.reset = counting_reset
|
||||
results = []
|
||||
for _ in range(steps):
|
||||
lead_speed = v_lead(plant.current_time)
|
||||
result = plant.step(v_lead=lead_speed, v_cruise=v_cruise)
|
||||
results.append((result, plant.planner.mpc.source, plant.planner.output_a_target))
|
||||
return results, solver_failures
|
||||
|
||||
|
||||
class TestPlantSP(OpenpilotTestCase):
|
||||
@parameterized.expand(PARITY_SCENARIOS, names=("scenario",), ids=lambda scenario: scenario)
|
||||
def test_plant_sp_matches_stock_plant_on_shared_kwargs(self, scenario: str):
|
||||
kwargs = dict(PARITY_SCENARIOS[scenario])
|
||||
v_cruise = cast(float, kwargs.pop("v_cruise"))
|
||||
v_lead = cast(Callable[[float], float], kwargs.pop("v_lead"))
|
||||
steps = cast(int, kwargs.pop("steps"))
|
||||
|
||||
stock_results, stock_failures = _drive(Plant, v_cruise=v_cruise, v_lead=v_lead, steps=steps, **kwargs)
|
||||
sp_results, sp_failures = _drive(PlantSP, v_cruise=v_cruise, v_lead=v_lead, steps=steps, **kwargs)
|
||||
|
||||
assert stock_failures == 0, f"stock Plant solver failed {stock_failures} times in {scenario!r}"
|
||||
assert sp_failures == 0, f"PlantSP solver failed {sp_failures} times in {scenario!r}"
|
||||
|
||||
for frame, ((stock_result, stock_source, stock_a_target), (sp_result, sp_source, sp_a_target)) in enumerate(
|
||||
zip(stock_results, sp_results, strict=True),
|
||||
):
|
||||
for key in STOCK_STEP_KEYS:
|
||||
if isinstance(stock_result[key], float):
|
||||
self.assertAlmostEqual(sp_result[key], stock_result[key], msg=f"{scenario} frame {frame} key {key}")
|
||||
else:
|
||||
assert sp_result[key] == stock_result[key], f"{scenario} frame {frame} key {key}"
|
||||
assert sp_source == stock_source, f"{scenario} frame {frame} mpc.source"
|
||||
self.assertAlmostEqual(sp_a_target, stock_a_target, msg=f"{scenario} frame {frame} output_a_target")
|
||||
|
||||
if scenario == "stop_then_depart":
|
||||
departure_frame = round(1.0 / DT_MDL)
|
||||
for results in (stock_results, sp_results):
|
||||
assert all(result["speed"] < 0.01 for result, _, _ in results[:departure_frame])
|
||||
assert results[departure_frame - 1][0]["should_stop"]
|
||||
assert any(not result["should_stop"] for result, _, _ in results[departure_frame:])
|
||||
assert any(result["speed"] > 0.05 for result, _, _ in results[departure_frame:])
|
||||
stock_release = next(frame for frame, (result, _, _) in enumerate(stock_results)
|
||||
if frame >= departure_frame and not result["should_stop"])
|
||||
sp_release = next(frame for frame, (result, _, _) in enumerate(sp_results)
|
||||
if frame >= departure_frame and not result["should_stop"])
|
||||
stock_motion = next(frame for frame, (result, _, _) in enumerate(stock_results)
|
||||
if frame >= departure_frame and result["speed"] > 0.05)
|
||||
sp_motion = next(frame for frame, (result, _, _) in enumerate(sp_results)
|
||||
if frame >= departure_frame and result["speed"] > 0.05)
|
||||
assert sp_release == stock_release
|
||||
assert sp_motion == stock_motion
|
||||
|
||||
def test_full_lead_observation_is_independent_from_truth(self):
|
||||
callback_inputs = []
|
||||
|
||||
def observe_lead(current_time, lead_name, truth):
|
||||
callback_inputs.append((current_time, lead_name, truth))
|
||||
if lead_name == "leadOne":
|
||||
return {
|
||||
"dRel": 12.5,
|
||||
"vRel": -4.0,
|
||||
"vLead": 6.0,
|
||||
"vLeadK": 5.5,
|
||||
"aLeadK": -1.25,
|
||||
"aLeadTau": 0.7,
|
||||
"present": True,
|
||||
"modelProb": 0.9,
|
||||
"radarTrackId": 42,
|
||||
}
|
||||
return None
|
||||
|
||||
plant = PlantSP(lead_relevancy=True, speed=10.0, distance_lead=50.0, lead_observation_fn=observe_lead)
|
||||
result = plant.step(v_lead=8.0)
|
||||
|
||||
assert [entry[1] for entry in callback_inputs] == ["leadOne", "leadTwo"]
|
||||
self.assertAlmostEqual(callback_inputs[0][2]["dRel"], 50.0)
|
||||
self.assertAlmostEqual(result["truth_lead"]["dRel"], 50.0)
|
||||
self.assertAlmostEqual(result["lead_one_observation"]["dRel"], 12.5)
|
||||
assert result["lead_one_observation"]["radarTrackId"] == 42
|
||||
assert result["lead_two_observation"] is None
|
||||
self.assertAlmostEqual(result["distance_lead"], 50.0 + 8.0 * DT_MDL)
|
||||
|
||||
def test_model_action_realized_acceleration_and_source_logging(self):
|
||||
def model_action(current_time, v_ego, a_ego):
|
||||
return -1.25, True
|
||||
|
||||
plant = PlantSP(speed=10.0, e2e=True, force_decel=True, model_action_fn=model_action, actuator_lag=0.5)
|
||||
first = plant.step()
|
||||
second = plant.step()
|
||||
|
||||
assert first["model_action"] == {"desiredAcceleration": -1.25, "shouldStop": True}
|
||||
self.assertAlmostEqual(first["published_a_ego"], 0.0)
|
||||
self.assertAlmostEqual(second["published_a_ego"], first["realized_acceleration"])
|
||||
assert first["acceleration"] == first["realized_acceleration"]
|
||||
assert abs(first["realized_acceleration"]) < abs(first["actuator_command"])
|
||||
assert first["mpc_source"] is not None
|
||||
assert first["dec_mode"] in ("acc", "blended")
|
||||
assert "controller_active" in first
|
||||
assert "controller_accel_max" in first
|
||||
assert "controller_early_decel" in first
|
||||
assert first["lead_one_observation"] is not None
|
||||
assert first["truth_lead"] == first["lead_one_observation"]
|
||||
|
||||
def test_default_model_action_matches_stock_plant(self):
|
||||
result = PlantSP(speed=10.0).step()
|
||||
|
||||
self.assertAlmostEqual(result["model_action"]["desiredAcceleration"], 0.5)
|
||||
assert not result["model_action"]["shouldStop"]
|
||||
|
||||
def test_configurable_transport_delay_and_first_order_lag(self):
|
||||
plant = PlantSP(speed=10.0, actuator_delay=2 * DT_MDL, actuator_lag=0.2)
|
||||
|
||||
self.assertAlmostEqual(plant.planner.CP.longitudinalActuatorDelay, 2 * DT_MDL)
|
||||
delayed_commands = [plant._update_actuator(-1.0) for _ in range(3)]
|
||||
assert [command for command, _ in delayed_commands[:2]] == [0.0, 0.0]
|
||||
|
||||
expected_acceleration = -(1.0 - math.exp(-DT_MDL / 0.2))
|
||||
assert delayed_commands[2][0] == -1.0
|
||||
self.assertAlmostEqual(delayed_commands[2][1], expected_acceleration)
|
||||
|
||||
@parameterized.expand(
|
||||
[(-0.1, 0.0), (float("nan"), 0.0), (float("inf"), 0.0), (None, -0.1), (None, float("nan")), (None, float("inf"))],
|
||||
names=("delay", "lag"),
|
||||
)
|
||||
def test_invalid_actuator_dynamics(self, delay, lag):
|
||||
with self.assertRaises(ValueError):
|
||||
PlantSP(actuator_delay=delay, actuator_lag=lag)
|
||||
@@ -652,6 +652,58 @@
|
||||
}
|
||||
]
|
||||
},
|
||||
{
|
||||
"key": "AccelPersonalityEnabled",
|
||||
"widget": "toggle",
|
||||
"title": "Enable Accel Controller",
|
||||
"description": "Begin slowing early and smoothly behind lead vehicles. Stock longitudinal control retains braking and stopping authority.",
|
||||
"visibility": [
|
||||
{
|
||||
"type": "capability",
|
||||
"field": "has_longitudinal_control",
|
||||
"equals": true
|
||||
}
|
||||
],
|
||||
"enablement": [
|
||||
{
|
||||
"type": "capability",
|
||||
"field": "has_longitudinal_control",
|
||||
"equals": true
|
||||
}
|
||||
]
|
||||
},
|
||||
{
|
||||
"key": "AccelPersonality",
|
||||
"widget": "multiple_button",
|
||||
"title": "Acceleration Profile",
|
||||
"description": "Eco slows earliest and recovers gently, Normal balances comfort and response, and Sport reacts and recovers more quickly.",
|
||||
"options": [
|
||||
{
|
||||
"value": 0,
|
||||
"label": "Eco"
|
||||
},
|
||||
{
|
||||
"value": 1,
|
||||
"label": "Normal"
|
||||
},
|
||||
{
|
||||
"value": 2,
|
||||
"label": "Sport"
|
||||
}
|
||||
],
|
||||
"enablement": [
|
||||
{
|
||||
"type": "capability",
|
||||
"field": "has_longitudinal_control",
|
||||
"equals": true
|
||||
},
|
||||
{
|
||||
"type": "param",
|
||||
"key": "AccelPersonalityEnabled",
|
||||
"equals": true
|
||||
}
|
||||
]
|
||||
},
|
||||
{
|
||||
"key": "IntelligentCruiseButtonManagement",
|
||||
"widget": "toggle",
|
||||
|
||||
@@ -43,6 +43,32 @@ sections:
|
||||
label: Relaxed
|
||||
enablement:
|
||||
- $ref: '#/macros/longitudinal'
|
||||
- key: AccelPersonalityEnabled
|
||||
widget: toggle
|
||||
title: Enable Accel Controller
|
||||
description: Begin slowing early and smoothly behind lead vehicles. Stock longitudinal control retains braking
|
||||
and stopping authority.
|
||||
visibility:
|
||||
- $ref: '#/macros/longitudinal'
|
||||
enablement:
|
||||
- $ref: '#/macros/longitudinal'
|
||||
- key: AccelPersonality
|
||||
widget: multiple_button
|
||||
title: Acceleration Profile
|
||||
description: Eco slows earliest and recovers gently, Normal balances comfort and response, and Sport reacts
|
||||
and recovers more quickly.
|
||||
options:
|
||||
- value: 0
|
||||
label: Eco
|
||||
- value: 1
|
||||
label: Normal
|
||||
- value: 2
|
||||
label: Sport
|
||||
enablement:
|
||||
- $ref: '#/macros/longitudinal'
|
||||
- type: param
|
||||
key: AccelPersonalityEnabled
|
||||
equals: true
|
||||
- key: IntelligentCruiseButtonManagement
|
||||
widget: toggle
|
||||
title: Intelligent Cruise Button Management (ICBM) (Alpha)
|
||||
|
||||
@@ -276,6 +276,22 @@ class TestKnownPanels(OpenpilotTestCase):
|
||||
enhanced_enable_keys = {r.get("key") for r in enhanced.get("enablement", []) if r.get("type") == "param"}
|
||||
assert "NeuralNetworkLateralControl" in enhanced_enable_keys
|
||||
|
||||
def test_accel_controller_profile_mapping_and_enablement(self, schema):
|
||||
cruise = next(p for p in schema["panels"] if p["id"] == "cruise")
|
||||
items = {item["key"]: item for item in _iter_panel_items(cruise)}
|
||||
|
||||
assert items["AccelPersonalityEnabled"]["widget"] == "toggle"
|
||||
assert items["AccelPersonality"]["options"] == [
|
||||
{"value": 0, "label": "Eco"},
|
||||
{"value": 1, "label": "Normal"},
|
||||
{"value": 2, "label": "Sport"},
|
||||
]
|
||||
assert {
|
||||
"type": "param",
|
||||
"key": "AccelPersonalityEnabled",
|
||||
"equals": True,
|
||||
} in items["AccelPersonality"]["enablement"]
|
||||
|
||||
|
||||
class TestKnownVehicleSettings(OpenpilotTestCase):
|
||||
def test_hyundai_has_longitudinal_tuning(self, schema):
|
||||
|
||||
Reference in New Issue
Block a user