HKG: Add lead distance indicator on cluster

CHANGELOGS.md

@@ -16,8 +16,12 @@ sunnypilot - 0.9.5.1 (2023-10-xx)
   * Dynamic Experimental Control is only active while in Experimental Mode
   * Dynamic Experimental Control must be enabled via "SP - Controls", then toggle via the new Onroad Settings Panel
   * When Dynamic Experimental Control is ON, initially setting cruise speed will set to the vehicle's current speed
-* NEW❗: Hyundai CAN longitudinal: Parse lead info for camera-based SCC platforms
-  * Improve lead tracking when using openpilot longitudinal
+* NEW❗: Hyundai CAN longitudinal:
+  * NEW❗: Parse lead info for camera-based SCC platforms
+    * Improve lead tracking when using openpilot longitudinal
+  * NEW❗: Parse lead distance to display on car cluster
+    * Introduced better lead distance calculation to display on the car's cluster, replacing the binary "lead visible" indication on the SCC cluster
+    * Lead distance is now categorized into different ranges for more detailed and comprehensive information to the driver similar to how stock ACC does it
 * NEW❗: Subaru - Stop and Go auto-resume support thanks to martinl!
   * Global (excluding Gen 2 and Hybrid) and Pre-Global support
 * NEW❗: Toyota - Stop and Go hack

selfdrive/car/hyundai/carcontroller.py

@@ -63,7 +63,11 @@ class CarController:
     self.lat_disengage_init = False
     self.lat_active_last = False
 
-    self.sm = messaging.SubMaster(['longitudinalPlanSP'])
+    sub_service = 'longitudinalPlanSP' if not CP.pcmCruiseSpeed else \
+                  'radarState' if CP.openpilotLongitudinalControl else \
+                  None
+    if sub_service is not None:
+      self.sm = messaging.SubMaster([sub_service])
     self.param_s = Params()
     self.is_metric = self.param_s.get_bool("IsMetric")
     self.speed_limit_control_enabled = False
@@ -93,11 +97,24 @@ class CarController:
     self.m_tsc = 0
     self.steady_speed = 0
     self.hkg_can_smooth_stop = self.param_s.get_bool("HkgSmoothStop")
+    self.lead_distance = 0
+
+  def calculate_lead_distance(self, hud_control: car.CarControl.HUDControl) -> float:
+    lead_one = self.sm["radarState"].leadOne
+    lead_two = self.sm["radarState"].leadTwo
+
+    if lead_one.status and (not lead_two.status or lead_one.dRel < lead_two.dRel):
+      return lead_one.dRel
+    if lead_two.status:
+      return lead_two.dRel
+
+    return 19 if hud_control.leadVisible else 0
 
   def update(self, CC, CS, now_nanos):
-    if not self.CP.pcmCruiseSpeed:
+    if not self.CP.pcmCruiseSpeed or (self.CP.openpilotLongitudinalControl and self.frame % 5 == 0):
       self.sm.update(0)
 
+    if not self.CP.pcmCruiseSpeed:
       if self.sm.updated['longitudinalPlanSP']:
         self.v_tsc_state = self.sm['longitudinalPlanSP'].visionTurnControllerState
         self.slc_state = self.sm['longitudinalPlanSP'].speedLimitControlState
@@ -245,14 +262,19 @@ class CarController:
             if self.frame % 2 == 0:
               can_sends.extend([hyundaican.create_clu11(self.packer, (self.frame // 2) + 1, CS.clu11, self.cruise_button, self.CP.carFingerprint)] * 25)
 
+      # Parse lead distance from radarState and display the corresponding distance in the car's cluster
+      if self.sm.updated['radarState'] and self.frame % 5 == 0 and self.CP.openpilotLongitudinalControl:
+        self.lead_distance = self.calculate_lead_distance(hud_control)
+
       if self.frame % 2 == 0 and self.CP.openpilotLongitudinalControl:
         if self.hkg_can_smooth_stop:
           stopping = stopping and CS.out.vEgoRaw < 0.05
+
         # TODO: unclear if this is needed
         jerk = 3.0 if actuators.longControlState == LongCtrlState.pid else 1.0
         use_fca = self.CP.flags & HyundaiFlags.USE_FCA.value
         can_sends.extend(hyundaican.create_acc_commands(self.packer, CC.enabled and CS.out.cruiseState.enabled, accel, jerk, int(self.frame / 2),
-                                                        hud_control.leadVisible, set_speed_in_units, stopping,
+                                                        self.lead_distance, set_speed_in_units, stopping,
                                                         CC.cruiseControl.override, use_fca, CS.mainEnabled, CS, escc, self.CP.carFingerprint))
 
       # 20 Hz LFA MFA message

selfdrive/car/hyundai/hyundaican.py

@@ -129,7 +129,7 @@ def create_lfahda_mfc(packer, enabled, lat_active, lateral_paused, blinking_icon
   }
   return packer.make_can_msg("LFAHDA_MFC", 0, values)
 
-def create_acc_commands(packer, enabled, accel, upper_jerk, idx, lead_visible, set_speed, stopping, long_override, use_fca,
+def create_acc_commands(packer, enabled, accel, upper_jerk, idx, lead_distance, set_speed, stopping, long_override, use_fca,
                         main_enabled, CS, escc, car_fingerprint):
   commands = []
 
@@ -176,7 +176,7 @@ def create_acc_commands(packer, enabled, accel, upper_jerk, idx, lead_visible, s
     "JerkUpperLimit": upper_jerk, # stock usually is 1.0 but sometimes uses higher values
     "JerkLowerLimit": 5.0, # stock usually is 0.5 but sometimes uses higher values
     "ACCMode": 2 if enabled and long_override else 1 if enabled else 4, # stock will always be 4 instead of 0 after first disengage
-    "ObjGap": 2 if lead_visible else 0, # 5: >30, m, 4: 25-30 m, 3: 20-25 m, 2: < 20 m, 0: no lead
+    "ObjGap": get_object_gap(lead_distance) # 5: >30, m, 4: 25-30 m, 3: 20-25 m, 2: < 20 m, 0: no lead
   }
   commands.append(packer.make_can_msg("SCC14", 0, scc14_values))
 
@@ -236,3 +236,25 @@ def create_frt_radar_opt(packer):
     "CF_FCA_Equip_Front_Radar": 1,
   }
   return packer.make_can_msg("FRT_RADAR11", 0, frt_radar11_values)
+
+
+def get_object_gap(lead_distance: float) -> int:
+  if lead_distance <= 0:
+    return 0
+
+  # Define distance ranges and corresponding values.
+  ranges = [(30, 5),
+            (25, 4),
+            (20, 3),
+            (0,  2)]
+
+  # The next function scans through 'ranges' in ascending order,
+  # returning the associated distance range for the first range
+  # whose lower bound is exceeded by lead_distance.
+  #
+  # If lead_distance does not exceed any bounds, it defaults to 0 (no lead).
+  #
+  # For example: if lead_distance is 22, then 3 (20-25 m) is returned.
+  # This is because 22 is greater than the lower bound of this range (20),
+  # but does not exceed the next range's lower bound (25).
+  return next((gap for dist, gap in ranges if lead_distance > dist), 0)