mawei/sunnypilot
1
0
Fork 0
mirror of https://github.com/sunnypilot/sunnypilot.git synced 2026-07-24 22:52:05 +08:00
Code Issues Packages Projects Releases Wiki Activity

Merge branch 'dec-redo' into master-dev-c3-new

Browse Source
This commit is contained in:
Jason Wen
2025-01-16 12:09:28 -05:00
parent e2ff34417f baf0506033
commit bb83e1fe8f
1 changed files with 18 additions and 6 deletions
Download Patch File Download Diff File Expand all files Collapse all files
sunnypilot/selfdrive/controls/lib/dec/dec.py
+18 -6
View File
@@ -173,8 +173,8 @@ class DynamicExperimentalController:
"""
Smoothing the lead detection to avoid erratic behavior.
"""
self._has_lead_filtered = (1 - smoothing_factor) * self._has_lead_filtered + smoothing_factor * lead_prob
return self._has_lead_filtered > WMACConstants.LEAD_PROB
lead_filtering: float = (1 - smoothing_factor) * self._has_lead_filtered + smoothing_factor * lead_prob
return lead_filtering > WMACConstants.LEAD_PROB
def _adaptive_lead_prob_threshold(self) -> float:
"""
@@ -196,7 +196,10 @@ class DynamicExperimentalController:
# fcw detection
self._mpc_fcw_gmac.add_data(self._mpc_fcw_crash_cnt > 0)
self._has_mpc_fcw = self._mpc_fcw_gmac.get_weighted_average() > WMACConstants.MPC_FCW_PROB
if _mpc_fcw_weighted_average := self._mpc_fcw_gmac.get_weighted_average():
self._has_mpc_fcw = _mpc_fcw_weighted_average > WMACConstants.MPC_FCW_PROB
else:
self._has_mpc_fcw = False
# nav enable detection
# self._has_nav_instruction = md.navEnabledDEPRECATED and maneuver_distance / max(car_state.vEgo, 1) < 13
@@ -211,7 +214,10 @@ class DynamicExperimentalController:
adaptive_threshold = self._adaptive_slowdown_threshold()
slow_down_trigger = len(md.orientation.x) == len(md.position.x) == TRAJECTORY_SIZE and md.position.x[TRAJECTORY_SIZE - 1] < adaptive_threshold
self._slow_down_gmac.add_data(slow_down_trigger)
self._has_slow_down = self._slow_down_gmac.get_weighted_average() > WMACConstants.SLOW_DOWN_PROB
if _has_slow_down_weighted_average := self._slow_down_gmac.get_weighted_average():
self._has_slow_down = _has_slow_down_weighted_average > WMACConstants.SLOW_DOWN_PROB
else:
self._has_slow_down = False
# anomaly detection for slow down events
if self._anomaly_detection(self._slow_down_gmac.data):
@@ -238,7 +244,10 @@ class DynamicExperimentalController:
# slowness detection
if not self._has_standstill:
self._slowness_gmac.add_data(self._v_ego_kph <= (self._v_cruise_kph * WMACConstants.SLOWNESS_CRUISE_OFFSET))
self._has_slowness = self._slowness_gmac.get_weighted_average() > WMACConstants.SLOWNESS_PROB
if _slowness_weighted_average := self._slowness_gmac.get_weighted_average():
self._has_slowness = _slowness_weighted_average > WMACConstants.SLOWNESS_PROB
else:
self._has_slowness = False
# dangerous TTC detection
if not self._has_lead_filtered and self._has_lead_filtered_prev:
@@ -248,7 +257,10 @@ class DynamicExperimentalController:
if self._has_lead and car_state.vEgo >= 0.01:
self._dangerous_ttc_gmac.add_data(lead_one.dRel / car_state.vEgo)
self._has_dangerous_ttc = self._dangerous_ttc_gmac.get_weighted_average() is not None and self._dangerous_ttc_gmac.get_weighted_average() <= WMACConstants.DANGEROUS_TTC
if _dangerous_ttc_weighted_average := self._dangerous_ttc_gmac.get_weighted_average():
self._has_dangerous_ttc = _dangerous_ttc_weighted_average <= WMACConstants.DANGEROUS_TTC
else:
self._has_dangerous_ttc = False
# keep prev values
self._has_standstill_prev = self._has_standstill