From 9f30661f6dbf72dc3f597edb0f43ee922a34bce7 Mon Sep 17 00:00:00 2001 From: firestar5683 <168790843+firestar5683@users.noreply.github.com> Date: Tue, 2 Jun 2026 11:18:34 -0500 Subject: [PATCH] long planner / i5 / sonata --- selfdrive/controls/lib/latcontrol_torque.py | 106 +++++++++++++++--- .../lib/longitudinal_mpc_lib/long_mpc.py | 3 +- .../controls/lib/longitudinal_planner.py | 80 +++++++++++++ selfdrive/controls/tests/test_latcontrol.py | 45 ++++++-- .../tests/test_longitudinal_planner.py | 74 ++++++++++++ 5 files changed, 285 insertions(+), 23 deletions(-) diff --git a/selfdrive/controls/lib/latcontrol_torque.py b/selfdrive/controls/lib/latcontrol_torque.py index 0654b7dd5..e891b445c 100644 --- a/selfdrive/controls/lib/latcontrol_torque.py +++ b/selfdrive/controls/lib/latcontrol_torque.py @@ -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: diff --git a/selfdrive/controls/lib/longitudinal_mpc_lib/long_mpc.py b/selfdrive/controls/lib/longitudinal_mpc_lib/long_mpc.py index f1eed1b26..8e6e02fbf 100755 --- a/selfdrive/controls/lib/longitudinal_mpc_lib/long_mpc.py +++ b/selfdrive/controls/lib/longitudinal_mpc_lib/long_mpc.py @@ -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) diff --git a/selfdrive/controls/lib/longitudinal_planner.py b/selfdrive/controls/lib/longitudinal_planner.py index 83bbcafa0..a0bab90af 100755 --- a/selfdrive/controls/lib/longitudinal_planner.py +++ b/selfdrive/controls/lib/longitudinal_planner.py @@ -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)) diff --git a/selfdrive/controls/tests/test_latcontrol.py b/selfdrive/controls/tests/test_latcontrol.py index c5a376e4c..89a78fbba 100644 --- a/selfdrive/controls/tests/test_latcontrol.py +++ b/selfdrive/controls/tests/test_latcontrol.py @@ -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] diff --git a/selfdrive/controls/tests/test_longitudinal_planner.py b/selfdrive/controls/tests/test_longitudinal_planner.py index d215856ca..50072b9ac 100644 --- a/selfdrive/controls/tests/test_longitudinal_planner.py +++ b/selfdrive/controls/tests/test_longitudinal_planner.py @@ -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