nighty night

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,
         )

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."""

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)
 
 

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,

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)
 

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 (

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

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)

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
 
 

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

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)

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())))

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 == []