Compiled on Mar.5,2025

RELEASES.md

@@ -1,5 +1,6 @@
 Version 0.9.6 (2024-02-27)
 ========================
+* 跟车起步距离控制 (2025-03-05)
 * 优化自动跟车功能 (2025-02-24)
 * 外挂自动跟车起步 (2024-12-07)
 * 轻柔变道支持红熊 (2024-05-28)

selfdrive/controls/controlsd.py

@@ -183,6 +183,7 @@ class Controls:
     self.params_memory = Params("/dev/shm/params")
     # auto_resume
     self.standstill_time = 0
+    self.leaddepart_time = 0
 
     self.ignore_controls_mismatch = False
 
@@ -665,7 +666,10 @@ class Controls:
     if self.lead_departing_alert and self.sm.frame % 50 == 0:
       lead = self.sm['radarState'].leadOne
       lead_distance = lead.dRel
-      lead_departing = lead_distance - self.previous_lead_distance > 0.5 and self.previous_lead_distance != 0 and CS.standstill
+      lead_departing = lead_distance - self.previous_lead_distance > 0.5 and CS.standstill #and self.previous_lead_distance != 0
+      # below 15 meters
+      lead_departing &= self.previous_lead_distance > 0 and self.previous_lead_distance <= 15 
+      previous_lead = self.previous_lead_distance
       self.previous_lead_distance = lead_distance
 
       lead_departing &= not CS.gasPressed
@@ -674,18 +678,27 @@ class Controls:
       
       # auto_resume
       if lead_departing:
+        self.params_memory.put_int("LeadDepartDistance", previous_lead * 10)
         # wait time 3 seconds
         if (int(time.time()) - self.standstill_time) >= 3:
           # read param only when lead_departing = true
           cruise_auto_resume = self.params.get_bool("CruiseAutoResume") and self.params_memory.get_bool("ESP32HasIP") #auto_resume
-          long_personality = self.params.get_int("LongitudinalPersonality") == 0
-          if long_personality and cruise_auto_resume and self.state == State.enabled and not CS.brakePressed and self.v_cruise_helper.v_cruise_cluster_kph < 24.0:
+          conversion = 1 if self.is_metric else CV.FOOT_TO_METER
+          cruise_auto_resume &= previous_lead <= self.params.get_int("AutoResumeDistance")*conversion
+          # long_personality = self.params.get_int("LongitudinalPersonality") == 0
+          # if long_personality and cruise_auto_resume and self.state == State.enabled and not CS.brakePressed and self.v_cruise_helper.v_cruise_cluster_kph < 24.0:
+          if cruise_auto_resume and self.state == State.enabled and not CS.brakePressed and self.v_cruise_helper.v_cruise_cluster_kph < 24.0:
             self.params_memory.put_bool("ESP32AutoResume", True)
             self.events.add(EventName.autoResumeEvent)
-          else:
+            self.previous_lead_distance = 0
+            self.standstill_time = int(time.time()) + 10
+            self.leaddepart_time = int(time.time()) + 10
+          elif (int(time.time()) - self.leaddepart_time) > 10:
             self.events.add(EventName.leadDeparting)
-        else:
+            self.leaddepart_time = int(time.time())
+        elif (int(time.time()) - self.leaddepart_time) > 10:
           self.events.add(EventName.leadDeparting)
+          self.leaddepart_time = int(time.time())
 
     # Speed limit changed alert
     if self.speed_limit_alert or self.speed_limit_confirmation:

selfdrive/controls/lib/events.py

@@ -21,7 +21,6 @@ VisualAlert = car.CarControl.HUDControl.VisualAlert
 AudibleAlert = car.CarControl.HUDControl.AudibleAlert
 EventName = car.CarEvent.EventName
 
-
 # Alert priorities
 class Priority(IntEnum):
   LOWEST = 0
@@ -248,6 +247,15 @@ def below_steer_speed_alert(CP: car.CarParams, CS: car.CarState, sm: messaging.S
     AlertStatus.userPrompt, AlertSize.small,
     Priority.LOW, VisualAlert.steerRequired, AudibleAlert.prompt, 0.4)
 
+def lead_departing_alert(CP: car.CarParams, CS: car.CarState, sm: messaging.SubMaster, metric: bool, soft_disable_time: int) -> Alert:
+  params_memory = Params("/dev/shm/params")
+  dis = params_memory.get_int("LeadDepartDistance") / 10
+  return Alert(
+      f"Lead departed {dis} meters",
+      "",
+      AlertStatus.frogpilot, AlertSize.small,
+      Priority.MID, VisualAlert.none, AudibleAlert.prompt, 3.)
+
 
 def calibration_incomplete_alert(CP: car.CarParams, CS: car.CarState, sm: messaging.SubMaster, metric: bool, soft_disable_time: int) -> Alert:
   first_word = 'Recalibration' if sm['liveCalibration'].calStatus == log.LiveCalibrationData.Status.recalibrating else 'Calibration'
@@ -1082,13 +1090,17 @@ EVENTS: Dict[int, Dict[str, Union[Alert, AlertCallbackType]]] = {
       Priority.LOW, VisualAlert.none, AudibleAlert.warningSoft, .1),
   },
 
+  # EventName.leadDeparting: {
+  #   ET.PERMANENT: Alert(
+  #     "Lead departed",
+  #     "",
+  #     AlertStatus.frogpilot, AlertSize.small,
+  #     Priority.MID, VisualAlert.none, AudibleAlert.prompt, 3.),
+  # },
   EventName.leadDeparting: {
-    ET.PERMANENT: Alert(
-      "Lead departed",
-      "",
-      AlertStatus.frogpilot, AlertSize.small,
-      Priority.MID, VisualAlert.none, AudibleAlert.prompt, 3.),
+    ET.PERMANENT: lead_departing_alert,
   },
+  
   # auto_resume
   EventName.autoResumeEvent: {
     ET.PERMANENT: Alert(

selfdrive/manager/manager.py

@@ -259,7 +259,7 @@ def manager_init() -> None:
     ("SLCPriority2", "Offline Maps"),
     ("SLCPriority3", "Navigation"),
     ("SmoothBraking", "1"),
-    ("SNGHack", "1"),
+    ("SNGHack", "1"),    
     ("SpeedLimitChangedAlert", "1"),
     ("SpeedLimitController", "1"),
     ("StandardFollow", "1.45"),
@@ -295,6 +295,7 @@ def manager_init() -> None:
     ("FrogPilotPrebuilt", "0"), #FROGPILOT_PREBUILT_TEST
     ("UseRedPanda", "0"), #Red Panda Config BUS 0/1/2/3 -> 4/5/6/7
     ("CruiseAutoResume", "0"), #auto_resume
+    ("AutoResumeDistance", "10"),
   ]
   if not PC:
     default_params.append(("LastUpdateTime", datetime.datetime.utcnow().isoformat().encode('utf8')))