mirror of
https://github.com/MoreTore/openpilot.git
synced 2026-08-06 00:36:32 +08:00
Dom's Plan
This commit is contained in:
firestar5683
@@ -71,6 +71,13 @@ DIST_ADAPTS = [0.04, 0.06, 0.06, 0.05] # Balanced across speeds
|
||||
|
||||
# ===== END TUNING PARAMETERS =====
|
||||
|
||||
FAR_RADAR_LEAD_ACCEL_TAPER_MAX = 1.0
|
||||
FAR_RADAR_LEAD_ACCEL_TAPER_MAX_CLOSING = 2.5
|
||||
FAR_RADAR_LEAD_ACCEL_TAPER_MIN_GAP_EXCESS = 8.0
|
||||
FAR_RADAR_LEAD_ACCEL_TAPER_MIN_GAP_GAIN = 0.25
|
||||
FAR_RADAR_LEAD_ACCEL_TAPER_FULL_GAP_EXCESS = 25.0
|
||||
FAR_RADAR_LEAD_ACCEL_TAPER_FULL_GAP_GAIN = 0.9
|
||||
|
||||
# Function to get parameter value based on current speed
|
||||
def get_speed_based_param(speed_mph, param_array):
|
||||
"""Get parameter value based on current speed using smooth interpolation"""
|
||||
@@ -167,6 +174,28 @@ def desired_follow_distance(v_ego, v_lead, t_follow=None):
|
||||
return get_safe_obstacle_distance(v_ego, t_follow) - get_stopped_equivalence_factor(v_lead)
|
||||
|
||||
|
||||
def soften_far_radar_lead_accel(d_rel, v_lead, a_lead, v_ego, t_follow, *, radar=True):
|
||||
if not radar or a_lead >= 0.0:
|
||||
return float(a_lead)
|
||||
|
||||
desired_gap = float(desired_follow_distance(v_ego, v_lead, t_follow))
|
||||
closing_speed = max(0.0, float(v_ego) - float(v_lead))
|
||||
gap_excess = float(d_rel) - desired_gap
|
||||
|
||||
taper_start = max(FAR_RADAR_LEAD_ACCEL_TAPER_MIN_GAP_EXCESS,
|
||||
FAR_RADAR_LEAD_ACCEL_TAPER_MIN_GAP_GAIN * float(v_ego))
|
||||
if gap_excess <= taper_start or closing_speed >= FAR_RADAR_LEAD_ACCEL_TAPER_MAX_CLOSING:
|
||||
return float(a_lead)
|
||||
|
||||
taper_scale = max(FAR_RADAR_LEAD_ACCEL_TAPER_FULL_GAP_EXCESS,
|
||||
FAR_RADAR_LEAD_ACCEL_TAPER_FULL_GAP_GAIN * float(v_ego))
|
||||
distance_factor = float(np.clip((gap_excess - taper_start) / taper_scale, 0.0, 1.0))
|
||||
closing_factor = float(np.clip((FAR_RADAR_LEAD_ACCEL_TAPER_MAX_CLOSING - closing_speed) /
|
||||
FAR_RADAR_LEAD_ACCEL_TAPER_MAX_CLOSING, 0.0, 1.0))
|
||||
taper = FAR_RADAR_LEAD_ACCEL_TAPER_MAX * distance_factor * closing_factor
|
||||
return float(a_lead * (1.0 - taper))
|
||||
|
||||
|
||||
def gen_long_model():
|
||||
model = AcadosModel()
|
||||
model.name = MODEL_NAME
|
||||
@@ -484,13 +513,16 @@ class LongitudinalMpc:
|
||||
lead_xv = np.column_stack((x_lead_traj, v_lead_traj))
|
||||
return lead_xv
|
||||
|
||||
def process_lead(self, lead, tracking_lead=True):
|
||||
def process_lead(self, lead, tracking_lead=True, t_follow=None):
|
||||
v_ego = self.x0[1]
|
||||
if lead is not None and lead.status and tracking_lead:
|
||||
x_lead = lead.dRel
|
||||
v_lead = lead.vLead
|
||||
a_lead = lead.aLeadK
|
||||
a_lead_tau = lead.aLeadTau
|
||||
a_lead = soften_far_radar_lead_accel(x_lead, v_lead, a_lead, v_ego,
|
||||
get_T_FOLLOW() if t_follow is None else t_follow,
|
||||
radar=bool(getattr(lead, "radar", False)))
|
||||
else:
|
||||
# Fake a fast lead car, so mpc can keep running in the same mode
|
||||
x_lead = 50.0
|
||||
@@ -525,8 +557,8 @@ class LongitudinalMpc:
|
||||
lead_two = radarstate.leadTwo
|
||||
self.status = tracking_lead and (lead_one.status or lead_two.status)
|
||||
|
||||
lead_xv_0 = self.process_lead(lead_one, tracking_lead)
|
||||
lead_xv_1 = self.process_lead(lead_two, tracking_lead)
|
||||
lead_xv_0 = self.process_lead(lead_one, tracking_lead, t_follow=t_follow)
|
||||
lead_xv_1 = self.process_lead(lead_two, tracking_lead, t_follow=t_follow)
|
||||
|
||||
# To estimate a safe distance from a moving lead, we calculate how much stopping
|
||||
# distance that lead needs as a minimum. We can add that to the current distance
|
||||
|
||||
@@ -29,6 +29,7 @@ MIN_ALLOW_THROTTLE_SPEED = 2.5
|
||||
RAW_LEAD_SAFETY_MIN_CLOSING_SPEED = 0.5
|
||||
RAW_LEAD_SAFETY_TTC = 7.0
|
||||
RAW_LEAD_SAFETY_DISTANCE = 40.0
|
||||
CLOSE_LEAD_BRAKE_CAP_MAX_TTC = 25.0
|
||||
|
||||
# Uncertainty-based filter disable thresholds
|
||||
UNCERT_SLOPE_TRIG = 0.12 # per second
|
||||
@@ -238,6 +239,9 @@ class LongitudinalPlanner:
|
||||
target_gap = float(np.clip(2.0 + 0.2 * v_ego, 2.0, 6.0))
|
||||
delay_buffer = projected_closing_speed * reaction_t
|
||||
available_gap = max(float(lead.dRel) - target_gap - delay_buffer, 0.5)
|
||||
projected_ttc = available_gap / max(projected_closing_speed, 0.1)
|
||||
if projected_ttc > CLOSE_LEAD_BRAKE_CAP_MAX_TTC:
|
||||
return None
|
||||
required_decel = (projected_closing_speed ** 2) / (2.0 * available_gap) + 0.7 * lead_brake
|
||||
if required_decel < 0.2:
|
||||
return None
|
||||
|
||||
@@ -10,6 +10,7 @@ from opendbc.car.honda.interface import CarInterface
|
||||
from opendbc.car.honda.values import CAR
|
||||
from openpilot.selfdrive.controls.lib.longcontrol import LongCtrlState
|
||||
from openpilot.selfdrive.controls.lib.longitudinal_planner import LongitudinalPlanner, get_vehicle_min_accel
|
||||
from openpilot.selfdrive.controls.lib.longitudinal_mpc_lib.long_mpc import soften_far_radar_lead_accel
|
||||
from openpilot.selfdrive.modeld.constants import ModelConstants, Plan
|
||||
|
||||
|
||||
@@ -188,6 +189,58 @@ def test_acc_mode_matches_no_lead_baseline_for_far_vision_only_lead_without_trac
|
||||
np.testing.assert_allclose(far_vision_outputs, no_lead_outputs, atol=1e-6)
|
||||
|
||||
|
||||
def test_soften_far_radar_lead_accel_reduces_gentle_far_brake():
|
||||
softened = soften_far_radar_lead_accel(114.8, 28.88, -0.75, 29.26, 1.45, radar=True)
|
||||
assert softened > -0.35
|
||||
assert softened < 0.0
|
||||
|
||||
|
||||
def test_soften_far_radar_lead_accel_keeps_close_closing_brake():
|
||||
baseline = -0.76
|
||||
softened = soften_far_radar_lead_accel(68.0, 26.38, baseline, 29.38, 1.45, radar=True)
|
||||
assert softened == pytest.approx(baseline)
|
||||
|
||||
|
||||
@pytest.mark.parametrize("model_version", ["v11", "v12"])
|
||||
def test_acc_mode_damps_far_radar_mild_lead_brake_more_than_close_brake(model_version):
|
||||
far_v_ego = 29.26
|
||||
far_v_cruise = 32.22
|
||||
close_v_ego = 29.38
|
||||
close_v_cruise = 32.22
|
||||
|
||||
CP = CarInterface.get_non_essential_params(CAR.HONDA_CIVIC)
|
||||
planner_far = LongitudinalPlanner(CP, init_v=far_v_ego)
|
||||
planner_close = LongitudinalPlanner(CP, init_v=close_v_ego)
|
||||
|
||||
sm_far = make_sm(
|
||||
far_v_ego,
|
||||
desired_accel=0.2,
|
||||
min_accel=-1.0,
|
||||
experimental_mode=False,
|
||||
tracking_lead=True,
|
||||
lead_one=make_lead(status=True, d_rel=114.8, v_lead=28.88, a_lead=-0.75, radar=True, model_prob=0.9),
|
||||
)
|
||||
sm_close = make_sm(
|
||||
close_v_ego,
|
||||
desired_accel=0.2,
|
||||
min_accel=-1.0,
|
||||
experimental_mode=False,
|
||||
tracking_lead=True,
|
||||
lead_one=make_lead(status=True, d_rel=68.0, v_lead=26.38, a_lead=-0.76, radar=True, model_prob=0.9),
|
||||
)
|
||||
sm_far["starpilotPlan"].vCruise = far_v_cruise
|
||||
sm_close["starpilotPlan"].vCruise = close_v_cruise
|
||||
|
||||
for _ in range(80):
|
||||
planner_far.update(sm_far, make_toggles(model_version))
|
||||
planner_close.update(sm_close, make_toggles(model_version))
|
||||
|
||||
assert planner_far.mode == "acc"
|
||||
assert planner_close.mode == "acc"
|
||||
assert planner_far.output_a_target > -0.4
|
||||
assert planner_close.output_a_target < planner_far.output_a_target - 0.1
|
||||
|
||||
|
||||
def test_modeld_action_passes_tomb_raider_longitudinal_params(monkeypatch):
|
||||
monkeypatch.setenv("DEBUG", "0")
|
||||
fake_commonmodel = types.ModuleType("openpilot.selfdrive.modeld.models.commonmodel_pyx")
|
||||
|
||||
Reference in New Issue
Block a user