mirror of
https://github.com/firestar5683/StarPilot.git
synced 2026-08-21 00:03:45 +08:00
túne
This commit is contained in:
@@ -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))
|
||||
|
||||
|
||||
@@ -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
|
||||
|
||||
Reference in New Issue
Block a user