Sleppy time

selfdrive/car/cruise.py

@@ -214,7 +214,7 @@ class VCruiseHelper:
 
     engage_floor_kph = max(V_CRUISE_MIN, 7.0 * CV.MPH_TO_KPH)
     resume_pressed = any(b.type in (ButtonType.accelCruise, ButtonType.resumeCruise) for b in CS.buttonEvents)
-    remembered_resume = resume_prev_button and (self.gm_cc_only or self.redneck_non_pcm)
+    remembered_resume = resume_prev_button and self._uses_software_cruise()
 
     if self.v_cruise_initialized and (resume_pressed or remembered_resume):
       self.v_cruise_kph = self.v_cruise_kph_last

selfdrive/car/tests/test_cruise_speed.py

@@ -313,6 +313,22 @@ class TestVCruiseHelper:
         assert V_CRUISE_MIN <= self.v_cruise_helper.v_cruise_kph <= V_CRUISE_MAX
         assert self.v_cruise_helper.v_cruise_initialized
 
+  def test_resume_keeps_previous_software_cruise_speed(self):
+    engage_cs = car.CarState(vEgo=75 * CV.MPH_TO_MS)
+    self.v_cruise_helper.initialize_v_cruise(engage_cs, experimental_mode=False, resume_prev_button=False,
+                                             starpilot_toggles=self.starpilot_toggles)
+
+    disabled_cs = car.CarState(cruiseState={"available": True})
+    self.v_cruise_helper.update_v_cruise(disabled_cs, enabled=False, is_metric=False,
+                                         speed_limit_changed=False, starpilot_toggles=self.starpilot_toggles)
+
+    resume_cs = car.CarState(vEgo=22 * CV.MPH_TO_MS)
+    self.v_cruise_helper.initialize_v_cruise(resume_cs, experimental_mode=False, resume_prev_button=True,
+                                             starpilot_toggles=self.starpilot_toggles)
+
+    assert self.v_cruise_helper.v_cruise_kph == pytest.approx(75 * CV.MPH_TO_KPH)
+    assert self.v_cruise_helper.v_cruise_cluster_kph == pytest.approx(75 * CV.MPH_TO_KPH)
+
   def test_initialize_v_cruise_matches_speed_limit(self):
     self.reset_cruise_speed_state()
     self.starpilot_toggles.set_speed_limit = True

selfdrive/controls/lib/latcontrol_vehicle_tunes.py

@@ -275,7 +275,7 @@ GENESIS_G70_FRICTION_JERK_DEADZONE_LAT = 0.30
 GENESIS_G70_FRICTION_JERK_DEADZONE_LAT_WIDTH = 0.08
 GENESIS_G70_FRICTION_JERK_DEADZONE_SPEED = 12.0
 GENESIS_G70_FRICTION_JERK_DEADZONE_SPEED_WIDTH = 3.5
-GENESIS_G70_CURVE_UNWIND_FRICTION_JERK_DEADZONE_MAX = 0.16
+GENESIS_G70_CURVE_UNWIND_FRICTION_JERK_DEADZONE_MAX = 0.22
 GENESIS_G70_CURVE_UNWIND_FRICTION_JERK_DEADZONE_SPEED = 35.0 * CV.MPH_TO_MS
 GENESIS_G70_CURVE_UNWIND_FRICTION_JERK_DEADZONE_SPEED_WIDTH = 8.0 * CV.MPH_TO_MS
 GENESIS_G70_CURVE_UNWIND_FRICTION_JERK_DEADZONE_LAT = 0.35
@@ -284,7 +284,7 @@ GENESIS_G70_CURVE_UNWIND_FRICTION_JERK_DEADZONE_LAT_CUTOFF = 1.25
 GENESIS_G70_CURVE_UNWIND_FRICTION_JERK_DEADZONE_LAT_CUTOFF_WIDTH = 0.25
 GENESIS_G70_CURVE_UNWIND_FRICTION_JERK_DEADZONE_JERK = 0.20
 GENESIS_G70_CURVE_UNWIND_FRICTION_JERK_DEADZONE_JERK_WIDTH = 0.12
-GENESIS_G70_CENTER_OUTPUT_TAPER_MAX = 0.22
+GENESIS_G70_CENTER_OUTPUT_TAPER_MAX = 0.26
 GENESIS_G70_CENTER_OUTPUT_TAPER_LAT = 0.30
 GENESIS_G70_CENTER_OUTPUT_TAPER_LAT_WIDTH = 0.10
 GENESIS_G70_CENTER_OUTPUT_TAPER_SPEED = 18.0

selfdrive/controls/lib/longitudinal_planner.py

@@ -397,6 +397,27 @@ def get_vehicle_min_accel(CP, v_ego):
   return float(ACCEL_MIN)
 
 
