From f501a4de372a4f695f8d52b21905d53160c2c740 Mon Sep 17 00:00:00 2001 From: firestar5683 <168790843+firestar5683@users.noreply.github.com> Date: Fri, 28 Aug 2026 00:01:11 -0500 Subject: [PATCH] nighty night --- selfdrive/controls/lib/latcontrol_torque.py | 3 + .../controls/lib/latcontrol_vehicle_tunes.py | 28 ++++++ selfdrive/controls/lib/lead_behavior.py | 5 +- .../lib/longitudinal_mpc_lib/long_mpc.py | 5 +- .../controls/lib/longitudinal_planner.py | 51 ++++++++++- .../lib/longitudinal_vehicle_tunes.py | 79 ++++++++++++++++- selfdrive/controls/tests/test_latcontrol.py | 14 +++ .../tests/test_longitudinal_planner.py | 85 ++++++++++++++++++- .../controls/tests/test_starpilot_planner.py | 1 + starpilot/controls/starpilot_planner.py | 4 - starpilot/starpilot_process.py | 2 + starpilot/system/starpilot_tracking.py | 57 ++++++++----- .../system/tests/test_starpilot_tracking.py | 47 ++++++++++ 13 files changed, 347 insertions(+), 34 deletions(-) create mode 100644 starpilot/system/tests/test_starpilot_tracking.py diff --git a/selfdrive/controls/lib/latcontrol_torque.py b/selfdrive/controls/lib/latcontrol_torque.py index c03821b61f..654ad3ab4a 100644 --- a/selfdrive/controls/lib/latcontrol_torque.py +++ b/selfdrive/controls/lib/latcontrol_torque.py @@ -516,6 +516,8 @@ class LatControlTorque(LatControl): vehicle_friction_jerk_deadzone = get_prius_friction_jerk_deadzone(CS.vEgo, setpoint) elif genesis_g70_active: vehicle_friction_jerk_deadzone = get_genesis_g70_friction_jerk_deadzone(CS.vEgo, setpoint) + elif self.is_genesis_gv70: + vehicle_friction_jerk_deadzone = get_genesis_gv70_friction_jerk_deadzone(CS.vEgo, setpoint) elif kia_carnival_active: vehicle_friction_jerk_deadzone = get_kia_carnival_friction_jerk_deadzone( CS.vEgo, setpoint, desired_lateral_jerk, @@ -638,6 +640,7 @@ class LatControlTorque(LatControl): low_speed_output_limit = get_genesis_g70_low_speed_output_limit(setpoint, CS.vEgo) output_torque = float(np.clip(output_torque, -low_speed_output_limit, low_speed_output_limit)) elif self.is_genesis_gv70: + output_torque *= get_genesis_gv70_center_output_scale(setpoint, CS.vEgo) output_torque *= get_genesis_gv70_high_speed_error_scale( setpoint, measurement, desired_lateral_jerk, CS.vEgo, ) diff --git a/selfdrive/controls/lib/latcontrol_vehicle_tunes.py b/selfdrive/controls/lib/latcontrol_vehicle_tunes.py index badda996f3..4f91b95340 100644 --- a/selfdrive/controls/lib/latcontrol_vehicle_tunes.py +++ b/selfdrive/controls/lib/latcontrol_vehicle_tunes.py @@ -218,6 +218,16 @@ GENESIS_GV70_FRICTION_CENTER_LAT = 0.28 GENESIS_GV70_FRICTION_CENTER_LAT_WIDTH = 0.12 GENESIS_GV70_FRICTION_CALM_JERK = 0.35 GENESIS_GV70_FRICTION_CALM_JERK_WIDTH = 0.10 +GENESIS_GV70_FRICTION_JERK_DEADZONE_MAX = 0.30 +GENESIS_GV70_FRICTION_JERK_DEADZONE_LAT = 0.30 +GENESIS_GV70_FRICTION_JERK_DEADZONE_LAT_WIDTH = 0.08 +GENESIS_GV70_FRICTION_JERK_DEADZONE_SPEED = 12.0 * CV.MPH_TO_MS +GENESIS_GV70_FRICTION_JERK_DEADZONE_SPEED_WIDTH = 3.5 * CV.MPH_TO_MS +GENESIS_GV70_CENTER_OUTPUT_TAPER_MAX = 0.14 +GENESIS_GV70_CENTER_OUTPUT_TAPER_LAT = 0.30 +GENESIS_GV70_CENTER_OUTPUT_TAPER_LAT_WIDTH = 0.10 +GENESIS_GV70_CENTER_OUTPUT_TAPER_SPEED = 22.0 * CV.MPH_TO_MS +GENESIS_GV70_CENTER_OUTPUT_TAPER_SPEED_WIDTH = 3.0 * CV.MPH_TO_MS GENESIS_GV70_UNWIND_FF_REDUCTION_MAX = 0.35 GENESIS_GV70_UNWIND_FF_OVERSHOOT = 0.15 GENESIS_GV70_UNWIND_FF_OVERSHOOT_WIDTH = 0.18 @@ -3029,6 +3039,24 @@ def get_genesis_gv70_friction_threshold(v_ego: float, desired_lateral_accel: flo return base_threshold * (1.0 + gain) +def get_genesis_gv70_friction_jerk_deadzone(v_ego: float, desired_lateral_accel: float) -> float: + """Suppress small jerk-driven friction flips around the GV70 lane center.""" + speed_weight = _sigmoid((v_ego - GENESIS_GV70_FRICTION_JERK_DEADZONE_SPEED) / + GENESIS_GV70_FRICTION_JERK_DEADZONE_SPEED_WIDTH) + center_weight = _sigmoid((GENESIS_GV70_FRICTION_JERK_DEADZONE_LAT - abs(desired_lateral_accel)) / + GENESIS_GV70_FRICTION_JERK_DEADZONE_LAT_WIDTH) + return GENESIS_GV70_FRICTION_JERK_DEADZONE_MAX * speed_weight * center_weight + + +def get_genesis_gv70_center_output_scale(desired_lateral_accel: float, v_ego: float) -> float: + """Dampen high-speed center corrections without reducing turn authority.""" + speed_weight = _sigmoid((v_ego - GENESIS_GV70_CENTER_OUTPUT_TAPER_SPEED) / + GENESIS_GV70_CENTER_OUTPUT_TAPER_SPEED_WIDTH) + center_weight = _sigmoid((GENESIS_GV70_CENTER_OUTPUT_TAPER_LAT - abs(desired_lateral_accel)) / + GENESIS_GV70_CENTER_OUTPUT_TAPER_LAT_WIDTH) + return 1.0 - (GENESIS_GV70_CENTER_OUTPUT_TAPER_MAX * speed_weight * center_weight) + + def get_genesis_gv70_unwind_ff_scale(setpoint: float, measured_lateral_accel: float, desired_lateral_jerk: float, v_ego: float) -> float: """Remove old-turn feedforward when the GV70 has already over-rotated.""" diff --git a/selfdrive/controls/lib/lead_behavior.py b/selfdrive/controls/lib/lead_behavior.py index 91aa2f5872..600b855ea9 100644 --- a/selfdrive/controls/lib/lead_behavior.py +++ b/selfdrive/controls/lib/lead_behavior.py @@ -115,7 +115,8 @@ def is_radarless_matched_follow_window(v_ego: float, lead_distance: float, v_lea def get_tracked_lead_catchup_bias(v_ego: float, lead_distance: float, desired_gap: float, closing_speed: float, v_cruise: float | None = None, y_rel: float | None = None, min_headway_margin: float = TRACKED_LEAD_CATCHUP_BIAS_MIN_HEADWAY_MARGIN, - full_headway_margin: float = TRACKED_LEAD_CATCHUP_BIAS_FULL_HEADWAY_MARGIN) -> float: + full_headway_margin: float = TRACKED_LEAD_CATCHUP_BIAS_FULL_HEADWAY_MARGIN, + bias_gain: float = TRACKED_LEAD_CATCHUP_BIAS_GAIN) -> float: gap_error = lead_distance - desired_gap actual_hw = lead_distance / max(v_ego, 1e-3) desired_hw = desired_gap / max(v_ego, 1e-3) @@ -155,7 +156,7 @@ def get_tracked_lead_catchup_bias(v_ego: float, lead_distance: float, desired_ga TRACKED_LEAD_CATCHUP_BIAS_MAX_LATERAL_OFFSET) bias_cap = max(10.0, TRACKED_LEAD_CATCHUP_BIAS_SPEED_FACTOR * v_ego) - return (min(gap_error * TRACKED_LEAD_CATCHUP_BIAS_GAIN, bias_cap) * speed_factor * cruise_factor * + return (min(gap_error * max(0.0, float(bias_gain)), bias_cap) * speed_factor * cruise_factor * entry_factor * exit_factor * closing_factor * lateral_factor) diff --git a/selfdrive/controls/lib/longitudinal_mpc_lib/long_mpc.py b/selfdrive/controls/lib/longitudinal_mpc_lib/long_mpc.py index f8b8a29bb3..6d1d737e5c 100755 --- a/selfdrive/controls/lib/longitudinal_mpc_lib/long_mpc.py +++ b/selfdrive/controls/lib/longitudinal_mpc_lib/long_mpc.py @@ -924,7 +924,8 @@ class LongitudinalMpc: personality=log.LongitudinalPersonality.standard, tracking_lead=True, optional_far_lead_comfort=True, smooth_duplicate_vision=False, stop_x=None, silverado_early_follow=False, modelV2=None, - lead_obstacle_bias=(0.0, 0.0), tracked_lead_catchup_headway_margins=None): + lead_obstacle_bias=(0.0, 0.0), tracked_lead_catchup_headway_margins=None, + tracked_lead_catchup_bias_gain=None): v_ego = self.x0[1] lead_one = radarstate.leadOne lead_two = radarstate.leadTwo @@ -978,6 +979,8 @@ class LongitudinalMpc: "min_headway_margin": tracked_lead_catchup_headway_margins[0], "full_headway_margin": tracked_lead_catchup_headway_margins[1], } + if tracked_lead_catchup_bias_gain is not None: + catchup_kwargs["bias_gain"] = tracked_lead_catchup_bias_gain cruise_obstacle += get_tracked_lead_catchup_bias( v_ego, lead_one.dRel, diff --git a/selfdrive/controls/lib/longitudinal_planner.py b/selfdrive/controls/lib/longitudinal_planner.py index 1fbc9d00a3..10b56514a5 100755 --- a/selfdrive/controls/lib/longitudinal_planner.py +++ b/selfdrive/controls/lib/longitudinal_planner.py @@ -23,6 +23,8 @@ from openpilot.selfdrive.controls.lib.longitudinal_vehicle_tunes import ( get_far_follow_output_slew_rates, get_follow_prebrake_min_headway, get_honda_accord_lead_departure_tune, + get_honda_accord_stop_go_accel_cap, + get_honda_accord_stop_go_accel_rise_rate, get_toyota_rav4_tss2_lead_departure_tune, get_force_stop_distance_bias, get_force_stop_handoff_distance, @@ -41,6 +43,7 @@ from openpilot.selfdrive.controls.lib.longitudinal_vehicle_tunes import ( get_standstill_gap_settle_max_extra_gap, get_standstill_stopped_lead_guard_distance_margin, get_standstill_stopped_lead_guard_max_lead_speed, + get_tracked_lead_catchup_bias_gain, get_tracked_lead_catchup_headway_margins, ) from openpilot.selfdrive.controls.lib.drive_helpers import CONTROL_N @@ -574,6 +577,7 @@ class LongitudinalPlanner: self.lead_depart_release_hold_remaining = 0.0 self.radar_standstill_gap_settle_elapsed = 0.0 self.radar_standstill_gap_settle_active = False + self.tracked_lead_catchup_bias_gain = get_tracked_lead_catchup_bias_gain(CP) self.v_desired_trajectory = np.zeros(CONTROL_N) self.a_desired_trajectory = np.zeros(CONTROL_N) @@ -1625,6 +1629,25 @@ class LongitudinalPlanner: LOW_SPEED_WEAK_LEAD_ACCEL_CAP_MAX_ACCEL - LOW_SPEED_WEAK_LEAD_ACCEL_CAP_MIN_ACCEL ) * cap_strength + def get_honda_accord_stop_go_accel_target(self, lead, v_ego, previous_target, target, blocked): + """Smooth the Accord's low-speed lead launch without touching braking targets.""" + if blocked or target <= 0.0: + return float(target) + + cap = get_honda_accord_stop_go_accel_cap(self.CP, lead, v_ego) + if cap is None: + return float(target) + + limited_target = min(float(target), cap) + if limited_target > float(previous_target): + rise_rate = get_honda_accord_stop_go_accel_rise_rate(self.CP) + if rise_rate > 0.0: + limited_target = min( + limited_target, + max(0.0, float(previous_target) + rise_rate * self.dt), + ) + return float(limited_target) + 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: @@ -2269,7 +2292,8 @@ class LongitudinalPlanner: silverado_early_follow=early_truck_follow, modelV2=sm['modelV2'], lead_obstacle_bias=stopped_lead_obstacle_bias, - tracked_lead_catchup_headway_margins=self.tracked_lead_catchup_headway_margins) + tracked_lead_catchup_headway_margins=self.tracked_lead_catchup_headway_margins, + tracked_lead_catchup_bias_gain=self.tracked_lead_catchup_bias_gain) self.a_desired_trajectory_full = np.interp(CONTROL_N_T_IDX, T_IDXS_MPC, self.mpc.a_solution) self.v_desired_trajectory = np.interp(CONTROL_N_T_IDX, T_IDXS_MPC, self.mpc.v_solution) @@ -2975,6 +2999,31 @@ class LongitudinalPlanner: if force_slow_decel and scene_v_ego > 0.1: output_a_target = min(output_a_target, FORCE_DECEL_MIN_ACCEL) + try: + starpilot_car_state = sm['starpilotCarState'] + except KeyError: + starpilot_car_state = None + driver_accel_pressed = bool( + getattr(sm['carState'], 'gasPressed', False) or + getattr(starpilot_car_state, 'accelPressed', False) + ) + accord_stop_go_blocked = bool( + not lead_control_active or + output_should_stop or + vision_low_speed_stop_active or + depart_safety_veto or + radar_gap_settle_active or + getattr(sm['starpilotPlan'], 'forcingStop', False) or + getattr(sm['starpilotPlan'], 'redLight', False) or + driver_accel_pressed + ) + accord_stop_go_target = self.get_honda_accord_stop_go_accel_target( + policy_lead, scene_v_ego, prev_output_a_target, output_a_target, accord_stop_go_blocked, + ) + if accord_stop_go_target < output_a_target: + self.a_desired = min(self.a_desired, accord_stop_go_target) + output_a_target = accord_stop_go_target + self.output_a_target = output_a_target self.output_should_stop = bool(output_should_stop or vision_low_speed_stop_active) diff --git a/selfdrive/controls/lib/longitudinal_vehicle_tunes.py b/selfdrive/controls/lib/longitudinal_vehicle_tunes.py index fe6466e814..6ed420054a 100644 --- a/selfdrive/controls/lib/longitudinal_vehicle_tunes.py +++ b/selfdrive/controls/lib/longitudinal_vehicle_tunes.py @@ -3,10 +3,22 @@ import numpy as np HONDA_HRV_3G_FAR_FOLLOW_BRAKE_SLEW_RATE = 3.0 HONDA_HRV_3G_FAR_FOLLOW_RELEASE_SLEW_RATE = 2.0 +HONDA_CRV_5G_FAR_FOLLOW_BRAKE_SLEW_RATE = 2.5 +HONDA_CRV_5G_FAR_FOLLOW_RELEASE_SLEW_RATE = 1.75 HONDA_HRV_3G_UNTRACKED_SLOW_LEAD_DECEL_SCALE = 1.35 HONDA_ACCORD_LEAD_DEPART_ACCEL_HOLD_MAX_ACCEL = 0.85 HONDA_ACCORD_LEAD_DEPART_ACCEL_ASSIST = 0.25 +HONDA_ACCORD_STOP_GO_MAX_EGO_SPEED = 4.5 +HONDA_ACCORD_STOP_GO_MIN_DISTANCE = 5.5 +HONDA_ACCORD_STOP_GO_MAX_DISTANCE = 14.0 +HONDA_ACCORD_STOP_GO_MAX_LEAD_SPEED = 4.5 +HONDA_ACCORD_STOP_GO_MIN_LEAD_SPEED = 0.4 +HONDA_ACCORD_STOP_GO_MAX_LEAD_BRAKE = 0.25 +HONDA_ACCORD_STOP_GO_MAX_LATERAL_OFFSET = 1.25 +HONDA_ACCORD_STOP_GO_MIN_MODEL_PROB = 0.95 +HONDA_ACCORD_STOP_GO_ACCEL_RISE_RATE = 4.0 HYUNDAI_ELANTRA_LEAD_FOLLOW_JERK_SCALE = 1.25 +GENESIS_GV70_ELECTRIFIED_LEAD_FOLLOW_JERK_SCALE = 1.35 GM_SILVERADO_EARLY_FOLLOW_MIN_EGO_SPEED = 18.0 GM_SILVERADO_EARLY_FOLLOW_MAX_DISTANCE = 130.0 GM_SILVERADO_EARLY_FOLLOW_MIN_MODEL_PROB = 0.85 @@ -14,8 +26,11 @@ GM_SILVERADO_EARLY_FOLLOW_MAX_LATERAL_OFFSET = 1.2 DEFAULT_FOLLOW_PREBRAKE_MIN_HEADWAY = 1.25 GM_SILVERADO_FOLLOW_PREBRAKE_MIN_HEADWAY = 1.25 FORD_LIGHTNING_FOLLOW_PREBRAKE_MIN_HEADWAY = 1.0 -FORD_LIGHTNING_TRACKED_LEAD_CATCHUP_MIN_HEADWAY_MARGIN = 0.20 -FORD_LIGHTNING_TRACKED_LEAD_CATCHUP_FULL_HEADWAY_MARGIN = 0.45 +FORD_LIGHTNING_TRACKED_LEAD_CATCHUP_MIN_HEADWAY_MARGIN = 0.10 +FORD_LIGHTNING_TRACKED_LEAD_CATCHUP_FULL_HEADWAY_MARGIN = 0.25 +FORD_LIGHTNING_TRACKED_LEAD_CATCHUP_BIAS_GAIN = 0.65 +FORD_LIGHTNING_FAR_FOLLOW_BRAKE_SLEW_RATE = 2.5 +FORD_LIGHTNING_FAR_FOLLOW_RELEASE_SLEW_RATE = 1.75 FORD_LIGHTNING_STANDSTILL_GUARD_DISTANCE_MARGIN = 5.0 FORD_LIGHTNING_STANDSTILL_GUARD_MAX_LEAD_SPEED = 0.60 TOYOTA_SIENNA_POST_DEPARTURE_RESTOP_MAX_EGO_SPEED = 2.0 @@ -265,6 +280,12 @@ def get_tracked_lead_catchup_headway_margins(CP): return None +def get_tracked_lead_catchup_bias_gain(CP): + if is_ford_f150_lightning(CP): + return FORD_LIGHTNING_TRACKED_LEAD_CATCHUP_BIAS_GAIN + return None + + def is_ford_f150_lightning(CP): return ( getattr(CP, "brand", "") == "ford" and @@ -371,11 +392,21 @@ def get_far_follow_output_slew_rates(CP): HONDA_HRV_3G_FAR_FOLLOW_BRAKE_SLEW_RATE, HONDA_HRV_3G_FAR_FOLLOW_RELEASE_SLEW_RATE, ) + if is_honda_crv_5g(CP): + return ( + HONDA_CRV_5G_FAR_FOLLOW_BRAKE_SLEW_RATE, + HONDA_CRV_5G_FAR_FOLLOW_RELEASE_SLEW_RATE, + ) if is_toyota_rav4_tss2_post_departure_tune(CP): return ( TOYOTA_RAV4_TSS2_FAR_FOLLOW_BRAKE_SLEW_RATE, TOYOTA_RAV4_TSS2_FAR_FOLLOW_RELEASE_SLEW_RATE, ) + if is_ford_f150_lightning(CP): + return ( + FORD_LIGHTNING_FAR_FOLLOW_BRAKE_SLEW_RATE, + FORD_LIGHTNING_FAR_FOLLOW_RELEASE_SLEW_RATE, + ) return 0.0, 0.0 @@ -389,6 +420,11 @@ def get_lead_follow_jerk_scale(CP): """Spread the lead-source transition for cars with a sharp vision-lead handoff.""" if getattr(CP, "brand", "") == "hyundai" and str(getattr(CP, "carFingerprint", "")) == "HYUNDAI_ELANTRA_2021": return HYUNDAI_ELANTRA_LEAD_FOLLOW_JERK_SCALE + if ( + getattr(CP, "brand", "") == "hyundai" and + str(getattr(CP, "carFingerprint", "")) == "GENESIS_GV70_ELECTRIFIED_1ST_GEN" + ): + return GENESIS_GV70_ELECTRIFIED_LEAD_FOLLOW_JERK_SCALE return 1.0 @@ -401,6 +437,45 @@ def get_honda_accord_lead_departure_tune(CP): return None +def get_honda_accord_stop_go_accel_cap(CP, lead, v_ego): + """Keep the Accord from launching at the full cruise acceleration into a close lead.""" + if ( + CP.brand != "honda" or str(CP.carFingerprint) != "HONDA_ACCORD" or + lead is None or not bool(getattr(lead, "status", False)) or + bool(getattr(lead, "radar", False)) or + float(getattr(lead, "modelProb", 0.0)) < HONDA_ACCORD_STOP_GO_MIN_MODEL_PROB or + float(v_ego) < 0.0 or float(v_ego) > HONDA_ACCORD_STOP_GO_MAX_EGO_SPEED or + abs(float(getattr(lead, "yRel", 0.0))) > HONDA_ACCORD_STOP_GO_MAX_LATERAL_OFFSET + ): + return None + + distance = float(getattr(lead, "dRel", float("inf"))) + lead_speed = max(float(getattr(lead, "vLead", 0.0)), 0.0) + lead_delta = lead_speed - float(v_ego) + lead_brake = max(0.0, -float(getattr(lead, "aLeadK", 0.0))) + if ( + not HONDA_ACCORD_STOP_GO_MIN_DISTANCE <= distance <= HONDA_ACCORD_STOP_GO_MAX_DISTANCE or + not HONDA_ACCORD_STOP_GO_MIN_LEAD_SPEED <= lead_speed <= HONDA_ACCORD_STOP_GO_MAX_LEAD_SPEED or + lead_delta < -0.25 or + lead_brake > HONDA_ACCORD_STOP_GO_MAX_LEAD_BRAKE + ): + return None + + speed_factor = float(np.clip(float(v_ego) / 2.5, 0.0, 1.0)) + gap_factor = float(np.clip( + (distance - HONDA_ACCORD_STOP_GO_MIN_DISTANCE) / + max(HONDA_ACCORD_STOP_GO_MAX_DISTANCE - HONDA_ACCORD_STOP_GO_MIN_DISTANCE, 0.1), + 0.0, 1.0, + )) + return float(0.90 + 0.12 * speed_factor + 0.10 * gap_factor) + + +def get_honda_accord_stop_go_accel_rise_rate(CP): + if CP.brand == "honda" and str(CP.carFingerprint) == "HONDA_ACCORD": + return HONDA_ACCORD_STOP_GO_ACCEL_RISE_RATE + return 0.0 + + def is_gm_silverado_early_follow_lead(CP, lead, v_ego): """Admit a credible centered vision lead before it becomes a close lead.""" if ( diff --git a/selfdrive/controls/tests/test_latcontrol.py b/selfdrive/controls/tests/test_latcontrol.py index 75ea766a5d..e46e9a39e4 100644 --- a/selfdrive/controls/tests/test_latcontrol.py +++ b/selfdrive/controls/tests/test_latcontrol.py @@ -91,6 +91,8 @@ from openpilot.selfdrive.controls.lib.latcontrol_torque import ( get_genesis_g70_low_speed_angle_damping, get_genesis_g70_low_speed_output_limit, get_genesis_g70_unwind_ff_scale, + get_genesis_gv70_center_output_scale, + get_genesis_gv70_friction_jerk_deadzone, get_genesis_gv70_friction_threshold, get_genesis_gv70_high_speed_error_scale, get_genesis_gv70_unwind_ff_scale, @@ -926,6 +928,18 @@ class TestLatControl: assert highway_turn == pytest.approx(base, rel=0.01) assert highway_center < center + def test_genesis_gv70_center_bounce_damping_preserves_turn_authority(self): + center_scale = get_genesis_gv70_center_output_scale(0.0, 30.0) + turn_scale = get_genesis_gv70_center_output_scale(0.8, 30.0) + low_speed_center_scale = get_genesis_gv70_center_output_scale(0.0, 5.0) + highway_center_deadzone = get_genesis_gv70_friction_jerk_deadzone(30.0, 0.0) + highway_turn_deadzone = get_genesis_gv70_friction_jerk_deadzone(30.0, 0.8) + + assert center_scale < low_speed_center_scale < 1.0 + assert turn_scale > center_scale + assert highway_center_deadzone > highway_turn_deadzone + assert highway_turn_deadzone < 0.05 + def test_genesis_gv70_high_speed_error_damping(self): assert get_genesis_gv70_high_speed_error_scale(0.2, 0.2, 0.8, 20.0) == 1.0 assert get_genesis_gv70_high_speed_error_scale(-0.7, 0.58, -0.8, 33.5) < 1.0 diff --git a/selfdrive/controls/tests/test_longitudinal_planner.py b/selfdrive/controls/tests/test_longitudinal_planner.py index 619b4cac39..ab69f3f113 100644 --- a/selfdrive/controls/tests/test_longitudinal_planner.py +++ b/selfdrive/controls/tests/test_longitudinal_planner.py @@ -30,6 +30,7 @@ from openpilot.selfdrive.controls.lib.longitudinal_vehicle_tunes import ( get_far_follow_output_slew_rates, get_follow_prebrake_min_headway, get_honda_accord_lead_departure_tune, + get_honda_accord_stop_go_accel_cap, get_honda_crv_5g_stopped_lead_obstacle_bias, get_honda_crv_5g_low_speed_stopped_lead_cap, allow_honda_crv_5g_vision_gap_settle, @@ -38,6 +39,7 @@ from openpilot.selfdrive.controls.lib.longitudinal_vehicle_tunes import ( get_standstill_gap_settle_max_extra_gap, get_standstill_stopped_lead_guard_distance_margin, get_standstill_stopped_lead_guard_max_lead_speed, + get_tracked_lead_catchup_bias_gain, get_tracked_lead_catchup_headway_margins, get_toyota_prius_stopped_lead_obstacle_bias, get_toyota_rav4_tss2_lead_departure_tune, @@ -319,6 +321,28 @@ def test_hrv_far_follow_output_slew_damps_only_continuous_safe_follow(): assert smoothed == pytest.approx(-0.5) +def test_crv_far_follow_output_slew_damps_nonurgent_lead_transition(): + v_ego = 24.0 + CP = CarInterface.get_non_essential_params(CAR.HONDA_CRV_5G) + planner = LongitudinalPlanner(CP, init_v=v_ego) + planner.lead_one = make_lead(status=True, d_rel=58.0, v_lead=20.0, model_prob=0.99) + planner.lead_two = make_lead(status=False) + + brake_rate, release_rate = get_far_follow_output_slew_rates(CP) + assert brake_rate == pytest.approx(2.5) + assert release_rate == pytest.approx(1.75) + + initial = planner.get_vehicle_far_follow_slew_target( + v_ego, prev_target=0.0, target=-0.6, output_should_stop=False, panic_bypass=False, + ) + smoothed = planner.get_vehicle_far_follow_slew_target( + v_ego, prev_target=initial, target=0.4, output_should_stop=False, panic_bypass=False, + ) + + assert initial == pytest.approx(-0.6) + assert smoothed == pytest.approx(initial + release_rate * planner.dt) + + @pytest.mark.parametrize("d_rel,v_lead,output_should_stop,panic_bypass", [ (20.0, 20.0, False, False), (35.0, 18.0, False, False), @@ -731,8 +755,32 @@ def test_lightning_stopped_lead_guard_tune_is_vehicle_specific(): assert get_standstill_stopped_lead_guard_distance_margin(civic) == pytest.approx(3.0) assert get_standstill_stopped_lead_guard_max_lead_speed(lightning, 0.45) == pytest.approx(0.60) assert get_standstill_stopped_lead_guard_max_lead_speed(civic, 0.45) == pytest.approx(0.45) - assert get_tracked_lead_catchup_headway_margins(lightning) == pytest.approx((0.20, 0.45)) + assert get_tracked_lead_catchup_headway_margins(lightning) == pytest.approx((0.10, 0.25)) + assert get_tracked_lead_catchup_bias_gain(lightning) == pytest.approx(0.65) assert get_tracked_lead_catchup_headway_margins(civic) is None + assert get_tracked_lead_catchup_bias_gain(civic) is None + + +def test_lightning_far_follow_output_slew_damps_nonurgent_lead_braking(): + v_ego = 24.0 + CP = FordCarInterface.get_non_essential_params(FORD_CAR.FORD_F_150_LIGHTNING_MK1) + planner = LongitudinalPlanner(CP, init_v=v_ego) + planner.lead_one = make_lead(status=True, d_rel=58.0, v_lead=20.0, model_prob=0.99) + planner.lead_two = make_lead(status=False) + + brake_rate, release_rate = get_far_follow_output_slew_rates(CP) + assert brake_rate == pytest.approx(2.5) + assert release_rate == pytest.approx(1.75) + + initial = planner.get_vehicle_far_follow_slew_target( + v_ego, prev_target=0.0, target=-0.6, output_should_stop=False, panic_bypass=False, + ) + smoothed = planner.get_vehicle_far_follow_slew_target( + v_ego, prev_target=initial, target=-2.0, output_should_stop=False, panic_bypass=False, + ) + + assert initial == pytest.approx(-0.6) + assert smoothed == pytest.approx(initial - brake_rate * planner.dt) def test_silverado_vision_follow_hold_survives_nonurgent_far_lead_crossover(): @@ -2697,6 +2745,41 @@ def test_honda_accord_lead_departure_assist_is_stronger_but_vehicle_scoped(): assert accord_floor <= get_honda_accord_lead_departure_tune(accord)[0] +def test_honda_accord_stop_go_departure_cap_is_vehicle_and_scene_scoped(): + accord = CarInterface.get_non_essential_params(CAR.HONDA_ACCORD) + civic = CarInterface.get_non_essential_params(CAR.HONDA_CIVIC) + lead = make_lead( + status=True, + d_rel=7.7, + v_lead=1.5, + a_lead=0.6, + radar=False, + model_prob=1.0, + ) + + cap = get_honda_accord_stop_go_accel_cap(accord, lead, v_ego=0.0) + assert cap is not None + assert cap < 1.0 + assert get_honda_accord_stop_go_accel_cap(civic, lead, v_ego=0.0) is None + assert get_honda_accord_stop_go_accel_cap( + accord, make_lead(status=True, d_rel=7.7, v_lead=0.0, model_prob=1.0), v_ego=0.0, + ) is None + assert get_honda_accord_stop_go_accel_cap( + accord, make_lead(status=True, d_rel=30.0, v_lead=1.5, model_prob=1.0), v_ego=0.0, + ) is None + + planner = LongitudinalPlanner(accord, init_v=0.0) + assert planner.get_honda_accord_stop_go_accel_target( + lead, v_ego=0.0, previous_target=-0.2, target=1.5, blocked=False, + ) == pytest.approx(0.0) + assert planner.get_honda_accord_stop_go_accel_target( + lead, v_ego=0.0, previous_target=0.0, target=1.5, blocked=True, + ) == pytest.approx(1.5) + assert planner.get_honda_accord_stop_go_accel_target( + lead, v_ego=0.0, previous_target=0.9, target=-1.5, blocked=False, + ) == pytest.approx(-1.5) + + def test_rav4_tss2_lead_departure_assist_is_vehicle_scoped(): rav4 = ToyotaCarInterface.get_non_essential_params(TOYOTA_CAR.TOYOTA_RAV4_TSS2_2023) civic = CarInterface.get_non_essential_params(CAR.HONDA_CIVIC) diff --git a/selfdrive/controls/tests/test_starpilot_planner.py b/selfdrive/controls/tests/test_starpilot_planner.py index 8625ba6b6c..cf4ef80818 100644 --- a/selfdrive/controls/tests/test_starpilot_planner.py +++ b/selfdrive/controls/tests/test_starpilot_planner.py @@ -42,6 +42,7 @@ def test_force_stop_jerk_scale_is_platform_specific(): def test_lead_follow_jerk_scale_is_platform_specific(): assert get_lead_follow_jerk_scale(SimpleNamespace(brand="hyundai", carFingerprint="HYUNDAI_ELANTRA_2021")) == 1.25 + assert get_lead_follow_jerk_scale(SimpleNamespace(brand="hyundai", carFingerprint="GENESIS_GV70_ELECTRIFIED_1ST_GEN")) == 1.35 assert get_lead_follow_jerk_scale(SimpleNamespace(brand="other", carFingerprint="OTHER_CAR")) == 1.0 diff --git a/starpilot/controls/starpilot_planner.py b/starpilot/controls/starpilot_planner.py index 4cc96b91f6..9083944830 100644 --- a/starpilot/controls/starpilot_planner.py +++ b/starpilot/controls/starpilot_planner.py @@ -316,10 +316,6 @@ class StarPilotPlanner: if self.starpilot_vcruise.forcing_stop or self.starpilot_vcruise.approach_stop_length > 0.0: jerk_scale = get_force_stop_jerk_scale(car_params) elif self.tracking_lead: - # Elantra vision leads can hand off from cruise to lead0 while closing - # quickly. A slightly higher accel-change cost makes that handoff begin - # earlier instead of arriving as a sharp brake request, without changing - # the safety stop distance or the force-stop path. jerk_scale = get_lead_follow_jerk_scale(car_params) else: jerk_scale = 1.0 diff --git a/starpilot/starpilot_process.py b/starpilot/starpilot_process.py index c2db1b463e..166b281031 100644 --- a/starpilot/starpilot_process.py +++ b/starpilot/starpilot_process.py @@ -285,6 +285,7 @@ def starpilot_thread(): run_update_checks = False safe_mode_active = safe_mode_enabled(params_raw) started_previously = False + starpilot_tracking = None model_randomizer_previously = params.get_bool("ModelRandomizer") time_validated = False @@ -304,6 +305,7 @@ def starpilot_thread(): started = sm["deviceState"].started if not started and started_previously: + starpilot_tracking.flush(now, time_validated) starpilot_planner.shutdown() starpilot_toggles = update_toggles(starpilot_variables, started, theme_manager, thread_manager, time_validated, params, starpilot_toggles) diff --git a/starpilot/system/starpilot_tracking.py b/starpilot/system/starpilot_tracking.py index 0828c251bb..40ad1f9025 100644 --- a/starpilot/system/starpilot_tracking.py +++ b/starpilot/system/starpilot_tracking.py @@ -36,6 +36,39 @@ class StarPilotTracking: self.model_name = clean_model_name(starpilot_toggles.model_name) + def _commit_tracked_time(self, now=None, time_validated=False): + if not self.previously_enabled or self.tracked_time <= 0: + return + + if time_validated and now is not None: + current_month = now.month + if current_month != self.starpilot_stats.get("Month"): + self.starpilot_stats.update({ + "CurrentMonthsMeters": 0, + "Month": current_month + }) + + self.starpilot_stats["StarPilotSeconds"] = self.starpilot_stats.get("StarPilotSeconds", 0) + self.tracked_time + + total_model_times = self.starpilot_stats.get("ModelTimes", {}) + total_model_times[self.model_name] = total_model_times.get(self.model_name, 0) + self.tracked_time + self.starpilot_stats["ModelTimes"] = total_model_times + + self.starpilot_stats["TrackedTime"] = self.starpilot_stats.get("TrackedTime", 0) + self.tracked_time + self.tracked_time = 0 + + if not self.drive_added: + self.starpilot_stats["StarPilotDrives"] = self.starpilot_stats.get("StarPilotDrives", 0) + 1 + self.drive_added = True + + def flush(self, now=None, time_validated=False): + """Persist the current drive, including time since the last stop checkpoint.""" + if not self.previously_enabled: + return + + self._commit_tracked_time(now, time_validated) + self.params.put("StarPilotStats", dict(sorted(self.starpilot_stats.items()))) + def update(self, now, time_validated, sm, starpilot_toggles): v_cruise = min(sm["carState"].vCruiseCluster, V_CRUISE_MAX) * CV.KPH_TO_MS v_ego = max(sm["carState"].vEgo, 0) @@ -141,27 +174,5 @@ class StarPilotTracking: self.starpilot_stats["WeatherTimes"] = weather_times if self.tracked_time >= 60 and sm["carState"].standstill and self.previously_enabled: - if time_validated: - current_month = now.month - if current_month != self.starpilot_stats.get("Month"): - self.starpilot_stats.update({ - "CurrentMonthsMeters": 0, - "Month": current_month - }) - - self.starpilot_stats["StarPilotSeconds"] = self.starpilot_stats.get("StarPilotSeconds", 0) + self.tracked_time - - current_model = self.model_name - total_model_times = self.starpilot_stats.get("ModelTimes", {}) - total_model_times[current_model] = total_model_times.get(current_model, 0) + self.tracked_time - self.starpilot_stats["ModelTimes"] = total_model_times - - self.starpilot_stats["TrackedTime"] = self.starpilot_stats.get("TrackedTime", 0) + self.tracked_time - - self.tracked_time = 0 - - if not self.drive_added: - self.starpilot_stats["StarPilotDrives"] = self.starpilot_stats.get("StarPilotDrives", 0) + 1 - self.drive_added = True - + self._commit_tracked_time(now, time_validated) self.params.put_nonblocking("StarPilotStats", dict(sorted(self.starpilot_stats.items()))) diff --git a/starpilot/system/tests/test_starpilot_tracking.py b/starpilot/system/tests/test_starpilot_tracking.py new file mode 100644 index 0000000000..a77cb2e3e9 --- /dev/null +++ b/starpilot/system/tests/test_starpilot_tracking.py @@ -0,0 +1,47 @@ +from datetime import UTC, datetime + +from openpilot.starpilot.system.starpilot_tracking import StarPilotTracking + + +class FakeParams: + def __init__(self): + self.writes = [] + + def put(self, key, value): + self.writes.append((key, value)) + + +def test_flush_persists_time_since_last_checkpoint(): + params = FakeParams() + tracking = StarPilotTracking.__new__(StarPilotTracking) + tracking.params = params + tracking.starpilot_stats = {"StarPilotMeters": 1000} + tracking.tracked_time = 12.5 + tracking.previously_enabled = True + tracking.drive_added = False + tracking.model_name = "test-model" + + tracking.flush(datetime(2026, 8, 27, tzinfo=UTC), time_validated=True) + + assert tracking.tracked_time == 0 + assert tracking.starpilot_stats["StarPilotSeconds"] == 12.5 + assert tracking.starpilot_stats["TrackedTime"] == 12.5 + assert tracking.starpilot_stats["ModelTimes"] == {"test-model": 12.5} + assert tracking.starpilot_stats["StarPilotDrives"] == 1 + assert params.writes == [("StarPilotStats", dict(sorted(tracking.starpilot_stats.items())))] + + +def test_flush_does_not_count_a_drive_without_star_pilot_engagement(): + params = FakeParams() + tracking = StarPilotTracking.__new__(StarPilotTracking) + tracking.params = params + tracking.starpilot_stats = {"StarPilotMeters": 1000} + tracking.tracked_time = 12.5 + tracking.previously_enabled = False + tracking.drive_added = False + tracking.model_name = "test-model" + + tracking.flush() + + assert tracking.tracked_time == 12.5 + assert params.writes == []