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
+128
@@ -0,0 +1,128 @@
|
||||
"""
|
||||
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 custom
|
||||
from opendbc.car.structs import car
|
||||
from opendbc.car import structs, apply_hysteresis
|
||||
from openpilot.common.constants import CV
|
||||
from openpilot.common.realtime import DT_CTRL
|
||||
from openpilot.sunnypilot.selfdrive.car.intelligent_cruise_button_management.helpers import get_minimum_set_speed
|
||||
from openpilot.sunnypilot.selfdrive.car.cruise_ext import CRUISE_BUTTON_TIMER, update_manual_button_timers
|
||||
|
||||
LongitudinalPlanSource = custom.LongitudinalPlanSP.LongitudinalPlanSource
|
||||
State = custom.IntelligentCruiseButtonManagement.IntelligentCruiseButtonManagementState
|
||||
SendButtonState = custom.IntelligentCruiseButtonManagement.SendButtonState
|
||||
|
||||
ALLOWED_SPEED_THRESHOLD = 1.8 # m/s, ~4 MPH
|
||||
HYST_GAP = 0.0 # currently disabled; TODO-SP: might need to be brand-specific
|
||||
INACTIVE_TIMER = 0.4
|
||||
|
||||
|
||||
SEND_BUTTONS = {
|
||||
State.increasing: SendButtonState.increase,
|
||||
State.decreasing: SendButtonState.decrease,
|
||||
}
|
||||
|
||||
|
||||
class IntelligentCruiseButtonManagement:
|
||||
def __init__(self, CP: structs.CarParams, CP_SP: structs.CarParamsSP):
|
||||
self.CP = CP
|
||||
self.CP_SP = CP_SP
|
||||
|
||||
self.v_target = 0
|
||||
self.v_cruise_cluster = 0
|
||||
self.v_cruise_min = 0
|
||||
self.cruise_button = SendButtonState.none
|
||||
self.state = State.inactive
|
||||
self.pre_active_timer = 0
|
||||
|
||||
self.is_ready = False
|
||||
self.is_ready_prev = False
|
||||
self.v_target_ms_last = 0.0
|
||||
self.is_metric = False
|
||||
|
||||
self.cruise_button_timers = CRUISE_BUTTON_TIMER
|
||||
|
||||
@property
|
||||
def v_cruise_equal(self) -> bool:
|
||||
return self.v_target == self.v_cruise_cluster
|
||||
|
||||
def update_calculations(self, CS: car.CarState, LP_SP: custom.LongitudinalPlanSP) -> None:
|
||||
speed_conv = CV.MS_TO_KPH if self.is_metric else CV.MS_TO_MPH
|
||||
ms_conv = CV.KPH_TO_MS if self.is_metric else CV.MPH_TO_MS
|
||||
|
||||
self.v_target_ms_last = apply_hysteresis(LP_SP.vTarget, self.v_target_ms_last, HYST_GAP * ms_conv)
|
||||
|
||||
self.v_target = round(self.v_target_ms_last * speed_conv)
|
||||
self.v_cruise_min = get_minimum_set_speed(self.is_metric)
|
||||
self.v_cruise_cluster = round(CS.cruiseState.speedCluster * speed_conv)
|
||||
|
||||
def update_state_machine(self) -> custom.IntelligentCruiseButtonManagement.SendButtonState:
|
||||
self.pre_active_timer = max(0, self.pre_active_timer - 1)
|
||||
|
||||
# HOLDING, ACCELERATING, DECELERATING, PRE_ACTIVE
|
||||
if self.state != State.inactive:
|
||||
if not self.is_ready:
|
||||
self.state = State.inactive
|
||||
|
||||
else:
|
||||
# PRE_ACTIVE
|
||||
if self.state == State.preActive:
|
||||
if self.pre_active_timer <= 0:
|
||||
if self.v_cruise_equal:
|
||||
self.state = State.holding
|
||||
|
||||
elif self.v_target > self.v_cruise_cluster:
|
||||
self.state = State.increasing
|
||||
|
||||
elif self.v_target < self.v_cruise_cluster and self.v_cruise_cluster > self.v_cruise_min:
|
||||
self.state = State.decreasing
|
||||
|
||||
# HOLDING
|
||||
elif self.state == State.holding:
|
||||
if not self.v_cruise_equal:
|
||||
self.state = State.preActive
|
||||
|
||||
# ACCELERATING
|
||||
elif self.state == State.increasing:
|
||||
if self.v_target <= self.v_cruise_cluster:
|
||||
self.state = State.holding
|
||||
|
||||
# DECELERATING
|
||||
elif self.state == State.decreasing:
|
||||
if self.v_target >= self.v_cruise_cluster or self.v_cruise_cluster <= self.v_cruise_min:
|
||||
self.state = State.holding
|
||||
|
||||
# INACTIVE
|
||||
elif self.state == State.inactive:
|
||||
if self.is_ready and not self.is_ready_prev:
|
||||
self.pre_active_timer = int(INACTIVE_TIMER / DT_CTRL)
|
||||
self.state = State.preActive
|
||||
|
||||
send_button = SEND_BUTTONS.get(self.state, SendButtonState.none)
|
||||
|
||||
return send_button
|
||||
|
||||
def update_readiness(self, CS: car.CarState, CC: car.CarControl) -> None:
|
||||
update_manual_button_timers(CS, self.cruise_button_timers)
|
||||
|
||||
ready = CC.enabled and not CC.cruiseControl.override and not CC.cruiseControl.cancel and not CC.cruiseControl.resume
|
||||
button_pressed = any(self.cruise_button_timers[k] > 0 for k in self.cruise_button_timers)
|
||||
|
||||
self.is_ready = ready and not button_pressed
|
||||
|
||||
def run(self, CS: car.CarState, CC: car.CarControl, LP_SP: custom.LongitudinalPlanSP, is_metric: bool) -> None:
|
||||
if self.CP_SP.pcmCruiseSpeed:
|
||||
return
|
||||
|
||||
self.is_metric = is_metric
|
||||
|
||||
self.update_calculations(CS, LP_SP)
|
||||
self.update_readiness(CS, CC)
|
||||
|
||||
self.cruise_button = self.update_state_machine()
|
||||
|
||||
self.is_ready_prev = self.is_ready
|
||||
Reference in new issue
Block a user