+def get_far_lead_coast_cap(lead, v_ego, desired_gap, output_a_target):
+  if lead is None or not bool(getattr(lead, "status", False)):
+    return float(output_a_target)
+
+  v_ego = float(v_ego)
+  lead_distance = float(getattr(lead, "dRel", float("inf")))
+  lead_speed = float(getattr(lead, "vLead", v_ego))
+  closing_speed = v_ego - lead_speed
+  if (
+    v_ego <= 10.0 or
+    closing_speed <= 0.5 or
+    lead_distance < FAR_LEAD_COAST_MIN_DISTANCE or
+    lead_distance <= float(desired_gap) + FAR_LEAD_COAST_MIN_GAP_MARGIN or
+    lead_distance / max(closing_speed, 0.1) < FAR_LEAD_COAST_MIN_TTC or
+    max(0.0, -float(getattr(lead, "aLeadK", 0.0))) > FAR_LEAD_COAST_MAX_LEAD_BRAKE
+  ):
+    return float(output_a_target)
+
+  return max(float(output_a_target), -FAR_LEAD_COAST_MAX_DECEL)
+
+
 # Restored planner constants retained by CEM, stop, and departure paths.
 A_CRUISE_MIN = -1.0
 # The stop distance runs ~9 m long through the mid-approach, which leaves the obstacle slack
@@ -433,6 +454,11 @@ VEHICLE_FAR_FOLLOW_SLEW_MIN_DISTANCE_TIME = 1.35
 VEHICLE_FAR_FOLLOW_SLEW_MIN_HEADWAY = 1.35
 VEHICLE_FAR_FOLLOW_SLEW_MIN_TTC = 8.0
 VEHICLE_FAR_FOLLOW_SLEW_MAX_LATERAL_OFFSET = 1.5
+FAR_LEAD_COAST_MIN_DISTANCE = 45.0
+FAR_LEAD_COAST_MIN_TTC = 8.0
+FAR_LEAD_COAST_MIN_GAP_MARGIN = 6.0
+FAR_LEAD_COAST_MAX_LEAD_BRAKE = 0.35
+FAR_LEAD_COAST_MAX_DECEL = 0.20
 RADAR_DEPART_CONFLICT_MAX_EGO_SPEED = 1.6
 RADAR_DEPART_CONFLICT_MIN_RADAR_LATERAL = 1.5
 RADAR_DEPART_CONFLICT_MAX_RADAR_DISTANCE = 18.0
@@ -3069,6 +3095,28 @@ class LongitudinalPlanner:
       panic_bypass,
     )
 
+    far_lead_coast_allowed = (
+      not experimental_mode and
+      comfort_lead is not None and
+      desired_gap is not None and
+      not output_should_stop and
+      not vision_low_speed_stop_active and
+      not close_lead_caps and
+      not panic_bypass and
+      not depart_safety_veto and
+      inside_gap_closing_cap is None and
+      not bool(getattr(sm['starpilotPlan'], 'forcingStop', False)) and
+      not bool(getattr(sm['starpilotPlan'], 'redLight', False)) and
+      not bool(getattr(sm['starpilotPlan'], 'stopSignConfirmed', False))
+    )
+    if far_lead_coast_allowed:
+      output_a_target = get_far_lead_coast_cap(
+        comfort_lead,
+        scene_v_ego,
+        desired_gap,
+        output_a_target,
+      )
+
     if radar_gap_settle_active:
       output_a_target = RADAR_STANDSTILL_GAP_SETTLE_ACCEL
       output_should_stop = False

selfdrive/controls/radard.py

@@ -7,6 +7,7 @@ from typing import Any
 
 import capnp
 from cereal import messaging, log, car, custom
+from cereal.services import SERVICE_LIST
 from openpilot.common.filter_simple import FirstOrderFilter
 from openpilot.common.params import Params
 from openpilot.common.realtime import DT_MDL, Priority, config_realtime_process
@@ -42,6 +43,11 @@ def is_bosch_a_radar_car(CP) -> bool:
   return CP.brand == "honda" and CP.carFingerprint in HONDA_BOSCH_A and not CP.radarUnavailable
 
 
+def has_slow_radar_tracks(CP) -> bool:
+  radar_ts = float(getattr(CP, "radarTimeStepDEPRECATED", DT_MDL) or DT_MDL)
+  return not CP.radarUnavailable and radar_ts > 2.0 / SERVICE_LIST["liveTracks"].frequency
+
+
 # Adjacent-lane stopped-vehicle detector, used as a stop-line hint on red-light
 # approaches. The qualifier is the DECELERATION HISTORY, not the current speed: roadside
 # furniture and curb-parked cars never show a moving -> stopped transition, so testing
