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

pcm long only

Browse Source
This commit is contained in:
Jason Wen
2025-09-23 00:27:41 -04:00
parent 6fe1921fe6
commit 024086ee5a
1 changed files with 15 additions and 17 deletions
Download Patch File Download Diff File Expand all files Collapse all files
sunnypilot/selfdrive/controls/lib/speed_limit/speed_limit_assist.py
+15 -17
View File
@@ -68,7 +68,7 @@ class SpeedLimitAssist:
self._distance = 0.
self.state = SpeedLimitAssistState.disabled
self._state_prev = SpeedLimitAssistState.disabled
self.pcm_cruise_long = CP.openpilotLongitudinalControl and CP.pcmCruise
self.pcm_op_long = CP.openpilotLongitudinalControl and CP.pcmCruise
# TODO-SP: SLA's own output_a_target for planner
# Solution functions mapped to respective states
@@ -137,6 +137,16 @@ class SpeedLimitAssist:
def get_active_state_target_acceleration(self) -> float:
return self.v_offset / float(ModelConstants.T_IDXS[CONTROL_N])
def _update_pcm_op_long_state(self):
if self.initial_max_set_confirmed():
if self._speed_limit > 0:
if self.v_offset < LIMIT_SPEED_OFFSET_TH:
self.state = SpeedLimitAssistState.adapting
else:
self.state = SpeedLimitAssistState.active
else:
self.state = SpeedLimitAssistState.pending
def update_state_machine(self):
self._state_prev = self.state
@@ -173,14 +183,8 @@ class SpeedLimitAssist:
# PRE_ACTIVE
elif self.state == SpeedLimitAssistState.preActive:
if self.initial_max_set_confirmed():
if self._speed_limit > 0:
if self.v_offset < LIMIT_SPEED_OFFSET_TH:
self.state = SpeedLimitAssistState.adapting
else:
self.state = SpeedLimitAssistState.active
else:
self.state = SpeedLimitAssistState.pending
if self.pcm_op_long:
self._update_pcm_op_long_state()
elif self.pre_active_timer <= PRE_ACTIVE_GUARD_PERIOD:
# Timeout - session ended
self.state = SpeedLimitAssistState.inactive
@@ -197,14 +201,8 @@ class SpeedLimitAssist:
self.long_engaged_timer = int(DISABLED_GUARD_PERIOD / DT_MDL)
elif self.long_engaged_timer <= 0:
if self.initial_max_set_confirmed():
if self._speed_limit > 0:
if self.v_offset < LIMIT_SPEED_OFFSET_TH:
self.state = SpeedLimitAssistState.adapting
else:
self.state = SpeedLimitAssistState.active
else:
self.state = SpeedLimitAssistState.pending
if self.pcm_op_long:
self._update_pcm_op_long_state()
else:
self.state = SpeedLimitAssistState.preActive
self.pre_active_timer = int(PRE_ACTIVE_GUARD_PERIOD / DT_MDL)