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

cst!

Browse Source
This commit is contained in:
Jason Wen
2025-09-25 01:06:23 -04:00
parent 81ead529f5
commit 6fe2d05ac8
3 changed files with 39 additions and 9 deletions
Download Patch File Download Diff File Expand all files Collapse all files
sunnypilot/selfdrive/controls/lib/speed_limit/__init__.py
+2 -2
View File
@@ -9,6 +9,6 @@ LIMIT_MAX_MAP_DATA_AGE = 10. # s Maximum time to hold to map data, then conside
# Speed Limit Assist constants
PCM_LONG_REQUIRED_MAX_SET_SPEED = {
True: 36.1111, # 130 km/h
False: 35.7632, # 80 mph
True: (33.3333, 36.1111), # km/h, (120, 130)
False: (31.2928, 35.7632), # mph, (70, 80)
}
sunnypilot/selfdrive/controls/lib/speed_limit/speed_limit_assist.py
+31 -6
View File
@@ -31,6 +31,10 @@ LIMIT_MAX_ACC = 1.0 # m/s^2 Maximum acceleration allowed for limit controllers
LIMIT_MIN_SPEED = 8.33 # m/s, Minimum speed limit to provide as solution on limit controllers.
LIMIT_SPEED_OFFSET_TH = -1. # m/s Maximum offset between speed limit and current speed for adapting state.
V_CRUISE_UNSET = 255
CONFIRM_SPEED_THRESHOLD = {
True: 80, # km/h
False: 50, # mph
}
CRUISE_BUTTONS_PLUS = (ButtonType.accelCruise, ButtonType.resumeCruise)
CRUISE_BUTTONS_MINUS = (ButtonType.decelCruise, ButtonType.setCruise)
@@ -65,8 +69,8 @@ class SpeedLimitAssist:
self._speed_limit = 0.
self._speed_limit_final_last = 0.
self.speed_limit_prev = 0.
self.speed_limit_final_conv = 0
self.prev_speed_limit_final_conv = 0
self.speed_limit_final_last_conv = 0
self.prev_speed_limit_final_last_conv = 0
self._distance = 0.
self.state = SpeedLimitAssistState.disabled
self._state_prev = SpeedLimitAssistState.disabled
@@ -118,10 +122,24 @@ class SpeedLimitAssist:
# Update current velocity offset (error)
self.v_offset = self._speed_limit_final_last - self.v_ego
self.speed_limit_final_conv = round(self._speed_limit_final_last * speed_conv)
self.speed_limit_final_last_conv = round(self._speed_limit_final_last * speed_conv)
self.v_cruise_cluster_conv = round(self.v_cruise_cluster * speed_conv)
pcm_long_required_max_set_speed_conv = round(PCM_LONG_REQUIRED_MAX_SET_SPEED[self.is_metric] * speed_conv)
self.target_set_speed_conv = pcm_long_required_max_set_speed_conv if self.pcm_op_long else self.speed_limit_final_conv
cst_low, cst_high = PCM_LONG_REQUIRED_MAX_SET_SPEED[self.is_metric]
pcm_long_required_max = cst_low if self.speed_limit_final_last_conv < CONFIRM_SPEED_THRESHOLD[self.is_metric] else cst_high
pcm_long_required_max_set_speed_conv = round(pcm_long_required_max * speed_conv)
self.target_set_speed_conv = pcm_long_required_max_set_speed_conv if self.pcm_op_long else self.speed_limit_final_last_conv
def apply_confirm_speed_threshold(self) -> bool:
# below CST: always require user confirmation
if self.v_cruise_cluster_conv < CONFIRM_SPEED_THRESHOLD[self.is_metric]:
return True
# at/above CST:
# - new speed limit >= CST: auto change
# - new speed limit < CST: user confirmation required
return self.speed_limit_final_last_conv < CONFIRM_SPEED_THRESHOLD[self.is_metric]
def get_current_acceleration_as_target(self) -> float:
return self.a_ego
@@ -171,6 +189,8 @@ class SpeedLimitAssist:
if self.state == SpeedLimitAssistState.active:
if self.v_cruise_cluster_changed:
self.state = SpeedLimitAssistState.inactive
elif self.speed_limit_changed and self.apply_confirm_speed_threshold():
self.state = SpeedLimitAssistState.preActive
elif self._has_speed_limit and self.v_offset < LIMIT_SPEED_OFFSET_TH:
self.state = SpeedLimitAssistState.adapting
@@ -178,6 +198,8 @@ class SpeedLimitAssist:
elif self.state == SpeedLimitAssistState.adapting:
if self.v_cruise_cluster_changed:
self.state = SpeedLimitAssistState.inactive
elif self.speed_limit_changed and self.apply_confirm_speed_threshold():
self.state = SpeedLimitAssistState.preActive
elif self.v_offset >= LIMIT_SPEED_OFFSET_TH:
self.state = SpeedLimitAssistState.active
@@ -235,6 +257,9 @@ class SpeedLimitAssist:
if self.v_cruise_cluster_changed:
self.state = SpeedLimitAssistState.inactive
elif self.speed_limit_changed and self.apply_confirm_speed_threshold():
self.state = SpeedLimitAssistState.preActive
# PRE_ACTIVE
elif self.state == SpeedLimitAssistState.preActive:
if self._update_non_pcm_long_confirmed_state(CS):
@@ -316,7 +341,7 @@ class SpeedLimitAssist:
self.long_enabled_prev = self.long_enabled
self.prev_target_set_speed_conv = self.target_set_speed_conv
self.prev_v_cruise_cluster_conv = self.v_cruise_cluster_conv
self.prev_speed_limit_final_conv = self.speed_limit_final_conv
self.prev_speed_limit_final_last_conv = self.speed_limit_final_last_conv
self.output_v_target = self.get_v_target_from_control()
self.output_a_target = self.get_a_target_from_control()
sunnypilot/selfdrive/selfdrived/events.py
+6 -1
View File
@@ -4,6 +4,7 @@ from openpilot.common.constants import CV
from openpilot.sunnypilot.selfdrive.selfdrived.events_base import EventsBase, Priority, ET, Alert, \
NoEntryAlert, ImmediateDisableAlert, EngagementAlert, NormalPermanentAlert, AlertCallbackType, wrong_car_mode_alert
from openpilot.sunnypilot.selfdrive.controls.lib.speed_limit import PCM_LONG_REQUIRED_MAX_SET_SPEED
from openpilot.sunnypilot.selfdrive.controls.lib.speed_limit.speed_limit_assist import CONFIRM_SPEED_THRESHOLD
AlertSize = log.SelfdriveState.AlertSize
@@ -31,7 +32,11 @@ def speed_limit_adjust_alert(CP: car.CarParams, CS: car.CarState, sm: messaging.
def speed_limit_pre_active_alert(CP: car.CarParams, CS: car.CarState, sm: messaging.SubMaster, metric: bool, soft_disable_time: int, personality) -> Alert:
if CP.openpilotLongitudinalControl and CP.pcmCruise:
# PCM long
pcm_long_required_max_set_speed_conv = round(PCM_LONG_REQUIRED_MAX_SET_SPEED[metric] * (CV.MS_TO_KPH if metric else CV.MS_TO_MPH))
speed_conv = CV.MS_TO_KPH if metric else CV.MS_TO_MPH
cst_low, cst_high = PCM_LONG_REQUIRED_MAX_SET_SPEED[metric]
speed_limit_final_last_conv = round(sm['longitudinalPlanSP'].speedLimit.resolver.speedLimitFinalLast * speed_conv)
pcm_long_required_max = cst_low if speed_limit_final_last_conv < CONFIRM_SPEED_THRESHOLD[metric] else cst_high
pcm_long_required_max_set_speed_conv = round(pcm_long_required_max * speed_conv)
speed_unit = "km/h" if metric else "mph"
alert_2_str = f"Manually change set speed to {pcm_long_required_max_set_speed_conv} {speed_unit} to activate"
else: