diff --git a/CHANGELOGS.md b/CHANGELOGS.md index 8c44e7194e..89881ed347 100644 --- a/CHANGELOGS.md +++ b/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 diff --git a/selfdrive/car/hyundai/carcontroller.py b/selfdrive/car/hyundai/carcontroller.py index 4c2b1e1a1a..9d79efdf26 100644 --- a/selfdrive/car/hyundai/carcontroller.py +++ b/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 diff --git a/selfdrive/car/hyundai/hyundaican.py b/selfdrive/car/hyundai/hyundaican.py index c5c27c2e53..d905470605 100644 --- a/selfdrive/car/hyundai/hyundaican.py +++ b/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)