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
Whitespace-only changes.
@@ -0,0 +1 @@
|
||||
../../../../opendbc_repo/opendbc/sunnypilot/car/car_list.json
|
||||
@@ -0,0 +1,51 @@
|
||||
"""
|
||||
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, custom
|
||||
from opendbc.car import structs
|
||||
|
||||
from opendbc.car.chrysler.values import RAM_DT
|
||||
from openpilot.selfdrive.selfdrived.events import Events
|
||||
from openpilot.sunnypilot.selfdrive.selfdrived.events import EventsSP
|
||||
|
||||
EventName = log.OnroadEvent.EventName
|
||||
EventNameSP = custom.OnroadEventSP.EventName
|
||||
GearShifter = structs.CarState.GearShifter
|
||||
|
||||
|
||||
class CarSpecificEventsSP:
|
||||
def __init__(self, CP: structs.CarParams, CP_SP: structs.CarParamsSP):
|
||||
self.CP = CP
|
||||
self.CP_SP = CP_SP
|
||||
|
||||
self.low_speed_alert = False
|
||||
|
||||
def update(self, CS: structs.CarState, events: Events):
|
||||
events_sp = EventsSP()
|
||||
|
||||
if self.CP.brand == 'chrysler':
|
||||
if self.CP.carFingerprint in RAM_DT:
|
||||
# remove belowSteerSpeed event from CarSpecificEvents as RAM_DT uses a different logic
|
||||
if events.has(EventName.belowSteerSpeed):
|
||||
events.remove(EventName.belowSteerSpeed)
|
||||
|
||||
# TODO-SP: use if/elif to have the gear shifter condition takes precedence over the speed condition
|
||||
# TODO-SP: add 1 m/s hysteresis
|
||||
if CS.vEgo >= self.CP.minEnableSpeed:
|
||||
self.low_speed_alert = False
|
||||
if self.CP.minEnableSpeed >= 14.5 and CS.gearShifter != GearShifter.drive:
|
||||
self.low_speed_alert = True
|
||||
if self.low_speed_alert:
|
||||
events.add(EventName.belowSteerSpeed)
|
||||
|
||||
elif self.CP.brand == 'toyota':
|
||||
if self.CP.openpilotLongitudinalControl:
|
||||
if CS.cruiseState.standstill and not CS.brakePressed and self.CP_SP.enableGasInterceptor:
|
||||
if events.has(EventName.resumeRequired):
|
||||
events.remove(EventName.resumeRequired)
|
||||
|
||||
return events_sp
|
||||
@@ -0,0 +1,139 @@
|
||||
"""
|
||||
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.
|
||||
"""
|
||||
import numpy as np
|
||||
|
||||
from openpilot.cereal import custom
|
||||
from opendbc.car.structs import car
|
||||
from opendbc.car import structs
|
||||
from openpilot.common.constants import CV
|
||||
from openpilot.common.params import Params
|
||||
from openpilot.sunnypilot.selfdrive.car.intelligent_cruise_button_management.helpers import get_minimum_set_speed
|
||||
from openpilot.sunnypilot.selfdrive.controls.lib.speed_limit.speed_limit_assist import ACTIVE_STATES as SLA_ACTIVE_STATES
|
||||
from openpilot.sunnypilot.selfdrive.controls.lib.speed_limit.helpers import compare_cluster_target
|
||||
|
||||
ButtonType = car.CarState.ButtonEvent.Type
|
||||
SpeedLimitAssistState = custom.LongitudinalPlanSP.SpeedLimit.AssistState
|
||||
|
||||
CRUISE_BUTTON_TIMER = {ButtonType.decelCruise: 0, ButtonType.accelCruise: 0,
|
||||
ButtonType.setCruise: 0, ButtonType.resumeCruise: 0,
|
||||
ButtonType.cancel: 0, ButtonType.mainCruise: 0}
|
||||
|
||||
V_CRUISE_MIN = 8
|
||||
V_CRUISE_MAX = 145
|
||||
V_CRUISE_UNSET = 255
|
||||
|
||||
|
||||
def update_manual_button_timers(CS: car.CarState, button_timers: dict[car.CarState.ButtonEvent.Type, int]) -> None:
|
||||
# increment timer for buttons still pressed
|
||||
for k in button_timers:
|
||||
if button_timers[k] > 0:
|
||||
button_timers[k] += 1
|
||||
|
||||
for b in CS.buttonEvents:
|
||||
if b.type.raw in button_timers:
|
||||
# Start/end timer and store current state on change of button pressed
|
||||
button_timers[b.type.raw] = 1 if b.pressed else 0
|
||||
|
||||
|
||||
class VCruiseHelperSP:
|
||||
def __init__(self, CP: structs.CarParams, CP_SP: structs.CarParamsSP) -> None:
|
||||
self.CP = CP
|
||||
self.CP_SP = CP_SP
|
||||
self.v_cruise_kph = V_CRUISE_UNSET
|
||||
self.v_cruise_cluster_kph = V_CRUISE_UNSET
|
||||
self.params = Params()
|
||||
self.v_cruise_min = 0
|
||||
self.enabled_prev = False
|
||||
|
||||
self.custom_acc_enabled = self.params.get_bool("CustomAccIncrementsEnabled")
|
||||
self.short_increment = self.params.get("CustomAccShortPressIncrement", return_default=True)
|
||||
self.long_increment = self.params.get("CustomAccLongPressIncrement", return_default=True)
|
||||
|
||||
self.enable_button_timers = CRUISE_BUTTON_TIMER
|
||||
|
||||
# Speed Limit Assist
|
||||
self.sla_state = SpeedLimitAssistState.disabled
|
||||
self.prev_sla_state = SpeedLimitAssistState.disabled
|
||||
self.has_speed_limit = False
|
||||
self.speed_limit_final_last = 0.
|
||||
self.speed_limit_final_last_kph = 0.
|
||||
self.prev_speed_limit_final_last_kph = 0.
|
||||
self.req_plus = False
|
||||
self.req_minus = False
|
||||
|
||||
def read_custom_set_speed_params(self) -> None:
|
||||
self.custom_acc_enabled = self.params.get_bool("CustomAccIncrementsEnabled")
|
||||
self.short_increment = self.params.get("CustomAccShortPressIncrement", return_default=True)
|
||||
self.long_increment = self.params.get("CustomAccLongPressIncrement", return_default=True)
|
||||
|
||||
def update_v_cruise_delta(self, long_press: bool, v_cruise_delta: float) -> tuple[bool, float]:
|
||||
if not self.custom_acc_enabled:
|
||||
v_cruise_delta = v_cruise_delta * (5 if long_press else 1)
|
||||
return long_press, v_cruise_delta
|
||||
|
||||
# Apply user-specified multipliers to the base increment
|
||||
short_increment = np.clip(self.short_increment, 1, 10)
|
||||
long_increment = np.clip(self.long_increment, 1, 10)
|
||||
|
||||
actual_increment = long_increment if long_press else short_increment
|
||||
round_to_nearest = actual_increment in (5, 10)
|
||||
v_cruise_delta = v_cruise_delta * actual_increment
|
||||
|
||||
return round_to_nearest, v_cruise_delta
|
||||
|
||||
def get_minimum_set_speed(self, is_metric: bool) -> None:
|
||||
if self.CP_SP.pcmCruiseSpeed:
|
||||
self.v_cruise_min = V_CRUISE_MIN
|
||||
return
|
||||
|
||||
self.v_cruise_min = get_minimum_set_speed(is_metric)
|
||||
|
||||
def update_enabled_state(self, CS: car.CarState, enabled: bool) -> bool:
|
||||
# special enabled state for non pcmCruiseSpeed, unchanged for non pcmCruise
|
||||
if not self.CP_SP.pcmCruiseSpeed:
|
||||
update_manual_button_timers(CS, self.enable_button_timers)
|
||||
button_pressed = any(self.enable_button_timers[k] > 0 for k in self.enable_button_timers)
|
||||
|
||||
if enabled and not self.enabled_prev:
|
||||
self.enabled_prev = not button_pressed
|
||||
enabled = False
|
||||
elif not enabled:
|
||||
self.enabled_prev = enabled
|
||||
|
||||
return enabled and self.enabled_prev
|
||||
|
||||
return enabled
|
||||
|
||||
def update_speed_limit_assist(self, is_metric, LP_SP: custom.LongitudinalPlanSP) -> None:
|
||||
resolver = LP_SP.speedLimit.resolver
|
||||
self.has_speed_limit = resolver.speedLimitValid or resolver.speedLimitLastValid
|
||||
self.speed_limit_final_last = LP_SP.speedLimit.resolver.speedLimitFinalLast
|
||||
self.speed_limit_final_last_kph = self.speed_limit_final_last * CV.MS_TO_KPH
|
||||
self.sla_state = LP_SP.speedLimit.assist.state
|
||||
self.req_plus, self.req_minus = compare_cluster_target(self.v_cruise_cluster_kph * CV.KPH_TO_MS,
|
||||
self.speed_limit_final_last, is_metric)
|
||||
|
||||
@property
|
||||
def update_speed_limit_final_last_changed(self) -> bool:
|
||||
return self.has_speed_limit and bool(self.speed_limit_final_last_kph != self.prev_speed_limit_final_last_kph)
|
||||
|
||||
def update_speed_limit_assist_pre_active_confirmed(self, button_type: car.CarState.ButtonEvent.Type) -> bool:
|
||||
if self.sla_state == SpeedLimitAssistState.preActive or self.prev_sla_state == SpeedLimitAssistState.preActive:
|
||||
if button_type == ButtonType.decelCruise and self.req_minus:
|
||||
return True
|
||||
if button_type == ButtonType.accelCruise and self.req_plus:
|
||||
return True
|
||||
|
||||
return False
|
||||
|
||||
def update_speed_limit_assist_v_cruise_non_pcm(self) -> None:
|
||||
if self.sla_state in SLA_ACTIVE_STATES and (self.prev_sla_state not in SLA_ACTIVE_STATES or
|
||||
self.update_speed_limit_final_last_changed):
|
||||
self.v_cruise_kph = np.clip(round(self.speed_limit_final_last_kph, 1), self.v_cruise_min, V_CRUISE_MAX)
|
||||
|
||||
self.prev_sla_state = self.sla_state
|
||||
self.prev_speed_limit_final_last_kph = self.speed_limit_final_last_kph
|
||||
@@ -0,0 +1,51 @@
|
||||
"""
|
||||
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
|
||||
from openpilot.common.params import Params
|
||||
|
||||
ButtonType = car.CarState.ButtonEvent.Type
|
||||
EventNameSP = custom.OnroadEventSP.EventName
|
||||
|
||||
DISTANCE_LONG_PRESS = 50
|
||||
|
||||
|
||||
class CruiseHelper:
|
||||
def __init__(self, CP: structs.CarParams):
|
||||
self.CP = CP
|
||||
self.params = Params()
|
||||
|
||||
self.button_frame_counts = {ButtonType.gapAdjustCruise: 0}
|
||||
self._experimental_mode = False
|
||||
self.experimental_mode_switched = False
|
||||
|
||||
def update(self, CS, events, experimental_mode) -> None:
|
||||
if self.CP.openpilotLongitudinalControl:
|
||||
if CS.cruiseState.available:
|
||||
self.update_button_frame_counts(CS)
|
||||
|
||||
# toggle experimental mode once on distance button hold
|
||||
self.update_experimental_mode(events, experimental_mode)
|
||||
|
||||
def update_button_frame_counts(self, CS) -> None:
|
||||
for button in self.button_frame_counts:
|
||||
if self.button_frame_counts[button] > 0:
|
||||
self.button_frame_counts[button] += 1
|
||||
|
||||
for button_event in CS.buttonEvents:
|
||||
button = button_event.type.raw
|
||||
if button in self.button_frame_counts:
|
||||
self.button_frame_counts[button] = int(button_event.pressed)
|
||||
|
||||
def update_experimental_mode(self, events, experimental_mode) -> None:
|
||||
if self.button_frame_counts[ButtonType.gapAdjustCruise] >= DISTANCE_LONG_PRESS and not self.experimental_mode_switched:
|
||||
self._experimental_mode = not experimental_mode
|
||||
self.params.put_bool("ExperimentalMode", self._experimental_mode)
|
||||
events.add(EventNameSP.experimentalModeSwitched)
|
||||
self.experimental_mode_switched = True
|
||||
Whitespace-only changes.
+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
|
||||
@@ -0,0 +1,8 @@
|
||||
"""
|
||||
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.
|
||||
"""
|
||||
def get_minimum_set_speed(is_metric: bool) -> int:
|
||||
return 30 if is_metric else 20
|
||||
@@ -0,0 +1,134 @@
|
||||
"""
|
||||
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 typing import Any
|
||||
|
||||
from opendbc.car import structs
|
||||
from opendbc.car.interfaces import CarInterfaceBase
|
||||
from openpilot.common.params import Params
|
||||
from openpilot.common.swaglog import cloudlog
|
||||
from openpilot.sunnypilot.selfdrive.controls.lib.nnlc.helpers import get_nn_model_path
|
||||
from openpilot.sunnypilot.selfdrive.controls.lib.speed_limit.helpers import set_speed_limit_assist_availability
|
||||
|
||||
import openpilot.system.sentry as sentry
|
||||
|
||||
from openpilot.sunnypilot.sunnylink.statsd import STATSLOGSP
|
||||
|
||||
|
||||
def log_fingerprint(CP: structs.CarParams) -> None:
|
||||
if CP.carFingerprint == "MOCK":
|
||||
sentry.capture_fingerprint_mock()
|
||||
else:
|
||||
sentry.capture_fingerprint(CP.carFingerprint, CP.brand)
|
||||
|
||||
|
||||
def _enforce_torque_lateral_control(CP: structs.CarParams, params: Params = None, enabled: bool = False) -> bool:
|
||||
if params is None:
|
||||
params = Params()
|
||||
|
||||
if CP.steerControlType != structs.CarParams.SteerControlType.angle:
|
||||
enabled = params.get_bool("EnforceTorqueControl")
|
||||
|
||||
return enabled
|
||||
|
||||
|
||||
def _initialize_neural_network_lateral_control(CP: structs.CarParams, CP_SP: structs.CarParamsSP,
|
||||
params: Params = None, enabled: bool = False) -> bool:
|
||||
if params is None:
|
||||
params = Params()
|
||||
|
||||
nnlc_model_path, nnlc_model_name, exact_match = get_nn_model_path(CP)
|
||||
|
||||
if nnlc_model_name == "MOCK":
|
||||
cloudlog.error({"nnlc event": "car doesn't match any Neural Network model"})
|
||||
|
||||
if nnlc_model_name != "MOCK" and CP.steerControlType != structs.CarParams.SteerControlType.angle:
|
||||
enabled = params.get_bool("NeuralNetworkLateralControl")
|
||||
|
||||
CP_SP.neuralNetworkLateralControl.model.path = nnlc_model_path
|
||||
CP_SP.neuralNetworkLateralControl.model.name = nnlc_model_name
|
||||
CP_SP.neuralNetworkLateralControl.fuzzyFingerprint = not exact_match
|
||||
|
||||
return enabled
|
||||
|
||||
|
||||
def _initialize_intelligent_cruise_button_management(CP: structs.CarParams, CP_SP: structs.CarParamsSP, params: Params = None) -> None:
|
||||
if params is None:
|
||||
params = Params()
|
||||
|
||||
icbm_enabled = params.get_bool("IntelligentCruiseButtonManagement")
|
||||
if icbm_enabled and CP_SP.intelligentCruiseButtonManagementAvailable and not CP.openpilotLongitudinalControl:
|
||||
CP_SP.pcmCruiseSpeed = False
|
||||
|
||||
|
||||
def _initialize_torque_lateral_control(CI: CarInterfaceBase, CP: structs.CarParams, enforce_torque: bool, nnlc_enabled: bool) -> None:
|
||||
if nnlc_enabled or enforce_torque:
|
||||
CI.configure_torque_tune(CP.carFingerprint, CP.lateralTuning)
|
||||
|
||||
|
||||
def _cleanup_unsupported_params(CP: structs.CarParams, CP_SP: structs.CarParamsSP, params: Params = None) -> None:
|
||||
if params is None:
|
||||
params = Params()
|
||||
|
||||
if CP.steerControlType == structs.CarParams.SteerControlType.angle:
|
||||
cloudlog.warning("SteerControlType is angle, cleaning up params")
|
||||
params.remove("NeuralNetworkLateralControl")
|
||||
params.remove("EnforceTorqueControl")
|
||||
|
||||
if not CP_SP.intelligentCruiseButtonManagementAvailable or CP.openpilotLongitudinalControl:
|
||||
cloudlog.warning("ICBM not available or openpilot Longitudinal Control enabled, cleaning up params")
|
||||
params.remove("IntelligentCruiseButtonManagement")
|
||||
|
||||
if not CP.openpilotLongitudinalControl and CP_SP.pcmCruiseSpeed:
|
||||
cloudlog.warning("openpilot Longitudinal Control and ICBM not available, cleaning up params")
|
||||
params.remove("DynamicExperimentalControl")
|
||||
params.remove("CustomAccIncrementsEnabled")
|
||||
params.remove("SmartCruiseControlVision")
|
||||
params.remove("SmartCruiseControlMap")
|
||||
|
||||
set_speed_limit_assist_availability(CP, CP_SP, params)
|
||||
|
||||
|
||||
def setup_interfaces(CI: CarInterfaceBase, params: Params = None) -> None:
|
||||
enforce_torque = _enforce_torque_lateral_control(CI.CP, params)
|
||||
nnlc_enabled = _initialize_neural_network_lateral_control(CI.CP, CI.CP_SP, params)
|
||||
_initialize_intelligent_cruise_button_management(CI.CP, CI.CP_SP, params)
|
||||
_initialize_torque_lateral_control(CI, CI.CP, enforce_torque, nnlc_enabled)
|
||||
_cleanup_unsupported_params(CI.CP, CI.CP_SP)
|
||||
|
||||
try:
|
||||
STATSLOGSP.raw('sunnypilot.car_params', CI.CP.to_dict())
|
||||
except RuntimeError:
|
||||
pass # to_dict fails on macOS due to library issues.
|
||||
# STATSLOGSP.raw('sunnypilot_params.car_params_sp', CP_SP.to_dict()) # https://github.com/sunnypilot/opendbc/pull/361
|
||||
|
||||
|
||||
def initialize_params(params) -> list[dict[str, Any]]:
|
||||
keys: list = []
|
||||
|
||||
# hyundai
|
||||
keys.extend([
|
||||
"HyundaiLongitudinalTuning",
|
||||
])
|
||||
|
||||
# subaru
|
||||
keys.extend([
|
||||
"SubaruStopAndGo",
|
||||
"SubaruStopAndGoManualParkingBrake",
|
||||
])
|
||||
|
||||
# tesla
|
||||
keys.extend([
|
||||
"TeslaCoopSteering",
|
||||
])
|
||||
|
||||
# toyota
|
||||
keys.extend([
|
||||
"ToyotaEnforceStockLongitudinal",
|
||||
"ToyotaStopAndGoHack",
|
||||
])
|
||||
|
||||
return [{k: params.get(k, return_default=True)} for k in keys]
|
||||
@@ -0,0 +1,31 @@
|
||||
#!/usr/bin/env python3
|
||||
"""
|
||||
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.
|
||||
"""
|
||||
import json
|
||||
import os
|
||||
|
||||
from openpilot.common.basedir import BASEDIR
|
||||
from openpilot.common.params import Params
|
||||
from openpilot.common.swaglog import cloudlog
|
||||
|
||||
CAR_LIST_JSON_OUT = os.path.join(BASEDIR, "openpilot", "sunnypilot", "selfdrive", "car", "car_list.json")
|
||||
|
||||
|
||||
def update_car_list_param():
|
||||
with open(CAR_LIST_JSON_OUT) as f:
|
||||
current_car_list = json.load(f)
|
||||
|
||||
params = Params()
|
||||
if params.get("CarList") != current_car_list:
|
||||
params.put("CarList", current_car_list, block=True)
|
||||
cloudlog.warning("Updated CarList param with latest platform list")
|
||||
else:
|
||||
cloudlog.warning("CarList param is up to date, no need to update")
|
||||
|
||||
|
||||
if __name__ == "__main__":
|
||||
update_car_list_param()
|
||||
Whitespace-only changes.
@@ -0,0 +1,66 @@
|
||||
from opendbc.car.structs import car
|
||||
from openpilot.common.parameterized import parameterized_class
|
||||
from openpilot.selfdrive.selfdrived.events import Events
|
||||
from openpilot.sunnypilot.selfdrive.car.cruise_helpers import CruiseHelper, DISTANCE_LONG_PRESS
|
||||
|
||||
ButtonEvent = car.CarState.ButtonEvent
|
||||
ButtonType = car.CarState.ButtonEvent.Type
|
||||
|
||||
|
||||
@parameterized_class(('openpilot_longitudinal',), [(True,)])
|
||||
class TestCruiseHelper:
|
||||
def setup_method(self):
|
||||
self.CP = car.CarParams(openpilotLongitudinalControl=self.openpilot_longitudinal)
|
||||
self.cruise_helper = CruiseHelper(self.CP)
|
||||
self.cruise_helper.experimental_mode_switched = False
|
||||
self.events = Events()
|
||||
|
||||
def reset(self):
|
||||
for _ in range(2):
|
||||
CS = car.CarState(cruiseState={"available": False})
|
||||
CS.buttonEvents = [ButtonEvent(type=ButtonType.gapAdjustCruise, pressed=False)]
|
||||
self.cruise_helper._experimental_mode = False
|
||||
self.cruise_helper.experimental_mode_switched = False
|
||||
self.cruise_helper.update(CS, self.events, False)
|
||||
|
||||
|
||||
def test_gap_adjust_cruise_long_press_toggle_mode(self) -> None:
|
||||
for pressed in (True, False):
|
||||
for experimental_mode in (True, False):
|
||||
self.reset()
|
||||
self.cruise_helper._experimental_mode = experimental_mode
|
||||
toggled_mode = not experimental_mode if pressed else experimental_mode
|
||||
|
||||
for i in range(DISTANCE_LONG_PRESS):
|
||||
CS = car.CarState(cruiseState={"available": True})
|
||||
CS.buttonEvents = [ButtonEvent(type=ButtonType.gapAdjustCruise, pressed=pressed)] if i == 0 else []
|
||||
self.cruise_helper.update(CS, self.events, experimental_mode)
|
||||
|
||||
# mode should be toggled
|
||||
assert self.cruise_helper._experimental_mode == toggled_mode
|
||||
assert self.cruise_helper.experimental_mode_switched is pressed
|
||||
|
||||
# keep holding button after switching mode
|
||||
for _ in range(DISTANCE_LONG_PRESS):
|
||||
CS = car.CarState(cruiseState={"available": True})
|
||||
CS.buttonEvents = [ButtonEvent(type=ButtonType.gapAdjustCruise, pressed=pressed)]
|
||||
self.cruise_helper.update(CS, self.events, toggled_mode)
|
||||
|
||||
# mode should not be toggled
|
||||
assert self.cruise_helper._experimental_mode == toggled_mode
|
||||
assert self.cruise_helper.experimental_mode_switched is pressed
|
||||
|
||||
def test_gap_adjust_cruise_short_press_toggle_mode(self) -> None:
|
||||
for pressed in (True, False):
|
||||
for experimental_mode in (True, False):
|
||||
self.reset()
|
||||
self.cruise_helper._experimental_mode = experimental_mode
|
||||
|
||||
for i in range(DISTANCE_LONG_PRESS - 1):
|
||||
CS = car.CarState(cruiseState={"available": True})
|
||||
CS.buttonEvents = [ButtonEvent(type=ButtonType.gapAdjustCruise, pressed=pressed)] if i == 0 else []
|
||||
self.cruise_helper.update(CS, self.events, experimental_mode)
|
||||
|
||||
# mode should not be toggled
|
||||
assert self.cruise_helper._experimental_mode == experimental_mode
|
||||
assert self.cruise_helper.experimental_mode_switched is False
|
||||
@@ -0,0 +1,150 @@
|
||||
import pytest
|
||||
|
||||
from opendbc.car.structs import car
|
||||
from openpilot.common.constants import CV
|
||||
from openpilot.common.parameterized import parameterized_class
|
||||
from openpilot.common.params import Params
|
||||
from openpilot.selfdrive.car.cruise import V_CRUISE_INITIAL
|
||||
from openpilot.selfdrive.car.tests.test_cruise_speed import TestVCruiseHelper
|
||||
|
||||
ButtonEvent = car.CarState.ButtonEvent
|
||||
ButtonType = car.CarState.ButtonEvent.Type
|
||||
|
||||
|
||||
# TODO: test pcmCruise and pcmCruiseSpeed
|
||||
@parameterized_class(('pcm_cruise', 'pcm_cruise_speed'), [(False, True)])
|
||||
class TestCustomAccIncrements(TestVCruiseHelper):
|
||||
def setup_method(self):
|
||||
TestVCruiseHelper.setup_method(self)
|
||||
self.params = Params()
|
||||
self.reset_custom_params()
|
||||
|
||||
def reset_custom_params(self) -> None:
|
||||
"""Reset to default custom ACC parameters"""
|
||||
self.params.put_bool("CustomAccIncrementsEnabled", False, block=True)
|
||||
self.params.put("CustomAccShortPressIncrement", 1, block=True)
|
||||
self.params.put("CustomAccLongPressIncrement", 5, block=True)
|
||||
self.v_cruise_helper.read_custom_set_speed_params()
|
||||
|
||||
def press_button_short(self, button_type: car.CarState.ButtonEvent.Type) -> None:
|
||||
"""Simulate a short button press (press + release)"""
|
||||
CS = car.CarState(cruiseState={"available": True})
|
||||
CS.buttonEvents = [ButtonEvent(type=button_type, pressed=True)]
|
||||
self.v_cruise_helper.update_v_cruise(CS, enabled=True, is_metric=True)
|
||||
|
||||
CS.buttonEvents = [ButtonEvent(type=button_type, pressed=False)]
|
||||
self.v_cruise_helper.update_v_cruise(CS, enabled=True, is_metric=True)
|
||||
|
||||
def press_button_long(self, button_type: car.CarState.ButtonEvent.Type) -> None:
|
||||
"""Simulate a long button press (50+ frames)"""
|
||||
CS = car.CarState(cruiseState={"available": True})
|
||||
CS.buttonEvents = [ButtonEvent(type=button_type, pressed=True)]
|
||||
self.v_cruise_helper.update_v_cruise(CS, enabled=True, is_metric=True)
|
||||
|
||||
# Hold for 50 frames to trigger long press
|
||||
CS.buttonEvents = []
|
||||
for _ in range(50):
|
||||
self.v_cruise_helper.update_v_cruise(CS, enabled=True, is_metric=True)
|
||||
|
||||
CS.buttonEvents = [ButtonEvent(type=button_type, pressed=False)]
|
||||
self.v_cruise_helper.update_v_cruise(CS, enabled=True, is_metric=True)
|
||||
|
||||
def set_custom_increments(self, enabled: bool, short_inc: int, long_inc: int) -> None:
|
||||
"""Set custom ACC increment parameters"""
|
||||
self.params.put_bool("CustomAccIncrementsEnabled", enabled, block=True)
|
||||
self.params.put("CustomAccShortPressIncrement", short_inc, block=True)
|
||||
self.params.put("CustomAccLongPressIncrement", long_inc, block=True)
|
||||
self.v_cruise_helper.read_custom_set_speed_params()
|
||||
|
||||
def test_default_behavior_when_disabled(self):
|
||||
"""Test that default increments are used when custom ACC is disabled"""
|
||||
self.set_custom_increments(enabled=False, short_inc=5, long_inc=10)
|
||||
self.enable(V_CRUISE_INITIAL * CV.KPH_TO_MS, False, False)
|
||||
|
||||
initial_speed = self.v_cruise_helper.v_cruise_kph
|
||||
|
||||
# Short press should increment by 1 (default)
|
||||
self.press_button_short(ButtonType.accelCruise)
|
||||
assert self.v_cruise_helper.v_cruise_kph == initial_speed + 1
|
||||
|
||||
@pytest.mark.parametrize("increment", (1, 2, 3, 4, 5, 6, 7, 8, 9, 10))
|
||||
def test_custom_short_press_increments(self, increment):
|
||||
"""Test custom short press increments (1-10)"""
|
||||
self.set_custom_increments(enabled=True, short_inc=increment, long_inc=5)
|
||||
self.enable(50 * CV.KPH_TO_MS, False, False)
|
||||
|
||||
initial_speed = self.v_cruise_helper.v_cruise_kph
|
||||
self.press_button_short(ButtonType.accelCruise)
|
||||
|
||||
if increment in (5, 10):
|
||||
# Should round to nearest increment
|
||||
expected_speed = ((initial_speed // increment) + 1) * increment
|
||||
else:
|
||||
expected_speed = initial_speed + increment
|
||||
|
||||
assert self.v_cruise_helper.v_cruise_kph == expected_speed
|
||||
|
||||
@pytest.mark.parametrize("increment", (1, 5, 10))
|
||||
def test_custom_long_press_increments(self, increment):
|
||||
"""Test custom long press increments (1, 5, 10)"""
|
||||
self.set_custom_increments(enabled=True, short_inc=1, long_inc=increment)
|
||||
self.enable(50 * CV.KPH_TO_MS, False, False)
|
||||
|
||||
initial_speed = self.v_cruise_helper.v_cruise_kph
|
||||
self.press_button_long(ButtonType.accelCruise)
|
||||
|
||||
if increment in (5, 10):
|
||||
# Should round to nearest increment
|
||||
expected_speed = ((initial_speed // increment) + 1) * increment
|
||||
else:
|
||||
expected_speed = initial_speed + increment
|
||||
|
||||
assert self.v_cruise_helper.v_cruise_kph == expected_speed
|
||||
|
||||
@pytest.mark.parametrize("button_type", [ButtonType.accelCruise, ButtonType.decelCruise])
|
||||
def test_accel_decel_symmetry(self, button_type):
|
||||
"""Test that acceleration and deceleration work symmetrically"""
|
||||
self.set_custom_increments(enabled=True, short_inc=3, long_inc=5)
|
||||
self.enable(50 * CV.KPH_TO_MS, False, False)
|
||||
|
||||
initial_speed = self.v_cruise_helper.v_cruise_kph
|
||||
self.press_button_short(button_type)
|
||||
|
||||
expected_change = 3 if button_type == ButtonType.accelCruise else -3
|
||||
assert self.v_cruise_helper.v_cruise_kph == initial_speed + expected_change
|
||||
|
||||
def test_rounding_behavior(self):
|
||||
"""Test rounding behavior for 5 and 10 increments"""
|
||||
test_cases = [
|
||||
(47, 5, 50), # 47 -> 50 (round up to next 5)
|
||||
(45, 5, 50), # 45 -> 50 (already at 5, increment by 5)
|
||||
(43, 10, 50), # 43 -> 50 (round up to next 10)
|
||||
(40, 10, 50), # 40 -> 50 (already at 10, increment by 10)
|
||||
]
|
||||
|
||||
for initial, increment, expected in test_cases:
|
||||
self.set_custom_increments(enabled=True, short_inc=increment, long_inc=increment)
|
||||
self.reset_cruise_speed_state()
|
||||
self.enable(initial * CV.KPH_TO_MS, False, False)
|
||||
|
||||
self.press_button_short(ButtonType.accelCruise)
|
||||
assert self.v_cruise_helper.v_cruise_kph == expected
|
||||
|
||||
def test_invalid_values_fallback(self):
|
||||
"""Test that invalid values fallback to safe defaults"""
|
||||
# Test invalid short increment
|
||||
self.set_custom_increments(enabled=True, short_inc=-1, long_inc=5)
|
||||
self.enable(50 * CV.KPH_TO_MS, False, False)
|
||||
|
||||
initial_speed = self.v_cruise_helper.v_cruise_kph
|
||||
self.press_button_short(ButtonType.accelCruise)
|
||||
assert self.v_cruise_helper.v_cruise_kph == initial_speed + 1 # Should fallback to 1
|
||||
|
||||
# Test invalid long increment
|
||||
self.reset_cruise_speed_state()
|
||||
self.set_custom_increments(enabled=True, short_inc=1, long_inc=99)
|
||||
self.enable(50 * CV.KPH_TO_MS, False, False)
|
||||
|
||||
initial_speed = self.v_cruise_helper.v_cruise_kph
|
||||
self.press_button_long(ButtonType.accelCruise)
|
||||
assert self.v_cruise_helper.v_cruise_kph == initial_speed + 10 # Should fallback to 10
|
||||
Reference in new issue
Block a user