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,112 @@
|
||||
"""
|
||||
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 openpilot.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()
|
||||
Reference in new issue
Block a user