github-actions[bot]
54e6124925
version: sunnypilot v2026.001.000 (dev) date: 2026-05-07T23:07:19 master commit: c28eb958740187620f2282023b8f1997cf90f583
113 lines
3.6 KiB
Python
113 lines
3.6 KiB
Python
"""
|
|
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.
|
|
"""
|
|
from cereal import log
|
|
|
|
from openpilot.common.params import Params
|
|
from openpilot.common.realtime import DT_MDL
|
|
|
|
|
|
class AutoLaneChangeMode:
|
|
OFF = -1
|
|
NUDGE = 0 # default
|
|
NUDGELESS = 1
|
|
HALF_SECOND = 2
|
|
ONE_SECOND = 3
|
|
TWO_SECONDS = 4
|
|
THREE_SECONDS = 5
|
|
|
|
|
|
AUTO_LANE_CHANGE_TIMER = {
|
|
AutoLaneChangeMode.OFF: 0.0, # Off
|
|
AutoLaneChangeMode.NUDGE: 0.0, # Nudge
|
|
AutoLaneChangeMode.NUDGELESS: 0.05, # Nudgeless
|
|
AutoLaneChangeMode.HALF_SECOND: 0.5, # 0.5-second delay
|
|
AutoLaneChangeMode.ONE_SECOND: 1.0, # 1-second delay
|
|
AutoLaneChangeMode.TWO_SECONDS: 2.0, # 2-second delay
|
|
AutoLaneChangeMode.THREE_SECONDS: 3.0, # 3-second delay
|
|
}
|
|
|
|
ONE_SECOND_DELAY = -1
|
|
|
|
|
|
class AutoLaneChangeController:
|
|
def __init__(self, desire_helper):
|
|
self.DH = desire_helper
|
|
self.params = Params()
|
|
|
|
self.lane_change_wait_timer = 0.0
|
|
self.param_read_counter = 0
|
|
self.lane_change_delay = 0.0
|
|
|
|
self.lane_change_set_timer = self.params.get("AutoLaneChangeTimer", return_default=True)
|
|
self.lane_change_bsm_delay = False
|
|
|
|
self.prev_brake_pressed = False
|
|
self.auto_lane_change_allowed = False
|
|
self.prev_lane_change = False
|
|
|
|
self.read_params()
|
|
|
|
def reset(self) -> None:
|
|
# Auto reset if parent state indicates we should
|
|
if self.DH.lane_change_state == log.LaneChangeState.off and \
|
|
self.DH.lane_change_direction == log.LaneChangeDirection.none:
|
|
self.lane_change_wait_timer = 0.0
|
|
self.prev_brake_pressed = False
|
|
self.prev_lane_change = False
|
|
|
|
def read_params(self) -> None:
|
|
self.lane_change_bsm_delay = self.params.get_bool("AutoLaneChangeBsmDelay")
|
|
self.lane_change_set_timer = self.params.get("AutoLaneChangeTimer", return_default=True)
|
|
|
|
def update_params(self) -> None:
|
|
if self.param_read_counter % 50 == 0:
|
|
self.read_params()
|
|
self.param_read_counter += 1
|
|
|
|
def update_lane_change_timers(self, blindspot_detected: bool) -> None:
|
|
self.lane_change_delay = AUTO_LANE_CHANGE_TIMER.get(self.lane_change_set_timer,
|
|
AUTO_LANE_CHANGE_TIMER[AutoLaneChangeMode.NUDGE])
|
|
|
|
self.lane_change_wait_timer += DT_MDL
|
|
|
|
if self.lane_change_bsm_delay and blindspot_detected and self.lane_change_delay > 0:
|
|
if self.lane_change_delay == AUTO_LANE_CHANGE_TIMER[AutoLaneChangeMode.NUDGELESS]:
|
|
self.lane_change_wait_timer = ONE_SECOND_DELAY
|
|
else:
|
|
self.lane_change_wait_timer = self.lane_change_delay + ONE_SECOND_DELAY
|
|
|
|
def update_allowed(self) -> bool:
|
|
# Auto lane change allowed if:
|
|
# 1. A valid delay is set (non-zero)
|
|
# 2. Brake wasn't previously pressed
|
|
# 3. We've waited long enough
|
|
|
|
if self.lane_change_set_timer in (AutoLaneChangeMode.OFF, AutoLaneChangeMode.NUDGE):
|
|
return False
|
|
|
|
if self.prev_brake_pressed:
|
|
return False
|
|
|
|
if self.prev_lane_change:
|
|
return False
|
|
|
|
return bool(self.lane_change_wait_timer > self.lane_change_delay)
|
|
|
|
def update_lane_change(self, blindspot_detected: bool, brake_pressed: bool) -> None:
|
|
if brake_pressed and not self.prev_brake_pressed:
|
|
self.prev_brake_pressed = brake_pressed
|
|
|
|
self.update_lane_change_timers(blindspot_detected)
|
|
|
|
self.auto_lane_change_allowed = self.update_allowed()
|
|
|
|
def update_state(self):
|
|
if self.DH.lane_change_state == log.LaneChangeState.laneChangeStarting:
|
|
self.prev_lane_change = True
|
|
|
|
self.reset()
|