mawei/StarPilot
1
0
Fork 0
mirror of https://github.com/firestar5683/StarPilot.git synced 2026-07-03 04:22:09 +08:00
Code Issues Packages Projects Releases Wiki Activity

Doms plan small update

Browse Source
This commit is contained in:
firestar5683
2026-04-29 15:38:45 -05:00
parent 3e82e773a9
commit f9d6c0d648
2 changed files with 23 additions and 14 deletions
Download Patch File Download Diff File Expand all files Collapse all files
selfdrive/controls/lib/longitudinal_planner.py
+19 -10
View File
@@ -35,11 +35,11 @@ VISION_LEAD_APPROACH_MIN_CLOSING_SPEED = 2.0
VISION_LEAD_APPROACH_TRIGGER_TIME = 4.5
VISION_LEAD_APPROACH_FULL_TIME = 1.0
VISION_LEAD_APPROACH_TIGHT_BUFFER = 2.0
VISION_LEAD_APPROACH_MAX_DECEL = 0.65
VISION_LEAD_APPROACH_MAX_DECEL = 0.80
VISION_LEAD_APPROACH_MIN_DECEL = 0.15
VISION_LEAD_APPROACH_MIN_MODEL_PROB = 0.85
VISION_LEAD_APPROACH_FULL_MODEL_PROB = 0.98
VISION_LEAD_APPROACH_DEFICIT_MAX_DECEL = 1.15
VISION_LEAD_APPROACH_DEFICIT_MAX_DECEL = 1.30
VISION_LEAD_APPROACH_DEFICIT_BUFFER_MIN = 3.0
VISION_LEAD_APPROACH_DEFICIT_BUFFER_GAIN = 0.20
VISION_SLOW_LEAD_MAX_SPEED = 5.0
@@ -68,12 +68,12 @@ VISION_LOW_SPEED_STOP_BUFFER_MAX_EGO_SPEED = 5.5
VISION_LOW_SPEED_STOP_BUFFER_MAX_LEAD_SPEED = 1.75
VISION_LOW_SPEED_STOP_BUFFER_MIN_MODEL_PROB = 0.9
VISION_LOW_SPEED_STOP_BUFFER_MIN_CLOSING_SPEED = 0.35
VISION_LOW_SPEED_STOP_BUFFER_BASE = 2.8
VISION_LOW_SPEED_STOP_BUFFER_BASE = 3.2
VISION_LOW_SPEED_STOP_BUFFER_EGO_GAIN = 0.80
VISION_LOW_SPEED_STOP_BUFFER_LEAD_GAIN = 0.25
VISION_LOW_SPEED_STOP_BUFFER_RELEASE_MARGIN = 0.75
VISION_LOW_SPEED_STOP_BUFFER_HOLD_TIME = 0.6
VISION_LOW_SPEED_STOP_BUFFER_MIN_BRAKE = 0.9
VISION_LOW_SPEED_STOP_BUFFER_RELEASE_MARGIN = 0.9
VISION_LOW_SPEED_STOP_BUFFER_HOLD_TIME = 0.8
VISION_LOW_SPEED_STOP_BUFFER_MIN_BRAKE = 1.1
VISION_LOW_SPEED_STOP_BUFFER_BRAKE_GAIN = 0.25
# Uncertainty-based filter disable thresholds
@@ -736,24 +736,30 @@ class LongitudinalPlanner:
self.v_desired_trajectory, self.a_desired_trajectory,
action_t=action_t, vEgoStopping=starpilot_toggles.vEgoStopping)
output_accel_min = get_vehicle_min_accel(self.CP, v_ego) if experimental_mlsim else accel_limits_turns[0]
comfort_output_accel_min = get_vehicle_min_accel(self.CP, v_ego) if experimental_mlsim else accel_limits_turns[0]
vision_cap_accel_min = min(comfort_output_accel_min, get_vehicle_min_accel(self.CP, v_ego))
output_accel_min = comfort_output_accel_min
close_lead_caps = []
vision_low_speed_stop_active = False
vision_brake_cap_active = False
if lead_control_active:
for lead in (self.lead_one, self.lead_two):
cap = self.get_close_lead_brake_cap(lead, v_ego, output_accel_min)
if cap is not None:
close_lead_caps.append(cap)
slow_stop_cap = self.get_vision_slow_stopped_lead_cap(lead, v_ego, output_accel_min, effective_t_follow)
slow_stop_cap = self.get_vision_slow_stopped_lead_cap(lead, v_ego, vision_cap_accel_min, effective_t_follow)
if slow_stop_cap is not None:
close_lead_caps.append(slow_stop_cap)
approach_cap = self.get_vision_lead_approach_cap(lead, v_ego, output_accel_min, effective_t_follow)
vision_brake_cap_active = True
approach_cap = self.get_vision_lead_approach_cap(lead, v_ego, vision_cap_accel_min, effective_t_follow)
if approach_cap is not None:
close_lead_caps.append(approach_cap)
low_speed_stop_cap, low_speed_stop_active = self.get_vision_low_speed_stop_buffer_cap(lead, v_ego, output_accel_min)
vision_brake_cap_active = True
low_speed_stop_cap, low_speed_stop_active = self.get_vision_low_speed_stop_buffer_cap(lead, v_ego, vision_cap_accel_min)
if low_speed_stop_cap is not None:
close_lead_caps.append(low_speed_stop_cap)
vision_brake_cap_active = True
vision_low_speed_stop_active |= low_speed_stop_active
if close_lead_caps:
close_lead_brake_cap = min(close_lead_caps)
@@ -772,6 +778,9 @@ class LongitudinalPlanner:
cruise_accel_cap = (v_cruise - v_ego + 0.01) / max(action_t, self.dt)
output_a_target = min(output_a_target, cruise_accel_cap)
if vision_brake_cap_active:
output_accel_min = min(output_accel_min, vision_cap_accel_min)
output_accel_max = no_throttle_output_max if not self.allow_throttle else accel_limits_turns[1]
output_a_target = float(np.clip(output_a_target, output_accel_min, output_accel_max))
selfdrive/controls/tests/test_longitudinal_planner.py
+4 -4
View File
@@ -345,7 +345,7 @@ def test_acc_mode_vision_lead_approach_cap_smooths_before_close_brake(model_vers
sm_approach = make_sm(
approach_v_ego,
desired_accel=0.2,
min_accel=-3.0,
min_accel=-0.5,
experimental_mode=False,
tracking_lead=True,
lead_one=make_lead(status=True, d_rel=38.9, v_lead=18.04, a_lead=-0.026, radar=False, model_prob=0.984),
@@ -353,7 +353,7 @@ def test_acc_mode_vision_lead_approach_cap_smooths_before_close_brake(model_vers
sm_close = make_sm(
close_v_ego,
desired_accel=0.2,
min_accel=-3.0,
min_accel=-0.5,
experimental_mode=False,
tracking_lead=True,
lead_one=make_lead(status=True, d_rel=27.18, v_lead=15.76, a_lead=-0.824, radar=False, model_prob=0.988),
@@ -366,7 +366,7 @@ def test_acc_mode_vision_lead_approach_cap_smooths_before_close_brake(model_vers
assert planner_approach.mode == "acc"
assert planner_close.mode == "acc"
assert planner_approach.output_a_target < -0.5
assert planner_approach.output_a_target < -0.6
assert planner_close.output_a_target < planner_approach.output_a_target - 0.25
@@ -379,7 +379,7 @@ def test_acc_mode_low_speed_vision_stop_buffer_sets_should_stop_before_tiny_gap(
sm = make_sm(
v_ego,
desired_accel=0.1,
min_accel=-3.0,
min_accel=-0.5,
experimental_mode=False,
tracking_lead=True,
lead_one=make_lead(status=True, d_rel=5.75, v_lead=0.58, a_lead=-0.1, radar=False, model_prob=0.99),