mawei/sunnypilot
1
0
Fork 0
mirror of https://github.com/sunnypilot/sunnypilot.git synced 2026-09-18 10:23:43 +08:00
Code Issues Packages Projects Releases Wiki Activity

refine stopping acceleration calculation in longcontrol.py

Browse Source 
Update longcontrol.py

smooth
This commit is contained in:
rav4kumar
2026-08-06 10:31:52 -07:00
committed by rav4kumar
parent cab08c9516
commit 8c5eb73674
2 changed files with 20 additions and 3 deletions
Download Patch File Download Diff File Expand all files Collapse all files
openpilot/selfdrive/controls/lib/longcontrol.py
+4 -3
View File
@@ -4,6 +4,7 @@ from openpilot.common.realtime import DT_CTRL
from openpilot.selfdrive.controls.lib.drive_helpers import CONTROL_N
from openpilot.common.pid import PIDController
from openpilot.selfdrive.modeld.constants import ModelConstants
from openpilot.sunnypilot.selfdrive.controls.lib.longcontrol import LongControlSP
CONTROL_N_T_IDX = ModelConstants.T_IDXS[:CONTROL_N]
@@ -39,7 +40,7 @@ def long_control_state_trans(CP_SP, active, long_control_state,
return long_control_state
class LongControl:
class LongControl(LongControlSP):
def __init__(self, CP, CP_SP):
self.CP = CP
self.CP_SP = CP_SP
@@ -65,10 +66,10 @@ class LongControl:
elif self.long_control_state == LongCtrlState.stopping:
output_accel = self.last_output_accel
if output_accel > self.CP.stopAccel:
if output_accel > self.CP.stopAccel and not LongControlSP.should_hold_stopping(self, CS, a_target):
output_accel = min(output_accel, 0.0)
# TODO: can we just go straight to stopAccel?
output_accel -= 1.0 * DT_CTRL # m/s^2/s while trying to stop
output_accel -= np.interp(CS.vEgo, [0.0, 1.0, 3.0], [0.0, 0.25, 0.5]) * DT_CTRL
self.reset()
else: # LongCtrlState.pid
openpilot/sunnypilot/selfdrive/controls/lib/longcontrol.py
+16
View File
@@ -0,0 +1,16 @@
"""
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.
"""
STOPPING_DISTANCE = 1.5
STOPPED_SPEED = 0.02
STOPPING_TIME = 2.5
class LongControlSP:
def should_hold_stopping(self, CS, a_target: float) -> bool:
return (self.last_output_accel <= 0.0 and a_target >= self.last_output_accel and CS.vEgo > STOPPED_SPEED and CS.aEgo < 0.0
and CS.vEgo <= -CS.aEgo * STOPPING_TIME and CS.vEgo ** 2 <= -2.0 * CS.aEgo * STOPPING_DISTANCE)