From bc6dbf8ca16b5149c6c969db5d0acd2e32d0b5ca Mon Sep 17 00:00:00 2001 From: rav4kumar <36933347+rav4kumar@users.noreply.github.com> Date: Tue, 9 Jun 2026 14:18:36 -0700 Subject: [PATCH] feat(long): acceleration controller --- cereal/custom.capnp | 18 ++ common/params_keys.h | 5 + .../controls/lib/longitudinal_planner.py | 15 +- selfdrive/ui/layouts/settings/toggles.py | 37 +++- .../lib/accel_personality/__init__.py | 0 .../lib/accel_personality/accel_controller.py | 195 +++++++++++++++++ .../lib/accel_personality/constants.py | 82 +++++++ .../lib/accel_personality/tests/__init__.py | 0 .../tests/test_accel_controller.py | 201 ++++++++++++++++++ .../controls/lib/longitudinal_planner.py | 13 ++ sunnypilot/sunnylink/settings_ui.json | 74 ++++++- .../settings_ui_src/pages/cruise.yaml | 18 +- 12 files changed, 640 insertions(+), 18 deletions(-) create mode 100644 sunnypilot/selfdrive/controls/lib/accel_personality/__init__.py create mode 100644 sunnypilot/selfdrive/controls/lib/accel_personality/accel_controller.py create mode 100644 sunnypilot/selfdrive/controls/lib/accel_personality/constants.py create mode 100644 sunnypilot/selfdrive/controls/lib/accel_personality/tests/__init__.py create mode 100644 sunnypilot/selfdrive/controls/lib/accel_personality/tests/test_accel_controller.py diff --git a/cereal/custom.capnp b/cereal/custom.capnp index 237ec79e64..8fb6c81d73 100644 --- a/cereal/custom.capnp +++ b/cereal/custom.capnp @@ -194,6 +194,7 @@ struct LongitudinalPlanSP @0xf35cc4560bbf6ec2 { aTarget @5 :Float32; events @6 :List(OnroadEventSP.Event); e2eAlerts @7 :E2eAlerts; + acceleration @8 :Acceleration; struct DynamicExperimentalControl { state @0 :DynamicExperimentalControlState; @@ -296,6 +297,23 @@ struct LongitudinalPlanSP @0xf35cc4560bbf6ec2 { greenLightAlert @0 :Bool; leadDepartAlert @1 :Bool; } + + # Acceleration Personality (Eco / Normal / Sport) + struct Acceleration { + personality @0 :AccelerationPersonality; + enabled @1 :Bool; + maxAccel @2 :Float32; # current speed-indexed accel ceiling + brakeNeed @3 :Float32; # predicted decel demand from the lookahead (m/s^2, positive) + decelTarget @4 :Float32; # early-soft comfort decel target (m/s^2, negative) + smoothActive @5 :Bool; # early-soft braking currently shaping the target + bypassed @6 :Bool; # passthrough to stock plan (hard brake / FCW / should_stop / closing lead / e2e) + } + + enum AccelerationPersonality { + eco @0; + normal @1; + sport @2; + } } struct OnroadEventSP @0xda96579883444c35 { diff --git a/common/params_keys.h b/common/params_keys.h index 84f484057a..053dd574ce 100644 --- a/common/params_keys.h +++ b/common/params_keys.h @@ -4,6 +4,7 @@ #include #include "cereal/gen/cpp/log.capnp.h" +#include "cereal/gen/cpp/custom.capnp.h" inline static std::unordered_map keys = { {"AccessToken", {CLEAR_ON_MANAGER_START | DONT_LOG, STRING}}, @@ -235,6 +236,10 @@ inline static std::unordered_map keys = { {"DynamicExperimentalControl", {PERSISTENT | BACKUP, BOOL, "0"}}, {"BlindSpot", {PERSISTENT | BACKUP, BOOL, "0"}}, + // Acceleration Personality (Eco / Normal / Sport) + {"AccelPersonalityEnabled", {PERSISTENT | BACKUP, BOOL, "0"}}, + {"AccelPersonality", {PERSISTENT | BACKUP, INT, std::to_string(static_cast(cereal::LongitudinalPlanSP::AccelerationPersonality::NORMAL))}}, + // sunnypilot model params {"CameraOffset", {PERSISTENT | BACKUP, FLOAT, "0.0"}}, {"LagdToggle", {PERSISTENT | BACKUP, BOOL, "1"}}, diff --git a/selfdrive/controls/lib/longitudinal_planner.py b/selfdrive/controls/lib/longitudinal_planner.py index e02b02d2e0..02e2a3c5bc 100755 --- a/selfdrive/controls/lib/longitudinal_planner.py +++ b/selfdrive/controls/lib/longitudinal_planner.py @@ -110,7 +110,7 @@ class LongitudinalPlanner(LongitudinalPlannerSP): # No change cost when user is controlling the speed, or when standstill prev_accel_constraint = not (reset_state or sm['carState'].standstill) - accel_clip = [ACCEL_MIN, get_max_accel(v_ego)] + accel_clip = [ACCEL_MIN, self.accel.get_max_accel(v_ego)] steer_angle_without_offset = sm['carState'].steeringAngleDeg - sm['liveParameters'].angleOffsetDeg accel_clip = limit_accel_in_turns(v_ego, steer_angle_without_offset, accel_clip, self.CP) @@ -160,7 +160,8 @@ class LongitudinalPlanner(LongitudinalPlannerSP): output_a_target_e2e = sm['modelV2'].action.desiredAcceleration output_should_stop_e2e = sm['modelV2'].action.shouldStop - if self.is_e2e(sm): + is_e2e = self.is_e2e(sm) + if is_e2e: output_a_target = min(output_a_target_e2e, output_a_target_mpc) self.output_should_stop = output_should_stop_e2e or output_should_stop_mpc if output_a_target < output_a_target_mpc: @@ -169,8 +170,14 @@ class LongitudinalPlanner(LongitudinalPlannerSP): output_a_target = output_a_target_mpc self.output_should_stop = output_should_stop_mpc - for idx in range(2): - accel_clip[idx] = np.clip(accel_clip[idx], self.prev_accel_clip[idx] - 0.05, self.prev_accel_clip[idx] + 0.05) + # Acceleration Personality: early soft braking (never weaker than the plan). No-op when disabled. + output_a_target = self.accel.smooth_target_accel(output_a_target, self.a_desired_trajectory, CONTROL_N_T_IDX, + self.output_should_stop or force_slow_decel, reset=reset_state, stock_brake=is_e2e) + + # Lower (braking) bound and the ceiling's downward slew stay at the stock rate; only the ceiling's + # upward slew is tier-dependent (Acceleration Personality). + accel_clip[0] = np.clip(accel_clip[0], self.prev_accel_clip[0] - 0.05, self.prev_accel_clip[0] + 0.05) + accel_clip[1] = np.clip(accel_clip[1], self.prev_accel_clip[1] - 0.05, self.prev_accel_clip[1] + self.accel.get_rise_rate()) self.output_a_target = np.clip(output_a_target, accel_clip[0], accel_clip[1]) self.prev_accel_clip = accel_clip diff --git a/selfdrive/ui/layouts/settings/toggles.py b/selfdrive/ui/layouts/settings/toggles.py index 4c83584ad5..79cbeb91ee 100644 --- a/selfdrive/ui/layouts/settings/toggles.py +++ b/selfdrive/ui/layouts/settings/toggles.py @@ -31,6 +31,11 @@ DESCRIPTIONS = { "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)." ), + "AccelPersonalityEnabled": tr_noop("Enable Eco/Normal/Sport acceleration profiles, including early soft braking."), + "AccelPersonality": tr_noop( + "Eco accelerates gently and brakes early and soft; Sport accelerates briskly. " + + "Hard-braking authority is always preserved." + ), "AlwaysOnDM": tr_noop("Enable driver monitoring even when sunnypilot is not engaged."), 'RecordFront': tr_noop("Upload data from the driver facing camera and help improve the driver monitoring algorithm."), "IsMetric": tr_noop("Display speed in km/h instead of mph."), @@ -106,6 +111,24 @@ class TogglesLayout(Widget): icon="speed_limit.png" ) + self._accel_personality_enabled = toggle_item( + lambda: tr("Enable Acceleration Profiles"), + 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 +158,11 @@ class TogglesLayout(Widget): self._toggles[param] = toggle - # insert longitudinal personality after NDOG toggle + # insert longitudinal + acceleration personality 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) @@ -176,11 +201,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(True) 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.") @@ -247,3 +276,9 @@ 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) diff --git a/sunnypilot/selfdrive/controls/lib/accel_personality/__init__.py b/sunnypilot/selfdrive/controls/lib/accel_personality/__init__.py new file mode 100644 index 0000000000..e69de29bb2 diff --git a/sunnypilot/selfdrive/controls/lib/accel_personality/accel_controller.py b/sunnypilot/selfdrive/controls/lib/accel_personality/accel_controller.py new file mode 100644 index 0000000000..fce121b8b7 --- /dev/null +++ b/sunnypilot/selfdrive/controls/lib/accel_personality/accel_controller.py @@ -0,0 +1,195 @@ +""" +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. + +Acceleration Personality controller (Eco / Normal / Sport). + +Three independent, per-tier levers keyed by the cereal AccelerationPersonality ordinal: + 1. Accel ceiling - get_max_accel(v_ego), feeds the planner accel_clip upper bound. + 2. Accel rise rate - get_rise_rate(), slews the accel ceiling upward. + 3. Early soft braking - smooth_target_accel(), front-loads a gentle decel BEFORE the plan brakes, + never commanding less braking than the plan (never-weaken invariant), with hard-brake / FCW / + should_stop / closing-lead / e2e bypass back to the stock plan. + +Disabled or Normal == stock by construction: Normal tier uses the stock ceiling/rise literals, and a +disabled controller forces Normal and passes the target through untouched. +""" + +from collections.abc import Sequence + +import numpy as np + +from cereal import messaging +from opendbc.car import structs +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_personality.constants import \ + NORMAL, PERSONALITY_MIN, PERSONALITY_MAX, A_CRUISE_MAX_BP, A_CRUISE_MAX_V, RISE_RATE, SMOOTH_DECEL_BP, \ + SMOOTH_DECEL_V, BRAKE_DEEPENING_JERK, BRAKE_RELEASE_JERK, ACCEL_RISE_JERK, SMOOTH_DECEL_LOOKAHEAD_T, \ + MIN_SMOOTH_BRAKE_NEED, HARD_BRAKE_TARGET_ACCEL, HARD_BRAKE_NEED, CLOSING_LEAD_VREL, CLOSING_LEAD_TTC + +_ZERO_ACCEL_EPS = 1e-6 + + +class AccelController: + def __init__(self, CP: structs.CarParams, mpc, params=None): + self._CP = CP + self._mpc = mpc + self._params = params or Params() + self._frame = 0 + self._enabled: bool = self._params.get_bool("AccelPersonalityEnabled") + self._personality = NORMAL # cereal AccelerationPersonality ordinal + self._v_ego = 0.0 + self._lead_closing = False + self._last_target_accel = 0.0 + self._brake_need = 0.0 + self._decel_target = 0.0 + self._smooth_active = False + self._bypassed = False + self._read_params() + + def _read_params(self) -> None: + self._enabled = self._params.get_bool("AccelPersonalityEnabled") + if not self._enabled: + self._personality = NORMAL + return + + self._personality = get_sanitize_int_param("AccelPersonality", PERSONALITY_MIN, PERSONALITY_MAX, self._params) + + def update(self, sm: messaging.SubMaster) -> None: + if self._frame % int(1. / DT_MDL) == 0: + self._read_params() + self._v_ego = sm['carState'].vEgo + self._lead_closing = self._compute_lead_closing(sm) + self._frame += 1 + + @staticmethod + def _compute_lead_closing(sm: messaging.SubMaster) -> bool: + lead = sm['radarState'].leadOne + if not lead.status: + return False + v_rel = float(lead.vRel) + if v_rel >= 0.0: + return False + ttc = float(lead.dRel) / max(-v_rel, 1e-3) + return v_rel <= CLOSING_LEAD_VREL or ttc <= CLOSING_LEAD_TTC + + # --- positive accel levers --- + + def get_max_accel(self, v_ego: float) -> float: + return float(np.interp(v_ego, A_CRUISE_MAX_BP, A_CRUISE_MAX_V[self._personality])) + + def get_rise_rate(self) -> float: + return RISE_RATE[self._personality] + + # --- early soft braking --- + + def get_decel_target(self, brake_need: float) -> float: + return float(np.interp(max(0.0, float(brake_need)), SMOOTH_DECEL_BP, SMOOTH_DECEL_V[self._personality])) + + def smooth_target_accel(self, raw_target_accel: float, accel_trajectory: Sequence[float], t_idxs: Sequence[float], + should_stop: bool, reset: bool = False, stock_brake: bool = False) -> float: + raw_target_accel = float(raw_target_accel) + self._brake_need = self._compute_brake_need(raw_target_accel, accel_trajectory, t_idxs) + self._decel_target = 0.0 + + if reset or not self._enabled: + self._bypassed = False + return self._passthrough(raw_target_accel) + + # e2e/blended path: never reshape braking (the planner already min-blends e2e/mpc, and vision stops + # are the model's job per the lead->ACC policy). + if stock_brake and (raw_target_accel < 0.0 or self._brake_need >= MIN_SMOOTH_BRAKE_NEED): + self._bypassed = False + return self._passthrough(raw_target_accel) + + self._bypassed = self._emergency_bypass(raw_target_accel, should_stop) + if self._bypassed: + return self._passthrough(raw_target_accel) + + if self._brake_need < MIN_SMOOTH_BRAKE_NEED: + # no decel predicted: jerk-limit the (positive) accel, but never weaken an active brake + self._smooth_active = False + slewed = self._slew(raw_target_accel) + out = min(slewed, raw_target_accel) if raw_target_accel < 0.0 else slewed + return self._finalize(out) + + # decel predicted: front-load a gentle EARLY target, but NEVER weaker than the plan. + self._smooth_active = True + self._decel_target = self.get_decel_target(self._brake_need) + commanded = min(raw_target_accel, self._decel_target) # early-soft onset, can only brake >= plan + slewed = self._slew(commanded) + return self._finalize(min(slewed, raw_target_accel)) # post-slew clamp: never weaker than plan + + def _compute_brake_need(self, raw_target_accel: float, accel_trajectory: Sequence[float], t_idxs: Sequence[float]) -> float: + min_accel = float(raw_target_accel) + for accel, t in zip(accel_trajectory, t_idxs, strict=False): + if float(t) <= SMOOTH_DECEL_LOOKAHEAD_T: + min_accel = min(min_accel, float(accel)) + return max(0.0, -min_accel) + + def _emergency_bypass(self, raw_target_accel: float, should_stop: bool) -> bool: + return (self._mpc.crash_cnt > 0 or should_stop or + raw_target_accel <= HARD_BRAKE_TARGET_ACCEL or + self._brake_need >= HARD_BRAKE_NEED or + self._lead_closing) + + # --- slew / jerk limiting --- + + def _slew(self, target_accel: float) -> float: + target_accel = float(target_accel) + if target_accel > self._last_target_accel: + return self._slew_up(target_accel) + step = BRAKE_DEEPENING_JERK[self._personality] * DT_MDL + return self._clean_accel(max(target_accel, self._last_target_accel - step)) + + def _slew_up(self, target_accel: float) -> float: + if self._last_target_accel < 0.0: + released = min(target_accel, self._last_target_accel + BRAKE_RELEASE_JERK * DT_MDL) + if released <= 0.0: + return self._clean_accel(released) + return self._clean_accel(min(target_accel, ACCEL_RISE_JERK[self._personality] * DT_MDL)) + + step = ACCEL_RISE_JERK[self._personality] * DT_MDL + return self._clean_accel(min(target_accel, self._last_target_accel + step)) + + def _passthrough(self, target_accel: float) -> float: + self._smooth_active = False + return self._finalize(target_accel) + + def _finalize(self, target_accel: float) -> float: + target_accel = self._clean_accel(target_accel) + self._last_target_accel = target_accel + return target_accel + + @staticmethod + def _clean_accel(accel: float) -> float: + accel = float(accel) + return 0.0 if abs(accel) < _ZERO_ACCEL_EPS else accel + + # --- publishers (for longitudinalPlanSP.acceleration telemetry) --- + + def enabled(self) -> bool: + return self._enabled + + def personality(self): + return self._personality # cereal AccelerationPersonality ordinal + + def max_accel(self) -> float: + # Cached value for publishing; publish_longitudinal_plan_sp has no v_ego in scope. + return self.get_max_accel(self._v_ego) + + def brake_need(self) -> float: + return self._brake_need + + def decel_target(self) -> float: + return self._decel_target + + def smooth_active(self) -> bool: + return self._smooth_active + + def bypassed(self) -> bool: + return self._bypassed diff --git a/sunnypilot/selfdrive/controls/lib/accel_personality/constants.py b/sunnypilot/selfdrive/controls/lib/accel_personality/constants.py new file mode 100644 index 0000000000..4be7477116 --- /dev/null +++ b/sunnypilot/selfdrive/controls/lib/accel_personality/constants.py @@ -0,0 +1,82 @@ +""" +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 cereal import custom + +# Profile ids come from cereal: eco @0, normal @1, sport @2 (single source of truth). +AccelerationPersonality = custom.LongitudinalPlanSP.AccelerationPersonality +ECO = AccelerationPersonality.eco +NORMAL = AccelerationPersonality.normal +SPORT = AccelerationPersonality.sport + +PERSONALITY_MIN = min(AccelerationPersonality.schema.enumerants.values()) +PERSONALITY_MAX = max(AccelerationPersonality.schema.enumerants.values()) + +# --- Positive acceleration ceiling (feeds the planner accel_clip upper bound) --- +A_CRUISE_MAX_BP = [0., 10., 25., 40.] + +# Stock openpilot acceleration ceiling. Normal and disabled mode intentionally match this path. +STOCK_A_CRUISE_MAX_V = [1.6, 1.2, 0.8, 0.6] +STOCK_RISE_RATE = 0.05 + +# Speed-indexed accel ceiling. NORMAL is LOCKED to stock so a disabled controller (forced to NORMAL) +# is byte-identical to stock. Sport stays modestly above stock (responsive, not aggressive); eco gentle. +A_CRUISE_MAX_V = { + ECO: [1.20, 0.85, 0.45, 0.30], + NORMAL: STOCK_A_CRUISE_MAX_V, + SPORT: [1.75, 1.30, 0.90, 0.65], +} + +# Upward slew of the accel ceiling, m/s^2 per planner cycle (DT_MDL). NORMAL locked to stock. +# Sport only slightly quicker than stock (smooth roll-on, not a launch). +RISE_RATE = { + ECO: 0.02, + NORMAL: STOCK_RISE_RATE, + SPORT: 0.06, +} + +# --- Early soft braking --- +# Predicted brake need (m/s^2, positive) -> early comfort decel target (m/s^2, negative). +# Gentle, human-like progression: lead the brake early and softly rather than late and hard. +SMOOTH_DECEL_BP = [0.0, 0.4, 0.8, 1.2, 1.6, 2.0, 2.4] +SMOOTH_DECEL_V = { + ECO: [0.00, -0.10, -0.24, -0.44, -0.68, -0.92, -1.15], + NORMAL: [0.00, -0.13, -0.30, -0.55, -0.84, -1.12, -1.40], + SPORT: [0.00, -0.17, -0.40, -0.72, -1.05, -1.35, -1.65], +} + +# Jerk limits (m/s^3) - all kept gentle for smoothness ("no jerk" goal). +# Deepening only shapes the EARLY front-loaded brake; the never-weaken clamp lets a real plan brake +# through immediately, so a soft deepening rate never delays genuine braking. +BRAKE_DEEPENING_JERK = { + ECO: 0.6, + NORMAL: 0.8, + SPORT: 1.0, +} +BRAKE_RELEASE_JERK = 2.0 # how fast the brake lets off (kept brisk so resume/SnG isn't laggy) + +# Positive-accel onset jerk (m/s^3). This is the "smooth, not crazy fast" knob: stock has no output +# accel-jerk limit, so enabling the controller makes accel onset gentler than stock on every tier. +ACCEL_RISE_JERK = { + ECO: 0.7, + NORMAL: 1.2, + SPORT: 1.6, +} + +# Look this far into the planned decel trajectory to anticipate braking and start early. +SMOOTH_DECEL_LOOKAHEAD_T = 3.0 +# Below this predicted decel we treat the situation as "no braking coming". +MIN_SMOOTH_BRAKE_NEED = 0.05 + +# Hand the target fully back to the stock plan (never shape) once braking is genuinely hard. +HARD_BRAKE_TARGET_ACCEL = -2.0 +HARD_BRAKE_NEED = 2.6 + +# Closing-lead bypass: hand fully back to the plan on a real closing threat regardless of the fixed +# accel thresholds above (mirrors the route 000003da lesson - shaping must yield to closing dynamics). +CLOSING_LEAD_VREL = -8.0 # m/s, lead approaching faster than this +CLOSING_LEAD_TTC = 4.0 # s, time-to-collision below this diff --git a/sunnypilot/selfdrive/controls/lib/accel_personality/tests/__init__.py b/sunnypilot/selfdrive/controls/lib/accel_personality/tests/__init__.py new file mode 100644 index 0000000000..e69de29bb2 diff --git a/sunnypilot/selfdrive/controls/lib/accel_personality/tests/test_accel_controller.py b/sunnypilot/selfdrive/controls/lib/accel_personality/tests/test_accel_controller.py new file mode 100644 index 0000000000..626cb0994c --- /dev/null +++ b/sunnypilot/selfdrive/controls/lib/accel_personality/tests/test_accel_controller.py @@ -0,0 +1,201 @@ +""" +Copyright (c) 2021-, Haibin Wen, sunnypilot, and a number of other contributors. + +This file is part of sunnypilot and is licensed under the MIT License. +See the LICENSE.md file in the root directory for more details. +""" + +from types import SimpleNamespace + +import numpy as np +import pytest + +from openpilot.sunnypilot.selfdrive.controls.lib.accel_personality.accel_controller import AccelController +from openpilot.sunnypilot.selfdrive.controls.lib.accel_personality.constants import \ + ECO, NORMAL, SPORT, PERSONALITY_MIN, PERSONALITY_MAX, A_CRUISE_MAX_BP, RISE_RATE, \ + STOCK_A_CRUISE_MAX_V, STOCK_RISE_RATE, HARD_BRAKE_TARGET_ACCEL, AccelerationPersonality + +# t<=2.5 frames feed the lookahead; the rest are beyond it. +T_IDXS = [0.0, 0.2, 0.4, 0.6, 0.8, 1.0, 1.25, 1.5, 1.75, 2.0, 2.5, 3.0, 4.0] +_EPS = 1e-6 + + +class FakeParams: + def __init__(self, store=None): + self.store = dict(store or {}) + + def get_bool(self, key): + return bool(self.store.get(key, False)) + + def get(self, key, return_default=False): + return int(self.store.get(key, 1)) + + def put(self, key, val, block=False): + self.store[key] = val + + def put_bool(self, key, val, block=False): + self.store[key] = bool(val) + + +def make_sm(v_ego=20.0, lead_status=False, v_rel=0.0, d_rel=50.0): + lead = SimpleNamespace(status=lead_status, vRel=v_rel, dRel=d_rel, vLead=v_ego + v_rel, aLeadK=0.0, modelProb=0.9) + return { + 'carState': SimpleNamespace(vEgo=v_ego), + 'radarState': SimpleNamespace(leadOne=lead), + } + + +def make_controller(enabled=True, personality=NORMAL, crash_cnt=0): + store = {"AccelPersonalityEnabled": enabled, "AccelPersonality": int(personality)} + mpc = SimpleNamespace(crash_cnt=crash_cnt) + ctrl = AccelController(CP=SimpleNamespace(), mpc=mpc, params=FakeParams(store)) + ctrl.update(make_sm()) + return ctrl + + +def flat_traj(value): + return [float(value)] * len(T_IDXS) + + +# --- enum source of truth --- + +def test_enum_source_parity(): + assert (ECO, NORMAL, SPORT) == (AccelerationPersonality.eco, AccelerationPersonality.normal, AccelerationPersonality.sport) + assert (PERSONALITY_MIN, PERSONALITY_MAX) == (0, 2) + + +# --- disabled / normal == stock --- + +def test_disabled_forces_normal_and_stock_ceiling(): + ctrl = make_controller(enabled=False, personality=SPORT) + assert ctrl.personality() == NORMAL + assert not ctrl.enabled() + for v in (0.0, 10.0, 25.0, 40.0): + assert ctrl.get_max_accel(v) == pytest.approx(np.interp(v, A_CRUISE_MAX_BP, STOCK_A_CRUISE_MAX_V)) + assert ctrl.get_rise_rate() == STOCK_RISE_RATE + + +def test_disabled_passes_brake_through(): + ctrl = make_controller(enabled=False) + for raw in (-1.5, -0.5, 0.0, 1.0): + out = ctrl.smooth_target_accel(raw, flat_traj(raw), T_IDXS, should_stop=False) + assert out == pytest.approx(raw, abs=_EPS) + + +def test_normal_matches_stock(): + ctrl = make_controller(personality=NORMAL) + for v in (0.0, 5.0, 10.0, 25.0, 40.0): + assert ctrl.get_max_accel(v) == pytest.approx(np.interp(v, A_CRUISE_MAX_BP, STOCK_A_CRUISE_MAX_V)) + assert ctrl.get_rise_rate() == STOCK_RISE_RATE + + +# --- per-tier ordering --- + +def test_ceiling_ordering_eco_lt_normal_lt_sport(): + eco, normal, sport = (make_controller(personality=p) for p in (ECO, NORMAL, SPORT)) + for v in (0.0, 10.0, 25.0, 40.0): + assert eco.get_max_accel(v) < normal.get_max_accel(v) < sport.get_max_accel(v) + + +def test_rise_rate_ordering(): + assert RISE_RATE[ECO] < RISE_RATE[NORMAL] < RISE_RATE[SPORT] + assert make_controller(personality=ECO).get_rise_rate() == RISE_RATE[ECO] + assert make_controller(personality=SPORT).get_rise_rate() == RISE_RATE[SPORT] + + +# --- early soft braking front-loads --- + +def test_early_soft_braking_brakes_before_plan(): + # plan not braking yet (raw ~ 0) but a decel is predicted in the lookahead -> command an early gentle brake + ctrl = make_controller(personality=NORMAL) + out = ctrl.smooth_target_accel(0.0, flat_traj(-1.0), T_IDXS, should_stop=False) + assert out < 0.0 + assert ctrl.smooth_active() + assert ctrl.brake_need() == pytest.approx(1.0) + + +# --- never-weaken invariant (route 000003da regression guard) --- + +@pytest.mark.parametrize("personality", [ECO, NORMAL, SPORT]) +def test_never_weaker_than_plan_sustained_closing(personality): + # Sustained moderate closing lead: plan ramps to -1.5 and holds. The controller must NEVER command + # less braking than the plan on any frame (this is the 000003da driver-takeover failure mode). + ctrl = make_controller(personality=personality) + raw_seq = [0.0, -0.2, -0.5, -0.9, -1.2, -1.5] + [-1.5] * 40 + for raw in raw_seq: + ctrl.update(make_sm(v_ego=15.0)) # no closing-bypass lead -> stays in the shaping zone + out = ctrl.smooth_target_accel(raw, flat_traj(raw), T_IDXS, should_stop=False) + assert out <= raw + _EPS, f"under-braked: out={out} > raw={raw}" + + +@pytest.mark.parametrize("personality", [ECO, NORMAL, SPORT]) +def test_never_weaker_random_walk(personality): + # Braking invariant: whenever the plan is braking (raw < 0), the controller must never command less + # braking. (For raw >= 0 the accel rate-limiter may legitimately ride above the plan.) + rng = np.random.default_rng(0) + ctrl = make_controller(personality=personality) + for _ in range(500): + raw = float(rng.uniform(-1.9, 1.5)) # stay above the hard-brake bypass threshold + traj_min = raw - float(rng.uniform(0.0, 0.6)) # predicted decel at or below the current plan + traj = flat_traj(traj_min) + ctrl.update(make_sm(v_ego=20.0)) + out = ctrl.smooth_target_accel(raw, traj, T_IDXS, should_stop=False) + if raw < 0.0: + assert out <= raw + _EPS + + +# --- bypasses hand fully back to the plan --- + +def test_hard_brake_bypass(): + ctrl = make_controller(personality=ECO) + raw = HARD_BRAKE_TARGET_ACCEL - 0.5 + out = ctrl.smooth_target_accel(raw, flat_traj(raw), T_IDXS, should_stop=False) + assert out == pytest.approx(raw, abs=_EPS) + assert ctrl.bypassed() + + +def test_should_stop_bypass(): + ctrl = make_controller(personality=ECO) + out = ctrl.smooth_target_accel(-1.0, flat_traj(-1.0), T_IDXS, should_stop=True) + assert out == pytest.approx(-1.0, abs=_EPS) + assert ctrl.bypassed() + + +def test_fcw_crash_cnt_bypass(): + ctrl = make_controller(personality=ECO, crash_cnt=3) + out = ctrl.smooth_target_accel(-1.0, flat_traj(-1.0), T_IDXS, should_stop=False) + assert out == pytest.approx(-1.0, abs=_EPS) + assert ctrl.bypassed() + + +def test_closing_lead_bypass(): + # a fast-closing lead must bypass shaping even in the soft (-0.05..-2.0) zone + ctrl = make_controller(personality=ECO) + ctrl.update(make_sm(v_ego=20.0, lead_status=True, v_rel=-10.0, d_rel=40.0)) + out = ctrl.smooth_target_accel(-1.2, flat_traj(-1.2), T_IDXS, should_stop=False) + assert out == pytest.approx(-1.2, abs=_EPS) + assert ctrl.bypassed() + + +def test_e2e_brake_passthrough(): + # blended/e2e path: braking is never reshaped + ctrl = make_controller(personality=ECO) + out = ctrl.smooth_target_accel(-1.0, flat_traj(-1.0), T_IDXS, should_stop=False, stock_brake=True) + assert out == pytest.approx(-1.0, abs=_EPS) + assert not ctrl.smooth_active() + + +# --- param sanitation --- + +def test_out_of_range_personality_clamps(): + store = {"AccelPersonalityEnabled": True, "AccelPersonality": 99} + ctrl = AccelController(CP=SimpleNamespace(), mpc=SimpleNamespace(crash_cnt=0), params=FakeParams(store)) + ctrl.update(make_sm()) + assert ctrl.personality() == PERSONALITY_MAX + + +def test_reset_passes_through(): + ctrl = make_controller(personality=ECO) + out = ctrl.smooth_target_accel(0.0, flat_traj(-1.0), T_IDXS, should_stop=False, reset=True) + assert out == pytest.approx(0.0, abs=_EPS) + assert not ctrl.bypassed() diff --git a/sunnypilot/selfdrive/controls/lib/longitudinal_planner.py b/sunnypilot/selfdrive/controls/lib/longitudinal_planner.py index 6efda4585f..4b30318ce7 100644 --- a/sunnypilot/selfdrive/controls/lib/longitudinal_planner.py +++ b/sunnypilot/selfdrive/controls/lib/longitudinal_planner.py @@ -9,6 +9,7 @@ from cereal import messaging, custom from opendbc.car import structs from openpilot.common.constants import CV from openpilot.selfdrive.car.cruise import V_CRUISE_MAX +from openpilot.sunnypilot.selfdrive.controls.lib.accel_personality.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 @@ -26,6 +27,7 @@ class LongitudinalPlannerSP: self.events_sp = EventsSP() self.resolver = SpeedLimitResolver() self.dec = DynamicExperimentalController(CP, mpc) + self.accel = AccelController(CP, mpc) self.scc = SmartCruiseControl() self.resolver = SpeedLimitResolver() self.sla = SpeedLimitAssist(CP, CP_SP) @@ -76,6 +78,7 @@ class LongitudinalPlannerSP: def update(self, sm: messaging.SubMaster) -> None: self.events_sp.clear() self.dec.update(sm) + self.accel.update(sm) self.e2e_alerts_helper.update(sm, self.events_sp) def publish_longitudinal_plan_sp(self, sm: messaging.SubMaster, pm: messaging.PubMaster) -> None: @@ -138,4 +141,14 @@ class LongitudinalPlannerSP: e2eAlerts.greenLightAlert = self.e2e_alerts_helper.green_light_alert e2eAlerts.leadDepartAlert = self.e2e_alerts_helper.lead_depart_alert + # Acceleration Personality + acceleration = longitudinalPlanSP.acceleration + acceleration.personality = self.accel.personality() + acceleration.enabled = self.accel.enabled() + acceleration.maxAccel = float(self.accel.max_accel()) + acceleration.brakeNeed = float(self.accel.brake_need()) + acceleration.decelTarget = float(self.accel.decel_target()) + acceleration.smoothActive = self.accel.smooth_active() + acceleration.bypassed = self.accel.bypassed() + pm.send('longitudinalPlanSP', plan_sp_send) diff --git a/sunnypilot/sunnylink/settings_ui.json b/sunnypilot/sunnylink/settings_ui.json index 1638105651..f5e30239c7 100644 --- a/sunnypilot/sunnylink/settings_ui.json +++ b/sunnypilot/sunnylink/settings_ui.json @@ -519,12 +519,6 @@ } ] }, - { - "key": "RoadEdgeLaneChangeEnabled", - "widget": "toggle", - "title": "Block Lane Change: Road Edge Detection", - "description": "Blocks lane change when the model sees a road edge on the side you signal." - }, { "key": "AutoLaneChangeBsmDelay", "widget": "toggle", @@ -630,7 +624,7 @@ "key": "AccelPersonalityEnabled", "widget": "toggle", "title": "Enable Acceleration Profiles", - "description": "Enables acceleration profile selection for longitudinal control.", + "description": "Enables Eco/Normal/Sport acceleration profiles for longitudinal control, including early soft braking.", "visibility": [ { "type": "capability", @@ -650,10 +644,10 @@ "key": "AccelPersonality", "widget": "multiple_button", "title": "Acceleration Profile", - "description": "Controls how quickly sunnypilot accelerates while preserving braking and stop behavior.", + "description": "Eco accelerates gently and brakes early and soft; Sport accelerates briskly. Hard-braking authority is always preserved.", "options": [ { - "value": 2, + "value": 0, "label": "Eco" }, { @@ -661,7 +655,7 @@ "label": "Normal" }, { - "value": 0, + "value": 2, "label": "Sport" } ], @@ -2059,6 +2053,22 @@ "equals": true } ] + }, + { + "key": "PlanplusControl", + "widget": "option", + "title": "Plan Plus Controls", + "description": "Adjust planplus model recentering strength. The higher this number the more aggressively the model will recover to lane center; too high and it will ping-pong.", + "min": 0.0, + "max": 2.0, + "step": 0.1, + "enablement": [ + { + "type": "param", + "key": "ShowAdvancedControls", + "equals": true + } + ] } ] }, @@ -2226,6 +2236,50 @@ "title": "Toyota / Lexus Settings", "description": "", "items": [ + { + "key": "ToyotaAutoHold", + "widget": "toggle", + "needs_onroad_cycle": true, + "title": "Toyota: Auto Brake Hold FOR TSS2 HYBRID CARS", + "enablement": [ + { + "type": "not_engaged" + } + ] + }, + { + "key": "ToyotaEnhancedBsm", + "widget": "toggle", + "needs_onroad_cycle": true, + "title": "Toyota: Prius TSS2 BSM and some tssp", + "enablement": [ + { + "type": "not_engaged" + } + ] + }, + { + "key": "ToyotaTSS2Long", + "widget": "toggle", + "needs_onroad_cycle": true, + "title": "Toyota: custom longitudinal for TSS2", + "enablement": [ + { + "type": "not_engaged" + } + ] + }, + { + "key": "ToyotaDriveMode", + "widget": "toggle", + "needs_onroad_cycle": true, + "title": "Enable drive mode btn link", + "enablement": [ + { + "type": "not_engaged" + } + ] + }, { "key": "ToyotaEnforceStockLongitudinal", "widget": "toggle", diff --git a/sunnypilot/sunnylink/settings_ui_src/pages/cruise.yaml b/sunnypilot/sunnylink/settings_ui_src/pages/cruise.yaml index 667cfd1d40..78f029b4a5 100644 --- a/sunnypilot/sunnylink/settings_ui_src/pages/cruise.yaml +++ b/sunnypilot/sunnylink/settings_ui_src/pages/cruise.yaml @@ -43,19 +43,31 @@ sections: label: Relaxed enablement: - $ref: '#/macros/longitudinal' + - key: AccelPersonalityEnabled + widget: toggle + title: Enable Acceleration Profiles + description: Enables Eco/Normal/Sport acceleration profiles for longitudinal control, including early soft braking. + visibility: + - $ref: '#/macros/longitudinal' + enablement: + - $ref: '#/macros/longitudinal' - key: AccelPersonality widget: multiple_button title: Acceleration Profile - description: Controls how quickly sunnypilot accelerates while preserving braking and stop behavior. + description: Eco accelerates gently and brakes early and soft; Sport accelerates briskly. Hard-braking + authority is always preserved. options: - - value: 2 + - value: 0 label: Eco - value: 1 label: Normal - - value: 0 + - 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)