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

check ttc

Browse Source
This commit is contained in:
Jason Wen
2024-08-07 11:21:11 -04:00
parent 368a5e3eb4
commit bd5228e394
1 changed files with 12 additions and 2 deletions
Download Patch File Download Diff File Expand all files Collapse all files
selfdrive/controls/controlsd.py
+12 -2
View File
@@ -186,6 +186,8 @@ class Controls:
self.dynamic_personality = self.params.get_bool("DynamicPersonality")
self.overtaking_accel = self.params.get_bool("OvertakingAccelerationAssist")
self.overtaking_accel_engaged = False
self.prev_overtaking_accel_engaged = False
self.accel_personality = self.read_accel_personality_param()
@@ -824,9 +826,17 @@ class Controls:
overtaking_accel_allowed = ((blinker_svs.laneChangeDirection == LaneChangeDirection.right and dm_state.isRHD) or
(blinker_svs.laneChangeDirection == LaneChangeDirection.left and not dm_state.isRHD)) and \
(blinker_svs.laneChangeState in (LaneChangeState.preLaneChange, LaneChangeState.laneChangeStarting))
self.prev_overtaking_accel_engaged = self.overtaking_accel_engaged
ttc = self.sm['radarState'].leadOne.dRel / CS.vEgo if CS.vEgo > 0 else 255
overtaking_accel_engaged = self.overtaking_accel and overtaking_accel_allowed and \
CS.vEgo > (60 * CV.KPH_TO_MS) if self.is_metric else (40 * CV.MPH_TO_MS) and long_plan.hasLead and \
long_plan.aTarget > -0.2 and not (CS.leftBlinker and CS.rightBlinker)
long_plan.aTarget > -0.2 and not (CS.leftBlinker and CS.rightBlinker) and ttc > 0.75
if ttc <= 0.75 and self.prev_overtaking_accel_engaged and overtaking_accel_engaged:
overtaking_accel_engaged = False
if overtaking_accel_engaged and not self.prev_overtaking_accel_engaged:
self.overtaking_accel_engaged = True
elif not overtaking_accel_engaged:
self.overtaking_accel_engaged = False
# controlsState
dat = messaging.new_message('controlsState')
@@ -880,7 +890,7 @@ class Controls:
controlsStateSP.personality = self.personality
controlsStateSP.dynamicPersonality = self.dynamic_personality
controlsStateSP.accelPersonality = self.accel_personality
controlsStateSP.overtakingAccelerationAssist = overtaking_accel_engaged
controlsStateSP.overtakingAccelerationAssist = self.overtaking_accel_engaged
if self.enable_nnff and lat_tuning == 'torque':
controlsStateSP.lateralControlState.torqueState = self.LaC.pid_long_sp