From 136984d43dc93afac696ff0b2e668e90861c3c9f Mon Sep 17 00:00:00 2001 From: rav4kumar <36933347+rav4kumar@users.noreply.github.com> Date: Sat, 15 Aug 2026 02:54:44 -0700 Subject: [PATCH] feat(long): smooth lead following with acceleration profiles --- openpilot/cereal/custom.capnp | 30 ++ openpilot/common/params_keys.h | 4 + openpilot/common/tests/test_params.py | 4 + .../controls/lib/longitudinal_planner.py | 1 + .../test/longitudinal_maneuvers/plant.py | 12 +- .../selfdrive/ui/layouts/settings/toggles.py | 44 +- .../ui/mici/layouts/settings/toggles.py | 14 + openpilot/selfdrive/ui/mici/widgets/button.py | 7 +- .../controls/lib/accel_controller/__init__.py | 0 .../lib/accel_controller/accel_controller.py | 143 +++++++ .../lib/accel_controller/constants.py | 53 +++ .../controls/lib/accel_controller/lead.py | 107 +++++ .../lib/accel_controller/tests/__init__.py | 0 .../tests/test_accel_controller.py | 191 +++++++++ .../tests/test_accel_controller_interfaces.py | 212 ++++++++++ .../lib/dec/tests/test_dec_planner_gate.py | 1 + .../controls/lib/longitudinal_planner.py | 53 ++- .../test_accel_controller_closed_loop.py | 94 +++++ .../sunnypilot/selfdrive/test/__init__.py | 0 .../test/longitudinal_maneuvers/__init__.py | 0 .../test/longitudinal_maneuvers/plant.py | 397 ++++++++++++++++++ .../longitudinal_maneuvers/tests/__init__.py | 0 .../tests/test_plant_sp.py | 166 ++++++++ .../sunnypilot/sunnylink/settings_ui.json | 52 +++ .../settings_ui_src/pages/cruise.yaml | 26 ++ .../sunnylink/tests/test_settings_schema.py | 16 + 26 files changed, 1621 insertions(+), 6 deletions(-) create mode 100644 openpilot/sunnypilot/selfdrive/controls/lib/accel_controller/__init__.py create mode 100644 openpilot/sunnypilot/selfdrive/controls/lib/accel_controller/accel_controller.py create mode 100644 openpilot/sunnypilot/selfdrive/controls/lib/accel_controller/constants.py create mode 100644 openpilot/sunnypilot/selfdrive/controls/lib/accel_controller/lead.py create mode 100644 openpilot/sunnypilot/selfdrive/controls/lib/accel_controller/tests/__init__.py create mode 100644 openpilot/sunnypilot/selfdrive/controls/lib/accel_controller/tests/test_accel_controller.py create mode 100644 openpilot/sunnypilot/selfdrive/controls/lib/accel_controller/tests/test_accel_controller_interfaces.py create mode 100644 openpilot/sunnypilot/selfdrive/controls/lib/tests/test_accel_controller_closed_loop.py create mode 100644 openpilot/sunnypilot/selfdrive/test/__init__.py create mode 100644 openpilot/sunnypilot/selfdrive/test/longitudinal_maneuvers/__init__.py create mode 100644 openpilot/sunnypilot/selfdrive/test/longitudinal_maneuvers/plant.py create mode 100644 openpilot/sunnypilot/selfdrive/test/longitudinal_maneuvers/tests/__init__.py create mode 100644 openpilot/sunnypilot/selfdrive/test/longitudinal_maneuvers/tests/test_plant_sp.py diff --git a/openpilot/cereal/custom.capnp b/openpilot/cereal/custom.capnp index c20bf923be..f10aaf2c86 100644 --- a/openpilot/cereal/custom.capnp +++ b/openpilot/cereal/custom.capnp @@ -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 { diff --git a/openpilot/common/params_keys.h b/openpilot/common/params_keys.h index bb7989381b..3fee8184b6 100644 --- a/openpilot/common/params_keys.h +++ b/openpilot/common/params_keys.h @@ -231,6 +231,10 @@ inline static std::unordered_map 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"}}, diff --git a/openpilot/common/tests/test_params.py b/openpilot/common/tests/test_params.py index a81d346b06..ab60fa7c71 100644 --- a/openpilot/common/tests/test_params.py +++ b/openpilot/common/tests/test_params.py @@ -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 diff --git a/openpilot/selfdrive/controls/lib/longitudinal_planner.py b/openpilot/selfdrive/controls/lib/longitudinal_planner.py index 8b62808dc0..e89e4b0574 100755 --- a/openpilot/selfdrive/controls/lib/longitudinal_planner.py +++ b/openpilot/selfdrive/controls/lib/longitudinal_planner.py @@ -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) diff --git a/openpilot/selfdrive/test/longitudinal_maneuvers/plant.py b/openpilot/selfdrive/test/longitudinal_maneuvers/plant.py index b4e8d76d6d..e2474767eb 100755 --- a/openpilot/selfdrive/test/longitudinal_maneuvers/plant.py +++ b/openpilot/selfdrive/test/longitudinal_maneuvers/plant.py @@ -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: diff --git a/openpilot/selfdrive/ui/layouts/settings/toggles.py b/openpilot/selfdrive/ui/layouts/settings/toggles.py index ee76b7e4cc..f82d05e0dd 100644 --- a/openpilot/selfdrive/ui/layouts/settings/toggles.py +++ b/openpilot/selfdrive/ui/layouts/settings/toggles.py @@ -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) diff --git a/openpilot/selfdrive/ui/mici/layouts/settings/toggles.py b/openpilot/selfdrive/ui/mici/layouts/settings/toggles.py index 2dba124df5..40c6bb2d3a 100644 --- a/openpilot/selfdrive/ui/mici/layouts/settings/toggles.py +++ b/openpilot/selfdrive/ui/mici/layouts/settings/toggles.py @@ -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 diff --git a/openpilot/selfdrive/ui/mici/widgets/button.py b/openpilot/selfdrive/ui/mici/widgets/button.py index 0ecda6a0c5..553e23201b 100644 --- a/openpilot/selfdrive/ui/mici/widgets/button.py +++ b/openpilot/selfdrive/ui/mici/widgets/button.py @@ -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): diff --git a/openpilot/sunnypilot/selfdrive/controls/lib/accel_controller/__init__.py b/openpilot/sunnypilot/selfdrive/controls/lib/accel_controller/__init__.py new file mode 100644 index 0000000000..e69de29bb2 diff --git a/openpilot/sunnypilot/selfdrive/controls/lib/accel_controller/accel_controller.py b/openpilot/sunnypilot/selfdrive/controls/lib/accel_controller/accel_controller.py new file mode 100644 index 0000000000..4fdd8b86f7 --- /dev/null +++ b/openpilot/sunnypilot/selfdrive/controls/lib/accel_controller/accel_controller.py @@ -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, + ) diff --git a/openpilot/sunnypilot/selfdrive/controls/lib/accel_controller/constants.py b/openpilot/sunnypilot/selfdrive/controls/lib/accel_controller/constants.py new file mode 100644 index 0000000000..58712019db --- /dev/null +++ b/openpilot/sunnypilot/selfdrive/controls/lib/accel_controller/constants.py @@ -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) diff --git a/openpilot/sunnypilot/selfdrive/controls/lib/accel_controller/lead.py b/openpilot/sunnypilot/selfdrive/controls/lib/accel_controller/lead.py new file mode 100644 index 0000000000..da3ba7c3d8 --- /dev/null +++ b/openpilot/sunnypilot/selfdrive/controls/lib/accel_controller/lead.py @@ -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) diff --git a/openpilot/sunnypilot/selfdrive/controls/lib/accel_controller/tests/__init__.py b/openpilot/sunnypilot/selfdrive/controls/lib/accel_controller/tests/__init__.py new file mode 100644 index 0000000000..e69de29bb2 diff --git a/openpilot/sunnypilot/selfdrive/controls/lib/accel_controller/tests/test_accel_controller.py b/openpilot/sunnypilot/selfdrive/controls/lib/accel_controller/tests/test_accel_controller.py new file mode 100644 index 0000000000..859ff0dcf0 --- /dev/null +++ b/openpilot/sunnypilot/selfdrive/controls/lib/accel_controller/tests/test_accel_controller.py @@ -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) diff --git a/openpilot/sunnypilot/selfdrive/controls/lib/accel_controller/tests/test_accel_controller_interfaces.py b/openpilot/sunnypilot/selfdrive/controls/lib/accel_controller/tests/test_accel_controller_interfaces.py new file mode 100644 index 0000000000..397e0b4b31 --- /dev/null +++ b/openpilot/sunnypilot/selfdrive/controls/lib/accel_controller/tests/test_accel_controller_interfaces.py @@ -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", + }) diff --git a/openpilot/sunnypilot/selfdrive/controls/lib/dec/tests/test_dec_planner_gate.py b/openpilot/sunnypilot/selfdrive/controls/lib/dec/tests/test_dec_planner_gate.py index 1f5c577028..0e83ef7c99 100644 --- a/openpilot/sunnypilot/selfdrive/controls/lib/dec/tests/test_dec_planner_gate.py +++ b/openpilot/sunnypilot/selfdrive/controls/lib/dec/tests/test_dec_planner_gate.py @@ -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) diff --git a/openpilot/sunnypilot/selfdrive/controls/lib/longitudinal_planner.py b/openpilot/sunnypilot/selfdrive/controls/lib/longitudinal_planner.py index f1e0c36416..e4ad668606 100644 --- a/openpilot/sunnypilot/selfdrive/controls/lib/longitudinal_planner.py +++ b/openpilot/sunnypilot/selfdrive/controls/lib/longitudinal_planner.py @@ -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 diff --git a/openpilot/sunnypilot/selfdrive/controls/lib/tests/test_accel_controller_closed_loop.py b/openpilot/sunnypilot/selfdrive/controls/lib/tests/test_accel_controller_closed_loop.py new file mode 100644 index 0000000000..98ccb00f5e --- /dev/null +++ b/openpilot/sunnypilot/selfdrive/controls/lib/tests/test_accel_controller_closed_loop.py @@ -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]) diff --git a/openpilot/sunnypilot/selfdrive/test/__init__.py b/openpilot/sunnypilot/selfdrive/test/__init__.py new file mode 100644 index 0000000000..e69de29bb2 diff --git a/openpilot/sunnypilot/selfdrive/test/longitudinal_maneuvers/__init__.py b/openpilot/sunnypilot/selfdrive/test/longitudinal_maneuvers/__init__.py new file mode 100644 index 0000000000..e69de29bb2 diff --git a/openpilot/sunnypilot/selfdrive/test/longitudinal_maneuvers/plant.py b/openpilot/sunnypilot/selfdrive/test/longitudinal_maneuvers/plant.py new file mode 100644 index 0000000000..f4c66522c1 --- /dev/null +++ b/openpilot/sunnypilot/selfdrive/test/longitudinal_maneuvers/plant.py @@ -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), + } diff --git a/openpilot/sunnypilot/selfdrive/test/longitudinal_maneuvers/tests/__init__.py b/openpilot/sunnypilot/selfdrive/test/longitudinal_maneuvers/tests/__init__.py new file mode 100644 index 0000000000..e69de29bb2 diff --git a/openpilot/sunnypilot/selfdrive/test/longitudinal_maneuvers/tests/test_plant_sp.py b/openpilot/sunnypilot/selfdrive/test/longitudinal_maneuvers/tests/test_plant_sp.py new file mode 100644 index 0000000000..74d4ee06bf --- /dev/null +++ b/openpilot/sunnypilot/selfdrive/test/longitudinal_maneuvers/tests/test_plant_sp.py @@ -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) diff --git a/openpilot/sunnypilot/sunnylink/settings_ui.json b/openpilot/sunnypilot/sunnylink/settings_ui.json index fb4ba7cc2b..a0957ee83c 100644 --- a/openpilot/sunnypilot/sunnylink/settings_ui.json +++ b/openpilot/sunnypilot/sunnylink/settings_ui.json @@ -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", diff --git a/openpilot/sunnypilot/sunnylink/settings_ui_src/pages/cruise.yaml b/openpilot/sunnypilot/sunnylink/settings_ui_src/pages/cruise.yaml index 3ef73e0fb0..11688c306b 100644 --- a/openpilot/sunnypilot/sunnylink/settings_ui_src/pages/cruise.yaml +++ b/openpilot/sunnypilot/sunnylink/settings_ui_src/pages/cruise.yaml @@ -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) diff --git a/openpilot/sunnypilot/sunnylink/tests/test_settings_schema.py b/openpilot/sunnypilot/sunnylink/tests/test_settings_schema.py index e60ac000f4..1249907050 100644 --- a/openpilot/sunnypilot/sunnylink/tests/test_settings_schema.py +++ b/openpilot/sunnypilot/sunnylink/tests/test_settings_schema.py @@ -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):