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

tůne

Browse Source
This commit is contained in:
firestar5683
2026-05-06 10:38:47 -05:00
parent 8ec27d25c5
commit 49aad9cce3
3 changed files with 100 additions and 12 deletions
Download Patch File Download Diff File Expand all files Collapse all files
selfdrive/controls/lib/latcontrol_torque.py
+10 -10
View File
@@ -70,20 +70,20 @@ CIVIC_BOSCH_MODIFIED_A_VARIANT_FF_RESTORE_LEFT = 0.00
CIVIC_BOSCH_MODIFIED_A_VARIANT_FF_RESTORE_RIGHT = 0.00
CIVIC_BOSCH_MODIFIED_A_VARIANT_TURN_IN_BOOST_LEFT = 0.00
CIVIC_BOSCH_MODIFIED_A_VARIANT_TURN_IN_BOOST_RIGHT = 0.00
CIVIC_BOSCH_MODIFIED_A_VARIANT_UNWIND_TAPER_LEFT = 0.06
CIVIC_BOSCH_MODIFIED_A_VARIANT_UNWIND_TAPER_RIGHT = 0.14
CIVIC_BOSCH_MODIFIED_A_VARIANT_UNWIND_TAPER_LEFT = 0.04
CIVIC_BOSCH_MODIFIED_A_VARIANT_UNWIND_TAPER_RIGHT = 0.10
CIVIC_BOSCH_MODIFIED_A_VARIANT_TURN_IN_FRICTION_BOOST_LEFT = 0.00
CIVIC_BOSCH_MODIFIED_A_VARIANT_TURN_IN_FRICTION_BOOST_RIGHT = 0.00
CIVIC_BOSCH_MODIFIED_A_VARIANT_UNWIND_FRICTION_REDUCTION_LEFT = 0.04
CIVIC_BOSCH_MODIFIED_A_VARIANT_UNWIND_FRICTION_REDUCTION_RIGHT = 0.10
CIVIC_BOSCH_MODIFIED_A_VARIANT_UNWIND_FRICTION_REDUCTION_LEFT = 0.03
CIVIC_BOSCH_MODIFIED_A_VARIANT_UNWIND_FRICTION_REDUCTION_RIGHT = 0.07
CIVIC_BOSCH_MODIFIED_B_VARIANT_FF_REDUCTION_LEFT = 0.17
CIVIC_BOSCH_MODIFIED_B_VARIANT_FF_REDUCTION_RIGHT = 0.25
CIVIC_BOSCH_MODIFIED_B_VARIANT_TURN_IN_BOOST_LEFT = 0.06
CIVIC_BOSCH_MODIFIED_B_VARIANT_TURN_IN_BOOST_RIGHT = 0.05
CIVIC_BOSCH_MODIFIED_B_VARIANT_TURN_IN_BOOST_LEFT = 0.08
CIVIC_BOSCH_MODIFIED_B_VARIANT_TURN_IN_BOOST_RIGHT = 0.09
CIVIC_BOSCH_MODIFIED_B_VARIANT_UNWIND_TAPER_LEFT = 0.68
CIVIC_BOSCH_MODIFIED_B_VARIANT_UNWIND_TAPER_RIGHT = 0.86
CIVIC_BOSCH_MODIFIED_B_VARIANT_TURN_IN_FRICTION_BOOST_LEFT = 0.03
CIVIC_BOSCH_MODIFIED_B_VARIANT_TURN_IN_FRICTION_BOOST_RIGHT = 0.03
CIVIC_BOSCH_MODIFIED_B_VARIANT_TURN_IN_FRICTION_BOOST_LEFT = 0.04
CIVIC_BOSCH_MODIFIED_B_VARIANT_TURN_IN_FRICTION_BOOST_RIGHT = 0.045
CIVIC_BOSCH_MODIFIED_B_VARIANT_UNWIND_FRICTION_REDUCTION_LEFT = 0.46
CIVIC_BOSCH_MODIFIED_B_VARIANT_UNWIND_FRICTION_REDUCTION_RIGHT = 0.70
@@ -267,8 +267,8 @@ IONIQ_6_TURN_IN_FRICTION_BOOST_LEFT = 0.30
IONIQ_6_TURN_IN_FRICTION_BOOST_RIGHT = 0.50
IONIQ_6_UNWIND_FRICTION_REDUCTION_LEFT = 2.55
IONIQ_6_UNWIND_FRICTION_REDUCTION_RIGHT = 6.10
IONIQ_6_CENTER_TAPER_MAX = 0.056
IONIQ_6_CENTER_TAPER_LAT = 0.22
IONIQ_6_CENTER_TAPER_MAX = 0.060
IONIQ_6_CENTER_TAPER_LAT = 0.21
IONIQ_6_CENTER_TAPER_LAT_WIDTH = 0.02
IONIQ_6_CENTER_TAPER_SPEED = 18.0
IONIQ_6_CENTER_TAPER_SPEED_WIDTH = 2.5
selfdrive/controls/lib/longitudinal_planner.py
+18 -2
View File
@@ -62,6 +62,8 @@ VISION_UNTRACKED_SLOW_LEAD_RELAXED_MAX_LEAD_SPEED = 8.0
VISION_UNTRACKED_SLOW_LEAD_RELAXED_MAX_TTC = 10.0
VISION_UNTRACKED_SLOW_LEAD_RELAXED_MIN_CLOSING_SPEED = 10.0
VISION_UNTRACKED_SLOW_LEAD_RELAXED_FULL_CLOSING_SPEED = 16.0
VISION_UNTRACKED_SLOW_LEAD_CONFIRM_TIME = 0.30
VISION_UNTRACKED_SLOW_LEAD_IMMEDIATE_DECEL = 0.55
VISION_SLOW_LEAD_MAX_SPEED = 5.0
VISION_SLOW_LEAD_MIN_CLOSING_SPEED = 1.5
VISION_SLOW_LEAD_TRIGGER_TTC = 4.5
@@ -273,6 +275,7 @@ class LongitudinalPlanner:
self._uncert_last_t = None
self.effective_t_follow = None
self.vision_low_speed_stop_hold_until = 0.0
self.untracked_slow_lead_confirm_t = 0.0
if self.is_preap:
try:
@@ -909,8 +912,21 @@ class LongitudinalPlanner:
if pretracking_vision_caps:
pretracking_vision_cap = min(pretracking_vision_caps)
self.a_desired = min(self.a_desired, pretracking_vision_cap)
output_a_target = min(output_a_target, pretracking_vision_cap)
if pretracking_vision_cap <= -VISION_UNTRACKED_SLOW_LEAD_IMMEDIATE_DECEL:
self.untracked_slow_lead_confirm_t = VISION_UNTRACKED_SLOW_LEAD_CONFIRM_TIME
else:
self.untracked_slow_lead_confirm_t = min(
self.untracked_slow_lead_confirm_t + self.dt,
VISION_UNTRACKED_SLOW_LEAD_CONFIRM_TIME,
)
if self.untracked_slow_lead_confirm_t >= VISION_UNTRACKED_SLOW_LEAD_CONFIRM_TIME:
self.a_desired = min(self.a_desired, pretracking_vision_cap)
output_a_target = min(output_a_target, pretracking_vision_cap)
else:
self.untracked_slow_lead_confirm_t = 0.0
else:
self.untracked_slow_lead_confirm_t = 0.0
close_lead_caps = []
vision_low_speed_stop_active = False
selfdrive/controls/tests/test_longitudinal_planner.py
+72
View File
@@ -500,6 +500,78 @@ def test_acc_mode_pretracking_vision_far_slower_lead_starts_braking_before_track
assert lead_outputs[-1] < no_lead_outputs[-1] - 0.15
@pytest.mark.parametrize("model_version", ["v11", "v12", "v13", "v14"])
def test_acc_mode_pretracking_vision_far_slower_lead_can_still_brake_immediately(model_version):
v_ego = 21.48
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=False,
lead_one=make_lead(status=True, d_rel=93.0, v_lead=12.84, a_lead=0.0, radar=False, model_prob=0.935),
)
sm["starpilotPlan"].vCruise = v_ego + 6.0
planner.update(sm, make_toggles(model_version))
assert planner.mode == "acc"
assert planner.output_a_target < -0.45
@pytest.mark.parametrize("model_version", ["v11", "v12", "v13", "v14"])
def test_acc_mode_pretracking_flappy_far_lead_requires_persistence(model_version):
v_ego = 26.09
CP = CarInterface.get_non_essential_params(CAR.HONDA_CIVIC)
planner_no_lead = LongitudinalPlanner(CP, init_v=v_ego)
planner_flappy = LongitudinalPlanner(CP, init_v=v_ego)
sm_no_lead = make_sm(
v_ego,
desired_accel=0.2,
min_accel=-1.0,
experimental_mode=False,
tracking_lead=False,
)
sm_flappy = make_sm(
v_ego,
desired_accel=0.2,
min_accel=-1.0,
experimental_mode=False,
tracking_lead=False,
lead_one=make_lead(status=True, d_rel=74.75, v_lead=26.63, a_lead=0.01, radar=False, model_prob=0.989),
)
sm_no_lead["starpilotPlan"].vCruise = v_ego + 6.0
sm_flappy["starpilotPlan"].vCruise = v_ego + 6.0
flappy_sequence = [
(74.75, 26.63, 0.01, 0.989),
(68.17, 20.81, 0.094, 0.971),
(69.73, 24.12, 0.057, 0.981),
(62.15, 21.38, 0.064, 0.983),
(66.29, 23.19, 0.069, 0.985),
(70.58, 27.51, 0.036, 0.988),
]
no_lead_outputs = []
flappy_outputs = []
for d_rel, v_lead, a_lead, model_prob in flappy_sequence:
planner_no_lead.update(sm_no_lead, make_toggles(model_version))
sm_flappy["radarState"].leadOne = make_lead(
status=True, d_rel=d_rel, v_lead=v_lead, a_lead=a_lead, radar=False, model_prob=model_prob,
)
planner_flappy.update(sm_flappy, make_toggles(model_version))
no_lead_outputs.append(planner_no_lead.output_a_target)
flappy_outputs.append(planner_flappy.output_a_target)
assert planner_flappy.mode == "acc"
assert min(flappy_outputs) > -0.05
assert min(flappy_outputs) > min(no_lead_outputs) - 0.12
@pytest.mark.parametrize("model_version", ["v11", "v12", "v13", "v14"])
def test_acc_mode_pretracking_near_stopped_vision_lead_does_not_relax_when_confidence_is_midrange(model_version):
v_ego = 20.35