sunnypilot v2026.002.000
version: sunnypilot v2026.002.000 (staging) date: 2026-07-21T07:44:02 master commit: 2c334ede443d7391d27575af8c854a095ba702a8
This commit is contained in:
commit
de77c89cf6
3995 files changed
+1260688
No files matched your search
@@ -0,0 +1,117 @@
|
||||
"""
|
||||
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)
|
||||
Reference in new issue
Block a user