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

Merge remote-tracking branch 'sunnypilot/sunnypilot/master' into hkg-angle-steering-2025

Browse Source
This commit is contained in:
Jason Wen
2025-10-17 23:42:01 -04:00
parent f9ae9192fa 523c92c6fe
commit 79ea7db103
4 changed files with 26 additions and 27 deletions
Download Patch File Download Diff File Expand all files Collapse all files
selfdrive/controls/controlsd.py
+3 -1
View File
@@ -99,7 +99,6 @@ class Controls(ControlsExt, ModelStateBase):
self.LaC.extension.update_model_v2(self.sm['modelV2'])
self.lat_delay = get_lat_delay(self.params, self.sm["liveDelay"].lateralDelay)
self.LaC.extension.update_lateral_lag(self.lat_delay)
long_plan = self.sm['longitudinalPlan']
@@ -234,6 +233,9 @@ class Controls(ControlsExt, ModelStateBase):
while not evt.is_set():
self.get_params_sp()
if self.CP.lateralTuning.which() == 'torque':
self.lat_delay = get_lat_delay(self.params, self.sm["liveDelay"].lateralDelay)
time.sleep(0.1)
def run(self):
sunnypilot/selfdrive/controls/lib/speed_limit/speed_limit_assist.py
+13 -9
View File
@@ -27,7 +27,11 @@ ACTIVE_STATES = (SpeedLimitAssistState.active, SpeedLimitAssistState.adapting)
ENABLED_STATES = (SpeedLimitAssistState.preActive, SpeedLimitAssistState.pending, *ACTIVE_STATES)
DISABLED_GUARD_PERIOD = 0.5 # secs.
PRE_ACTIVE_GUARD_PERIOD = 15 # secs. Time to wait after activation before considering temp deactivation signal.
# secs. Time to wait after activation before considering temp deactivation signal.
PRE_ACTIVE_GUARD_PERIOD = {
True: 15,
False: 5,
}
SPEED_LIMIT_CHANGED_HOLD_PERIOD = 1 # secs. Time to wait after speed limit change before switching to preActive.
LIMIT_MIN_ACC = -1.5 # m/s^2 Maximum deceleration allowed for limit controllers to provide.
@@ -241,7 +245,7 @@ class SpeedLimitAssist:
self.state = SpeedLimitAssistState.inactive
elif self.speed_limit_changed and self.apply_confirm_speed_threshold:
self.state = SpeedLimitAssistState.preActive
self.pre_active_timer = int(PRE_ACTIVE_GUARD_PERIOD / DT_MDL)
self.pre_active_timer = int(PRE_ACTIVE_GUARD_PERIOD[self.pcm_op_long] / DT_MDL)
elif self._has_speed_limit and self.v_offset < LIMIT_SPEED_OFFSET_TH:
self.state = SpeedLimitAssistState.adapting
@@ -251,7 +255,7 @@ class SpeedLimitAssist:
self.state = SpeedLimitAssistState.inactive
elif self.speed_limit_changed and self.apply_confirm_speed_threshold:
self.state = SpeedLimitAssistState.preActive
self.pre_active_timer = int(PRE_ACTIVE_GUARD_PERIOD / DT_MDL)
self.pre_active_timer = int(PRE_ACTIVE_GUARD_PERIOD[self.pcm_op_long] / DT_MDL)
elif self.v_offset >= LIMIT_SPEED_OFFSET_TH:
self.state = SpeedLimitAssistState.active
@@ -261,7 +265,7 @@ class SpeedLimitAssist:
self._update_confirmed_state()
elif self.speed_limit_changed:
self.state = SpeedLimitAssistState.preActive
self.pre_active_timer = int(PRE_ACTIVE_GUARD_PERIOD / DT_MDL)
self.pre_active_timer = int(PRE_ACTIVE_GUARD_PERIOD[self.pcm_op_long] / DT_MDL)
# PRE_ACTIVE
elif self.state == SpeedLimitAssistState.preActive:
@@ -287,7 +291,7 @@ class SpeedLimitAssist:
self._update_confirmed_state()
elif self._has_speed_limit:
self.state = SpeedLimitAssistState.preActive
self.pre_active_timer = int(PRE_ACTIVE_GUARD_PERIOD / DT_MDL)
self.pre_active_timer = int(PRE_ACTIVE_GUARD_PERIOD[self.pcm_op_long] / DT_MDL)
else:
self.state = SpeedLimitAssistState.pending
@@ -313,7 +317,7 @@ class SpeedLimitAssist:
elif self.speed_limit_changed and self.apply_confirm_speed_threshold:
self.state = SpeedLimitAssistState.preActive
self.pre_active_timer = int(PRE_ACTIVE_GUARD_PERIOD / DT_MDL)
self.pre_active_timer = int(PRE_ACTIVE_GUARD_PERIOD[self.pcm_op_long] / DT_MDL)
# PRE_ACTIVE
elif self.state == SpeedLimitAssistState.preActive:
@@ -327,7 +331,7 @@ class SpeedLimitAssist:
elif self.state == SpeedLimitAssistState.inactive:
if self.speed_limit_changed:
self.state = SpeedLimitAssistState.preActive
self.pre_active_timer = int(PRE_ACTIVE_GUARD_PERIOD / DT_MDL)
self.pre_active_timer = int(PRE_ACTIVE_GUARD_PERIOD[self.pcm_op_long] / DT_MDL)
elif self._update_non_pcm_long_confirmed_state():
self.state = SpeedLimitAssistState.active
@@ -343,7 +347,7 @@ class SpeedLimitAssist:
self.state = SpeedLimitAssistState.active
elif self._has_speed_limit:
self.state = SpeedLimitAssistState.preActive
self.pre_active_timer = int(PRE_ACTIVE_GUARD_PERIOD / DT_MDL)
self.pre_active_timer = int(PRE_ACTIVE_GUARD_PERIOD[self.pcm_op_long] / DT_MDL)
else:
self.state = SpeedLimitAssistState.inactive
@@ -369,7 +373,7 @@ class SpeedLimitAssist:
if self.speed_limit_prev <= 0:
self.update_active_event(events_sp)
elif self.speed_limit_prev > 0 and self._speed_limit > 0:
events_sp.add(EventNameSP.speedLimitChanged)
self.update_active_event(events_sp)
def update(self, long_enabled: bool, long_override: bool, v_ego: float, a_ego: float, v_cruise_cluster: float, speed_limit: float,
speed_limit_final_last: float, has_speed_limit: bool, distance: float, events_sp: EventsSP) -> None:
sunnypilot/selfdrive/controls/lib/speed_limit/tests/test_speed_limit_assist.py
+2 -2
View File
@@ -39,7 +39,7 @@ class TestSpeedLimitAssist:
self.events_sp = EventsSP()
CI = self._setup_platform(TOYOTA.TOYOTA_RAV4_TSS2)
self.sla = SpeedLimitAssist(CI.CP)
self.sla.pre_active_timer = int(PRE_ACTIVE_GUARD_PERIOD / DT_MDL)
self.sla.pre_active_timer = int(PRE_ACTIVE_GUARD_PERIOD[self.sla.pcm_op_long] / DT_MDL)
self.pcm_long_max_set_speed = PCM_LONG_REQUIRED_MAX_SET_SPEED[self.sla.is_metric][1] # use 80 MPH for now
self.speed_conv = CV.MS_TO_KPH if self.sla.is_metric else CV.MS_TO_MPH
@@ -114,7 +114,7 @@ class TestSpeedLimitAssist:
self.sla.state = SpeedLimitAssistState.preActive
self.sla.update(True, False, SPEED_LIMITS['city'], 0, SPEED_LIMITS['highway'], SPEED_LIMITS['city'], SPEED_LIMITS['city'], True, 0, self.events_sp)
for _ in range(int(PRE_ACTIVE_GUARD_PERIOD / DT_MDL)):
for _ in range(int(PRE_ACTIVE_GUARD_PERIOD[self.sla.pcm_op_long] / DT_MDL)):
self.sla.update(True, False, SPEED_LIMITS['city'], 0, SPEED_LIMITS['highway'], SPEED_LIMITS['city'], SPEED_LIMITS['city'], True, 0, self.events_sp)
assert self.sla.state == SpeedLimitAssistState.inactive
sunnypilot/selfdrive/selfdrived/events.py
+8 -15
View File
@@ -4,7 +4,6 @@ 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, CONFIRM_SPEED_THRESHOLD
from openpilot.sunnypilot.selfdrive.controls.lib.speed_limit.helpers import compare_cluster_target
AlertSize = log.SelfdriveState.AlertSize
@@ -34,6 +33,9 @@ def speed_limit_pre_active_alert(CP: car.CarParams, CS: car.CarState, sm: messag
speed_conv = CV.MS_TO_KPH if metric else CV.MS_TO_MPH
speed_limit_final_last = sm['longitudinalPlanSP'].speedLimit.resolver.speedLimitFinalLast
speed_limit_final_last_conv = round(speed_limit_final_last * speed_conv)
alert_1_str = ""
alert_2_str = ""
alert_size = AlertSize.none
if CP.openpilotLongitudinalControl and CP.pcmCruise:
# PCM long
@@ -41,24 +43,15 @@ def speed_limit_pre_active_alert(CP: car.CarParams, CS: car.CarState, sm: messag
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_1_str = "Speed Limit Assist: Activation Required"
alert_2_str = f"Manually change set speed to {pcm_long_required_max_set_speed_conv} {speed_unit} to activate"
else:
# Non PCM long
v_cruise_cluster = CS.vCruiseCluster * CV.KPH_TO_MS
req_plus, req_minus = compare_cluster_target(v_cruise_cluster, speed_limit_final_last, metric)
arrow_str = ""
if req_plus:
arrow_str = "RES/+"
elif req_minus:
arrow_str = "SET/-"
alert_2_str = f"Operate the {arrow_str} cruise control button to activate"
alert_size = AlertSize.mid
return Alert(
"Speed Limit Assist: Activation Required",
alert_1_str,
alert_2_str,
AlertStatus.normal, AlertSize.mid,
AlertStatus.normal, alert_size,
Priority.LOW, VisualAlert.none, AudibleAlertSP.promptSingleLow, .1)