Files
sunnypilot-byd/opendbc_repo/opendbc/sunnypilot/car/hyundai/lead_data_ext.py
T
github-actions[bot] de77c89cf6 sunnypilot v2026.002.000
version: sunnypilot v2026.002.000 (staging)
date: 2026-07-21T07:44:02
master commit: 2c334ede443d7391d27575af8c854a095ba702a8
2026-07-21 07:44:02 +00:00

118 lines
4.2 KiB
Python

"""
Copyright (c) 2021-, Haibin Wen, sunnypilot, and a number of other contributors.
This file is part of sunnypilot and is licensed under the MIT License.
See the LICENSE.md file in the root directory for more details.
"""
from abc import ABC, abstractmethod
from opendbc.car import structs
from opendbc.car.hyundai.values import HyundaiFlags
class LeadData(ABC):
def __init__(self, object_gap: int, lead_distance: float, lead_rel_speed: float, lead_visible: bool):
self.object_gap = object_gap
self.lead_distance = lead_distance
self.lead_rel_speed = lead_rel_speed
self.lead_visible = lead_visible
@property
@abstractmethod
def object_rel_gap(self) -> int:
raise NotImplementedError("Subclasses must implement this method")
class CanLeadData(LeadData):
@property
def object_rel_gap(self) -> int:
return 0 if self.lead_distance == 0 else 2 if self.lead_rel_speed < -0.2 else 1
class CanFdLeadData(LeadData):
@property
def object_rel_gap(self) -> int:
return 0 if not self.lead_visible else 2 if self.lead_rel_speed < 0 else 1
def _hysteresis_update(current, new_value, counter, threshold):
"""
Updates a value based on a hysteresis threshold mechanism. This function
compares a new value against the current value and uses a counter to detect
when a transition should occur, avoiding rapid oscillations between states.
A new value will only be adopted if it differs from the current value and
the counter reaches the specified threshold.
:param current: The current value being tracked.
:param new_value: The potential new value to compare against the current.
:param counter: The count of consecutive different values encountered.
:param threshold: The minimum count required before switching to the new value.
:return: A tuple containing:
- The updated current value, which is either the original current
value or the new value if the hysteresis condition was met.
- The updated counter, reset to 0 if the new value was adopted,
or incremented by 1 otherwise.
"""
if new_value == current:
return current, 0
counter += 1
return (new_value, 0) if counter >= threshold else (current, counter)
class LeadDataCarController:
# Hysteresis parameters
LEAD_HYSTERESIS_FRAMES: int = 50
def __init__(self, CP: structs.CarParams):
self.CP = CP
self.lead_one = {}
self.lead_two = {}
self._lead_on_counter = 0
self._lead_off_counter = 0
self.lead_visible = False
self.gap_counter = 0
self.object_gap = 0
self.lead_distance = 0
self.lead_rel_speed = 0
def _update_object_gap(self, lead_distance: float | None):
new_gap = 5 # Default gap value if no lead distance is provided
if lead_distance is None or lead_distance == 0:
new_gap = 0
elif lead_distance < 20:
new_gap = 2
elif lead_distance < 25:
new_gap = 3
elif lead_distance < 30:
new_gap = 4
self.object_gap, self.gap_counter = _hysteresis_update(self.object_gap, new_gap, self.gap_counter, self.LEAD_HYSTERESIS_FRAMES)
def _update_lead_visible_hysteresis(self, raw_lead_visible: bool):
counter = self._lead_on_counter if raw_lead_visible else self._lead_off_counter
self.lead_visible, counter = _hysteresis_update(self.lead_visible, raw_lead_visible, counter, self.LEAD_HYSTERESIS_FRAMES)
if raw_lead_visible:
self._lead_on_counter = counter
self._lead_off_counter = 0 # reset opposite counter
else:
self._lead_off_counter = counter
self._lead_on_counter = 0 # reset opposite counter
def update(self, CC_SP: structs.CarControlSP) -> None:
self.lead_one = CC_SP.leadOne
self.lead_two = CC_SP.leadTwo
self.lead_distance = self.lead_one.dRel
self.lead_rel_speed = self.lead_one.vRel
self._update_lead_visible_hysteresis(self.lead_one.status)
self._update_object_gap(self.lead_distance)
@property
def lead_data(self) -> CanLeadData | CanFdLeadData:
if self.CP.flags & HyundaiFlags.CANFD:
return CanFdLeadData(self.object_gap, self.lead_distance, self.lead_rel_speed, self.lead_visible)
return CanLeadData(self.object_gap, self.lead_distance, self.lead_rel_speed, self.lead_visible)