mirror of
https://github.com/firestar5683/StarPilot.git
synced 2026-08-04 15:56:04 +08:00
long planner / i5 / sonata
This commit is contained in:
firestar5683
@@ -130,6 +130,9 @@ IONIQ_6_CARS = (
|
||||
SONATA_HYBRID_CARS = (
|
||||
HYUNDAI_CAR.HYUNDAI_SONATA_HYBRID,
|
||||
)
|
||||
SONATA_CARS = (
|
||||
HYUNDAI_CAR.HYUNDAI_SONATA,
|
||||
)
|
||||
ELANTRA_NON_SCC_CARS = (
|
||||
HYUNDAI_CAR.HYUNDAI_ELANTRA_2022_NON_SCC,
|
||||
HYUNDAI_CAR.HYUNDAI_ELANTRA_HEV_2022_NON_SCC,
|
||||
@@ -272,6 +275,29 @@ SONATA_HYBRID_LOW_SPEED_CENTER_TAPER_LAT_WIDTH = 0.02
|
||||
SONATA_HYBRID_LOW_SPEED_CENTER_TAPER_SPEED_MAX = 7.5
|
||||
SONATA_HYBRID_LOW_SPEED_CENTER_TAPER_SPEED_WIDTH = 1.0
|
||||
|
||||
SONATA_FF_REDUCTION_LEFT = 0.04
|
||||
SONATA_FF_REDUCTION_RIGHT = 0.26
|
||||
SONATA_FF_ONSET = 0.18
|
||||
SONATA_FF_ONSET_WIDTH = 0.08
|
||||
SONATA_FF_CUTOFF = 1.40
|
||||
SONATA_FF_CUTOFF_WIDTH = 0.42
|
||||
SONATA_TRANSITION_SPEED = 8.5
|
||||
SONATA_PHASE_SCALE = 0.12
|
||||
SONATA_TURN_IN_BOOST_LEFT = 0.18
|
||||
SONATA_TURN_IN_BOOST_RIGHT = 0.00
|
||||
SONATA_UNWIND_TAPER_LEFT = 0.28
|
||||
SONATA_UNWIND_TAPER_RIGHT = 0.00
|
||||
SONATA_CENTER_TAPER_MAX = 0.04
|
||||
SONATA_CENTER_TAPER_LAT = 0.15
|
||||
SONATA_CENTER_TAPER_LAT_WIDTH = 0.025
|
||||
SONATA_CENTER_TAPER_SPEED = 22.0
|
||||
SONATA_CENTER_TAPER_SPEED_WIDTH = 2.5
|
||||
SONATA_LOW_SPEED_CENTER_TAPER_MAX = 0.08
|
||||
SONATA_LOW_SPEED_CENTER_TAPER_LAT = 0.10
|
||||
SONATA_LOW_SPEED_CENTER_TAPER_LAT_WIDTH = 0.02
|
||||
SONATA_LOW_SPEED_CENTER_TAPER_SPEED_MAX = 7.0
|
||||
SONATA_LOW_SPEED_CENTER_TAPER_SPEED_WIDTH = 1.0
|
||||
|
||||
ELANTRA_NON_SCC_FF_ADJUST_LEFT = 0.02
|
||||
ELANTRA_NON_SCC_FF_ADJUST_RIGHT = -0.02
|
||||
ELANTRA_NON_SCC_FF_ONSET = 0.14
|
||||
@@ -359,26 +385,26 @@ IONIQ_5_FF_ONSET = 0.10
|
||||
IONIQ_5_FF_ONSET_WIDTH = 0.05
|
||||
IONIQ_5_FF_CUTOFF = 1.20
|
||||
IONIQ_5_FF_CUTOFF_WIDTH = 0.30
|
||||
IONIQ_5_TRANSITION_SPEED = 11.0
|
||||
IONIQ_5_TRANSITION_SPEED = 12.5
|
||||
IONIQ_5_PHASE_SCALE = 0.10
|
||||
IONIQ_5_FF_REDUCTION_LEFT = 0.12
|
||||
IONIQ_5_FF_REDUCTION_RIGHT = 0.22
|
||||
IONIQ_5_TURN_IN_BOOST_LEFT = 0.11
|
||||
IONIQ_5_TURN_IN_BOOST_RIGHT = 0.00
|
||||
IONIQ_5_UNWIND_TAPER_LEFT = 0.68
|
||||
IONIQ_5_UNWIND_TAPER_RIGHT = 0.64
|
||||
IONIQ_5_TURN_IN_BOOST_LEFT = 0.14
|
||||
IONIQ_5_TURN_IN_BOOST_RIGHT = 0.06
|
||||
IONIQ_5_UNWIND_TAPER_LEFT = 0.76
|
||||
IONIQ_5_UNWIND_TAPER_RIGHT = 0.86
|
||||
IONIQ_5_TURN_IN_THRESHOLD_REDUCTION_LEFT = 0.08
|
||||
IONIQ_5_TURN_IN_THRESHOLD_REDUCTION_RIGHT = 0.00
|
||||
IONIQ_5_UNWIND_THRESHOLD_INCREASE_LEFT = 0.32
|
||||
IONIQ_5_UNWIND_THRESHOLD_INCREASE_RIGHT = 0.24
|
||||
IONIQ_5_TURN_IN_THRESHOLD_REDUCTION_RIGHT = 0.05
|
||||
IONIQ_5_UNWIND_THRESHOLD_INCREASE_LEFT = 0.36
|
||||
IONIQ_5_UNWIND_THRESHOLD_INCREASE_RIGHT = 0.38
|
||||
IONIQ_5_TURN_IN_FRICTION_BOOST_LEFT = 0.04
|
||||
IONIQ_5_TURN_IN_FRICTION_BOOST_RIGHT = 0.00
|
||||
IONIQ_5_UNWIND_FRICTION_REDUCTION_LEFT = 0.30
|
||||
IONIQ_5_UNWIND_FRICTION_REDUCTION_RIGHT = 0.22
|
||||
IONIQ_5_CENTER_TAPER_MAX = 0.12
|
||||
IONIQ_5_CENTER_TAPER_LAT = 0.13
|
||||
IONIQ_5_TURN_IN_FRICTION_BOOST_RIGHT = 0.03
|
||||
IONIQ_5_UNWIND_FRICTION_REDUCTION_LEFT = 0.34
|
||||
IONIQ_5_UNWIND_FRICTION_REDUCTION_RIGHT = 0.34
|
||||
IONIQ_5_CENTER_TAPER_MAX = 0.14
|
||||
IONIQ_5_CENTER_TAPER_LAT = 0.12
|
||||
IONIQ_5_CENTER_TAPER_LAT_WIDTH = 0.03
|
||||
IONIQ_5_CENTER_TAPER_SPEED = 16.5
|
||||
IONIQ_5_CENTER_TAPER_SPEED = 16.0
|
||||
IONIQ_5_CENTER_TAPER_SPEED_WIDTH = 2.5
|
||||
|
||||
IONIQ_EV_OLD_BASE_LAT_ACCEL_FACTOR_MULT = 1.16
|
||||
@@ -1022,6 +1048,53 @@ def get_sonata_hybrid_center_taper_scale(desired_lateral_accel: float, v_ego: fl
|
||||
return 1.0 - reduction
|
||||
|
||||
|
||||
def _sonata_sigmoid(x: float) -> float:
|
||||
return _sigmoid(x)
|
||||
|
||||
|
||||
def _sonata_low_speed_factor(v_ego: float) -> float:
|
||||
return 1.0 / (1.0 + (max(v_ego, 0.0) / SONATA_TRANSITION_SPEED) ** 2)
|
||||
|
||||
|
||||
def _sonata_transition_phase(desired_lateral_accel: float, desired_lateral_jerk: float) -> float:
|
||||
return math.tanh((desired_lateral_accel * desired_lateral_jerk) / SONATA_PHASE_SCALE)
|
||||
|
||||
|
||||
def _sonata_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_sonata_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 = _sonata_sigmoid((abs_lateral_accel - SONATA_FF_ONSET) / SONATA_FF_ONSET_WIDTH)
|
||||
cutoff = _sonata_sigmoid((SONATA_FF_CUTOFF - abs_lateral_accel) / SONATA_FF_CUTOFF_WIDTH)
|
||||
base_reduction = _sonata_side_value(desired_lateral_accel, SONATA_FF_REDUCTION_LEFT, SONATA_FF_REDUCTION_RIGHT) * onset * cutoff
|
||||
phase = _sonata_transition_phase(desired_lateral_accel, desired_lateral_jerk)
|
||||
turn_in_weight = max(phase, 0.0)
|
||||
unwind_weight = max(-phase, 0.0)
|
||||
low_speed_factor = _sonata_low_speed_factor(v_ego)
|
||||
turn_in_boost = 1.0 + (_sonata_side_value(desired_lateral_accel, SONATA_TURN_IN_BOOST_LEFT, SONATA_TURN_IN_BOOST_RIGHT) *
|
||||
turn_in_weight * low_speed_factor)
|
||||
unwind_taper = 1.0 - (_sonata_side_value(desired_lateral_accel, SONATA_UNWIND_TAPER_LEFT, SONATA_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_sonata_center_taper_scale(desired_lateral_accel: float, v_ego: float) -> float:
|
||||
speed_weight = _sonata_sigmoid((v_ego - SONATA_CENTER_TAPER_SPEED) / SONATA_CENTER_TAPER_SPEED_WIDTH)
|
||||
center_weight = _sonata_sigmoid((SONATA_CENTER_TAPER_LAT - abs(desired_lateral_accel)) / SONATA_CENTER_TAPER_LAT_WIDTH)
|
||||
reduction = SONATA_CENTER_TAPER_MAX * speed_weight * center_weight
|
||||
low_speed_weight = _sonata_sigmoid((SONATA_LOW_SPEED_CENTER_TAPER_SPEED_MAX - v_ego) /
|
||||
SONATA_LOW_SPEED_CENTER_TAPER_SPEED_WIDTH)
|
||||
low_speed_center_weight = _sonata_sigmoid((SONATA_LOW_SPEED_CENTER_TAPER_LAT - abs(desired_lateral_accel)) /
|
||||
SONATA_LOW_SPEED_CENTER_TAPER_LAT_WIDTH)
|
||||
reduction += SONATA_LOW_SPEED_CENTER_TAPER_MAX * low_speed_weight * low_speed_center_weight
|
||||
return 1.0 - reduction
|
||||
|
||||
|
||||
def _elantra_non_scc_sigmoid(x: float) -> float:
|
||||
return _sigmoid(x)
|
||||
|
||||
@@ -1678,6 +1751,7 @@ class LatControlTorque(LatControl):
|
||||
self.is_ioniq_5 = CP.carFingerprint in IONIQ_5_CARS
|
||||
self.is_ioniq_ev_old = CP.carFingerprint in IONIQ_EV_OLD_CARS
|
||||
self.is_ioniq_6 = CP.carFingerprint in IONIQ_6_CARS
|
||||
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_forte = CP.carFingerprint in KIA_FORTE_CARS
|
||||
@@ -1809,6 +1883,7 @@ class LatControlTorque(LatControl):
|
||||
ioniq_5_active = self.is_ioniq_5
|
||||
ioniq_ev_old_active = self.is_ioniq_ev_old
|
||||
ioniq_6_active = self.is_ioniq_6
|
||||
sonata_active = self.is_sonata
|
||||
sonata_hybrid_active = self.is_sonata_hybrid
|
||||
elantra_non_scc_active = self.is_elantra_non_scc
|
||||
kia_forte_active = self.is_kia_forte
|
||||
@@ -1818,6 +1893,7 @@ class LatControlTorque(LatControl):
|
||||
volt_standard_center_taper = get_volt_standard_center_taper_scale(setpoint, CS.vEgo) if volt_standard_test_active else 1.0
|
||||
ioniq_ev_old_center_taper = get_ioniq_ev_old_center_taper_scale(setpoint, CS.vEgo) if ioniq_ev_old_active else 1.0
|
||||
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_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
|
||||
@@ -1859,6 +1935,8 @@ class LatControlTorque(LatControl):
|
||||
friction_threshold = get_ioniq_6_friction_threshold(CS.vEgo, setpoint, desired_lateral_jerk) / max(ioniq_6_center_taper, 1e-3)
|
||||
friction_scale = get_ioniq_6_friction_scale(CS.vEgo, setpoint, desired_lateral_jerk)
|
||||
friction_scale = 1.0 + ((friction_scale - 1.0) * ioniq_6_center_taper)
|
||||
elif sonata_active:
|
||||
ff *= get_sonata_ff_scale(setpoint, desired_lateral_jerk, CS.vEgo) * sonata_center_taper
|
||||
elif sonata_hybrid_active:
|
||||
ff *= get_sonata_hybrid_ff_scale(setpoint, desired_lateral_jerk, CS.vEgo) * sonata_hybrid_center_taper
|
||||
elif elantra_non_scc_active:
|
||||
|
||||
@@ -499,9 +499,9 @@ class LongitudinalMpc:
|
||||
|
||||
# Adjust filter time constants for complex scenes
|
||||
if abs(filter_time_factor - getattr(self, 'prev_filter_time_factor', 1.0)) > 0.05:
|
||||
new_filter_time = self.current_filter_time * filter_time_factor
|
||||
current_a = self.lead_a_filter.x if hasattr(self.lead_a_filter, 'x') else 0.0
|
||||
current_v = self.lead_v_filter.x if hasattr(self.lead_v_filter, 'x') else 0.0
|
||||
new_filter_time = self.current_filter_time * filter_time_factor
|
||||
self.lead_a_filter = FirstOrderFilter(current_a, new_filter_time, self.dt)
|
||||
self.lead_v_filter = FirstOrderFilter(current_v, new_filter_time, self.dt)
|
||||
self.prev_filter_time_factor = filter_time_factor
|
||||
@@ -640,7 +640,6 @@ class LongitudinalMpc:
|
||||
lead_one = radarstate.leadOne
|
||||
lead_two = radarstate.leadTwo
|
||||
self.status = tracking_lead and (lead_one.status or lead_two.status)
|
||||
|
||||
lead_xv_0 = self.process_lead(lead_one, tracking_lead, t_follow=t_follow)
|
||||
lead_xv_1 = self.process_lead(lead_two, tracking_lead, t_follow=t_follow)
|
||||
|
||||
|
||||
@@ -237,6 +237,17 @@ MATCHED_FOLLOW_TRANSITION_MAX_POSITIVE_STEP = 0.18
|
||||
MATCHED_FOLLOW_TRANSITION_MIN_NEGATIVE_STEP = 0.08
|
||||
MATCHED_FOLLOW_TRANSITION_MAX_NEGATIVE_STEP = 0.16
|
||||
MATCHED_FOLLOW_TRANSITION_SIGN_CROSS_STEP = 0.10
|
||||
NEAR_DUPLICATE_LEAD_TRANSITION_MIN_SPEED = 20.0
|
||||
NEAR_DUPLICATE_LEAD_TRANSITION_MIN_MODEL_PROB = 0.95
|
||||
NEAR_DUPLICATE_LEAD_TRANSITION_MAX_LEAD_BRAKE = 0.35
|
||||
NEAR_DUPLICATE_LEAD_TRANSITION_MAX_CLOSING_SPEED = 3.5
|
||||
NEAR_DUPLICATE_LEAD_TRANSITION_MIN_TTC = 8.0
|
||||
NEAR_DUPLICATE_LEAD_TRANSITION_MIN_HEADWAY_BELOW_TARGET = 0.45
|
||||
NEAR_DUPLICATE_LEAD_TRANSITION_MAX_HEADWAY_ABOVE_TARGET = 0.85
|
||||
NEAR_DUPLICATE_LEAD_TRANSITION_MIN_DELTA_A = 0.35
|
||||
NEAR_DUPLICATE_LEAD_TRANSITION_POSITIVE_STEP = 0.22
|
||||
NEAR_DUPLICATE_LEAD_TRANSITION_NEGATIVE_STEP = 0.32
|
||||
NEAR_DUPLICATE_LEAD_TRANSITION_SIGN_CROSS_STEP = 0.18
|
||||
TRACKED_VISION_MODEL_FLOOR_MIN_SPEED = 10.0
|
||||
TRACKED_VISION_MODEL_FLOOR_MIN_MODEL_PROB = 0.95
|
||||
TRACKED_VISION_MODEL_FLOOR_MIN_MODEL_DECEL = 0.80
|
||||
@@ -1275,6 +1286,58 @@ class LongitudinalPlanner:
|
||||
smoothed_target = float(np.clip(output_a_target, lower, upper))
|
||||
return smoothed_target if abs(smoothed_target - float(output_a_target)) > 1e-6 else None
|
||||
|
||||
def get_near_duplicate_lead_transition_target(self, lead, v_ego, base_t_follow,
|
||||
prev_output_a_target, output_a_target,
|
||||
current_source, tracking_lead_active):
|
||||
if lead is None or not lead.status:
|
||||
return None
|
||||
if current_source not in ("lead0", "lead1") and not tracking_lead_active:
|
||||
return None
|
||||
if not (self.lead_one.status and self.lead_two.status):
|
||||
return None
|
||||
if not self.mpc.leads_are_near_duplicates(self.lead_one, self.lead_two, v_ego):
|
||||
return None
|
||||
if float(v_ego) < NEAR_DUPLICATE_LEAD_TRANSITION_MIN_SPEED:
|
||||
return None
|
||||
|
||||
lead_prob = float(getattr(lead, "modelProb", 0.0))
|
||||
if bool(getattr(lead, "radar", False)) or lead_prob < NEAR_DUPLICATE_LEAD_TRANSITION_MIN_MODEL_PROB:
|
||||
return None
|
||||
|
||||
lead_brake = max(0.0, -float(getattr(lead, "aLeadK", 0.0)))
|
||||
if lead_brake > NEAR_DUPLICATE_LEAD_TRANSITION_MAX_LEAD_BRAKE:
|
||||
return None
|
||||
|
||||
relative_speed = float(v_ego) - float(lead.vLead)
|
||||
closing_speed = max(0.0, relative_speed)
|
||||
if closing_speed > NEAR_DUPLICATE_LEAD_TRANSITION_MAX_CLOSING_SPEED:
|
||||
return None
|
||||
|
||||
ttc = float(lead.dRel) / max(closing_speed, 0.1) if closing_speed > 0.1 else float("inf")
|
||||
if ttc < NEAR_DUPLICATE_LEAD_TRANSITION_MIN_TTC:
|
||||
return None
|
||||
|
||||
actual_headway = float(lead.dRel) / max(float(v_ego), 1e-3)
|
||||
if actual_headway < max(0.0, float(base_t_follow) - NEAR_DUPLICATE_LEAD_TRANSITION_MIN_HEADWAY_BELOW_TARGET):
|
||||
return None
|
||||
if actual_headway > float(base_t_follow) + NEAR_DUPLICATE_LEAD_TRANSITION_MAX_HEADWAY_ABOVE_TARGET:
|
||||
return None
|
||||
|
||||
target_delta = float(output_a_target) - float(prev_output_a_target)
|
||||
if abs(target_delta) < NEAR_DUPLICATE_LEAD_TRANSITION_MIN_DELTA_A:
|
||||
return None
|
||||
|
||||
positive_step = NEAR_DUPLICATE_LEAD_TRANSITION_POSITIVE_STEP
|
||||
negative_step = NEAR_DUPLICATE_LEAD_TRANSITION_NEGATIVE_STEP
|
||||
if float(prev_output_a_target) * float(output_a_target) < 0.0:
|
||||
positive_step = min(positive_step, NEAR_DUPLICATE_LEAD_TRANSITION_SIGN_CROSS_STEP)
|
||||
negative_step = min(negative_step, NEAR_DUPLICATE_LEAD_TRANSITION_SIGN_CROSS_STEP)
|
||||
|
||||
lower = float(prev_output_a_target) - negative_step
|
||||
upper = float(prev_output_a_target) + positive_step
|
||||
smoothed_target = float(np.clip(output_a_target, lower, upper))
|
||||
return smoothed_target if abs(smoothed_target - float(output_a_target)) > 1e-6 else None
|
||||
|
||||
def get_tracked_vision_model_brake_floor(self, lead, v_ego, accel_min, t_follow, model_desired):
|
||||
if lead is None or not lead.status or bool(getattr(lead, "radar", False)):
|
||||
return None
|
||||
@@ -1990,6 +2053,23 @@ class LongitudinalPlanner:
|
||||
self.a_desired = max(self.a_desired, matched_follow_transition_target)
|
||||
output_a_target = matched_follow_transition_target
|
||||
|
||||
if optional_far_lead_comfort and comfort_lead is not None and not panic_bypass and not output_should_stop and not vision_low_speed_stop_active:
|
||||
near_duplicate_transition_target = self.get_near_duplicate_lead_transition_target(
|
||||
comfort_lead,
|
||||
scene_v_ego,
|
||||
effective_t_follow,
|
||||
prev_output_a_target,
|
||||
output_a_target,
|
||||
self.mpc.source,
|
||||
bool(getattr(sm["starpilotPlan"], "trackingLead", False)),
|
||||
)
|
||||
if near_duplicate_transition_target is not None:
|
||||
if near_duplicate_transition_target < output_a_target:
|
||||
self.a_desired = min(self.a_desired, near_duplicate_transition_target)
|
||||
else:
|
||||
self.a_desired = max(self.a_desired, near_duplicate_transition_target)
|
||||
output_a_target = near_duplicate_transition_target
|
||||
|
||||
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))
|
||||
|
||||
|
||||
@@ -61,6 +61,8 @@ from openpilot.selfdrive.controls.lib.latcontrol_torque import (
|
||||
get_kia_ev6_ff_scale,
|
||||
get_kia_ev6_friction_scale,
|
||||
get_kia_ev6_friction_threshold,
|
||||
get_sonata_center_taper_scale,
|
||||
get_sonata_ff_scale,
|
||||
get_sonata_hybrid_center_taper_scale,
|
||||
get_sonata_hybrid_ff_scale,
|
||||
get_volt_standard_center_taper_scale,
|
||||
@@ -243,6 +245,26 @@ class TestLatControl:
|
||||
assert get_sonata_hybrid_center_taper_scale(0.0, 3.0) < get_sonata_hybrid_center_taper_scale(0.0, 10.0)
|
||||
assert get_sonata_hybrid_center_taper_scale(0.0, 30.0) < get_sonata_hybrid_center_taper_scale(0.20, 30.0) <= 1.0
|
||||
|
||||
def test_sonata_ff_scale_curve(self):
|
||||
assert get_sonata_ff_scale(0.0, 0.0, 20.0) == 1.0
|
||||
steady_left = get_sonata_ff_scale(0.45, 0.0, 8.0)
|
||||
steady_right = get_sonata_ff_scale(-0.45, 0.0, 8.0)
|
||||
turn_in_left = get_sonata_ff_scale(0.45, 0.8, 8.0)
|
||||
turn_in_right = get_sonata_ff_scale(-0.45, -0.8, 8.0)
|
||||
unwind_left = get_sonata_ff_scale(0.45, -0.8, 8.0)
|
||||
unwind_right = get_sonata_ff_scale(-0.45, 0.8, 8.0)
|
||||
assert steady_left < 1.0
|
||||
assert steady_right < steady_left
|
||||
assert turn_in_left > steady_left
|
||||
assert turn_in_right == pytest.approx(steady_right)
|
||||
assert unwind_left < steady_left
|
||||
assert unwind_right == pytest.approx(steady_right)
|
||||
|
||||
def test_sonata_center_taper_curve(self):
|
||||
assert get_sonata_center_taper_scale(0.0, 30.0) < get_sonata_center_taper_scale(0.0, 15.0)
|
||||
assert get_sonata_center_taper_scale(0.0, 3.0) < get_sonata_center_taper_scale(0.0, 10.0)
|
||||
assert get_sonata_center_taper_scale(0.0, 30.0) < get_sonata_center_taper_scale(0.20, 30.0) <= 1.0
|
||||
|
||||
def test_elantra_non_scc_ff_scale_curve(self):
|
||||
assert get_elantra_non_scc_ff_scale(0.0, 0.0, 20.0) == 1.0
|
||||
steady_left = get_elantra_non_scc_ff_scale(0.45, 0.0, 8.0)
|
||||
@@ -352,9 +374,9 @@ class TestLatControl:
|
||||
assert steady_left < 1.0
|
||||
assert steady_right < steady_left
|
||||
assert turn_in_left > steady_left
|
||||
assert turn_in_right >= steady_right
|
||||
assert turn_in_right > steady_right
|
||||
assert unwind_left < steady_left
|
||||
assert unwind_right > unwind_left
|
||||
assert unwind_right < unwind_left
|
||||
|
||||
def test_ioniq_5_friction_curves(self):
|
||||
base = get_friction_threshold(12.0)
|
||||
@@ -363,18 +385,17 @@ class TestLatControl:
|
||||
unwind_left_threshold = get_ioniq_5_friction_threshold(12.0, 0.7, -0.8)
|
||||
unwind_right_threshold = get_ioniq_5_friction_threshold(12.0, -0.7, 0.8)
|
||||
assert turn_in_left_threshold < base
|
||||
assert turn_in_right_threshold == pytest.approx(base)
|
||||
assert turn_in_left_threshold < turn_in_right_threshold < base
|
||||
assert unwind_left_threshold > base
|
||||
assert unwind_right_threshold < unwind_left_threshold
|
||||
assert unwind_right_threshold > unwind_left_threshold
|
||||
|
||||
turn_in_left_scale = get_ioniq_5_friction_scale(12.0, 0.7, 0.8)
|
||||
turn_in_right_scale = get_ioniq_5_friction_scale(12.0, -0.7, -0.8)
|
||||
unwind_left_scale = get_ioniq_5_friction_scale(12.0, 0.7, -0.8)
|
||||
unwind_right_scale = get_ioniq_5_friction_scale(12.0, -0.7, 0.8)
|
||||
assert turn_in_left_scale > 1.0
|
||||
assert turn_in_right_scale == pytest.approx(1.0)
|
||||
assert turn_in_left_scale > turn_in_right_scale > 1.0
|
||||
assert unwind_left_scale < 1.0
|
||||
assert unwind_right_scale > unwind_left_scale
|
||||
assert unwind_right_scale <= unwind_left_scale
|
||||
|
||||
def test_ioniq_5_center_taper_curve(self):
|
||||
assert get_ioniq_5_center_taper_scale(0.0, 25.0) < get_ioniq_5_center_taper_scale(0.0, 10.0)
|
||||
@@ -537,6 +558,16 @@ class TestLatControl:
|
||||
assert lac_log.active
|
||||
assert controller.torque_params.latAccelFactor == pytest.approx(CP.lateralTuning.torque.latAccelFactor * 0.98)
|
||||
|
||||
def test_sonata_default_update_path(self):
|
||||
controller, VM, CS, params, starpilot_toggles = self._build_torque_controller(HYUNDAI.HYUNDAI_SONATA)
|
||||
CarInterface = interfaces[HYUNDAI.HYUNDAI_SONATA]
|
||||
CP = CarInterface.get_non_essential_params(HYUNDAI.HYUNDAI_SONATA)
|
||||
|
||||
_, _, lac_log = controller.update(True, CS, VM, params, False, 0.0025, False, 0.2, None, None, starpilot_toggles)
|
||||
|
||||
assert lac_log.active
|
||||
assert controller.torque_params.latAccelFactor == pytest.approx(CP.lateralTuning.torque.latAccelFactor)
|
||||
|
||||
def test_ioniq_5_default_update_path(self):
|
||||
controller, VM, CS, params, starpilot_toggles = self._build_torque_controller(HYUNDAI.HYUNDAI_IONIQ_5)
|
||||
CarInterface = interfaces[HYUNDAI.HYUNDAI_IONIQ_5]
|
||||
|
||||
@@ -1826,3 +1826,77 @@ def test_near_duplicate_lead_source_hysteresis_skips_distinct_leads():
|
||||
|
||||
assert lead_0_bias == 0.0
|
||||
assert lead_1_bias == 0.0
|
||||
|
||||
|
||||
def test_near_duplicate_lead_transition_target_damps_same_source_sign_flip():
|
||||
v_ego = 25.0
|
||||
CP = CarInterface.get_non_essential_params(CAR.HONDA_CIVIC)
|
||||
planner = LongitudinalPlanner(CP, init_v=v_ego)
|
||||
lead_one = make_lead(status=True, d_rel=44.6, v_lead=24.05, a_lead=-0.03, radar=False, model_prob=0.99)
|
||||
lead_two = make_lead(status=True, d_rel=45.1, v_lead=24.0, a_lead=-0.04, radar=False, model_prob=0.99)
|
||||
lead_one.vRel = -0.95
|
||||
lead_two.vRel = -1.00
|
||||
planner.lead_one = lead_one
|
||||
planner.lead_two = lead_two
|
||||
|
||||
smoothed = planner.get_near_duplicate_lead_transition_target(
|
||||
lead_two,
|
||||
v_ego,
|
||||
1.45,
|
||||
prev_output_a_target=-1.10,
|
||||
output_a_target=0.13,
|
||||
current_source="lead1",
|
||||
tracking_lead_active=True,
|
||||
)
|
||||
|
||||
assert smoothed is not None
|
||||
assert smoothed == pytest.approx(-0.92, abs=1e-6)
|
||||
|
||||
|
||||
def test_near_duplicate_lead_transition_target_damps_tracking_cruise_sign_flip():
|
||||
v_ego = 25.0
|
||||
CP = CarInterface.get_non_essential_params(CAR.HONDA_CIVIC)
|
||||
planner = LongitudinalPlanner(CP, init_v=v_ego)
|
||||
lead_one = make_lead(status=True, d_rel=44.6, v_lead=24.05, a_lead=-0.03, radar=False, model_prob=0.99)
|
||||
lead_two = make_lead(status=True, d_rel=45.1, v_lead=24.0, a_lead=-0.04, radar=False, model_prob=0.99)
|
||||
lead_one.vRel = -0.95
|
||||
lead_two.vRel = -1.00
|
||||
planner.lead_one = lead_one
|
||||
planner.lead_two = lead_two
|
||||
|
||||
smoothed = planner.get_near_duplicate_lead_transition_target(
|
||||
lead_two,
|
||||
v_ego,
|
||||
1.45,
|
||||
prev_output_a_target=-1.10,
|
||||
output_a_target=0.13,
|
||||
current_source="cruise",
|
||||
tracking_lead_active=True,
|
||||
)
|
||||
|
||||
assert smoothed is not None
|
||||
assert smoothed == pytest.approx(-0.92, abs=1e-6)
|
||||
|
||||
|
||||
def test_near_duplicate_lead_transition_target_skips_plain_cruise_without_tracking():
|
||||
v_ego = 25.0
|
||||
CP = CarInterface.get_non_essential_params(CAR.HONDA_CIVIC)
|
||||
planner = LongitudinalPlanner(CP, init_v=v_ego)
|
||||
lead_one = make_lead(status=True, d_rel=44.6, v_lead=24.05, a_lead=-0.03, radar=False, model_prob=0.99)
|
||||
lead_two = make_lead(status=True, d_rel=45.1, v_lead=24.0, a_lead=-0.04, radar=False, model_prob=0.99)
|
||||
lead_one.vRel = -0.95
|
||||
lead_two.vRel = -1.00
|
||||
planner.lead_one = lead_one
|
||||
planner.lead_two = lead_two
|
||||
|
||||
smoothed = planner.get_near_duplicate_lead_transition_target(
|
||||
lead_two,
|
||||
v_ego,
|
||||
1.45,
|
||||
prev_output_a_target=-1.10,
|
||||
output_a_target=0.13,
|
||||
current_source="cruise",
|
||||
tracking_lead_active=False,
|
||||
)
|
||||
|
||||
assert smoothed is None
|
||||
|
||||
Reference in New Issue
Block a user