sunnypilot v2026.002.000
version: sunnypilot v2026.002.000 (staging) date: 2026-07-21T07:44:02 master commit: 2c334ede443d7391d27575af8c854a095ba702a8
This commit is contained in:
commit
de77c89cf6
3995 files changed
+1260688
No files matched your search
@@ -0,0 +1,104 @@
|
||||
from openpilot.cereal import log, custom
|
||||
from openpilot.common.constants import CV
|
||||
from openpilot.common.realtime import DT_MDL
|
||||
from openpilot.sunnypilot.selfdrive.controls.lib.auto_lane_change import AutoLaneChangeController, AutoLaneChangeMode
|
||||
from openpilot.sunnypilot.selfdrive.controls.lib.lane_turn_desire import LaneTurnController
|
||||
|
||||
LaneChangeState = log.LaneChangeState
|
||||
LaneChangeDirection = log.LaneChangeDirection
|
||||
TurnDirection = custom.ModelDataV2SP.TurnDirection
|
||||
|
||||
LANE_CHANGE_SPEED_MIN = 20 * CV.MPH_TO_MS
|
||||
LANE_CHANGE_TIME_MAX = 10.
|
||||
LANE_CHANGE_START_TIME = 0.5
|
||||
|
||||
TURN_DESIRES = {
|
||||
TurnDirection.none: log.Desire.none,
|
||||
TurnDirection.turnLeft: log.Desire.turnLeft,
|
||||
TurnDirection.turnRight: log.Desire.turnRight,
|
||||
}
|
||||
|
||||
class DesireHelper:
|
||||
def __init__(self):
|
||||
self.lane_change_state = LaneChangeState.off
|
||||
self.lane_change_direction = LaneChangeDirection.none
|
||||
self.lane_change_timer = 0.0
|
||||
self.prev_one_blinker = False
|
||||
self.desire = log.Desire.none
|
||||
self.alc = AutoLaneChangeController(self)
|
||||
self.lane_turn_controller = LaneTurnController(self)
|
||||
self.lane_turn_direction = TurnDirection.none
|
||||
|
||||
@staticmethod
|
||||
def get_lane_change_direction(CS):
|
||||
return LaneChangeDirection.left if CS.leftBlinker else LaneChangeDirection.right
|
||||
|
||||
def update(self, carstate, lateral_active, lane_change_prob):
|
||||
self.alc.update_params()
|
||||
self.lane_turn_controller.update_params()
|
||||
v_ego = carstate.vEgo
|
||||
one_blinker = carstate.leftBlinker != carstate.rightBlinker
|
||||
below_lane_change_speed = v_ego < LANE_CHANGE_SPEED_MIN
|
||||
|
||||
# Lane turn controller update
|
||||
self.lane_turn_controller.update_lane_turn(blindspot_left=carstate.leftBlindspot, blindspot_right=carstate.rightBlindspot,
|
||||
left_blinker=carstate.leftBlinker, right_blinker=carstate.rightBlinker, v_ego=v_ego)
|
||||
self.lane_turn_direction = self.lane_turn_controller.get_turn_direction()
|
||||
|
||||
if not lateral_active or self.lane_change_timer > LANE_CHANGE_TIME_MAX or self.alc.lane_change_set_timer == AutoLaneChangeMode.OFF:
|
||||
self.lane_change_state = LaneChangeState.off
|
||||
self.lane_change_direction = LaneChangeDirection.none
|
||||
self.lane_change_timer = 0.0
|
||||
else:
|
||||
if self.lane_change_state == LaneChangeState.off and one_blinker and not self.prev_one_blinker and not below_lane_change_speed:
|
||||
self.lane_change_state = LaneChangeState.preLaneChange
|
||||
self.lane_change_timer = 0.0
|
||||
# Initialize lane change direction to prevent UI alert flicker
|
||||
self.lane_change_direction = self.get_lane_change_direction(carstate)
|
||||
|
||||
elif self.lane_change_state == LaneChangeState.preLaneChange:
|
||||
# Update lane change direction
|
||||
self.lane_change_direction = self.get_lane_change_direction(carstate)
|
||||
|
||||
torque_applied = carstate.steeringPressed and \
|
||||
((carstate.steeringTorque > 0 and self.lane_change_direction == LaneChangeDirection.left) or
|
||||
(carstate.steeringTorque < 0 and self.lane_change_direction == LaneChangeDirection.right))
|
||||
|
||||
blindspot_detected = ((carstate.leftBlindspot and self.lane_change_direction == LaneChangeDirection.left) or
|
||||
(carstate.rightBlindspot and self.lane_change_direction == LaneChangeDirection.right))
|
||||
|
||||
self.alc.update_lane_change(blindspot_detected, carstate.brakePressed)
|
||||
|
||||
if not one_blinker or below_lane_change_speed:
|
||||
self.lane_change_state = LaneChangeState.off
|
||||
self.lane_change_direction = LaneChangeDirection.none
|
||||
self.lane_change_timer = 0.0
|
||||
elif (torque_applied or self.alc.auto_lane_change_allowed) and not blindspot_detected:
|
||||
self.lane_change_state = LaneChangeState.laneChangeStarting
|
||||
self.lane_change_timer = 0.0
|
||||
|
||||
elif self.lane_change_state == LaneChangeState.laneChangeStarting:
|
||||
self.lane_change_timer += DT_MDL
|
||||
|
||||
if lane_change_prob < 0.02 and self.lane_change_timer >= LANE_CHANGE_START_TIME:
|
||||
self.lane_change_timer = 0.0
|
||||
if one_blinker:
|
||||
self.lane_change_state = LaneChangeState.preLaneChange
|
||||
self.lane_change_direction = self.get_lane_change_direction(carstate)
|
||||
else:
|
||||
self.lane_change_state = LaneChangeState.off
|
||||
self.lane_change_direction = LaneChangeDirection.none
|
||||
|
||||
self.prev_one_blinker = one_blinker and lateral_active
|
||||
|
||||
if self.lane_turn_direction != TurnDirection.none:
|
||||
self.desire = TURN_DESIRES[self.lane_turn_direction]
|
||||
else:
|
||||
self.desire = log.Desire.none
|
||||
if self.lane_change_state == LaneChangeState.laneChangeStarting:
|
||||
if self.lane_change_direction == LaneChangeDirection.left:
|
||||
self.desire = log.Desire.laneChangeLeft
|
||||
elif self.lane_change_direction == LaneChangeDirection.right:
|
||||
self.desire = log.Desire.laneChangeRight
|
||||
|
||||
self.alc.update_state()
|
||||
Reference in new issue
Block a user