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-04-27 13:44:34 -05:00
parent ea1aad5ed1
commit 2fbe9dbea1
25 changed files with 1501 additions and 532 deletions
Download Patch File Download Diff File Expand all files Collapse all files

20
selfdrive/controls/lib/desire_helper.py
View File

@@ -37,7 +37,6 @@ TURN_DESIRES = {
TurnDirection.turnRight: log.Desire.turnRight,
}
class DesireHelper:
def __init__(self):
self.lane_change_state = LaneChangeState.off
@@ -70,7 +69,6 @@ class DesireHelper:
lane_available = True
else:
desired_lane = frogpilotPlan.laneWidthLeft if carstate.leftBlinker else frogpilotPlan.laneWidthRight
# Check if the lane width exceeds the threshold
lane_available = desired_lane >= self.lane_detection_width
if not lateral_active or self.lane_change_timer > LANE_CHANGE_TIME_MAX:
@@ -82,7 +80,6 @@ class DesireHelper:
# Set the "turn_completed" flag to prevent lane changes after completing a turn
self.turn_completed = True
else:
# TurnDirection.turnLeft / turnRight
self.turn_direction = TurnDirection.none
# LaneChangeState.off
@@ -104,19 +101,17 @@ class DesireHelper:
blindspot_detected = ((carstate.leftBlindspot and self.lane_change_direction == LaneChangeDirection.left) or
(carstate.rightBlindspot and self.lane_change_direction == LaneChangeDirection.right))
# Conduct a nudgeless lane change if all the conditions are met
self.lane_change_wait_timer += DT_MDL
if self.nudgeless and lane_available and not self.lane_change_completed and self.lane_change_wait_timer >= self.lane_change_delay:
self.lane_change_wait_timer = 0
torque_applied = True
self.lane_change_wait_timer = 0
if not one_blinker or below_lane_change_speed:
self.lane_change_state = LaneChangeState.off
self.lane_change_direction = LaneChangeDirection.none
elif torque_applied and not blindspot_detected:
# Set the "lane_change_completed" flag to prevent any more lane changes if the toggle is on
self.lane_change_completed = self.one_lane_change
self.lane_change_state = LaneChangeState.laneChangeStarting
self.lane_change_completed = True if self.one_lane_change else False
# LaneChangeState.laneChangeStarting
elif self.lane_change_state == LaneChangeState.laneChangeStarting:
@@ -144,10 +139,8 @@ class DesireHelper:
else:
self.lane_change_timer += DT_MDL
self.prev_one_blinker = one_blinker
# Reset the flags
self.lane_change_completed &= one_blinker
self.prev_one_blinker = one_blinker
self.turn_completed &= one_blinker
if self.turn_direction != TurnDirection.none:
@@ -173,10 +166,11 @@ class DesireHelper:
def update_frogpilot_params(self):
is_metric = self.params.get_bool("IsMetric")
lateral_tune = self.params.get_bool("LateralTune")
self.turn_desires = lateral_tune and self.params.get_bool("TurnDesires")
self.nudgeless = self.params.get_bool("NudgelessLaneChange")
self.lane_change_delay = self.params.get_int("LaneChangeTime") if self.nudgeless else 0
self.lane_detection = self.nudgeless and self.params.get_bool("LaneDetection")
self.lane_detection = self.nudgeless and self.params.get_int("LaneDetectionWidth") != 0
self.lane_detection_width = self.params.get_int("LaneDetectionWidth") * (1 if is_metric else CV.FOOT_TO_METER) / 10 if self.lane_detection else 0
self.one_lane_change = self.nudgeless and self.params.get_bool("OneLaneChange")
self.turn_desires = self.params.get_bool("TurnDesires")