This commit is contained in:
firestar5683
2026-05-15 12:30:35 -05:00
parent e2607ea6e8
commit 473e3efae9
4 changed files with 247 additions and 28 deletions
+22 -22
View File
@@ -66,16 +66,16 @@ CIVIC_BOSCH_MODIFIED_B_TURN_IN_FRICTION_BOOST_LEFT = 0.02
CIVIC_BOSCH_MODIFIED_B_TURN_IN_FRICTION_BOOST_RIGHT = 0.00
CIVIC_BOSCH_MODIFIED_B_UNWIND_FRICTION_REDUCTION_LEFT = 0.26
CIVIC_BOSCH_MODIFIED_B_UNWIND_FRICTION_REDUCTION_RIGHT = 0.40
CIVIC_BOSCH_MODIFIED_A_VARIANT_FF_RESTORE_LEFT = -0.03
CIVIC_BOSCH_MODIFIED_A_VARIANT_FF_RESTORE_RIGHT = 0.05
CIVIC_BOSCH_MODIFIED_A_VARIANT_TURN_IN_BOOST_LEFT = 0.00
CIVIC_BOSCH_MODIFIED_A_VARIANT_TURN_IN_BOOST_RIGHT = 0.04
CIVIC_BOSCH_MODIFIED_A_VARIANT_UNWIND_TAPER_LEFT = 0.10
CIVIC_BOSCH_MODIFIED_A_VARIANT_UNWIND_TAPER_RIGHT = 0.04
CIVIC_BOSCH_MODIFIED_A_VARIANT_TURN_IN_FRICTION_BOOST_LEFT = 0.00
CIVIC_BOSCH_MODIFIED_A_VARIANT_TURN_IN_FRICTION_BOOST_RIGHT = 0.03
CIVIC_BOSCH_MODIFIED_A_VARIANT_UNWIND_FRICTION_REDUCTION_LEFT = 0.08
CIVIC_BOSCH_MODIFIED_A_VARIANT_UNWIND_FRICTION_REDUCTION_RIGHT = 0.03
CIVIC_BOSCH_MODIFIED_A_VARIANT_FF_RESTORE_LEFT = -0.06
CIVIC_BOSCH_MODIFIED_A_VARIANT_FF_RESTORE_RIGHT = -0.02
CIVIC_BOSCH_MODIFIED_A_VARIANT_TURN_IN_BOOST_LEFT = -0.02
CIVIC_BOSCH_MODIFIED_A_VARIANT_TURN_IN_BOOST_RIGHT = 0.00
CIVIC_BOSCH_MODIFIED_A_VARIANT_UNWIND_TAPER_LEFT = 0.16
CIVIC_BOSCH_MODIFIED_A_VARIANT_UNWIND_TAPER_RIGHT = 0.10
CIVIC_BOSCH_MODIFIED_A_VARIANT_TURN_IN_FRICTION_BOOST_LEFT = -0.01
CIVIC_BOSCH_MODIFIED_A_VARIANT_TURN_IN_FRICTION_BOOST_RIGHT = 0.00
CIVIC_BOSCH_MODIFIED_A_VARIANT_UNWIND_FRICTION_REDUCTION_LEFT = 0.14
CIVIC_BOSCH_MODIFIED_A_VARIANT_UNWIND_FRICTION_REDUCTION_RIGHT = 0.08
CIVIC_BOSCH_MODIFIED_A_VARIANT_CENTER_TAPER_MAX = 0.12
CIVIC_BOSCH_MODIFIED_A_VARIANT_CENTER_TAPER_LAT = 0.24
CIVIC_BOSCH_MODIFIED_A_VARIANT_CENTER_TAPER_LAT_WIDTH = 0.05
@@ -337,21 +337,21 @@ 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.58
IONIQ_6_TURN_IN_BOOST_RIGHT = 1.82
IONIQ_6_UNWIND_TAPER_LEFT = 3.05
IONIQ_6_UNWIND_TAPER_RIGHT = 6.35
IONIQ_6_TURN_IN_BOOST_LEFT = 1.64
IONIQ_6_TURN_IN_BOOST_RIGHT = 1.88
IONIQ_6_UNWIND_TAPER_LEFT = 3.18
IONIQ_6_UNWIND_TAPER_RIGHT = 6.55
IONIQ_6_FRICTION_MULT = 0.928
IONIQ_6_FRICTION_LAT_RISE = 0.20
IONIQ_6_FRICTION_JERK_RISE = 0.24
IONIQ_6_TURN_IN_THRESHOLD_REDUCTION_LEFT = 0.74
IONIQ_6_TURN_IN_THRESHOLD_REDUCTION_RIGHT = 1.18
IONIQ_6_UNWIND_THRESHOLD_INCREASE_LEFT = 3.70
IONIQ_6_UNWIND_THRESHOLD_INCREASE_RIGHT = 7.85
IONIQ_6_TURN_IN_FRICTION_BOOST_LEFT = 0.40
IONIQ_6_TURN_IN_FRICTION_BOOST_RIGHT = 0.72
IONIQ_6_UNWIND_FRICTION_REDUCTION_LEFT = 3.35
IONIQ_6_UNWIND_FRICTION_REDUCTION_RIGHT = 7.30
IONIQ_6_TURN_IN_THRESHOLD_REDUCTION_LEFT = 0.78
IONIQ_6_TURN_IN_THRESHOLD_REDUCTION_RIGHT = 1.24
IONIQ_6_UNWIND_THRESHOLD_INCREASE_LEFT = 3.90
IONIQ_6_UNWIND_THRESHOLD_INCREASE_RIGHT = 8.10
IONIQ_6_TURN_IN_FRICTION_BOOST_LEFT = 0.44
IONIQ_6_TURN_IN_FRICTION_BOOST_RIGHT = 0.78
IONIQ_6_UNWIND_FRICTION_REDUCTION_LEFT = 3.55
IONIQ_6_UNWIND_FRICTION_REDUCTION_RIGHT = 7.65
IONIQ_6_CENTER_TAPER_MAX = 0.082
IONIQ_6_CENTER_TAPER_LAT = 0.24
IONIQ_6_CENTER_TAPER_LAT_WIDTH = 0.025
@@ -38,6 +38,18 @@ STANDSTILL_LEAD_DEPART_MAX_EGO_SPEED = 1.5
STANDSTILL_LEAD_DEPART_MIN_LEAD_SPEED = 0.6
STANDSTILL_LEAD_DEPART_MIN_GAP_MARGIN = 1.5
STANDSTILL_LEAD_DEPART_MIN_MODEL_ACCEL = 0.08
LEAD_DEPART_ACCEL_HOLD_TIME = 1.2
LEAD_DEPART_ACCEL_HOLD_MAX_EGO_SPEED = 1.5
LEAD_DEPART_ACCEL_HOLD_MIN_LEAD_SPEED = 0.6
LEAD_DEPART_ACCEL_HOLD_MIN_LEAD_DELTA = 0.5
LEAD_DEPART_ACCEL_HOLD_MIN_GAP = 3.5
LEAD_DEPART_ACCEL_HOLD_FULL_GAP = 6.0
LEAD_DEPART_ACCEL_HOLD_FULL_LEAD_SPEED = 2.2
LEAD_DEPART_ACCEL_HOLD_MIN_MODEL_PROB = 0.85
LEAD_DEPART_ACCEL_HOLD_MIN_MODEL_ACCEL = 0.12
LEAD_DEPART_ACCEL_HOLD_MAX_LEAD_BRAKE = 0.2
LEAD_DEPART_ACCEL_HOLD_MIN_ACCEL = 0.25
LEAD_DEPART_ACCEL_HOLD_MAX_ACCEL = 0.45
CLOSE_LEAD_BRAKE_CAP_MAX_TTC = 25.0
VISION_LEAD_APPROACH_MIN_CLOSING_SPEED = 2.0
VISION_LEAD_APPROACH_TRIGGER_TIME = 4.5
@@ -118,6 +130,12 @@ 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.25
VISION_LOW_SPEED_STOP_BUFFER_BRAKE_GAIN = 0.25
VISION_CLOSE_STOP_HOLD_MAX_EGO_SPEED = 0.75
VISION_CLOSE_STOP_HOLD_MAX_LEAD_SPEED = 0.8
VISION_CLOSE_STOP_HOLD_MAX_DISTANCE = 2.8
VISION_CLOSE_STOP_HOLD_MIN_MODEL_PROB = 0.95
VISION_CLOSE_STOP_HOLD_MIN_BRAKE = 0.12
VISION_CLOSE_STOP_HOLD_MAX_BRAKE = 0.28
MANUAL_STOP_RESUME_OVERRIDE_TIME = 3.0
MANUAL_STOP_RESUME_OVERRIDE_MAX_SPEED = 2.0
MANUAL_STOP_RESUME_OVERRIDE_MIN_ACCEL = 0.2
@@ -378,6 +396,7 @@ class LongitudinalPlanner:
self.vision_lead_approach_confirm_t = 0.0
self.untracked_slow_lead_confirm_t = 0.0
self.manual_stop_resume_override_until = 0.0
self.lead_depart_accel_hold_until = 0.0
if self.is_preap:
try:
@@ -713,6 +732,29 @@ class LongitudinalPlanner:
min_stop_brake = VISION_LOW_SPEED_STOP_BUFFER_MIN_BRAKE + VISION_LOW_SPEED_STOP_BUFFER_BRAKE_GAIN * float(v_ego)
return max(accel_min, -min_stop_brake), True
def get_vision_close_stop_hold_cap(self, lead, v_ego, accel_min, should_stop):
if not should_stop or lead is None or not lead.status or bool(getattr(lead, "radar", False)):
return None
lead_prob = float(getattr(lead, "modelProb", 0.0))
if lead_prob < VISION_CLOSE_STOP_HOLD_MIN_MODEL_PROB:
return None
lead_speed = max(float(lead.vLead), 0.0)
if (
float(v_ego) > VISION_CLOSE_STOP_HOLD_MAX_EGO_SPEED or
lead_speed > VISION_CLOSE_STOP_HOLD_MAX_LEAD_SPEED or
float(lead.dRel) > VISION_CLOSE_STOP_HOLD_MAX_DISTANCE
):
return None
distance_factor = float(np.clip((VISION_CLOSE_STOP_HOLD_MAX_DISTANCE - float(lead.dRel)) /
max(VISION_CLOSE_STOP_HOLD_MAX_DISTANCE - 1.8, 0.1), 0.0, 1.0))
speed_factor = float(np.clip(float(v_ego) / max(VISION_CLOSE_STOP_HOLD_MAX_EGO_SPEED, 0.1), 0.0, 1.0))
hold_brake = VISION_CLOSE_STOP_HOLD_MIN_BRAKE + 0.08 * distance_factor + 0.08 * speed_factor
hold_brake = float(np.clip(hold_brake, VISION_CLOSE_STOP_HOLD_MIN_BRAKE, VISION_CLOSE_STOP_HOLD_MAX_BRAKE))
return max(accel_min, -hold_brake)
def _update_manual_stop_resume_override(self, sm):
now_t = time.monotonic()
lead = sm["radarState"].leadOne
@@ -736,6 +778,36 @@ class LongitudinalPlanner:
now_t < self.manual_stop_resume_override_until
)
def get_lead_depart_accel_floor(self, lead, v_ego, model_desired_accel):
if lead is None or not lead.status:
return None
lead_radar = bool(getattr(lead, "radar", False))
lead_prob = float(getattr(lead, "modelProb", 1.0 if lead_radar else 0.0))
if not lead_radar and lead_prob < LEAD_DEPART_ACCEL_HOLD_MIN_MODEL_PROB:
return None
lead_speed = max(float(lead.vLead), 0.0)
lead_brake = max(0.0, -float(getattr(lead, "aLeadK", 0.0)))
lead_delta = lead_speed - float(v_ego)
if (
float(v_ego) > LEAD_DEPART_ACCEL_HOLD_MAX_EGO_SPEED or
lead_speed < LEAD_DEPART_ACCEL_HOLD_MIN_LEAD_SPEED or
lead_delta < LEAD_DEPART_ACCEL_HOLD_MIN_LEAD_DELTA or
float(lead.dRel) < LEAD_DEPART_ACCEL_HOLD_MIN_GAP or
lead_brake > LEAD_DEPART_ACCEL_HOLD_MAX_LEAD_BRAKE or
float(model_desired_accel) < LEAD_DEPART_ACCEL_HOLD_MIN_MODEL_ACCEL
):
return None
gap_factor = float(np.clip((float(lead.dRel) - LEAD_DEPART_ACCEL_HOLD_MIN_GAP) /
max(LEAD_DEPART_ACCEL_HOLD_FULL_GAP - LEAD_DEPART_ACCEL_HOLD_MIN_GAP, 0.1), 0.0, 1.0))
lead_factor = float(np.clip((lead_speed - LEAD_DEPART_ACCEL_HOLD_MIN_LEAD_SPEED) /
max(LEAD_DEPART_ACCEL_HOLD_FULL_LEAD_SPEED - LEAD_DEPART_ACCEL_HOLD_MIN_LEAD_SPEED, 0.1), 0.0, 1.0))
accel_cap = LEAD_DEPART_ACCEL_HOLD_MIN_ACCEL + (LEAD_DEPART_ACCEL_HOLD_MAX_ACCEL - LEAD_DEPART_ACCEL_HOLD_MIN_ACCEL) * np.clip(
0.55 * lead_factor + 0.45 * gap_factor, 0.0, 1.0)
return min(accel_cap, max(float(model_desired_accel), LEAD_DEPART_ACCEL_HOLD_MIN_ACCEL))
def get_lead_catchup_accel_cap(self, lead, v_ego, t_follow):
if lead is None or not lead.status:
return None
@@ -1511,6 +1583,40 @@ class LongitudinalPlanner:
if lead_control_active and lead_depart_ready and not output_should_stop and float(sm['carState'].vEgo) <= STANDSTILL_LEAD_DEPART_MAX_EGO_SPEED:
output_a_target = max(output_a_target, STANDSTILL_LEAD_DEPART_MIN_ACCEL)
if output_should_stop or bool(getattr(sm['starpilotPlan'], 'forcingStop', False)) or bool(getattr(sm['starpilotPlan'], 'redLight', False)):
self.lead_depart_accel_hold_until = 0.0
lead_depart_accel_floor = None
if lead_control_active and not output_should_stop:
lead_depart_accel_floors = [
floor for floor in (
self.get_lead_depart_accel_floor(self.lead_one, scene_v_ego, model_desired_accel),
self.get_lead_depart_accel_floor(self.lead_two, scene_v_ego, model_desired_accel),
) if floor is not None
]
if lead_depart_accel_floors:
lead_depart_accel_floor = max(lead_depart_accel_floors)
if sm['carState'].standstill:
self.lead_depart_accel_hold_until = now_t + LEAD_DEPART_ACCEL_HOLD_TIME
lead_depart_accel_hold_active = (
lead_depart_accel_floor is not None and
now_t < self.lead_depart_accel_hold_until and
float(sm['carState'].vEgo) <= LEAD_DEPART_ACCEL_HOLD_MAX_EGO_SPEED
)
if lead_control_active and output_should_stop:
close_stop_hold_caps = [
cap for cap in (
self.get_vision_close_stop_hold_cap(self.lead_one, scene_v_ego, output_accel_min, output_should_stop),
self.get_vision_close_stop_hold_cap(self.lead_two, scene_v_ego, output_accel_min, output_should_stop),
) if cap is not None
]
if close_stop_hold_caps:
close_stop_hold_cap = min(close_stop_hold_caps)
self.a_desired = min(self.a_desired, close_stop_hold_cap)
output_a_target = min(output_a_target, close_stop_hold_cap)
if lead_one_active:
lead_catchup_accel_cap = self.get_lead_catchup_accel_cap(self.lead_one, scene_v_ego, effective_t_follow)
if lead_catchup_accel_cap is not None:
@@ -1575,6 +1681,9 @@ class LongitudinalPlanner:
self.a_desired = max(self.a_desired, tracked_vision_model_brake_cap)
output_a_target = max(output_a_target, tracked_vision_model_brake_cap)
if lead_depart_accel_hold_active:
output_a_target = max(output_a_target, lead_depart_accel_floor)
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))
+5 -5
View File
@@ -604,14 +604,14 @@ class TestLatControl:
a_variant_unwind_right_friction = get_civic_bosch_modified_b_friction_scale(12.0, -0.5, 0.8)
assert a_variant_steady_left < base_steady_left
assert a_variant_steady_right > base_steady_right
assert a_variant_steady_right < base_steady_right
assert a_variant_turn_in_left < base_turn_in_left
assert a_variant_turn_in_right > base_turn_in_right
assert a_variant_turn_in_right < base_turn_in_right
assert a_variant_unwind_left < base_unwind_left
assert a_variant_unwind_right > base_unwind_right
assert a_variant_turn_in_right_friction > base_turn_in_right_friction
assert a_variant_unwind_right < base_unwind_right
assert a_variant_turn_in_right_friction <= base_turn_in_right_friction
assert a_variant_unwind_left_friction < base_unwind_left_friction
assert a_variant_unwind_right_friction >= 0.82
assert a_variant_unwind_right_friction < base_unwind_right_friction
def test_modified_civic_a_variant_center_taper_curve(self):
assert get_civic_bosch_modified_a_center_taper_scale(0.0, 25.0) < get_civic_bosch_modified_a_center_taper_scale(0.0, 10.0)
@@ -805,6 +805,40 @@ def test_acc_mode_low_speed_vision_stop_buffer_stays_latched_when_closure_soften
assert cap_held <= -1.25
@pytest.mark.parametrize("model_version", ["v11", "v12", "v13", "v14"])
def test_acc_mode_close_moving_vision_lead_keeps_negative_output_while_should_stop(model_version):
CP = CarInterface.get_non_essential_params(CAR.HONDA_CIVIC)
planner = LongitudinalPlanner(CP, init_v=0.242)
toggles = make_toggles(model_version)
stop_sequence = [
(0.242, 2.062, 0.284, -0.081),
(0.221, 1.963, 0.338, -0.076),
(0.194, 2.100, 0.451, -0.076),
(0.180, 2.001, 0.447, -0.066),
(0.166, 1.964, 0.451, -0.066),
(0.151, 2.075, 0.451, -0.060),
]
outputs = []
for v_ego, d_rel, v_lead, desired_accel in stop_sequence:
sm = make_sm(
v_ego,
desired_accel=desired_accel,
min_accel=-0.5,
experimental_mode=False,
tracking_lead=True,
lead_one=make_lead(status=True, d_rel=d_rel, v_lead=v_lead, a_lead=0.0, radar=False, model_prob=1.0),
)
sm["controlsState"].longControlState = LongCtrlState.stopping
sm["starpilotPlan"].vCruise = 10.0
planner.update(sm, toggles)
outputs.append(planner.output_a_target)
assert all(output <= -0.02 for output in outputs[2:])
@pytest.mark.parametrize("model_version", ["v11", "v12", "v13", "v14"])
def test_acc_mode_tracked_vision_model_brake_floor_prevents_positive_output_on_slower_lead(model_version):
v_ego = 19.1
@@ -833,7 +867,7 @@ def test_tracked_vision_model_brake_cap_relaxes_mild_model_brake_slam_window(mod
CP = CarInterface.get_non_essential_params(CAR.HONDA_CIVIC)
planner = LongitudinalPlanner(CP, init_v=v_ego)
lead = make_lead(status=True, d_rel=38.1, v_lead=19.07, a_lead=-0.30, radar=False, model_prob=0.999)
lead = make_lead(status=True, d_rel=48.0, v_lead=19.07, a_lead=-0.30, radar=False, model_prob=0.999)
cap = planner.get_tracked_vision_model_brake_cap(lead, v_ego, 1.45, -0.35)
@@ -942,6 +976,82 @@ def test_standstill_moving_lead_applies_resume_floor_once_stop_clears(model_vers
assert planner.output_a_target >= 0.2
@pytest.mark.parametrize("model_version", ["v11", "v12", "v13", "v14"])
def test_standstill_moving_lead_holds_depart_accel_floor_after_stop_release(model_version):
CP = CarInterface.get_non_essential_params(CAR.HONDA_CIVIC)
planner = LongitudinalPlanner(CP, init_v=0.0)
toggles = make_toggles(model_version)
sequence = [
(0.0, True, 15.0, 2.2, 0.10),
(0.0, True, 15.2, 2.4, 0.12),
(0.0, True, 15.5, 2.6, 0.15),
(0.10, False, 15.8, 2.8, 0.20),
(0.25, False, 16.2, 3.0, 0.25),
(0.45, False, 16.8, 3.2, 0.30),
]
outputs = []
for v_ego, standstill, d_rel, v_lead, desired_accel in sequence:
sm = make_sm(
v_ego,
desired_accel=desired_accel,
min_accel=-0.5,
experimental_mode=False,
tracking_lead=True,
lead_one=make_lead(status=True, d_rel=d_rel, v_lead=v_lead, a_lead=0.0, radar=False, model_prob=0.99),
)
sm["carState"].standstill = standstill
sm["controlsState"].longControlState = LongCtrlState.starting if standstill else LongCtrlState.pid
sm["starpilotPlan"].vCruise = 10.0
sm["modelV2"].action.shouldStop = False
planner.update(sm, toggles)
outputs.append(planner.output_a_target)
assert outputs[2] >= 0.25
assert outputs[3] >= 0.25
assert outputs[4] >= 0.25
@pytest.mark.parametrize("model_version", ["v11", "v12", "v13", "v14"])
def test_standstill_moving_lead_depart_accel_hold_cancels_if_lead_brakes(model_version):
CP = CarInterface.get_non_essential_params(CAR.HONDA_CIVIC)
planner = LongitudinalPlanner(CP, init_v=0.0)
toggles = make_toggles(model_version)
sm_release = make_sm(
0.0,
desired_accel=0.45,
min_accel=-0.5,
experimental_mode=False,
tracking_lead=True,
lead_one=make_lead(status=True, d_rel=4.8, v_lead=1.0, a_lead=0.0, radar=False, model_prob=0.99),
)
sm_release["carState"].standstill = True
sm_release["controlsState"].longControlState = LongCtrlState.starting
sm_release["starpilotPlan"].vCruise = 10.0
sm_release["modelV2"].action.shouldStop = False
planner.update(sm_release, toggles)
sm_brake = make_sm(
0.18,
desired_accel=0.18,
min_accel=-0.5,
experimental_mode=False,
tracking_lead=True,
lead_one=make_lead(status=True, d_rel=3.9, v_lead=0.1, a_lead=-0.4, radar=False, model_prob=0.99),
)
sm_brake["controlsState"].longControlState = LongCtrlState.pid
sm_brake["starpilotPlan"].vCruise = 10.0
sm_brake["modelV2"].action.shouldStop = True
planner.update(sm_brake, toggles)
assert planner.output_should_stop
assert planner.output_a_target < 0.1
@pytest.mark.parametrize("model_version", ["v11", "v12", "v13", "v14"])
def test_acc_mode_damps_far_radar_mild_lead_brake_more_than_close_brake(model_version):
far_v_ego = 29.26