brianhansonxyz/clearpilot
1
0
Fork 0
Code Issues Pull Requests Packages Projects Releases Wiki Activity

wip

Browse Source
This commit is contained in:
Your Name
2024-05-03 01:12:47 -05:00
parent b94be3e23e
commit 02513c94c6
5 changed files with 49 additions and 37 deletions
Download Patch File Download Diff File Expand all files Collapse all files

51
selfdrive/controls/controlsd.py
View File

@@ -311,28 +311,30 @@ class Controls:
# Handle lane change
# CLEARPILOT - Disabled lane change helper
# CLEARPILOT TODO: Make this a toggle
# if self.sm['modelV2'].meta.laneChangeState == LaneChangeState.preLaneChange:
# direction = self.sm['modelV2'].meta.laneChangeDirection
# if (CS.leftBlindspot and direction == LaneChangeDirection.left) or \
# (CS.rightBlindspot and direction == LaneChangeDirection.right):
# if self.loud_blindspot_alert:
# self.events.add(EventName.laneChangeBlockedLoud)
# else:
# self.events.add(EventName.laneChangeBlocked)
# else:
# if direction == LaneChangeDirection.left:
# if self.sm['frogpilotPlan'].laneWidthLeft >= self.lane_detection_width:
# self.events.add(EventName.preLaneChangeLeft)
# else:
# self.events.add(EventName.noLaneAvailable)
# else:
# if self.sm['frogpilotPlan'].laneWidthRight >= self.lane_detection_width:
# self.events.add(EventName.preLaneChangeRight)
# else:
# self.events.add(EventName.noLaneAvailable)
# elif self.sm['modelV2'].meta.laneChangeState in (LaneChangeState.laneChangeStarting,
# LaneChangeState.laneChangeFinishing):
# self.events.add(EventName.laneChange)
NoLaneChange = False
if not NoLaneChange:
if self.sm['modelV2'].meta.laneChangeState == LaneChangeState.preLaneChange:
direction = self.sm['modelV2'].meta.laneChangeDirection
if (CS.leftBlindspot and direction == LaneChangeDirection.left) or \
(CS.rightBlindspot and direction == LaneChangeDirection.right):
if self.loud_blindspot_alert:
self.events.add(EventName.laneChangeBlockedLoud)
else:
self.events.add(EventName.laneChangeBlocked)
else:
if direction == LaneChangeDirection.left:
if self.sm['frogpilotPlan'].laneWidthLeft >= self.lane_detection_width:
self.events.add(EventName.preLaneChangeLeft)
else:
self.events.add(EventName.noLaneAvailable)
else:
if self.sm['frogpilotPlan'].laneWidthRight >= self.lane_detection_width:
self.events.add(EventName.preLaneChangeRight)
else:
self.events.add(EventName.noLaneAvailable)
elif self.sm['modelV2'].meta.laneChangeState in (LaneChangeState.laneChangeStarting,
LaneChangeState.laneChangeFinishing):
self.events.add(EventName.laneChange)
for i, pandaState in enumerate(self.sm['pandaStates']):
# All pandas must match the list of safetyConfigs, and if outside this list, must be silent or noOutput
@@ -628,10 +630,15 @@ class Controls:
actuators = CC.actuators
actuators.longControlState = self.LoC.long_control_state
# CLEARPILOT test lane change
clearpilot_disable_lat_on_lane_change = True
# Enable blinkers while lane changing
if model_v2.meta.laneChangeState != LaneChangeState.off:
CC.leftBlinker = model_v2.meta.laneChangeDirection == LaneChangeDirection.left
CC.rightBlinker = model_v2.meta.laneChangeDirection == LaneChangeDirection.right
if clearpilot_disable_lat_on_lane_change:
CC.latActive = False
if CS.leftBlinker or CS.rightBlinker:
self.last_blinker_frame = self.sm.frame