mirror of
https://github.com/sunnypilot/sunnypilot.git
synced 2026-08-21 09:13:46 +08:00
long: bit faster
This commit is contained in:
rav4kumar
@@ -15,7 +15,7 @@ from openpilot.sunnypilot import get_sanitize_int_param
|
||||
AccelProfile = custom.LongitudinalPlanSP.AccelController.Profile
|
||||
|
||||
MAX_ACCEL_PROFILES = {
|
||||
AccelProfile.eco: [1.45, 1.40, 1.20, 0.65, 0.48, 0.36, 0.22, 0.085, 0.055, 0.045],
|
||||
AccelProfile.eco: [1.45, 1.40, 1.20, 0.85, 0.62, 0.36, 0.22, 0.085, 0.055, 0.045],
|
||||
AccelProfile.normal: [2.00, 1.95, 1.80, 1.06, 0.81, 0.69, 0.42, 0.160, 0.10, 0.08],
|
||||
AccelProfile.sport: [2.00, 1.99, 1.95, 1.45, 1.10, 0.82, 0.53, 0.240, 0.13, 0.09],
|
||||
}
|
||||
@@ -36,8 +36,12 @@ LEAD_GAP_WIDEN_PROFILES = {
|
||||
AccelProfile.normal: 0.20,
|
||||
AccelProfile.sport: 0.10,
|
||||
}
|
||||
LEAD_DECEL_FOR_MAX_WIDEN = 3.0 # m/s^2, lead decel that saturates the widen amount
|
||||
GAP_WIDEN_ONSET_ALPHA = 0.15
|
||||
LEAD_DECEL_FOR_MAX_WIDEN = 3.0 # m/s^2, lead decel (aLeadK) that saturates the widen amount
|
||||
# aLeadK is a differentiated, filtered estimate -- it can lag several tenths of a second behind
|
||||
# the lead actually closing. vRel is measured directly every frame, so closing speed alone (even
|
||||
# before aLeadK has caught up) can independently trigger the same widen.
|
||||
LEAD_CLOSING_FOR_MAX_WIDEN = 5.0 # m/s, closing speed (-vRel) that alone saturates the widen amount
|
||||
GAP_WIDEN_ONSET_ALPHA = 0.12
|
||||
GAP_WIDEN_RELEASE_ALPHA = 0.08
|
||||
# Taper out below city speed so the lever only shapes higher-speed anticipation.
|
||||
GAP_WIDEN_TAPER_LOW_SPEED = 3.0 # m/s, widen fully tapered out at/below this speed
|
||||
@@ -88,12 +92,11 @@ class AccelController:
|
||||
self.last_min_accel = min(self.last_min_accel, self.last_max_accel - 0.1)
|
||||
return float(self.last_min_accel)
|
||||
|
||||
def get_t_follow_multiplier(self, lead_present: bool, lead_accel: float, v_ego: float) -> float:
|
||||
def get_t_follow_multiplier(self, lead_present: bool, lead_accel: float, v_ego: float, lead_v_rel: float = 0.0) -> float:
|
||||
max_widen = LEAD_GAP_WIDEN_PROFILES[self._profile]
|
||||
if lead_present and lead_accel < 0.0:
|
||||
target_widen = min(-lead_accel / LEAD_DECEL_FOR_MAX_WIDEN, 1.0) * max_widen
|
||||
else:
|
||||
target_widen = 0.0
|
||||
decel_widen = min(-lead_accel / LEAD_DECEL_FOR_MAX_WIDEN, 1.0) if lead_accel < 0.0 else 0.0
|
||||
closing_widen = min(-lead_v_rel / LEAD_CLOSING_FOR_MAX_WIDEN, 1.0) if lead_v_rel < 0.0 else 0.0
|
||||
target_widen = max(decel_widen, closing_widen) * max_widen if lead_present else 0.0
|
||||
|
||||
alpha = GAP_WIDEN_ONSET_ALPHA if target_widen > self.last_t_follow_widen else GAP_WIDEN_RELEASE_ALPHA
|
||||
self.last_t_follow_widen += alpha * (target_widen - self.last_t_follow_widen)
|
||||
|
||||
+34
-1
@@ -28,7 +28,7 @@ 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, AccelProfile, MAX_ACCEL_BREAKPOINTS, MAX_ACCEL_PROFILES, MIN_ACCEL_BREAKPOINTS, MIN_ACCEL_PROFILES,
|
||||
LEAD_GAP_WIDEN_PROFILES, LEAD_DECEL_FOR_MAX_WIDEN,
|
||||
LEAD_GAP_WIDEN_PROFILES, LEAD_DECEL_FOR_MAX_WIDEN, LEAD_CLOSING_FOR_MAX_WIDEN,
|
||||
)
|
||||
|
||||
|
||||
@@ -233,6 +233,39 @@ class TestLeadGapWiden(OpenpilotTestCase):
|
||||
multiplier = self.controller.get_t_follow_multiplier(lead_present=True, lead_accel=-LEAD_DECEL_FOR_MAX_WIDEN * 2, v_ego=20.0)
|
||||
self.assertAlmostEqual(multiplier, 1.0 + LEAD_GAP_WIDEN_PROFILES[AccelProfile.normal], places=2)
|
||||
|
||||
def test_closing_lead_widens_even_with_zero_lead_accel(self):
|
||||
# aLeadK is a laggy, differentiated estimate -- vRel (measured directly) must be able to
|
||||
# trigger the same widen on its own, before aLeadK has caught up to a real closing event.
|
||||
for _ in range(200):
|
||||
multiplier = self.controller.get_t_follow_multiplier(lead_present=True, lead_accel=0.0, v_ego=20.0,
|
||||
lead_v_rel=-LEAD_CLOSING_FOR_MAX_WIDEN * 2)
|
||||
self.assertAlmostEqual(multiplier, 1.0 + LEAD_GAP_WIDEN_PROFILES[AccelProfile.normal], places=2)
|
||||
|
||||
def test_opening_lead_v_rel_never_widens(self):
|
||||
for _ in range(50):
|
||||
multiplier = self.controller.get_t_follow_multiplier(lead_present=True, lead_accel=0.0, v_ego=20.0, lead_v_rel=3.0)
|
||||
self.assertEqual(multiplier, 1.0)
|
||||
|
||||
def test_widen_takes_whichever_signal_is_more_urgent(self):
|
||||
# a laggy aLeadK=0 (not yet updated) shouldn't suppress an already-strong closing-rate signal
|
||||
for _ in range(200):
|
||||
multiplier = self.controller.get_t_follow_multiplier(lead_present=True, lead_accel=0.0, v_ego=20.0,
|
||||
lead_v_rel=-LEAD_CLOSING_FOR_MAX_WIDEN * 2)
|
||||
from_v_rel_only = multiplier
|
||||
for _ in range(200):
|
||||
multiplier = self.controller.get_t_follow_multiplier(lead_present=True, lead_accel=-LEAD_DECEL_FOR_MAX_WIDEN * 2,
|
||||
v_ego=20.0, lead_v_rel=0.0)
|
||||
from_a_lead_only = multiplier
|
||||
self.assertAlmostEqual(from_v_rel_only, from_a_lead_only, places=2)
|
||||
|
||||
def test_lead_v_rel_default_matches_pre_existing_callers(self):
|
||||
# regression guard: existing call sites that don't pass lead_v_rel must be unaffected
|
||||
controller_a, controller_b = AccelController(), AccelController()
|
||||
for _ in range(50):
|
||||
a = controller_a.get_t_follow_multiplier(lead_present=True, lead_accel=-1.0, v_ego=20.0)
|
||||
b = controller_b.get_t_follow_multiplier(lead_present=True, lead_accel=-1.0, v_ego=20.0, lead_v_rel=0.0)
|
||||
self.assertEqual(a, b)
|
||||
|
||||
def test_widen_never_shrinks_below_stock(self):
|
||||
for lead_accel in [-0.5, -1.5, -3.0, -6.0, 0.5, 0.0]:
|
||||
multiplier = self.controller.get_t_follow_multiplier(lead_present=True, lead_accel=lead_accel, v_ego=20.0)
|
||||
|
||||
@@ -58,7 +58,7 @@ class LongitudinalPlannerSP:
|
||||
if not self.accel_controller.is_enabled():
|
||||
return None
|
||||
lead = sm['radarState'].leadOne
|
||||
return self.accel_controller.get_t_follow_multiplier(lead.present, lead.aLeadK, v_ego)
|
||||
return self.accel_controller.get_t_follow_multiplier(lead.present, lead.aLeadK, v_ego, lead.vRel)
|
||||
|
||||
def update_targets(self, sm: messaging.SubMaster, v_ego: float, a_ego: float, v_cruise: float) -> tuple[float, float]:
|
||||
CS = sm['carState']
|
||||
|
||||
@@ -1,12 +1,17 @@
|
||||
from collections import deque
|
||||
from collections.abc import Callable
|
||||
import math
|
||||
from typing import cast
|
||||
from unittest import mock
|
||||
|
||||
from openpilot.common.parameterized import parameterized
|
||||
from openpilot.common.params import Params
|
||||
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
|
||||
from openpilot.sunnypilot.selfdrive.controls.lib.accel_controller import accel_controller as accel_controller_module
|
||||
from openpilot.sunnypilot.selfdrive.controls.lib.accel_controller.accel_controller import AccelController, AccelProfile
|
||||
from openpilot.sunnypilot.selfdrive.test.longitudinal_maneuvers.plant import PRIUS_TSS2_ROUTE_MODEL, PlantSP
|
||||
|
||||
STOCK_STEP_KEYS = ("distance", "speed", "acceleration", "should_stop", "distance_lead", "fcw")
|
||||
|
||||
@@ -162,3 +167,146 @@ class TestPlantSP(OpenpilotTestCase):
|
||||
def test_invalid_actuator_dynamics(self, delay, lag):
|
||||
with self.assertRaises(ValueError):
|
||||
PlantSP(actuator_delay=delay, actuator_lag=lag)
|
||||
|
||||
def test_v_rel_widen_anticipates_lagged_a_lead_k_without_safety_regression(self):
|
||||
# Real radar's aLeadK is a filtered/differentiated estimate that lags the lead actually
|
||||
# closing (confirmed on a recorded route: vRel escalated ~1s before aLeadK caught up).
|
||||
# get_t_follow_multiplier's vRel trigger exists to widen t_follow before aLeadK updates --
|
||||
# this drives that scenario through the real closed-loop MPC while keeping dRel/vRel live.
|
||||
params = Params()
|
||||
params.put_bool("AccelPersonalityEnabled", True, block=True)
|
||||
params.put("AccelPersonality", AccelProfile.normal, block=True)
|
||||
|
||||
lead_accel_lag = 1.0
|
||||
brake_start = 3.0 # let the lead/MPC state settle for well over two seconds first
|
||||
brake_end = 4.4
|
||||
lever_window_end = 4.45 # isolate pre-aLeadK widening while both synthetic arms remain solver-valid
|
||||
full_event_end = 10.0 # include the complete brake and recovery response
|
||||
lag_steps = round(lead_accel_lag / DT_MDL)
|
||||
assert brake_start >= 2.0
|
||||
real_get_t_follow_multiplier = AccelController.get_t_follow_multiplier
|
||||
|
||||
def run(strip_v_rel_widen: bool, onset_alpha: float, end_time: float):
|
||||
# PlantSP publishes leadOne and leadTwo every frame. A shared history would advance
|
||||
# twice per frame and silently turn this intended 1.0 s lag into 0.5 s.
|
||||
histories = {lead_name: deque([0.0] * lag_steps, maxlen=lag_steps + 1)
|
||||
for lead_name in ("leadOne", "leadTwo")}
|
||||
lag_transitions = {lead_name: [] for lead_name in histories}
|
||||
|
||||
def lagged_a_lead(current_time, lead_name, truth):
|
||||
history = histories[lead_name]
|
||||
history.append(truth["aLeadK"])
|
||||
out = dict(truth)
|
||||
out["aLeadK"] = history[0]
|
||||
lag_transitions[lead_name].append((current_time, truth["aLeadK"], out["aLeadK"]))
|
||||
return out
|
||||
|
||||
plant = PlantSP(lead_relevancy=True, speed=14.0, distance_lead=30.0, e2e=False,
|
||||
lead_observation_fn=lagged_a_lead, actuator_model=PRIUS_TSS2_ROUTE_MODEL, run_long_control=True)
|
||||
v_lead = 14.0
|
||||
plant.v_lead_prev = v_lead # do not manufacture a 280 m/s^2 acceleration on the first frame
|
||||
|
||||
solver_failures = 0
|
||||
original_reset = plant.planner.mpc.reset
|
||||
|
||||
def counting_reset(*args, **kwargs):
|
||||
nonlocal solver_failures
|
||||
if plant.planner.mpc.solution_status != 0:
|
||||
solver_failures += 1
|
||||
return original_reset(*args, **kwargs)
|
||||
|
||||
plant.planner.mpc.reset = counting_reset
|
||||
|
||||
def patched_t_follow(self, lead_present, lead_accel, v_ego, lead_v_rel=0.0):
|
||||
return real_get_t_follow_multiplier(self, lead_present, lead_accel, v_ego,
|
||||
0.0 if strip_v_rel_widen else lead_v_rel)
|
||||
|
||||
trace = []
|
||||
with mock.patch.object(accel_controller_module, "GAP_WIDEN_ONSET_ALPHA", onset_alpha), \
|
||||
mock.patch.object(AccelController, "get_t_follow_multiplier", patched_t_follow):
|
||||
for _ in range(round(end_time / DT_MDL)):
|
||||
current_time = plant.current_time
|
||||
a_lead_cmd = -2.5 if brake_start <= current_time < brake_end else 0.0
|
||||
v_lead = max(0.0, v_lead + a_lead_cmd * plant.ts)
|
||||
result = plant.step(v_lead=v_lead, v_cruise=22.0)
|
||||
truth_lead = result["truth_lead"]
|
||||
ttc = truth_lead["dRel"] / max(-truth_lead["vRel"], 1e-3) if truth_lead["vRel"] < 0.0 else None
|
||||
trace.append({
|
||||
"time": current_time,
|
||||
"realized_acceleration": result["realized_acceleration"],
|
||||
"target_acceleration": result["a_target"],
|
||||
"gap": result["distance_lead"] - result["distance"],
|
||||
"ttc": ttc,
|
||||
"fcw": result["fcw"],
|
||||
"t_follow": result["t_follow_multiplier"],
|
||||
})
|
||||
|
||||
event_index = round(brake_start / DT_MDL)
|
||||
pre_event_accel = trace[event_index - 1]["realized_acceleration"]
|
||||
onset_time = next(row["time"] for row in trace[event_index:]
|
||||
if row["realized_acceleration"] <= pre_event_accel - 0.1)
|
||||
measurement = trace[event_index - 1:]
|
||||
realized_jerks = [(after["realized_acceleration"] - before["realized_acceleration"]) / DT_MDL
|
||||
for before, after in zip(measurement, measurement[1:], strict=False)]
|
||||
target_jerks = [(after["target_acceleration"] - before["target_acceleration"]) / DT_MDL
|
||||
for before, after in zip(measurement, measurement[1:], strict=False)]
|
||||
|
||||
measured_lags = {}
|
||||
for lead_name, transitions in lag_transitions.items():
|
||||
true_onset = next(t for t, true_a, _ in transitions if true_a < -0.1)
|
||||
observed_onset = next(t for t, _, observed_a in transitions if observed_a < -0.1)
|
||||
measured_lags[lead_name] = observed_onset - true_onset
|
||||
|
||||
return {
|
||||
"solver_failures": solver_failures,
|
||||
"lag_s": measured_lags,
|
||||
"onset_s": onset_time,
|
||||
"worst_negative_realized_jerk_mps3": min(realized_jerks),
|
||||
"worst_negative_target_jerk_mps3": min(target_jerks),
|
||||
"worst_positive_realized_jerk_mps3": max(realized_jerks),
|
||||
"peak_realized_decel_mps2": min(row["realized_acceleration"] for row in measurement),
|
||||
"peak_target_decel_mps2": min(row["target_acceleration"] for row in measurement),
|
||||
"min_gap_m": min(row["gap"] for row in trace),
|
||||
"end_gap_m": trace[-1]["gap"],
|
||||
"min_ttc_s": min(row["ttc"] for row in trace if row["ttc"] is not None),
|
||||
"max_t_follow_pre_lag": max(row["t_follow"] for row in trace
|
||||
if brake_start <= row["time"] < brake_start + lead_accel_lag),
|
||||
"fcw": any(row["fcw"] for row in trace),
|
||||
}
|
||||
|
||||
production_alpha = accel_controller_module.GAP_WIDEN_ONSET_ALPHA
|
||||
reference_alpha = 0.15
|
||||
# Only isolate the controller's vRel-based t-follow widening in the short comparison.
|
||||
# The real MPC still receives the same live radar vRel in every arm.
|
||||
without_v_rel_widen = run(strip_v_rel_widen=True, onset_alpha=production_alpha, end_time=lever_window_end)
|
||||
reference_onset = run(strip_v_rel_widen=False, onset_alpha=reference_alpha, end_time=full_event_end)
|
||||
production = run(strip_v_rel_widen=False, onset_alpha=production_alpha, end_time=full_event_end)
|
||||
diagnostics = f"without_v_rel_widen={without_v_rel_widen}, reference_onset={reference_onset}, production={production}"
|
||||
|
||||
for metrics in (without_v_rel_widen, reference_onset, production):
|
||||
assert metrics["solver_failures"] == 0, diagnostics
|
||||
assert not metrics["fcw"], diagnostics
|
||||
assert metrics["min_gap_m"] > 29.0, diagnostics
|
||||
assert metrics["min_ttc_s"] > 15.0, diagnostics
|
||||
for measured_lag in metrics["lag_s"].values():
|
||||
self.assertAlmostEqual(measured_lag, lead_accel_lag, delta=DT_MDL / 2.0, msg=diagnostics)
|
||||
|
||||
self.assertAlmostEqual(without_v_rel_widen["max_t_follow_pre_lag"], 1.0, places=6, msg=diagnostics)
|
||||
self.assertGreater(production["max_t_follow_pre_lag"], 1.015, diagnostics)
|
||||
self.assertLessEqual(production["onset_s"], without_v_rel_widen["onset_s"] + DT_MDL, diagnostics)
|
||||
|
||||
# The gentler onset filter must improve braking jerk over the previous 0.15 value
|
||||
# without delaying the response or materially reducing separation.
|
||||
self.assertLessEqual(production["onset_s"], reference_onset["onset_s"] + DT_MDL, diagnostics)
|
||||
self.assertGreater(production["worst_negative_realized_jerk_mps3"],
|
||||
reference_onset["worst_negative_realized_jerk_mps3"], diagnostics)
|
||||
self.assertGreater(production["worst_negative_target_jerk_mps3"],
|
||||
reference_onset["worst_negative_target_jerk_mps3"], diagnostics)
|
||||
self.assertLessEqual(production["worst_positive_realized_jerk_mps3"],
|
||||
reference_onset["worst_positive_realized_jerk_mps3"] + 0.1, diagnostics)
|
||||
self.assertGreaterEqual(production["min_ttc_s"], reference_onset["min_ttc_s"] - 0.1, diagnostics)
|
||||
self.assertGreaterEqual(production["end_gap_m"], reference_onset["end_gap_m"] - 0.5, diagnostics)
|
||||
self.assertGreaterEqual(production["peak_realized_decel_mps2"],
|
||||
reference_onset["peak_realized_decel_mps2"], diagnostics)
|
||||
self.assertGreaterEqual(production["peak_target_decel_mps2"],
|
||||
reference_onset["peak_target_decel_mps2"] - 0.1, diagnostics)
|
||||
|
||||
Reference in New Issue
Block a user