@@ -636,8 +642,9 @@ def main() -> None:
   cloudlog.info("radard got CarParams")
 
   # *** setup messaging
+  ignore_avg_freq = ['liveTracks'] if has_slow_radar_tracks(CP) else None
   sm = messaging.SubMaster(['modelV2', 'carState', 'liveTracks'], poll='modelV2',
-                           ignore_valid=['starpilotPlan'])
+                           ignore_avg_freq=ignore_avg_freq, ignore_valid=['starpilotPlan'])
   pm = messaging.PubMaster(['radarState'])
 
   radar_ts = float(getattr(CP, "radarTimeStepDEPRECATED", DT_MDL) or DT_MDL)

selfdrive/controls/tests/test_leads.py

@@ -14,6 +14,7 @@ from openpilot.selfdrive.controls.radard import (
   RadarD,
   g90_low_speed_radar_lead_sane,
   g90_radar_lead_lateral_sane,
+  has_slow_radar_tracks,
   is_bosch_a_radar_car,
   match_vision_to_track,
 )
@@ -96,6 +97,15 @@ class TestLeads:
     assert bosch_a.lead_prob_filters[0].dt == pytest.approx(DT_MDL)
     assert bosch_a.kalman_params.A[0][1] == pytest.approx(HONDA_BOSCH_A_RADAR_TS)
 
+  def test_slow_radar_frequency_relaxation_is_scoped(self):
+    slow_radar = SimpleNamespace(radarTimeStepDEPRECATED=0.15, radarUnavailable=False)
+    normal_radar = SimpleNamespace(radarTimeStepDEPRECATED=0.1, radarUnavailable=False)
+    unavailable_radar = SimpleNamespace(radarTimeStepDEPRECATED=0.15, radarUnavailable=True)
+
+    assert has_slow_radar_tracks(slow_radar)
+    assert not has_slow_radar_tracks(normal_radar)
+    assert not has_slow_radar_tracks(unavailable_radar)
+
   @pytest.mark.skipif(platform.system() == "Darwin", reason="SocketEventHandle requires eventfd")
   def test_radar_fault(self):
     # if there's no radar-related can traffic, radard should either not respond or respond with an error

selfdrive/controls/tests/test_longitudinal_planner.py

@@ -18,7 +18,13 @@ from opendbc.car.toyota.values import CAR as TOYOTA_CAR
 import openpilot.selfdrive.controls.lib.longitudinal_planner as longitudinal_planner_module
 from openpilot.selfdrive.controls.lib.longcontrol import LongCtrlState
 from openpilot.selfdrive.controls.lib.drive_helpers import CONTROL_N
-from openpilot.selfdrive.controls.lib.longitudinal_planner import LongitudinalPlanner, get_coast_accel, get_vehicle_min_accel, should_publish_planner_fcw
+from openpilot.selfdrive.controls.lib.longitudinal_planner import (
+  LongitudinalPlanner,
+  get_coast_accel,
+  get_far_lead_coast_cap,
+  get_vehicle_min_accel,
+  should_publish_planner_fcw,
+)
 from openpilot.selfdrive.controls.lib.longitudinal_mpc_lib.long_mpc import (
   LongitudinalMpc,
   build_model_lead_trajectory,
@@ -310,6 +316,24 @@ def test_mpc_panic_bypass_immediately_removes_duplicate_vision_filter():
   assert mpc.lead_v_filter.x == pytest.approx(10.0)
 
 
+def test_far_lead_coast_cap_delays_nonurgent_deceleration():
+  lead = make_lead(status=True, d_rel=128.0, v_lead=16.7, a_lead=0.2, radar=True)
+
+  assert get_far_lead_coast_cap(lead, 26.6, 115.0, -0.43) == pytest.approx(-0.20)
+  assert get_far_lead_coast_cap(lead, 26.6, 115.0, 0.10) == pytest.approx(0.10)
+
+
+@pytest.mark.parametrize("d_rel,v_lead,a_lead,desired_gap", [
+  (50.0, 20.0, 0.2, 45.0),  # only a small gap remains
+  (128.0, 8.0, 0.2, 115.0),  # urgent closing time
+  (128.0, 16.7, -0.5, 115.0),  # the lead is braking materially
+])
+def test_far_lead_coast_cap_preserves_urgent_or_close_deceleration(d_rel, v_lead, a_lead, desired_gap):
+  lead = make_lead(status=True, d_rel=d_rel, v_lead=v_lead, a_lead=a_lead, radar=True)
+
+  assert get_far_lead_coast_cap(lead, 26.6, desired_gap, -0.43) == pytest.approx(-0.43)
+
+
 def test_hrv_far_follow_output_slew_damps_only_continuous_safe_follow():
   v_ego = 24.0
   CP = CarInterface.get_non_essential_params(CAR.HONDA_HRV_3G)