mirror of
https://github.com/MoreTore/openpilot.git
synced 2026-07-26 12:22:04 +08:00
Dingle's got dragonclaw?!
This commit is contained in:
firestar5683
@@ -74,6 +74,7 @@ legend = ["LAT_ACCEL_FACTOR", "MAX_LAT_ACCEL_MEASURED", "FRICTION"]
|
||||
"HYUNDAI_TUCSON_HEV_2025" = [2.5, 2.5, 0.1]
|
||||
"HYUNDAI_TUCSON_PHEV_2025" = [2.5, 2.5, 0.1]
|
||||
"KIA_SPORTAGE_5TH_GEN" = [2.6, 2.6, 0.1]
|
||||
"KIA_XCEED_PHEV" = [2.9638737459977467, 2.1259108157250735, 0.07813665616927593]
|
||||
"KIA_SPORTAGE_2026" = [nan, 2.5, nan]
|
||||
"KIA_SPORTAGE_HEV_2026" = [nan, 2.5, nan]
|
||||
"GENESIS_GV70_1ST_GEN" = [2.42, 2.42, 0.1]
|
||||
|
||||
@@ -24,7 +24,6 @@ legend = ["LAT_ACCEL_FACTOR", "MAX_LAT_ACCEL_MEASURED", "FRICTION"]
|
||||
"KIA_OPTIMA_H_G4_FL" = "HYUNDAI_SONATA"
|
||||
"KIA_FORTE" = "HYUNDAI_SONATA"
|
||||
"KIA_CEED" = "HYUNDAI_SONATA"
|
||||
"KIA_XCEED_PHEV" = "HYUNDAI_SONATA"
|
||||
"KIA_SELTOS" = "HYUNDAI_SONATA"
|
||||
"KIA_NIRO_PHEV" = "KIA_NIRO_EV"
|
||||
"KIA_NIRO_PHEV_2022" = "KIA_NIRO_EV"
|
||||
|
||||
@@ -141,6 +141,9 @@ ELANTRA_NON_SCC_CARS = (
|
||||
KIA_EV6_CARS = (
|
||||
HYUNDAI_CAR.KIA_EV6,
|
||||
)
|
||||
KIA_XCEED_CARS = (
|
||||
HYUNDAI_CAR.KIA_XCEED_PHEV,
|
||||
)
|
||||
KIA_FORTE_CARS = (
|
||||
HYUNDAI_CAR.KIA_FORTE,
|
||||
HYUNDAI_CAR.KIA_FORTE_2019_NON_SCC,
|
||||
@@ -317,6 +320,24 @@ ELANTRA_NON_SCC_TURN_IN_BOOST_RIGHT = 0.12
|
||||
ELANTRA_NON_SCC_UNWIND_TAPER_LEFT = 0.22
|
||||
ELANTRA_NON_SCC_UNWIND_TAPER_RIGHT = 0.12
|
||||
|
||||
KIA_XCEED_FF_REDUCTION_LEFT = 0.08
|
||||
KIA_XCEED_FF_REDUCTION_RIGHT = 0.10
|
||||
KIA_XCEED_FF_ONSET = 0.16
|
||||
KIA_XCEED_FF_ONSET_WIDTH = 0.06
|
||||
KIA_XCEED_FF_CUTOFF = 1.30
|
||||
KIA_XCEED_FF_CUTOFF_WIDTH = 0.36
|
||||
KIA_XCEED_TRANSITION_SPEED = 8.5
|
||||
KIA_XCEED_PHASE_SCALE = 0.10
|
||||
KIA_XCEED_TURN_IN_BOOST_LEFT = 0.08
|
||||
KIA_XCEED_TURN_IN_BOOST_RIGHT = 0.06
|
||||
KIA_XCEED_UNWIND_TAPER_LEFT = 0.16
|
||||
KIA_XCEED_UNWIND_TAPER_RIGHT = 0.14
|
||||
KIA_XCEED_CENTER_TAPER_MAX = 0.05
|
||||
KIA_XCEED_CENTER_TAPER_LAT = 0.14
|
||||
KIA_XCEED_CENTER_TAPER_LAT_WIDTH = 0.03
|
||||
KIA_XCEED_CENTER_TAPER_SPEED = 17.5
|
||||
KIA_XCEED_CENTER_TAPER_SPEED_WIDTH = 2.5
|
||||
|
||||
KIA_FORTE_BASE_LAT_ACCEL_FACTOR_MULT = 1.05
|
||||
KIA_FORTE_FF_REDUCTION_LEFT = 0.05
|
||||
KIA_FORTE_FF_REDUCTION_RIGHT = 0.10
|
||||
@@ -1274,6 +1295,54 @@ def get_elantra_non_scc_ff_scale(desired_lateral_accel: float, desired_lateral_j
|
||||
return base_scale * turn_in_boost * max(unwind_taper, 0.0)
|
||||
|
||||
|
||||
def _kia_xceed_sigmoid(x: float) -> float:
|
||||
return _sigmoid(x)
|
||||
|
||||
|
||||
def _kia_xceed_low_speed_factor(v_ego: float) -> float:
|
||||
return 1.0 / (1.0 + (max(v_ego, 0.0) / KIA_XCEED_TRANSITION_SPEED) ** 2)
|
||||
|
||||
|
||||
def _kia_xceed_transition_phase(desired_lateral_accel: float, desired_lateral_jerk: float) -> float:
|
||||
return math.tanh((desired_lateral_accel * desired_lateral_jerk) / KIA_XCEED_PHASE_SCALE)
|
||||
|
||||
|
||||
def _kia_xceed_side_value(desired_lateral_accel: float, left_value: float, right_value: float) -> float:
|
||||
return left_value if desired_lateral_accel >= 0.0 else right_value
|
||||
|
||||
|
||||
def get_kia_xceed_ff_scale(desired_lateral_accel: float, desired_lateral_jerk: float, v_ego: float) -> float:
|
||||
if desired_lateral_accel == 0.0:
|
||||
return 1.0
|
||||
|
||||
abs_lateral_accel = abs(desired_lateral_accel)
|
||||
onset = _kia_xceed_sigmoid((abs_lateral_accel - KIA_XCEED_FF_ONSET) / KIA_XCEED_FF_ONSET_WIDTH)
|
||||
cutoff = _kia_xceed_sigmoid((KIA_XCEED_FF_CUTOFF - abs_lateral_accel) / KIA_XCEED_FF_CUTOFF_WIDTH)
|
||||
base_reduction = _kia_xceed_side_value(desired_lateral_accel,
|
||||
KIA_XCEED_FF_REDUCTION_LEFT,
|
||||
KIA_XCEED_FF_REDUCTION_RIGHT) * onset * cutoff
|
||||
phase = _kia_xceed_transition_phase(desired_lateral_accel, desired_lateral_jerk)
|
||||
turn_in_weight = max(phase, 0.0)
|
||||
unwind_weight = max(-phase, 0.0)
|
||||
low_speed_factor = _kia_xceed_low_speed_factor(v_ego)
|
||||
turn_in_boost = 1.0 + (_kia_xceed_side_value(desired_lateral_accel,
|
||||
KIA_XCEED_TURN_IN_BOOST_LEFT,
|
||||
KIA_XCEED_TURN_IN_BOOST_RIGHT) *
|
||||
turn_in_weight * low_speed_factor)
|
||||
unwind_taper = 1.0 - (_kia_xceed_side_value(desired_lateral_accel,
|
||||
KIA_XCEED_UNWIND_TAPER_LEFT,
|
||||
KIA_XCEED_UNWIND_TAPER_RIGHT) *
|
||||
unwind_weight * (0.35 + 0.65 * low_speed_factor))
|
||||
return (1.0 - base_reduction) * turn_in_boost * max(unwind_taper, 0.0)
|
||||
|
||||
|
||||
def get_kia_xceed_center_taper_scale(desired_lateral_accel: float, v_ego: float) -> float:
|
||||
speed_weight = _kia_xceed_sigmoid((v_ego - KIA_XCEED_CENTER_TAPER_SPEED) / KIA_XCEED_CENTER_TAPER_SPEED_WIDTH)
|
||||
center_weight = _kia_xceed_sigmoid((KIA_XCEED_CENTER_TAPER_LAT - abs(desired_lateral_accel)) / KIA_XCEED_CENTER_TAPER_LAT_WIDTH)
|
||||
reduction = KIA_XCEED_CENTER_TAPER_MAX * speed_weight * center_weight
|
||||
return 1.0 - reduction
|
||||
|
||||
|
||||
def _kia_forte_sigmoid(x: float) -> float:
|
||||
return _sigmoid(x)
|
||||
|
||||
@@ -1917,6 +1986,7 @@ class LatControlTorque(LatControl):
|
||||
self.is_sonata = CP.carFingerprint in SONATA_CARS
|
||||
self.is_sonata_hybrid = CP.carFingerprint in SONATA_HYBRID_CARS
|
||||
self.is_elantra_non_scc = CP.carFingerprint in ELANTRA_NON_SCC_CARS
|
||||
self.is_kia_xceed = CP.carFingerprint in KIA_XCEED_CARS
|
||||
self.is_kia_forte = CP.carFingerprint in KIA_FORTE_CARS
|
||||
self.is_kia_ev6 = CP.carFingerprint in KIA_EV6_CARS
|
||||
self.is_civic_bosch_modified = CP.carFingerprint == HONDA_CAR.HONDA_CIVIC_BOSCH and bool(CP.flags & HondaFlags.EPS_MODIFIED)
|
||||
@@ -2051,6 +2121,7 @@ class LatControlTorque(LatControl):
|
||||
sonata_active = self.is_sonata
|
||||
sonata_hybrid_active = self.is_sonata_hybrid
|
||||
elantra_non_scc_active = self.is_elantra_non_scc
|
||||
kia_xceed_active = self.is_kia_xceed
|
||||
kia_forte_active = self.is_kia_forte
|
||||
kia_ev6_test_active = self.is_kia_ev6 and kia_ev6_lateral_testing_ground_active()
|
||||
volt_plexy_test_active = self.is_volt_cc and volt_plexy_lateral_testing_ground_active()
|
||||
@@ -2060,6 +2131,7 @@ class LatControlTorque(LatControl):
|
||||
ioniq_6_center_taper = get_ioniq_6_center_taper_scale(setpoint, CS.vEgo) if ioniq_6_active else 1.0
|
||||
sonata_center_taper = get_sonata_center_taper_scale(setpoint, CS.vEgo) if sonata_active else 1.0
|
||||
sonata_hybrid_center_taper = get_sonata_hybrid_center_taper_scale(setpoint, CS.vEgo) if sonata_hybrid_active else 1.0
|
||||
kia_xceed_center_taper = get_kia_xceed_center_taper_scale(setpoint, CS.vEgo) if kia_xceed_active else 1.0
|
||||
kia_forte_center_taper = get_kia_forte_center_taper_scale(setpoint, CS.vEgo) if kia_forte_active else 1.0
|
||||
kia_ev6_center_taper = get_kia_ev6_center_taper_scale(setpoint, CS.vEgo) if kia_ev6_test_active else 1.0
|
||||
civic_bosch_modified_a_center_taper = get_civic_bosch_modified_a_center_taper_scale(setpoint, CS.vEgo) if (
|
||||
@@ -2110,6 +2182,8 @@ class LatControlTorque(LatControl):
|
||||
ff *= get_sonata_hybrid_ff_scale(setpoint, desired_lateral_jerk, CS.vEgo) * sonata_hybrid_center_taper
|
||||
elif elantra_non_scc_active:
|
||||
ff *= get_elantra_non_scc_ff_scale(setpoint, desired_lateral_jerk, CS.vEgo)
|
||||
elif kia_xceed_active:
|
||||
ff *= get_kia_xceed_ff_scale(setpoint, desired_lateral_jerk, CS.vEgo) * kia_xceed_center_taper
|
||||
elif kia_forte_active:
|
||||
ff *= get_kia_forte_ff_scale(setpoint, desired_lateral_jerk, CS.vEgo) * kia_forte_center_taper
|
||||
friction_threshold = get_kia_forte_friction_threshold(CS.vEgo, setpoint, desired_lateral_jerk)
|
||||
|
||||
@@ -33,6 +33,11 @@ MIN_ALLOW_THROTTLE_SPEED = 5.0
|
||||
RAW_LEAD_SAFETY_MIN_CLOSING_SPEED = 0.5
|
||||
RAW_LEAD_SAFETY_TTC = 7.0
|
||||
RAW_LEAD_SAFETY_DISTANCE = 40.0
|
||||
RAW_LEAD_LOW_SPEED_HOLD_MAX_EGO_SPEED = 4.5
|
||||
RAW_LEAD_LOW_SPEED_HOLD_MAX_LEAD_SPEED = 3.5
|
||||
RAW_LEAD_LOW_SPEED_HOLD_MAX_DISTANCE = 10.0
|
||||
RAW_LEAD_LOW_SPEED_HOLD_MAX_LATERAL_OFFSET = 1.75
|
||||
RAW_LEAD_LOW_SPEED_HOLD_MIN_CLOSING_SPEED = 0.15
|
||||
STANDSTILL_LEAD_NUDGE_ACCEL = 0.05
|
||||
STANDSTILL_LEAD_NUDGE_MIN_SPEED = 0.0
|
||||
STANDSTILL_LEAD_DEPART_MIN_ACCEL = 0.35
|
||||
@@ -75,6 +80,23 @@ 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
|
||||
LOW_SPEED_WEAK_LEAD_ACCEL_CAP_MAX_EGO_SPEED = 4.5
|
||||
LOW_SPEED_WEAK_LEAD_ACCEL_CAP_MIN_DISTANCE = 4.0
|
||||
LOW_SPEED_WEAK_LEAD_ACCEL_CAP_MAX_DISTANCE = 18.0
|
||||
LOW_SPEED_WEAK_LEAD_ACCEL_CAP_MAX_LEAD_SPEED = 4.0
|
||||
LOW_SPEED_WEAK_LEAD_ACCEL_CAP_MAX_LATERAL_OFFSET = 1.75
|
||||
LOW_SPEED_WEAK_LEAD_ACCEL_CAP_MIN_MODEL_PROB = 0.9
|
||||
LOW_SPEED_WEAK_LEAD_ACCEL_CAP_MIN_LEAD_DELTA = -0.5
|
||||
LOW_SPEED_WEAK_LEAD_ACCEL_CAP_MAX_LEAD_DELTA = 0.75
|
||||
LOW_SPEED_WEAK_LEAD_ACCEL_CAP_MIN_LEAD_ACCEL = -0.4
|
||||
LOW_SPEED_WEAK_LEAD_ACCEL_CAP_MAX_LEAD_ACCEL = 0.25
|
||||
LOW_SPEED_WEAK_LEAD_ACCEL_CAP_MIN_ACCEL = 0.08
|
||||
LOW_SPEED_WEAK_LEAD_ACCEL_CAP_MAX_ACCEL = 0.22
|
||||
LOW_SPEED_WEAK_LEAD_ACCEL_CAP_STRONG_DEPART_MAX_EGO_SPEED = 1.25
|
||||
LOW_SPEED_WEAK_LEAD_ACCEL_CAP_STRONG_DEPART_MIN_GAP = 4.0
|
||||
LOW_SPEED_WEAK_LEAD_ACCEL_CAP_STRONG_DEPART_MIN_LEAD_SPEED = 1.2
|
||||
LOW_SPEED_WEAK_LEAD_ACCEL_CAP_STRONG_DEPART_MIN_LEAD_DELTA = 0.8
|
||||
LOW_SPEED_WEAK_LEAD_ACCEL_CAP_STRONG_DEPART_MIN_LEAD_ACCEL = 0.5
|
||||
CLOSE_LEAD_BRAKE_CAP_MAX_TTC = 25.0
|
||||
VISION_LEAD_APPROACH_MIN_CLOSING_SPEED = 2.0
|
||||
VISION_LEAD_APPROACH_TRIGGER_TIME = 4.5
|
||||
@@ -1118,6 +1140,59 @@ class LongitudinalPlanner:
|
||||
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_low_speed_weak_lead_accel_cap(self, lead, v_ego):
|
||||
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 < LOW_SPEED_WEAK_LEAD_ACCEL_CAP_MIN_MODEL_PROB:
|
||||
return None
|
||||
|
||||
d_rel = float(getattr(lead, "dRel", 0.0))
|
||||
lead_speed = max(float(getattr(lead, "vLead", 0.0)), 0.0)
|
||||
lead_delta = lead_speed - float(v_ego)
|
||||
lead_accel = float(getattr(lead, "aLeadK", 0.0))
|
||||
lead_lateral = abs(float(getattr(lead, "yRel", 0.0)))
|
||||
if (
|
||||
float(v_ego) > LOW_SPEED_WEAK_LEAD_ACCEL_CAP_MAX_EGO_SPEED or
|
||||
d_rel > LOW_SPEED_WEAK_LEAD_ACCEL_CAP_MAX_DISTANCE or
|
||||
lead_speed > LOW_SPEED_WEAK_LEAD_ACCEL_CAP_MAX_LEAD_SPEED or
|
||||
lead_lateral > LOW_SPEED_WEAK_LEAD_ACCEL_CAP_MAX_LATERAL_OFFSET or
|
||||
lead_delta > LOW_SPEED_WEAK_LEAD_ACCEL_CAP_MAX_LEAD_DELTA or
|
||||
lead_accel > LOW_SPEED_WEAK_LEAD_ACCEL_CAP_MAX_LEAD_ACCEL
|
||||
):
|
||||
return None
|
||||
|
||||
if (
|
||||
float(v_ego) <= LOW_SPEED_WEAK_LEAD_ACCEL_CAP_STRONG_DEPART_MAX_EGO_SPEED and
|
||||
d_rel >= LOW_SPEED_WEAK_LEAD_ACCEL_CAP_STRONG_DEPART_MIN_GAP and
|
||||
lead_speed >= LOW_SPEED_WEAK_LEAD_ACCEL_CAP_STRONG_DEPART_MIN_LEAD_SPEED and
|
||||
lead_delta >= LOW_SPEED_WEAK_LEAD_ACCEL_CAP_STRONG_DEPART_MIN_LEAD_DELTA and
|
||||
lead_accel >= LOW_SPEED_WEAK_LEAD_ACCEL_CAP_STRONG_DEPART_MIN_LEAD_ACCEL
|
||||
):
|
||||
return None
|
||||
|
||||
distance_factor = float(np.clip(
|
||||
(d_rel - LOW_SPEED_WEAK_LEAD_ACCEL_CAP_MIN_DISTANCE) /
|
||||
max(LOW_SPEED_WEAK_LEAD_ACCEL_CAP_MAX_DISTANCE - LOW_SPEED_WEAK_LEAD_ACCEL_CAP_MIN_DISTANCE, 0.1),
|
||||
0.0, 1.0,
|
||||
))
|
||||
delta_factor = float(np.clip(
|
||||
(lead_delta - LOW_SPEED_WEAK_LEAD_ACCEL_CAP_MIN_LEAD_DELTA) /
|
||||
max(LOW_SPEED_WEAK_LEAD_ACCEL_CAP_MAX_LEAD_DELTA - LOW_SPEED_WEAK_LEAD_ACCEL_CAP_MIN_LEAD_DELTA, 0.1),
|
||||
0.0, 1.0,
|
||||
))
|
||||
accel_factor = float(np.clip(
|
||||
(lead_accel - LOW_SPEED_WEAK_LEAD_ACCEL_CAP_MIN_LEAD_ACCEL) /
|
||||
max(LOW_SPEED_WEAK_LEAD_ACCEL_CAP_MAX_LEAD_ACCEL - LOW_SPEED_WEAK_LEAD_ACCEL_CAP_MIN_LEAD_ACCEL, 0.1),
|
||||
0.0, 1.0,
|
||||
))
|
||||
cap_strength = float(np.clip(0.5 * distance_factor + 0.3 * delta_factor + 0.2 * accel_factor, 0.0, 1.0))
|
||||
return LOW_SPEED_WEAK_LEAD_ACCEL_CAP_MIN_ACCEL + (
|
||||
LOW_SPEED_WEAK_LEAD_ACCEL_CAP_MAX_ACCEL - LOW_SPEED_WEAK_LEAD_ACCEL_CAP_MIN_ACCEL
|
||||
) * cap_strength
|
||||
|
||||
def get_standstill_stopped_lead_guard_cap(self, lead, v_ego, accel_min, stop_distance,
|
||||
release_ready, confident_depart_ready):
|
||||
if lead is None or not lead.status or release_ready or confident_depart_ready:
|
||||
@@ -1777,12 +1852,23 @@ class LongitudinalPlanner:
|
||||
if lead is None or not lead.status:
|
||||
return False
|
||||
|
||||
d_rel = max(float(lead.dRel), 0.0)
|
||||
lead_speed = max(float(getattr(lead, "vLead", 0.0)), 0.0)
|
||||
closing_speed = float(v_ego - lead.vLead)
|
||||
lead_braking = float(lead.aLeadK) < -0.5
|
||||
centered_lead = abs(float(getattr(lead, "yRel", 0.0))) <= RAW_LEAD_LOW_SPEED_HOLD_MAX_LATERAL_OFFSET
|
||||
if (
|
||||
centered_lead and
|
||||
float(v_ego) <= RAW_LEAD_LOW_SPEED_HOLD_MAX_EGO_SPEED and
|
||||
lead_speed <= RAW_LEAD_LOW_SPEED_HOLD_MAX_LEAD_SPEED and
|
||||
d_rel <= RAW_LEAD_LOW_SPEED_HOLD_MAX_DISTANCE and
|
||||
closing_speed >= RAW_LEAD_LOW_SPEED_HOLD_MIN_CLOSING_SPEED
|
||||
):
|
||||
return True
|
||||
|
||||
if closing_speed <= RAW_LEAD_SAFETY_MIN_CLOSING_SPEED and not lead_braking:
|
||||
return False
|
||||
|
||||
d_rel = max(float(lead.dRel), 0.0)
|
||||
dynamic_distance = max(RAW_LEAD_SAFETY_DISTANCE, 3.0 * float(v_ego))
|
||||
ttc = d_rel / max(closing_speed, 0.1) if closing_speed > 0.1 else float("inf")
|
||||
return d_rel < dynamic_distance and (ttc < RAW_LEAD_SAFETY_TTC or lead_braking)
|
||||
@@ -2317,6 +2403,17 @@ class LongitudinalPlanner:
|
||||
float(sm['carState'].vEgo) <= LEAD_DEPART_ACCEL_HOLD_MAX_EGO_SPEED
|
||||
)
|
||||
|
||||
low_speed_weak_lead_accel_cap = None
|
||||
if not output_should_stop:
|
||||
low_speed_weak_lead_accel_caps = [
|
||||
cap for cap in (
|
||||
self.get_low_speed_weak_lead_accel_cap(self.lead_one, scene_v_ego),
|
||||
self.get_low_speed_weak_lead_accel_cap(self.lead_two, scene_v_ego),
|
||||
) if cap is not None
|
||||
]
|
||||
if low_speed_weak_lead_accel_caps:
|
||||
low_speed_weak_lead_accel_cap = min(low_speed_weak_lead_accel_caps)
|
||||
|
||||
close_stop_active = bool(output_should_stop or vision_low_speed_stop_active)
|
||||
|
||||
close_stop_hold_cap = None
|
||||
@@ -2540,6 +2637,10 @@ class LongitudinalPlanner:
|
||||
if lead_depart_accel_hold_active:
|
||||
output_a_target = max(output_a_target, lead_depart_accel_floor)
|
||||
|
||||
if low_speed_weak_lead_accel_cap is not None:
|
||||
self.a_desired = min(self.a_desired, low_speed_weak_lead_accel_cap)
|
||||
output_a_target = min(output_a_target, low_speed_weak_lead_accel_cap)
|
||||
|
||||
force_stop_handoff = bool(
|
||||
getattr(sm['starpilotPlan'], 'forcingStop', False) and
|
||||
not lead_control_active and
|
||||
|
||||
@@ -567,6 +567,60 @@ def test_acc_mode_pretracking_vision_slow_lead_blocks_positive_catchup(model_ver
|
||||
assert planner_with_lead.output_a_target < -0.2
|
||||
|
||||
|
||||
@pytest.mark.parametrize("model_version", ["v11", "v12", "v13", "v14", "v15"])
|
||||
def test_acc_mode_keeps_close_slow_radar_lead_active_when_tracking_flaps(model_version):
|
||||
v_ego = 0.96
|
||||
|
||||
CP = CarInterface.get_non_essential_params(CAR.HONDA_CIVIC)
|
||||
planner_no_lead = LongitudinalPlanner(CP, init_v=v_ego)
|
||||
planner_with_lead = LongitudinalPlanner(CP, init_v=v_ego)
|
||||
sm_no_lead = make_sm(
|
||||
v_ego,
|
||||
desired_accel=0.45,
|
||||
min_accel=-0.5,
|
||||
experimental_mode=False,
|
||||
tracking_lead=False,
|
||||
)
|
||||
sm_with_lead = make_sm(
|
||||
v_ego,
|
||||
desired_accel=0.45,
|
||||
min_accel=-0.5,
|
||||
experimental_mode=False,
|
||||
tracking_lead=False,
|
||||
lead_one=make_lead(status=True, d_rel=8.15, v_lead=0.09, a_lead=-0.36, radar=True, model_prob=1.0, y_rel=0.1),
|
||||
)
|
||||
sm_no_lead["starpilotPlan"].vCruise = v_ego + 8.0
|
||||
sm_with_lead["starpilotPlan"].vCruise = v_ego + 8.0
|
||||
|
||||
planner_no_lead.update(sm_no_lead, make_toggles(model_version))
|
||||
planner_with_lead.update(sm_with_lead, make_toggles(model_version))
|
||||
|
||||
assert planner_with_lead.raw_close_lead_needs_control(sm_with_lead["radarState"].leadOne, v_ego)
|
||||
assert planner_with_lead.output_a_target < planner_no_lead.output_a_target - 0.15
|
||||
assert planner_with_lead.output_a_target <= 0.22
|
||||
|
||||
|
||||
@pytest.mark.parametrize("model_version", ["v11", "v12", "v13", "v14", "v15"])
|
||||
def test_low_speed_weak_departure_accel_cap_softens_voacc_follow_pulse(model_version):
|
||||
v_ego = 2.8
|
||||
|
||||
CP = CarInterface.get_non_essential_params(CAR.HONDA_CIVIC)
|
||||
planner = LongitudinalPlanner(CP, init_v=v_ego)
|
||||
sm = make_sm(
|
||||
v_ego,
|
||||
desired_accel=0.45,
|
||||
min_accel=-0.5,
|
||||
experimental_mode=False,
|
||||
tracking_lead=True,
|
||||
lead_one=make_lead(status=True, d_rel=16.6, v_lead=2.85, a_lead=0.05, radar=False, model_prob=0.99, y_rel=0.0),
|
||||
)
|
||||
sm["starpilotPlan"].vCruise = v_ego + 10.0
|
||||
|
||||
planner.update(sm, make_toggles(model_version))
|
||||
|
||||
assert planner.output_a_target <= 0.22
|
||||
|
||||
|
||||
@pytest.mark.parametrize("model_version", ["v11", "v12", "v13", "v14", "v15"])
|
||||
def test_acc_mode_pretracking_vision_far_slower_lead_starts_braking_before_tracking(model_version):
|
||||
v_ego = 21.48
|
||||
|
||||
@@ -79,6 +79,20 @@ def should_loud_blindspot_alert_without_lateral(CS, sm, starpilot_toggles) -> bo
|
||||
)
|
||||
|
||||
|
||||
def get_starpilot_alert_filters(current_alert_types: list[str], clear_event_types: set[str], starpilot_events: Events) -> tuple[list[str], set[str]]:
|
||||
starpilot_alert_types = list(current_alert_types)
|
||||
starpilot_clear_event_types = set(clear_event_types)
|
||||
|
||||
# This alert is explicitly allowed while lateral is paused/off. The state
|
||||
# machine only exposes WARNING while active/AOL, so let this warning through.
|
||||
if StarPilotEventName.laneChangeBlockedLoud in starpilot_events.names:
|
||||
if ET.WARNING not in starpilot_alert_types:
|
||||
starpilot_alert_types.append(ET.WARNING)
|
||||
starpilot_clear_event_types.discard(ET.WARNING)
|
||||
|
||||
return starpilot_alert_types, starpilot_clear_event_types
|
||||
|
||||
|
||||
class SelfdriveD:
|
||||
def __init__(self, CP=None):
|
||||
self.params = Params()
|
||||
@@ -691,11 +705,14 @@ class SelfdriveD:
|
||||
self.AM.add_many(self.sm.frame, alerts)
|
||||
self.AM.process_alerts(self.sm.frame, clear_event_types)
|
||||
|
||||
starpilot_alerts = self.starpilot_events.create_alerts(self.state_machine.current_alert_types, [self.CP, CS, self.sm, self.is_metric,
|
||||
starpilot_alert_types, starpilot_clear_event_types = get_starpilot_alert_filters(
|
||||
self.state_machine.current_alert_types, clear_event_types, self.starpilot_events
|
||||
)
|
||||
starpilot_alerts = self.starpilot_events.create_alerts(starpilot_alert_types, [self.CP, CS, self.sm, self.is_metric,
|
||||
self.state_machine.soft_disable_timer, pers,
|
||||
self.starpilot_toggles])
|
||||
self.starpilot_AM.add_many(self.sm.frame, starpilot_alerts)
|
||||
self.starpilot_AM.process_alerts(self.sm.frame, clear_event_types)
|
||||
self.starpilot_AM.process_alerts(self.sm.frame, starpilot_clear_event_types)
|
||||
|
||||
def publish_selfdriveState(self, CS):
|
||||
# selfdriveState
|
||||
|
||||
@@ -1,11 +1,14 @@
|
||||
from types import SimpleNamespace
|
||||
|
||||
from cereal import log
|
||||
from openpilot.selfdrive.selfdrived.selfdrived import should_loud_blindspot_alert_without_lateral
|
||||
from cereal import custom, log
|
||||
from openpilot.selfdrive.selfdrived.alertmanager import AlertManager
|
||||
from openpilot.selfdrive.selfdrived.events import ET, Events
|
||||
from openpilot.selfdrive.selfdrived.selfdrived import get_starpilot_alert_filters, should_loud_blindspot_alert_without_lateral
|
||||
|
||||
|
||||
LaneChangeState = log.LaneChangeState
|
||||
LaneChangeDirection = log.LaneChangeDirection
|
||||
StarPilotEventName = custom.StarPilotOnroadEvent.EventName
|
||||
|
||||
|
||||
def _car_state(left_blinker=False, right_blinker=False, left_blindspot=False, right_blindspot=False):
|
||||
@@ -66,3 +69,28 @@ def test_loud_blindspot_alert_without_lateral_handles_paused_pre_lane_change_wit
|
||||
sm = _sm(lane_change_state=LaneChangeState.preLaneChange, lat_active=True, lateral_check=True, pause_lateral=True)
|
||||
|
||||
assert should_loud_blindspot_alert_without_lateral(CS, sm, _toggles())
|
||||
|
||||
|
||||
def test_loud_blindspot_alert_survives_disabled_warning_filter():
|
||||
events = Events(starpilot=True)
|
||||
events.add(StarPilotEventName.laneChangeBlockedLoud)
|
||||
|
||||
alert_types, clear_event_types = get_starpilot_alert_filters([ET.PERMANENT], {ET.WARNING}, events)
|
||||
|
||||
alerts = events.create_alerts(alert_types)
|
||||
alert_manager = AlertManager()
|
||||
alert_manager.add_many(0, alerts)
|
||||
alert_manager.process_alerts(0, clear_event_types)
|
||||
|
||||
assert alert_manager.current_alert.alert_type == "laneChangeBlockedLoud/warning"
|
||||
|
||||
|
||||
def test_disabled_starpilot_warnings_stay_filtered_without_blindspot_event():
|
||||
events = Events(starpilot=True)
|
||||
events.add(StarPilotEventName.noLaneAvailable)
|
||||
|
||||
alert_types, clear_event_types = get_starpilot_alert_filters([ET.PERMANENT], {ET.WARNING}, events)
|
||||
|
||||
assert ET.WARNING not in alert_types
|
||||
assert ET.WARNING in clear_event_types
|
||||
assert events.create_alerts(alert_types) == []
|
||||
|
||||
Reference in New Issue
Block a user