mirror of
https://github.com/sunnypilot/sunnypilot.git
synced 2026-10-10 02:33:44 +08:00
feat(long): acceleration controller
This commit is contained in:
rav4kumar
@@ -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 {
|
||||
|
||||
@@ -4,6 +4,7 @@
|
||||
#include <unordered_map>
|
||||
|
||||
#include "cereal/gen/cpp/log.capnp.h"
|
||||
#include "cereal/gen/cpp/custom.capnp.h"
|
||||
|
||||
inline static std::unordered_map<std::string, ParamKeyAttributes> keys = {
|
||||
{"AccessToken", {CLEAR_ON_MANAGER_START | DONT_LOG, STRING}},
|
||||
@@ -235,6 +236,10 @@ inline static std::unordered_map<std::string, ParamKeyAttributes> 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<int>(cereal::LongitudinalPlanSP::AccelerationPersonality::NORMAL))}},
|
||||
|
||||
// sunnypilot model params
|
||||
{"CameraOffset", {PERSISTENT | BACKUP, FLOAT, "0.0"}},
|
||||
{"LagdToggle", {PERSISTENT | BACKUP, BOOL, "1"}},
|
||||
|
||||
@@ -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
|
||||
|
||||
|
||||
@@ -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)
|
||||
|
||||
@@ -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
|
||||
@@ -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
|
||||
@@ -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()
|
||||
@@ -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)
|
||||
|
||||
@@ -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",
|
||||
|
||||
@@ -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)
|
||||
|
||||
Reference in New Issue
Block a user