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

all the small things

Browse Source
This commit is contained in:
firestar5683
2026-05-09 08:54:04 -05:00
parent 1b2b13881c
commit 1714c516ab
3 changed files with 96 additions and 19 deletions
Download Patch File Download Diff File Expand all files Collapse all files
selfdrive/controls/lib/latcontrol_torque.py
+14 -14
View File
@@ -277,22 +277,22 @@ IONIQ_6_FF_CUTOFF = 0.48
IONIQ_6_FF_CUTOFF_WIDTH = 0.12
IONIQ_6_TRANSITION_SPEED = 10.0
IONIQ_6_PHASE_SCALE = 0.10
IONIQ_6_TURN_IN_BOOST_LEFT = 1.44
IONIQ_6_TURN_IN_BOOST_RIGHT = 1.56
IONIQ_6_UNWIND_TAPER_LEFT = 2.46
IONIQ_6_UNWIND_TAPER_RIGHT = 5.60
IONIQ_6_FRICTION_MULT = 0.955
IONIQ_6_TURN_IN_BOOST_LEFT = 1.50
IONIQ_6_TURN_IN_BOOST_RIGHT = 1.70
IONIQ_6_UNWIND_TAPER_LEFT = 2.62
IONIQ_6_UNWIND_TAPER_RIGHT = 6.05
IONIQ_6_FRICTION_MULT = 0.948
IONIQ_6_FRICTION_LAT_RISE = 0.20
IONIQ_6_FRICTION_JERK_RISE = 0.24
IONIQ_6_TURN_IN_THRESHOLD_REDUCTION_LEFT = 0.60
IONIQ_6_TURN_IN_THRESHOLD_REDUCTION_RIGHT = 0.90
IONIQ_6_UNWIND_THRESHOLD_INCREASE_LEFT = 2.90
IONIQ_6_UNWIND_THRESHOLD_INCREASE_RIGHT = 6.95
IONIQ_6_TURN_IN_FRICTION_BOOST_LEFT = 0.32
IONIQ_6_TURN_IN_FRICTION_BOOST_RIGHT = 0.55
IONIQ_6_UNWIND_FRICTION_REDUCTION_LEFT = 2.65
IONIQ_6_UNWIND_FRICTION_REDUCTION_RIGHT = 6.35
IONIQ_6_CENTER_TAPER_MAX = 0.066
IONIQ_6_TURN_IN_THRESHOLD_REDUCTION_LEFT = 0.66
IONIQ_6_TURN_IN_THRESHOLD_REDUCTION_RIGHT = 1.05
IONIQ_6_UNWIND_THRESHOLD_INCREASE_LEFT = 3.10
IONIQ_6_UNWIND_THRESHOLD_INCREASE_RIGHT = 7.40
IONIQ_6_TURN_IN_FRICTION_BOOST_LEFT = 0.36
IONIQ_6_TURN_IN_FRICTION_BOOST_RIGHT = 0.64
IONIQ_6_UNWIND_FRICTION_REDUCTION_LEFT = 2.85
IONIQ_6_UNWIND_FRICTION_REDUCTION_RIGHT = 6.90
IONIQ_6_CENTER_TAPER_MAX = 0.070
IONIQ_6_CENTER_TAPER_LAT = 0.215
IONIQ_6_CENTER_TAPER_LAT_WIDTH = 0.02
IONIQ_6_CENTER_TAPER_SPEED = 18.0
selfdrive/controls/lib/longitudinal_planner.py
+39 -2
View File
@@ -49,6 +49,12 @@ VISION_LEAD_APPROACH_BRAKING_MIN_LEAD_BRAKE = 0.45
VISION_LEAD_APPROACH_BRAKING_FULL_LEAD_BRAKE = 1.20
VISION_LEAD_APPROACH_BRAKING_FLOOR_MIN_DECEL = 1.30
VISION_LEAD_APPROACH_BRAKING_FLOOR_MAX_DECEL = 1.75
VISION_LEAD_APPROACH_CONFIRM_TIME = 0.25
VISION_LEAD_APPROACH_CONFIRM_BYPASS_DECEL = 1.0
VISION_LEAD_APPROACH_CONFIRM_BYPASS_CLOSING_SPEED = 4.0
VISION_LEAD_APPROACH_CONFIRM_BYPASS_LEAD_BRAKE = 0.20
VISION_LEAD_APPROACH_CONFIRM_BYPASS_DISTANCE_MIN = 28.0
VISION_LEAD_APPROACH_CONFIRM_BYPASS_DISTANCE_TIME = 0.85
VISION_UNTRACKED_SLOW_LEAD_MIN_MODEL_PROB = 0.9
VISION_UNTRACKED_SLOW_LEAD_FULL_MODEL_PROB = 0.97
VISION_UNTRACKED_SLOW_LEAD_MIN_CLOSING_SPEED = 3.0
@@ -282,6 +288,7 @@ class LongitudinalPlanner:
self._uncert_last_t = None
self.effective_t_follow = None
self.vision_low_speed_stop_hold_until = 0.0
self.vision_lead_approach_confirm_t = 0.0
self.untracked_slow_lead_confirm_t = 0.0
if self.is_preap:
@@ -519,6 +526,19 @@ class LongitudinalPlanner:
return max(accel_min, -approach_decel)
def tracked_vision_lead_approach_needs_immediate_brake(self, lead, v_ego, approach_cap):
lead_brake = max(0.0, -float(getattr(lead, "aLeadK", 0.0)))
reaction_t = max(self.CP.longitudinalActuatorDelay, self.dt)
projected_closing_speed = max(0.0, v_ego - float(lead.vLead)) + lead_brake * reaction_t
bypass_distance = max(VISION_LEAD_APPROACH_CONFIRM_BYPASS_DISTANCE_MIN,
VISION_LEAD_APPROACH_CONFIRM_BYPASS_DISTANCE_TIME * float(v_ego))
return (
approach_cap <= -VISION_LEAD_APPROACH_CONFIRM_BYPASS_DECEL or
projected_closing_speed >= VISION_LEAD_APPROACH_CONFIRM_BYPASS_CLOSING_SPEED or
lead_brake >= VISION_LEAD_APPROACH_CONFIRM_BYPASS_LEAD_BRAKE or
float(lead.dRel) <= bypass_distance
)
def get_dynamic_t_follow(self, base_t_follow, lead, v_ego):
base_t_follow = float(base_t_follow)
target_t_follow = base_t_follow
@@ -956,6 +976,7 @@ class LongitudinalPlanner:
self.untracked_slow_lead_confirm_t = 0.0
close_lead_caps = []
tracked_vision_approach_caps = []
vision_low_speed_stop_active = False
vision_brake_cap_active = False
if lead_control_active:
@@ -969,13 +990,29 @@ class LongitudinalPlanner:
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)
vision_brake_cap_active = True
tracked_vision_approach_caps.append((
approach_cap,
self.tracked_vision_lead_approach_needs_immediate_brake(lead, v_ego, approach_cap),
))
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 tracked_vision_approach_caps:
if any(immediate for _, immediate in tracked_vision_approach_caps):
self.vision_lead_approach_confirm_t = VISION_LEAD_APPROACH_CONFIRM_TIME
else:
self.vision_lead_approach_confirm_t = min(
self.vision_lead_approach_confirm_t + self.dt,
VISION_LEAD_APPROACH_CONFIRM_TIME,
)
if self.vision_lead_approach_confirm_t >= VISION_LEAD_APPROACH_CONFIRM_TIME:
close_lead_caps.append(min(cap for cap, _ in tracked_vision_approach_caps))
vision_brake_cap_active = True
else:
self.vision_lead_approach_confirm_t = 0.0
if close_lead_caps:
close_lead_brake_cap = min(close_lead_caps)
self.a_desired = min(self.a_desired, close_lead_brake_cap)
selfdrive/controls/tests/test_longitudinal_planner.py
+43 -3
View File
@@ -430,13 +430,53 @@ def test_acc_mode_vision_lead_approach_cap_smooths_before_close_brake(model_vers
sm_approach["starpilotPlan"].vCruise = approach_v_ego + 8.0
sm_close["starpilotPlan"].vCruise = close_v_ego + 8.0
planner_approach.update(sm_approach, make_toggles(model_version))
approach_outputs = []
for _ in range(6):
planner_approach.update(sm_approach, make_toggles(model_version))
approach_outputs.append(planner_approach.output_a_target)
planner_close.update(sm_close, make_toggles(model_version))
assert planner_approach.mode == "acc"
assert planner_close.mode == "acc"
assert planner_approach.output_a_target < -0.6
assert planner_close.output_a_target < planner_approach.output_a_target - 0.25
assert min(approach_outputs[:2]) > -0.55
assert approach_outputs[-1] < -1.3
assert planner_close.output_a_target < approach_outputs[0] - 0.8
@pytest.mark.parametrize("model_version", ["v11", "v12", "v13", "v14"])
def test_tracked_vision_far_mild_closure_does_not_bypass_persistence(model_version):
v_ego = 37.45
CP = CarInterface.get_non_essential_params(CAR.HONDA_CIVIC)
planner = LongitudinalPlanner(CP, init_v=v_ego)
lead = make_lead(status=True, d_rel=42.8, v_lead=35.31, a_lead=0.18, radar=False, model_prob=0.98)
approach_cap = planner.get_vision_lead_approach_cap(lead, v_ego, -1.0, 1.45)
assert approach_cap is not None
assert approach_cap > -1.0
assert not planner.tracked_vision_lead_approach_needs_immediate_brake(lead, v_ego, approach_cap)
@pytest.mark.parametrize("model_version", ["v11", "v12", "v13", "v14"])
def test_acc_mode_tracked_vision_close_or_braking_lead_bypasses_persistence(model_version):
v_ego = 19.50
CP = CarInterface.get_non_essential_params(CAR.HONDA_CIVIC)
planner = LongitudinalPlanner(CP, init_v=v_ego)
sm = make_sm(
v_ego,
desired_accel=0.2,
min_accel=-1.0,
experimental_mode=False,
tracking_lead=True,
lead_one=make_lead(status=True, d_rel=19.7, v_lead=16.25, a_lead=-0.83, radar=False, model_prob=0.98),
)
sm["starpilotPlan"].vCruise = v_ego + 6.0
planner.update(sm, make_toggles(model_version))
assert planner.output_a_target < -1.3
@pytest.mark.parametrize("model_version", ["v11", "v12", "v13", "v14"])