diff --git a/selfdrive/controls/lib/latcontrol_torque.py b/selfdrive/controls/lib/latcontrol_torque.py index f1d69dbd6..d18ec7c00 100644 --- a/selfdrive/controls/lib/latcontrol_torque.py +++ b/selfdrive/controls/lib/latcontrol_torque.py @@ -547,6 +547,12 @@ IONIQ_6_CRAWL_TURN_IN_FF_SPEED = 4.5 IONIQ_6_CRAWL_TURN_IN_FF_SPEED_WIDTH = 0.8 IONIQ_6_CRAWL_TURN_IN_FF_LAT = 0.10 IONIQ_6_CRAWL_TURN_IN_FF_LAT_WIDTH = 0.05 +IONIQ_6_HIGH_SPEED_RIGHT_TURN_IN_FF_BOOST = 0.10 +IONIQ_6_HIGH_SPEED_RIGHT_TURN_IN_FF_SPEED = 18.0 +IONIQ_6_HIGH_SPEED_RIGHT_TURN_IN_FF_SPEED_WIDTH = 2.5 +IONIQ_6_HIGH_SPEED_RIGHT_TURN_IN_FF_LAT_START = 0.06 +IONIQ_6_HIGH_SPEED_RIGHT_TURN_IN_FF_LAT_END = 0.22 +IONIQ_6_HIGH_SPEED_RIGHT_TURN_IN_FF_LAT_WIDTH = 0.035 IONIQ_6_HEAVY_DIRECTIONAL_TAPER_LAT_START = 0.82 IONIQ_6_HEAVY_DIRECTIONAL_TAPER_LAT_WIDTH = 0.12 IONIQ_6_HEAVY_DIRECTIONAL_TAPER_BASE_LEFT = 0.10 @@ -1742,7 +1748,17 @@ def get_ioniq_6_ff_scale(desired_lateral_accel: float, desired_lateral_jerk: flo IONIQ_6_CRAWL_TURN_IN_FF_LAT_WIDTH) crawl_turn_in_scale = _ioniq_6_side_value(desired_lateral_accel, IONIQ_6_CRAWL_TURN_IN_FF_BOOST_LEFT, IONIQ_6_CRAWL_TURN_IN_FF_BOOST_RIGHT) * crawl_speed_weight * crawl_lat_weight - return (1.0 + crawl_turn_in_scale + (extra_scale * turn_in_boost * max(unwind_taper, 0.0))) * get_ioniq_6_directional_taper_scale(desired_lateral_accel, desired_lateral_jerk, v_ego) + high_speed_right_turn_in_scale = 0.0 + if desired_lateral_accel < 0.0 and desired_lateral_accel * desired_lateral_jerk > 0.0: + high_speed_weight = _ioniq_6_sigmoid((max(v_ego, 0.0) - IONIQ_6_HIGH_SPEED_RIGHT_TURN_IN_FF_SPEED) / + IONIQ_6_HIGH_SPEED_RIGHT_TURN_IN_FF_SPEED_WIDTH) + high_speed_lat_onset = _ioniq_6_sigmoid((abs_lateral_accel - IONIQ_6_HIGH_SPEED_RIGHT_TURN_IN_FF_LAT_START) / + IONIQ_6_HIGH_SPEED_RIGHT_TURN_IN_FF_LAT_WIDTH) + high_speed_lat_cutoff = _ioniq_6_sigmoid((IONIQ_6_HIGH_SPEED_RIGHT_TURN_IN_FF_LAT_END - abs_lateral_accel) / + IONIQ_6_HIGH_SPEED_RIGHT_TURN_IN_FF_LAT_WIDTH) + high_speed_right_turn_in_scale = IONIQ_6_HIGH_SPEED_RIGHT_TURN_IN_FF_BOOST * high_speed_weight * high_speed_lat_onset * high_speed_lat_cutoff + return (1.0 + crawl_turn_in_scale + high_speed_right_turn_in_scale + + (extra_scale * turn_in_boost * max(unwind_taper, 0.0))) * get_ioniq_6_directional_taper_scale(desired_lateral_accel, desired_lateral_jerk, v_ego) def get_ioniq_6_friction_threshold(v_ego: float, desired_lateral_accel: float = 0.0, desired_lateral_jerk: float = 0.0) -> float: diff --git a/selfdrive/controls/lib/longitudinal_planner.py b/selfdrive/controls/lib/longitudinal_planner.py index 3607873ae..08ca94d58 100755 --- a/selfdrive/controls/lib/longitudinal_planner.py +++ b/selfdrive/controls/lib/longitudinal_planner.py @@ -211,6 +211,9 @@ LEAD_CATCHUP_ACCEL_MIN_EGO = 8.0 LEAD_CATCHUP_ACCEL_MIN_LEAD_DELTA = -0.5 LEAD_CATCHUP_ACCEL_MAX_GAP_BUFFER_MIN = 4.0 LEAD_CATCHUP_ACCEL_MAX_GAP_BUFFER_GAIN = 0.15 +RADAR_MATCHED_FOLLOW_CATCHUP_CAP_BUFFER_MARGIN = 0.75 +RADAR_MATCHED_FOLLOW_CATCHUP_HOLD_CAP = 0.04 +RADAR_MATCHED_FOLLOW_CATCHUP_HOLD_MAX_GAP_ERROR = 0.75 POST_DEPARTURE_FOLLOW_BYPASS_MIN_SPEED = 12.0 POST_DEPARTURE_FOLLOW_BYPASS_MIN_MODEL_PROB = 0.95 POST_DEPARTURE_FOLLOW_BYPASS_MIN_LEAD_DELTA = 0.35 @@ -1367,8 +1370,6 @@ class LongitudinalPlanner: lead_delta = float(lead.vLead) - float(v_ego) min_lead_delta = LOW_SPEED_FOLLOW_ACCEL_CAP_MIN_LEAD_DELTA if low_speed_follow_window else LEAD_CATCHUP_ACCEL_MIN_LEAD_DELTA - if lead_delta < min_lead_delta: - return None desired_gap = float(desired_follow_distance(v_ego, lead.vLead, t_follow)) if low_speed_follow_window: @@ -1378,6 +1379,23 @@ class LongitudinalPlanner: gap_buffer = max(LEAD_CATCHUP_ACCEL_MAX_GAP_BUFFER_MIN, LEAD_CATCHUP_ACCEL_MAX_GAP_BUFFER_GAIN * float(v_ego)) gap_error = float(lead.dRel) - desired_gap + + radar_matched_follow_active = ( + lead_radar and + tracking_lead_active and + self.lead_is_matched_follow_window(lead, v_ego, t_follow) + ) + if radar_matched_follow_active and gap_error > (gap_buffer - RADAR_MATCHED_FOLLOW_CATCHUP_CAP_BUFFER_MARGIN): + return None + + if (radar_matched_follow_active and current_source == "cruise" and + gap_error <= RADAR_MATCHED_FOLLOW_CATCHUP_HOLD_MAX_GAP_ERROR and + lead_delta < min_lead_delta): + return RADAR_MATCHED_FOLLOW_CATCHUP_HOLD_CAP + + if lead_delta < min_lead_delta: + return None + if gap_error > gap_buffer: return None diff --git a/selfdrive/controls/tests/test_longitudinal_planner.py b/selfdrive/controls/tests/test_longitudinal_planner.py index 242253c22..ca77f066e 100644 --- a/selfdrive/controls/tests/test_longitudinal_planner.py +++ b/selfdrive/controls/tests/test_longitudinal_planner.py @@ -2447,6 +2447,46 @@ def test_route_8bc6_catchup_cap_skips_slightly_negative_delta_when_lead_accelera assert cap is None +def test_route_8bc6_radar_matched_follow_catchup_cap_skips_buffer_edge_square_wave(): + v_ego = 22.0 + CP = CarInterface.get_non_essential_params(CAR.HONDA_CIVIC) + planner = LongitudinalPlanner(CP, init_v=v_ego) + lead = make_lead( + status=True, d_rel=41.558, v_lead=22.3, a_lead=0.0, radar=True, model_prob=1.0, y_rel=0.0, + ) + + cap = planner.get_lead_catchup_accel_cap( + lead, + v_ego, + 1.45, + current_source="cruise", + tracking_lead_active=True, + ) + + assert planner.lead_is_matched_follow_window(lead, v_ego, 1.45) + assert cap is None + + +def test_route_8bc6_radar_matched_follow_catchup_cap_holds_small_cap_for_slower_lead_on_cruise(): + v_ego = 22.0 + CP = CarInterface.get_non_essential_params(CAR.HONDA_CIVIC) + planner = LongitudinalPlanner(CP, init_v=v_ego) + lead = make_lead( + status=True, d_rel=39.108, v_lead=21.2, a_lead=0.0, radar=True, model_prob=1.0, y_rel=0.0, + ) + + cap = planner.get_lead_catchup_accel_cap( + lead, + v_ego, + 1.45, + current_source="cruise", + tracking_lead_active=True, + ) + + assert planner.lead_is_matched_follow_window(lead, v_ego, 1.45) + assert cap == pytest.approx(0.04, abs=1e-6) + + def test_post_departure_pullaway_bypass_does_not_skip_when_lead_brakes_again(): v_ego = 19.03 CP = CarInterface.get_non_essential_params(CAR.HONDA_CIVIC) diff --git a/selfdrive/controls/tests/test_starpilot_vcruise.py b/selfdrive/controls/tests/test_starpilot_vcruise.py index 3e6ec2f97..2f7461a6e 100644 --- a/selfdrive/controls/tests/test_starpilot_vcruise.py +++ b/selfdrive/controls/tests/test_starpilot_vcruise.py @@ -1,3 +1,5 @@ +import datetime + import pytest from openpilot.common.constants import CV @@ -206,6 +208,51 @@ def test_standstill_seeded_force_stop_hold_requires_clear_window_before_release( assert not vcruise.forcing_stop +def test_standstill_seeded_force_stop_hold_accepts_datetime_now_without_crashing(): + planner, vcruise = make_vcruise(red_light=True, raw_model_stopped=False, forcing_stop=False) + sm = make_sm(standstill=True) + toggles = make_toggles() + base = datetime.datetime(2026, 6, 18, tzinfo=datetime.timezone.utc) + + first = vcruise.update( + controls_enabled=True, + now=base, + time_validated=True, + v_cruise=20.0, + v_ego=0.0, + sm=sm, + starpilot_toggles=toggles, + ) + assert first == pytest.approx(0.0) + assert vcruise.standstill_force_stop_hold + + planner.starpilot_cem.stop_light_detected = False + second = vcruise.update( + controls_enabled=True, + now=base + datetime.timedelta(seconds=0.4), + time_validated=True, + v_cruise=20.0, + v_ego=0.0, + sm=sm, + starpilot_toggles=toggles, + ) + assert second == pytest.approx(0.0) + assert vcruise.standstill_force_stop_hold + + released = vcruise.update( + controls_enabled=True, + now=base + datetime.timedelta(seconds=1.2), + time_validated=True, + v_cruise=20.0, + v_ego=0.0, + sm=sm, + starpilot_toggles=toggles, + ) + assert released == pytest.approx(20.0) + assert not vcruise.standstill_force_stop_hold + assert not vcruise.forcing_stop + + def test_nav_turn_speed_control_default_off(): _, vcruise = make_vcruise(nav_state={ "valid": True, diff --git a/starpilot/controls/lib/starpilot_vcruise.py b/starpilot/controls/lib/starpilot_vcruise.py index 98f0fca9f..fa4245652 100644 --- a/starpilot/controls/lib/starpilot_vcruise.py +++ b/starpilot/controls/lib/starpilot_vcruise.py @@ -100,6 +100,11 @@ class StarPilotVCruise: self._nav_instruction_state = {} + @staticmethod + def _elapsed_seconds(now, since): + delta = now - since + return delta.total_seconds() if hasattr(delta, "total_seconds") else float(delta) + @staticmethod def _nav_maneuver_target_speed(maneuver_type, maneuver_modifier): maneuver_type = str(maneuver_type or "").strip().lower() @@ -241,7 +246,7 @@ class StarPilotVCruise: self.standstill_force_stop_clear_since = 0.0 elif self.standstill_force_stop_clear_since == 0.0: self.standstill_force_stop_clear_since = now - elif (now - self.standstill_force_stop_clear_since) >= STANDSTILL_FORCE_STOP_CLEAR_TIME: + elif self._elapsed_seconds(now, self.standstill_force_stop_clear_since) >= STANDSTILL_FORCE_STOP_CLEAR_TIME: self.standstill_force_stop_hold = False self.standstill_force_stop_clear_since = 0.0