mirror of
https://github.com/firestar5683/StarPilot.git
synced 2026-08-21 08:14:00 +08:00
Conditional Chill
This commit is contained in:
@@ -6,6 +6,9 @@ from openpilot.common.params import Params
|
||||
PERSIST_EXPERIMENTAL_STATE_PARAM = "PersistExperimentalState"
|
||||
PERSISTED_CE_STATUS_PARAM = "PersistedCEStatus"
|
||||
CE_STATUS_PARAM = "CEStatus"
|
||||
PERSIST_CHILL_STATE_PARAM = "PersistChillState"
|
||||
PERSISTED_CC_STATUS_PARAM = "PersistedCCStatus"
|
||||
CC_STATUS_PARAM = "CCStatus"
|
||||
|
||||
CEStatus = {
|
||||
"OFF": 0,
|
||||
@@ -19,35 +22,71 @@ CEStatus = {
|
||||
"STOP_LIGHT": 8,
|
||||
}
|
||||
|
||||
CCStatus = {
|
||||
"OFF": 0,
|
||||
"USER_EXPERIMENTAL": 1,
|
||||
"USER_CHILL": 2,
|
||||
"LEAD": 4,
|
||||
"SPEED": 6,
|
||||
}
|
||||
|
||||
MANUAL_CE_STATUSES = {
|
||||
CEStatus["USER_DISABLED"],
|
||||
CEStatus["USER_OVERRIDDEN"],
|
||||
}
|
||||
|
||||
MANUAL_CC_STATUSES = {
|
||||
CCStatus["USER_EXPERIMENTAL"],
|
||||
CCStatus["USER_CHILL"],
|
||||
}
|
||||
|
||||
|
||||
def is_manual_ce_status(status: int) -> bool:
|
||||
return int(status) in MANUAL_CE_STATUSES
|
||||
|
||||
|
||||
def is_manual_cc_status(status: int) -> bool:
|
||||
return int(status) in MANUAL_CC_STATUSES
|
||||
|
||||
|
||||
def normalize_persisted_ce_status(status: int) -> int:
|
||||
status = int(status)
|
||||
return status if status in MANUAL_CE_STATUSES else CEStatus["OFF"]
|
||||
|
||||
|
||||
def normalize_persisted_cc_status(status: int) -> int:
|
||||
status = int(status)
|
||||
return status if status in MANUAL_CC_STATUSES else CCStatus["OFF"]
|
||||
|
||||
|
||||
def get_persisted_ce_status(params: Params) -> int:
|
||||
return normalize_persisted_ce_status(params.get_int(PERSISTED_CE_STATUS_PARAM, default=CEStatus["OFF"]))
|
||||
|
||||
|
||||
def get_persisted_cc_status(params: Params) -> int:
|
||||
return normalize_persisted_cc_status(params.get_int(PERSISTED_CC_STATUS_PARAM, default=CCStatus["OFF"]))
|
||||
|
||||
|
||||
def set_persisted_ce_status(params: Params, status: int) -> int:
|
||||
normalized = normalize_persisted_ce_status(status)
|
||||
params.put_int(PERSISTED_CE_STATUS_PARAM, normalized)
|
||||
return normalized
|
||||
|
||||
|
||||
def set_persisted_cc_status(params: Params, status: int) -> int:
|
||||
normalized = normalize_persisted_cc_status(status)
|
||||
params.put_int(PERSISTED_CC_STATUS_PARAM, normalized)
|
||||
return normalized
|
||||
|
||||
|
||||
def clear_persisted_ce_status(params: Params) -> None:
|
||||
params.put_int(PERSISTED_CE_STATUS_PARAM, CEStatus["OFF"])
|
||||
|
||||
|
||||
def clear_persisted_cc_status(params: Params) -> None:
|
||||
params.put_int(PERSISTED_CC_STATUS_PARAM, CCStatus["OFF"])
|
||||
|
||||
|
||||
def sync_persist_experimental_state(params: Params, params_memory: Params | None, enabled: bool) -> None:
|
||||
params.put_bool(PERSIST_EXPERIMENTAL_STATE_PARAM, enabled)
|
||||
if enabled:
|
||||
@@ -57,21 +96,45 @@ def sync_persist_experimental_state(params: Params, params_memory: Params | None
|
||||
clear_persisted_ce_status(params)
|
||||
|
||||
|
||||
def sync_persist_chill_state(params: Params, params_memory: Params | None, enabled: bool) -> None:
|
||||
params.put_bool(PERSIST_CHILL_STATE_PARAM, enabled)
|
||||
if enabled:
|
||||
current_status = params_memory.get_int(CC_STATUS_PARAM, default=CCStatus["OFF"]) if params_memory is not None else CCStatus["OFF"]
|
||||
set_persisted_cc_status(params, current_status)
|
||||
else:
|
||||
clear_persisted_cc_status(params)
|
||||
|
||||
|
||||
def sync_manual_ce_state(params: Params, status: int) -> int:
|
||||
return set_persisted_ce_status(params, status) if params.get_bool(PERSIST_EXPERIMENTAL_STATE_PARAM) else clear_and_return_off(params)
|
||||
|
||||
|
||||
def sync_manual_cc_state(params: Params, status: int) -> int:
|
||||
return set_persisted_cc_status(params, status) if params.get_bool(PERSIST_CHILL_STATE_PARAM) else clear_cc_and_return_off(params)
|
||||
|
||||
|
||||
def clear_and_return_off(params: Params) -> int:
|
||||
clear_persisted_ce_status(params)
|
||||
return CEStatus["OFF"]
|
||||
|
||||
|
||||
def clear_cc_and_return_off(params: Params) -> int:
|
||||
clear_persisted_cc_status(params)
|
||||
return CCStatus["OFF"]
|
||||
|
||||
|
||||
def next_manual_ce_status(current_status: int, experimental_mode: bool) -> int:
|
||||
if is_manual_ce_status(current_status):
|
||||
return CEStatus["OFF"]
|
||||
return CEStatus["USER_DISABLED"] if experimental_mode else CEStatus["USER_OVERRIDDEN"]
|
||||
|
||||
|
||||
def next_manual_cc_status(current_status: int, experimental_mode: bool) -> int:
|
||||
if is_manual_cc_status(current_status):
|
||||
return CCStatus["OFF"]
|
||||
return CCStatus["USER_CHILL"] if experimental_mode else CCStatus["USER_EXPERIMENTAL"]
|
||||
|
||||
|
||||
def requested_experimental_mode(params: Params, params_memory: Params | None = None) -> bool:
|
||||
if params.get_bool("SafeMode"):
|
||||
return False
|
||||
@@ -82,6 +145,14 @@ def requested_experimental_mode(params: Params, params_memory: Params | None = N
|
||||
status = get_persisted_ce_status(params)
|
||||
return status == CEStatus["USER_OVERRIDDEN"]
|
||||
|
||||
if params.get_bool("ConditionalChill"):
|
||||
status = params_memory.get_int(CC_STATUS_PARAM, default=CCStatus["OFF"]) if params_memory is not None else CCStatus["OFF"]
|
||||
if not is_manual_cc_status(status):
|
||||
status = get_persisted_cc_status(params)
|
||||
if not is_manual_cc_status(status):
|
||||
return True
|
||||
return status == CCStatus["USER_EXPERIMENTAL"]
|
||||
|
||||
return params.get_bool("ExperimentalMode")
|
||||
|
||||
|
||||
@@ -100,3 +171,20 @@ def restore_persisted_ce_state(params: Params, params_memory: Params) -> int:
|
||||
return restored_status
|
||||
|
||||
return current_status
|
||||
|
||||
|
||||
def restore_persisted_cc_state(params: Params, params_memory: Params) -> int:
|
||||
current_status = params_memory.get_int(CC_STATUS_PARAM, default=CCStatus["OFF"])
|
||||
if is_manual_cc_status(current_status):
|
||||
sync_manual_cc_state(params, current_status)
|
||||
return current_status
|
||||
|
||||
if not params.get_bool(PERSIST_CHILL_STATE_PARAM):
|
||||
return current_status
|
||||
|
||||
restored_status = get_persisted_cc_status(params)
|
||||
if restored_status != CCStatus["OFF"]:
|
||||
params_memory.put_int(CC_STATUS_PARAM, restored_status)
|
||||
return restored_status
|
||||
|
||||
return current_status
|
||||
|
||||
@@ -104,6 +104,7 @@ SAFE_MODE_MANAGED_KEYS = (
|
||||
"ReduceLateralAccelerationRainStorm",
|
||||
"ReduceLateralAccelerationSnow",
|
||||
"ConditionalExperimental",
|
||||
"ConditionalChill",
|
||||
"CECurves",
|
||||
"CECurvesLead",
|
||||
"CELead",
|
||||
@@ -111,10 +112,16 @@ SAFE_MODE_MANAGED_KEYS = (
|
||||
"CEStoppedLead",
|
||||
"CESpeed",
|
||||
"CESpeedLead",
|
||||
"CCMLead",
|
||||
"CCMSetSpeedMargin",
|
||||
"CCMSpeed",
|
||||
"CCMSpeedLead",
|
||||
"CEModelStopTime",
|
||||
"CEStopLights",
|
||||
"CESignalSpeed",
|
||||
"CESignalLaneDetection",
|
||||
"PersistChillState",
|
||||
"ShowCCMStatus",
|
||||
"CurveSpeedController",
|
||||
"SpeedLimitController",
|
||||
"SetSpeedLimit",
|
||||
@@ -199,6 +206,7 @@ SAFE_MODE_STOCK_PARAM_MAP = {
|
||||
}
|
||||
|
||||
SAFE_MODE_MEMORY_VALUES = {
|
||||
"CCStatus": 0,
|
||||
"CEStatus": 0,
|
||||
}
|
||||
|
||||
|
||||
@@ -229,6 +229,7 @@ EXCLUDED_KEYS = {
|
||||
"openpilotMinutes",
|
||||
"OverpassRequests",
|
||||
"PandaSignatures",
|
||||
"PersistedCCStatus",
|
||||
"PersistedCEStatus",
|
||||
"SpeedLimits",
|
||||
"SpeedLimitsFiltered",
|
||||
@@ -707,6 +708,7 @@ class StarPilotVariables:
|
||||
toggle.cluster_offset = self.get_value("ClusterOffset", cast=float, condition=toggle.car_make == "toyota")
|
||||
|
||||
toggle.conditional_experimental_mode = toggle.openpilot_longitudinal and self.get_value("ConditionalExperimental")
|
||||
toggle.conditional_chill_mode = toggle.openpilot_longitudinal and not toggle.conditional_experimental_mode and self.get_value("ConditionalChill")
|
||||
toggle.conditional_curves = self.get_value("CECurves", condition=toggle.conditional_experimental_mode)
|
||||
toggle.conditional_curves_lead = self.get_value("CECurvesLead", condition=toggle.conditional_curves)
|
||||
toggle.conditional_lead = self.get_value("CELead", condition=toggle.conditional_experimental_mode)
|
||||
@@ -717,7 +719,15 @@ class StarPilotVariables:
|
||||
toggle.conditional_model_stop_time = self.get_value("CEModelStopTime", cast=float, condition=toggle.conditional_experimental_mode and self.get_value("CEStopLights"))
|
||||
toggle.conditional_signal = self.get_value("CESignalSpeed", cast=float, condition=toggle.conditional_experimental_mode, conversion=speed_conversion)
|
||||
toggle.conditional_signal_lane_detection = self.get_value("CESignalLaneDetection", condition=toggle.conditional_signal != 0)
|
||||
toggle.cem_status = self.get_value("ShowCEMStatus", condition=toggle.conditional_experimental_mode) or toggle.debug_mode
|
||||
toggle.conditional_chill_speed = self.get_value("CCMSpeed", cast=float, condition=toggle.conditional_chill_mode, conversion=speed_conversion)
|
||||
toggle.conditional_chill_speed_lead = self.get_value("CCMSpeedLead", cast=float, condition=toggle.conditional_chill_mode, conversion=speed_conversion)
|
||||
toggle.conditional_chill_speed_margin = self.get_value("CCMSetSpeedMargin", cast=float, condition=toggle.conditional_chill_mode, conversion=speed_conversion)
|
||||
toggle.conditional_chill_lead = self.get_value("CCMLead", condition=toggle.conditional_chill_mode)
|
||||
toggle.cem_status = (
|
||||
self.get_value("ShowCEMStatus", condition=toggle.conditional_experimental_mode) or
|
||||
self.get_value("ShowCCMStatus", condition=toggle.conditional_chill_mode) or
|
||||
toggle.debug_mode
|
||||
)
|
||||
|
||||
toggle.curve_speed_controller = toggle.openpilot_longitudinal and self.get_value("CurveSpeedController")
|
||||
toggle.csc_status = self.get_value("ShowCSCStatus", condition=toggle.curve_speed_controller) or toggle.debug_mode
|
||||
|
||||
@@ -0,0 +1,79 @@
|
||||
from openpilot.starpilot.common.experimental_state import (
|
||||
CC_STATUS_PARAM,
|
||||
CCStatus,
|
||||
CE_STATUS_PARAM,
|
||||
CEStatus,
|
||||
requested_experimental_mode,
|
||||
restore_persisted_cc_state,
|
||||
)
|
||||
|
||||
|
||||
class FakeParams:
|
||||
def __init__(self, bools=None, ints=None):
|
||||
self.bools = dict(bools or {})
|
||||
self.ints = dict(ints or {})
|
||||
|
||||
def get_bool(self, key):
|
||||
return bool(self.bools.get(key, False))
|
||||
|
||||
def put_bool(self, key, value):
|
||||
self.bools[key] = bool(value)
|
||||
|
||||
def get_int(self, key, default=0):
|
||||
return int(self.ints.get(key, default))
|
||||
|
||||
def put_int(self, key, value):
|
||||
self.ints[key] = int(value)
|
||||
|
||||
|
||||
def test_requested_experimental_mode_defaults_to_experimental_in_ccm_auto():
|
||||
params = FakeParams(bools={"ConditionalChill": True})
|
||||
params_memory = FakeParams(ints={CC_STATUS_PARAM: CCStatus["OFF"]})
|
||||
|
||||
assert requested_experimental_mode(params, params_memory) is True
|
||||
|
||||
|
||||
def test_requested_experimental_mode_respects_ccm_manual_override():
|
||||
params = FakeParams(bools={"ConditionalChill": True})
|
||||
|
||||
params_memory = FakeParams(ints={CC_STATUS_PARAM: CCStatus["USER_CHILL"]})
|
||||
assert requested_experimental_mode(params, params_memory) is False
|
||||
|
||||
params_memory = FakeParams(ints={CC_STATUS_PARAM: CCStatus["USER_EXPERIMENTAL"]})
|
||||
assert requested_experimental_mode(params, params_memory) is True
|
||||
|
||||
|
||||
def test_requested_experimental_mode_prefers_cem_if_both_conditional_modes_are_enabled():
|
||||
params = FakeParams(bools={"ConditionalExperimental": True, "ConditionalChill": True})
|
||||
|
||||
params_memory = FakeParams(ints={
|
||||
CE_STATUS_PARAM: CEStatus["OFF"],
|
||||
CC_STATUS_PARAM: CCStatus["USER_EXPERIMENTAL"],
|
||||
})
|
||||
assert requested_experimental_mode(params, params_memory) is False
|
||||
|
||||
params_memory = FakeParams(ints={
|
||||
CE_STATUS_PARAM: CEStatus["USER_OVERRIDDEN"],
|
||||
CC_STATUS_PARAM: CCStatus["USER_CHILL"],
|
||||
})
|
||||
assert requested_experimental_mode(params, params_memory) is True
|
||||
|
||||
|
||||
def test_requested_experimental_mode_safe_mode_overrides_ccm():
|
||||
params = FakeParams(bools={"SafeMode": True, "ConditionalChill": True})
|
||||
params_memory = FakeParams(ints={CC_STATUS_PARAM: CCStatus["USER_EXPERIMENTAL"]})
|
||||
|
||||
assert requested_experimental_mode(params, params_memory) is False
|
||||
|
||||
|
||||
def test_restore_persisted_cc_state_rehydrates_manual_override():
|
||||
params = FakeParams(
|
||||
bools={"PersistChillState": True},
|
||||
ints={"PersistedCCStatus": CCStatus["USER_CHILL"]},
|
||||
)
|
||||
params_memory = FakeParams(ints={CC_STATUS_PARAM: CCStatus["OFF"]})
|
||||
|
||||
restored_status = restore_persisted_cc_state(params, params_memory)
|
||||
|
||||
assert restored_status == CCStatus["USER_CHILL"]
|
||||
assert params_memory.get_int(CC_STATUS_PARAM) == CCStatus["USER_CHILL"]
|
||||
@@ -0,0 +1,214 @@
|
||||
#!/usr/bin/env python3
|
||||
import time
|
||||
|
||||
from openpilot.common.constants import CV
|
||||
|
||||
from openpilot.starpilot.common.experimental_state import (
|
||||
CCStatus,
|
||||
is_manual_cc_status,
|
||||
restore_persisted_cc_state,
|
||||
)
|
||||
|
||||
|
||||
class ConditionalChillMode:
|
||||
CCM_STOP_MODEL_TIME = 7.0
|
||||
CHILL_ENTRY_CONFIRM_TIME = 0.35
|
||||
CHILL_EXIT_BUFFER_TIME = 0.35
|
||||
CHILL_MIN_DWELL_TIME = 1.2
|
||||
|
||||
STABLE_LEAD_MIN_MODEL_PROB = 0.9
|
||||
STABLE_LEAD_MAX_BRAKE = 0.2
|
||||
STABLE_LEAD_MIN_SPEED = 1.5
|
||||
STABLE_LEAD_MAX_DISTANCE = 90.0
|
||||
STABLE_LEAD_MAX_DISTANCE_TIME = 4.5
|
||||
STABLE_LEAD_MAX_CLOSING_SPEED = 0.75
|
||||
STABLE_LEAD_MAX_CLOSING_RATIO = 0.03
|
||||
|
||||
ADJACENT_LEAD_VETO_MIN_SPEED = 1.0
|
||||
ADJACENT_LEAD_VETO_MAX_DISTANCE = 65.0
|
||||
ADJACENT_LEAD_VETO_MAX_DISTANCE_TIME = 3.5
|
||||
|
||||
LOW_SPEED_STOP_SCENE_MAX_SPEED = 18 * CV.MPH_TO_MS
|
||||
|
||||
def __init__(self, StarPilotPlanner, detector):
|
||||
self.starpilot_planner = StarPilotPlanner
|
||||
self.detector = detector
|
||||
self.params = self.starpilot_planner.params
|
||||
self.params_memory = self.starpilot_planner.params_memory
|
||||
|
||||
self.experimental_mode = True
|
||||
self.status_value = CCStatus["OFF"]
|
||||
self._active_auto_status = CCStatus["OFF"]
|
||||
self._candidate_since = 0.0
|
||||
self._soft_exit_since = 0.0
|
||||
self._chill_hold_until = 0.0
|
||||
self._prev_cc_status = None
|
||||
|
||||
def update(self, v_ego, v_cruise, sm, starpilot_toggles):
|
||||
now = time.monotonic()
|
||||
safe_mode = self.params.get_bool("SafeMode")
|
||||
|
||||
self.status_value = CCStatus["OFF"] if safe_mode else restore_persisted_cc_state(self.params, self.params_memory)
|
||||
|
||||
if is_manual_cc_status(self.status_value):
|
||||
self._reset_timers()
|
||||
self.experimental_mode = self.status_value == CCStatus["USER_EXPERIMENTAL"]
|
||||
self._write_status(self.status_value)
|
||||
return
|
||||
|
||||
self._refresh_detector(v_ego, sm)
|
||||
|
||||
if safe_mode or self._has_hard_veto(v_ego, sm):
|
||||
self._reset_timers()
|
||||
self.experimental_mode = False if safe_mode else True
|
||||
self.status_value = CCStatus["OFF"]
|
||||
self._write_status(CCStatus["OFF"])
|
||||
return
|
||||
|
||||
auto_status = self._get_chill_status(v_ego, v_cruise, sm, starpilot_toggles)
|
||||
chill_candidate = auto_status != CCStatus["OFF"]
|
||||
|
||||
if chill_candidate:
|
||||
if self._candidate_since == 0.0:
|
||||
self._candidate_since = now
|
||||
self._soft_exit_since = 0.0
|
||||
|
||||
if not self.experimental_mode or (now - self._candidate_since) >= self.CHILL_ENTRY_CONFIRM_TIME:
|
||||
self.experimental_mode = False
|
||||
self._active_auto_status = auto_status
|
||||
self._chill_hold_until = max(self._chill_hold_until, now + self.CHILL_MIN_DWELL_TIME)
|
||||
self.status_value = self._active_auto_status if not self.experimental_mode else CCStatus["OFF"]
|
||||
else:
|
||||
self._candidate_since = 0.0
|
||||
if not self.experimental_mode:
|
||||
if self._soft_exit_since == 0.0:
|
||||
self._soft_exit_since = now
|
||||
|
||||
hold_active = now < self._chill_hold_until
|
||||
exit_buffer_active = (now - self._soft_exit_since) < self.CHILL_EXIT_BUFFER_TIME
|
||||
if hold_active or exit_buffer_active:
|
||||
self.status_value = self._active_auto_status
|
||||
else:
|
||||
self.experimental_mode = True
|
||||
self._active_auto_status = CCStatus["OFF"]
|
||||
self.status_value = CCStatus["OFF"]
|
||||
else:
|
||||
self._soft_exit_since = 0.0
|
||||
self.status_value = CCStatus["OFF"]
|
||||
|
||||
self._write_status(self.status_value if not self.experimental_mode else CCStatus["OFF"])
|
||||
|
||||
def _reset_timers(self):
|
||||
self._active_auto_status = CCStatus["OFF"]
|
||||
self._candidate_since = 0.0
|
||||
self._soft_exit_since = 0.0
|
||||
self._chill_hold_until = 0.0
|
||||
|
||||
def _refresh_detector(self, v_ego, sm):
|
||||
detector_toggles = type("DetectorToggles", (), {
|
||||
"conditional_curves": True,
|
||||
"conditional_curves_lead": True,
|
||||
"conditional_lead": True,
|
||||
"conditional_slower_lead": True,
|
||||
"conditional_stopped_lead": True,
|
||||
})()
|
||||
self.detector.curve_detection(v_ego, detector_toggles)
|
||||
self.detector.slow_lead(detector_toggles, v_ego)
|
||||
self.detector.stop_sign_and_light(v_ego, sm, self.CCM_STOP_MODEL_TIME)
|
||||
|
||||
def _has_hard_veto(self, v_ego, sm):
|
||||
if sm["carState"].standstill:
|
||||
return True
|
||||
|
||||
if sm["carState"].leftBlinker or sm["carState"].rightBlinker:
|
||||
return True
|
||||
|
||||
if sm["starpilotCarState"].trafficModeEnabled:
|
||||
return True
|
||||
|
||||
if self.starpilot_planner.starpilot_vcruise.slc.experimental_mode:
|
||||
return True
|
||||
|
||||
if self.detector.curve_detected or self.detector.slow_lead_detected or self.detector.stop_light_detected:
|
||||
return True
|
||||
|
||||
if self.starpilot_planner.starpilot_vcruise.stop_sign_confirmed or self.starpilot_planner.starpilot_vcruise.forcing_stop:
|
||||
return True
|
||||
|
||||
if self._adjacent_lead_ambiguous(sm, v_ego):
|
||||
return True
|
||||
|
||||
return self._low_speed_stop_scene(v_ego)
|
||||
|
||||
def _low_speed_stop_scene(self, v_ego):
|
||||
if v_ego >= self.LOW_SPEED_STOP_SCENE_MAX_SPEED:
|
||||
return False
|
||||
|
||||
if self.starpilot_planner.raw_model_stopped or self.starpilot_planner.model_stopped or self.detector.stop_light_model_detected:
|
||||
return True
|
||||
|
||||
lead = self.starpilot_planner.lead_one
|
||||
if not getattr(lead, "status", False):
|
||||
return False
|
||||
|
||||
lead_distance = float(getattr(lead, "dRel", float("inf")))
|
||||
lead_speed = float(getattr(lead, "vLead", float("inf")))
|
||||
lead_distance_limit = max(40.0, v_ego * self.STABLE_LEAD_MAX_DISTANCE_TIME)
|
||||
return lead_distance < lead_distance_limit and lead_speed < max(6.0, v_ego + 0.5)
|
||||
|
||||
def _get_chill_status(self, v_ego, v_cruise, sm, starpilot_toggles):
|
||||
lead = self.starpilot_planner.lead_one
|
||||
lead_status = bool(getattr(lead, "status", False))
|
||||
tracking_lead = bool(getattr(self.starpilot_planner, "tracking_lead", False))
|
||||
set_speed_error = max(0.0, v_cruise - v_ego)
|
||||
|
||||
if (not lead_status and not tracking_lead and
|
||||
v_ego >= starpilot_toggles.conditional_chill_speed and
|
||||
set_speed_error >= starpilot_toggles.conditional_chill_speed_margin):
|
||||
return CCStatus["SPEED"]
|
||||
|
||||
if not starpilot_toggles.conditional_chill_lead:
|
||||
return CCStatus["OFF"]
|
||||
|
||||
if v_ego < starpilot_toggles.conditional_chill_speed_lead or not lead_status or not tracking_lead:
|
||||
return CCStatus["OFF"]
|
||||
|
||||
lead_distance = float(getattr(lead, "dRel", float("inf")))
|
||||
lead_speed = float(getattr(lead, "vLead", 0.0))
|
||||
lead_brake = max(0.0, -float(getattr(lead, "aLeadK", 0.0)))
|
||||
lead_prob = float(getattr(lead, "modelProb", 0.0))
|
||||
closing_speed = max(0.0, v_ego - lead_speed)
|
||||
max_closing_speed = max(self.STABLE_LEAD_MAX_CLOSING_SPEED, self.STABLE_LEAD_MAX_CLOSING_RATIO * v_ego)
|
||||
max_distance = min(self.STABLE_LEAD_MAX_DISTANCE, max(35.0, v_ego * self.STABLE_LEAD_MAX_DISTANCE_TIME))
|
||||
lead_confident = bool(getattr(lead, "radar", False)) or lead_prob >= self.STABLE_LEAD_MIN_MODEL_PROB
|
||||
|
||||
if not lead_confident:
|
||||
return CCStatus["OFF"]
|
||||
|
||||
if lead_distance >= max_distance or lead_speed <= self.STABLE_LEAD_MIN_SPEED:
|
||||
return CCStatus["OFF"]
|
||||
|
||||
if lead_brake > self.STABLE_LEAD_MAX_BRAKE or closing_speed > max_closing_speed:
|
||||
return CCStatus["OFF"]
|
||||
|
||||
return CCStatus["LEAD"]
|
||||
|
||||
def _adjacent_lead_ambiguous(self, sm, v_ego):
|
||||
radar_state = sm.get("starpilotRadarState")
|
||||
if radar_state is None:
|
||||
return False
|
||||
|
||||
max_distance = min(self.ADJACENT_LEAD_VETO_MAX_DISTANCE, max(25.0, v_ego * self.ADJACENT_LEAD_VETO_MAX_DISTANCE_TIME))
|
||||
for lead in (getattr(radar_state, "leadLeft", None), getattr(radar_state, "leadRight", None)):
|
||||
if lead is None or not getattr(lead, "status", False):
|
||||
continue
|
||||
|
||||
if float(getattr(lead, "dRel", float("inf"))) < max_distance and float(getattr(lead, "vLead", 0.0)) > self.ADJACENT_LEAD_VETO_MIN_SPEED:
|
||||
return True
|
||||
|
||||
return False
|
||||
|
||||
def _write_status(self, status_value):
|
||||
if status_value != self._prev_cc_status:
|
||||
self.params_memory.put_int("CCStatus", status_value)
|
||||
self._prev_cc_status = status_value
|
||||
@@ -6,8 +6,11 @@ from openpilot.selfdrive.car.cruise import CRUISE_LONG_PRESS, ButtonType
|
||||
from openpilot.selfdrive.selfdrived.events import ET
|
||||
|
||||
from openpilot.starpilot.common.experimental_state import (
|
||||
CCStatus,
|
||||
CEStatus,
|
||||
next_manual_cc_status,
|
||||
next_manual_ce_status,
|
||||
sync_manual_cc_state,
|
||||
sync_manual_ce_state,
|
||||
)
|
||||
from openpilot.starpilot.common.starpilot_utilities import is_FrogsGoMoo
|
||||
@@ -80,6 +83,11 @@ class StarPilotCard:
|
||||
override_value = next_manual_ce_status(current_status, sm["selfdriveState"].experimentalMode)
|
||||
self.params_memory.put_int("CEStatus", override_value)
|
||||
sync_manual_ce_state(self.params, override_value)
|
||||
elif getattr(starpilot_toggles, "conditional_chill_mode", False):
|
||||
current_status = self.params_memory.get_int("CCStatus", default=CCStatus["OFF"])
|
||||
override_value = next_manual_cc_status(current_status, sm["selfdriveState"].experimentalMode)
|
||||
self.params_memory.put_int("CCStatus", override_value)
|
||||
sync_manual_cc_state(self.params, override_value)
|
||||
else:
|
||||
self.params.put_bool_nonblocking("ExperimentalMode", not sm["selfdriveState"].experimentalMode)
|
||||
|
||||
|
||||
@@ -16,6 +16,7 @@ from openpilot.selfdrive.controls.lib.longitudinal_mpc_lib.long_mpc import A_CHA
|
||||
|
||||
from openpilot.starpilot.common.starpilot_utilities import calculate_lane_width, calculate_road_curvature
|
||||
from openpilot.starpilot.common.starpilot_variables import CRUISING_SPEED, MINIMUM_LATERAL_ACCELERATION, PLANNER_TIME, THRESHOLD
|
||||
from openpilot.starpilot.controls.lib.conditional_chill_mode import ConditionalChillMode
|
||||
from openpilot.starpilot.controls.lib.conditional_experimental_mode import ConditionalExperimentalMode
|
||||
from openpilot.starpilot.controls.lib.starpilot_acceleration import StarPilotAcceleration
|
||||
from openpilot.starpilot.controls.lib.starpilot_events import StarPilotEvents
|
||||
@@ -47,6 +48,7 @@ class StarPilotPlanner:
|
||||
|
||||
self.starpilot_acceleration = StarPilotAcceleration(self)
|
||||
self.starpilot_cem = ConditionalExperimentalMode(self)
|
||||
self.starpilot_ccm = ConditionalChillMode(self, self.starpilot_cem)
|
||||
self.starpilot_events = StarPilotEvents(self, error_log, ThemeManager)
|
||||
self.starpilot_following = StarPilotFollowing(self)
|
||||
self.starpilot_vcruise = StarPilotVCruise(self)
|
||||
@@ -194,11 +196,17 @@ class StarPilotPlanner:
|
||||
|
||||
self.starpilot_following.update(controls_enabled, v_ego, sm, starpilot_toggles)
|
||||
|
||||
cem_tracking_active = controls_enabled or sm["starpilotCarState"].alwaysOnLateralEnabled
|
||||
if cem_tracking_active and starpilot_toggles.conditional_experimental_mode:
|
||||
conditional_tracking_active = controls_enabled or sm["starpilotCarState"].alwaysOnLateralEnabled
|
||||
if conditional_tracking_active and starpilot_toggles.conditional_experimental_mode:
|
||||
# Keep CEM's filters warm in AOL so engagement can inherit the current scene.
|
||||
self.starpilot_cem.update(v_ego, sm, starpilot_toggles)
|
||||
self.starpilot_ccm.experimental_mode = True
|
||||
elif conditional_tracking_active and starpilot_toggles.conditional_chill_mode:
|
||||
self.starpilot_ccm.update(v_ego, v_cruise, sm, starpilot_toggles)
|
||||
self.starpilot_cem.experimental_mode = False
|
||||
else:
|
||||
self.starpilot_ccm.experimental_mode = True
|
||||
self.starpilot_cem.experimental_mode = False
|
||||
self.starpilot_cem.curve_detected = False
|
||||
self.starpilot_cem.stop_sign_and_light(v_ego, sm, PLANNER_TIME - 2)
|
||||
|
||||
@@ -263,7 +271,13 @@ class StarPilotPlanner:
|
||||
starpilotPlan.disableThrottle = self.starpilot_following.disable_throttle
|
||||
starpilotPlan.trackingLead = self.tracking_lead
|
||||
|
||||
starpilotPlan.experimentalMode = self.starpilot_cem.experimental_mode or self.starpilot_vcruise.slc.experimental_mode
|
||||
conditional_experimental_mode = False
|
||||
if starpilot_toggles.conditional_experimental_mode:
|
||||
conditional_experimental_mode = self.starpilot_cem.experimental_mode
|
||||
elif starpilot_toggles.conditional_chill_mode:
|
||||
conditional_experimental_mode = self.starpilot_ccm.experimental_mode
|
||||
|
||||
starpilotPlan.experimentalMode = conditional_experimental_mode or self.starpilot_vcruise.slc.experimental_mode
|
||||
|
||||
starpilotPlan.forcingStop = self.starpilot_vcruise.forcing_stop
|
||||
starpilotPlan.forcingStopLength = self.starpilot_vcruise.tracked_model_length
|
||||
|
||||
@@ -235,6 +235,23 @@ def test_pacifica_hybrid_main_aol_waits_for_set_press(monkeypatch, tmp_path):
|
||||
assert ret.alwaysOnLateralEnabled is False
|
||||
|
||||
|
||||
def test_conditional_chill_wheel_override_cycles_manual_state(monkeypatch, tmp_path):
|
||||
monkeypatch.setattr(spc, "Params", FakeParams)
|
||||
monkeypatch.setattr(spc, "is_FrogsGoMoo", lambda: False)
|
||||
monkeypatch.setattr(spc, "ERROR_LOGS_PATH", tmp_path)
|
||||
|
||||
card = spc.StarPilotCard(SimpleNamespace(brand="gm"), SimpleNamespace(alternativeExperience=0))
|
||||
sm = make_sm()
|
||||
toggles = make_toggles(conditional_chill_mode=True)
|
||||
|
||||
sm["selfdriveState"].experimentalMode = True
|
||||
card.handle_experimental_mode(sm, toggles)
|
||||
assert card.params_memory.get_int("CCStatus") == spc.CCStatus["USER_CHILL"]
|
||||
|
||||
card.handle_experimental_mode(sm, toggles)
|
||||
assert card.params_memory.get_int("CCStatus") == spc.CCStatus["OFF"]
|
||||
|
||||
|
||||
def test_cancel_button_short_press_can_run_independent_mapping(monkeypatch, tmp_path):
|
||||
monkeypatch.setattr(spc, "Params", FakeParams)
|
||||
monkeypatch.setattr(spc, "is_FrogsGoMoo", lambda: False)
|
||||
|
||||
@@ -199,7 +199,7 @@ def starpilot_thread():
|
||||
sm = messaging.SubMaster(["carControl", "carState", "controlsState", "deviceState", "driverMonitoringState",
|
||||
"gpsLocation", "gpsLocationExternal", "liveParameters", "managerState", "modelV2",
|
||||
"onroadEvents", "pandaStates", "radarState", "selfdriveState", "starpilotCarState",
|
||||
"starpilotSelfdriveState", "starpilotModelV2", "starpilotOnroadEvents", "mapdOut"],
|
||||
"starpilotRadarState", "starpilotSelfdriveState", "starpilotModelV2", "starpilotOnroadEvents", "mapdOut"],
|
||||
poll="modelV2")
|
||||
|
||||
params = Params(return_defaults=True)
|
||||
|
||||
File diff suppressed because it is too large
Load Diff
@@ -65,7 +65,7 @@ from openpilot.starpilot.common.model_versions import (
|
||||
uses_combined_driving_artifacts,
|
||||
uses_split_off_policy_artifacts,
|
||||
)
|
||||
from openpilot.starpilot.common.experimental_state import sync_persist_experimental_state
|
||||
from openpilot.starpilot.common.experimental_state import sync_persist_chill_state, sync_persist_experimental_state
|
||||
from openpilot.starpilot.common.starpilot_utilities import delete_file, get_lock_status, run_cmd
|
||||
from openpilot.starpilot.common.starpilot_variables import ACTIVE_THEME_PATH, ERROR_LOGS_PATH, EXCLUDED_KEYS, LEGACY_STARPILOT_PARAM_RENAMES, MAPS_PATH, MODELS_PATH, RESOURCES_REPO, SCREEN_RECORDINGS_PATH, STOCK_THEME_PATH, THEME_SAVE_PATH,\
|
||||
default_ev_tuning_enabled, migrate_cancel_button_controls, update_starpilot_toggles
|
||||
@@ -3794,6 +3794,22 @@ def setup(app):
|
||||
"updated": updated,
|
||||
}), 200
|
||||
|
||||
if key in {"ConditionalExperimental", "ConditionalChill"}:
|
||||
enabled = str_val.strip() in ("1", "true", "True")
|
||||
params.put_bool(key, enabled)
|
||||
|
||||
updated = {key: enabled}
|
||||
if enabled:
|
||||
other_key = "ConditionalChill" if key == "ConditionalExperimental" else "ConditionalExperimental"
|
||||
params.put_bool(other_key, False)
|
||||
updated[other_key] = False
|
||||
|
||||
update_starpilot_toggles()
|
||||
return jsonify({
|
||||
"message": f"Parameter '{key}' updated successfully.",
|
||||
"updated": updated,
|
||||
}), 200
|
||||
|
||||
if key == "CustomAccelProfile":
|
||||
enabled = str_val.strip() in ("1", "true", "True")
|
||||
params.put_bool(key, enabled)
|
||||
@@ -3825,6 +3841,18 @@ def setup(app):
|
||||
},
|
||||
}), 200
|
||||
|
||||
if key == "PersistChillState":
|
||||
enabled = str_val.strip() in ("1", "true", "True")
|
||||
sync_persist_chill_state(params, params_memory, enabled)
|
||||
update_starpilot_toggles()
|
||||
return jsonify({
|
||||
"message": f"Parameter '{key}' updated successfully.",
|
||||
"updated": {
|
||||
"PersistChillState": enabled,
|
||||
"PersistedCCStatus": params.get_int("PersistedCCStatus", default=0),
|
||||
},
|
||||
}), 200
|
||||
|
||||
if key == "IsRHD":
|
||||
enabled = str_val.strip() in ("1", "true", "True")
|
||||
params.put_bool("IsRHD", enabled)
|
||||
|
||||
@@ -16,6 +16,7 @@ StarPilotLongitudinalPanel::StarPilotLongitudinalPanel(StarPilotSettingsWindow *
|
||||
|
||||
StarPilotListWidget *advancedLongitudinalTuneList = new StarPilotListWidget(this);
|
||||
StarPilotListWidget *aggressivePersonalityList = new StarPilotListWidget(this);
|
||||
StarPilotListWidget *conditionalChillList = new StarPilotListWidget(this);
|
||||
StarPilotListWidget *conditionalExperimentalList = new StarPilotListWidget(this);
|
||||
StarPilotListWidget *curveSpeedList = new StarPilotListWidget(this);
|
||||
StarPilotListWidget *customDrivingPersonalityList = new StarPilotListWidget(this);
|
||||
@@ -36,6 +37,7 @@ StarPilotLongitudinalPanel::StarPilotLongitudinalPanel(StarPilotSettingsWindow *
|
||||
|
||||
ScrollView *advancedLongitudinalTunePanel = new ScrollView(advancedLongitudinalTuneList, this);
|
||||
ScrollView *aggressivePersonalityPanel = new ScrollView(aggressivePersonalityList, this);
|
||||
ScrollView *conditionalChillPanel = new ScrollView(conditionalChillList, this);
|
||||
ScrollView *conditionalExperimentalPanel = new ScrollView(conditionalExperimentalList, this);
|
||||
ScrollView *curveSpeedPanel = new ScrollView(curveSpeedList, this);
|
||||
ScrollView *customDrivingPersonalityPanel = new ScrollView(customDrivingPersonalityList, this);
|
||||
@@ -56,6 +58,7 @@ StarPilotLongitudinalPanel::StarPilotLongitudinalPanel(StarPilotSettingsWindow *
|
||||
|
||||
longitudinalLayout->addWidget(advancedLongitudinalTunePanel);
|
||||
longitudinalLayout->addWidget(aggressivePersonalityPanel);
|
||||
longitudinalLayout->addWidget(conditionalChillPanel);
|
||||
longitudinalLayout->addWidget(conditionalExperimentalPanel);
|
||||
longitudinalLayout->addWidget(curveSpeedPanel);
|
||||
longitudinalLayout->addWidget(customDrivingPersonalityPanel);
|
||||
@@ -95,6 +98,12 @@ StarPilotLongitudinalPanel::StarPilotLongitudinalPanel(StarPilotSettingsWindow *
|
||||
{"CEModelStopTime", tr("Predicted Stop In"), tr("<b>Switch to \"Experimental Mode\" when openpilot predicts a stop within the set time.</b> This is usually triggered when the model \"sees\" a red light or stop sign ahead.<br><br><i><b>Disclaimer</b>: openpilot does not explicitly detect traffic lights or stop signs. In \"Experimental Mode\", openpilot makes end-to-end driving decisions from camera input, which means it may stop even when there's no clear reason!</i>"), ""},
|
||||
{"CESignalSpeed", tr("Turn Signal Below"), tr("<b>Switch to \"Experimental Mode\" when using a turn signal below the set speed</b> to allow the model to choose an appropriate speed for smoother left and right turns."), ""},
|
||||
{"ShowCEMStatus", tr("Status Widget"), tr("<b>Show which condition triggered \"Experimental Mode\"</b> on the driving screen."), ""},
|
||||
{"ConditionalChill", tr("Conditional Chill Mode"), tr("<b>Keep \"Experimental Mode\" on by default, but temporarily switch to \"Chill Mode\" in simple cruising scenes where speed holding is usually better.</b>"), "../../starpilot/assets/toggle_icons/icon_conditional.png"},
|
||||
{"PersistChillState", tr("Persist Chill State"), tr("<b>Keep your manual Conditional Chill override through reboots</b> until you manually clear it."), ""},
|
||||
{"CCMSpeed", tr("Above"), tr("<b>Switch to \"Chill Mode\" on open roads above this speed when no lead is detected and the car is still below the set speed.</b>"), ""},
|
||||
{"CCMLead", tr("Stable Lead Ahead"), tr("<b>Switch to \"Chill Mode\" when following a steady, well-tracked lead vehicle at cruising speeds.</b>"), ""},
|
||||
{"CCMSetSpeedMargin", tr("Set Speed Margin"), tr("<b>How far below the set speed the car must be before open-road Conditional Chill can engage.</b>"), ""},
|
||||
{"ShowCCMStatus", tr("Status Widget"), tr("<b>Show which condition triggered \"Chill Mode\"</b> on the driving screen."), ""},
|
||||
|
||||
{"CurveSpeedController", tr("Curve Speed Controller"), tr("<b>Automatically slow down for upcoming curves</b> using data learned from your driving style, adapting to curves as you would."), "../../starpilot/assets/toggle_icons/icon_speed_map.png"},
|
||||
{"CalibratedLateralAcceleration", tr("Calibrated Lateral Acceleration"), tr("<b>The learned lateral acceleration from collected driving data.</b> This sets how fast openpilot will take curves. Higher values allow faster cornering; lower values slow the vehicle for gentler turns."), ""},
|
||||
@@ -253,11 +262,22 @@ StarPilotLongitudinalPanel::StarPilotLongitudinalPanel(StarPilotSettingsWindow *
|
||||
longitudinalLayout->setCurrentWidget(conditionalExperimentalPanel);
|
||||
});
|
||||
longitudinalToggle = conditionalExperimentalToggle;
|
||||
} else if (param == "ConditionalChill") {
|
||||
StarPilotManageControl *conditionalChillToggle = new StarPilotManageControl(param, title, desc, icon);
|
||||
QObject::connect(conditionalChillToggle, &StarPilotManageControl::manageButtonClicked, [longitudinalLayout, conditionalChillPanel]() {
|
||||
longitudinalLayout->setCurrentWidget(conditionalChillPanel);
|
||||
});
|
||||
longitudinalToggle = conditionalChillToggle;
|
||||
} else if (param == "CESpeed") {
|
||||
StarPilotParamValueControl *CESpeed = new StarPilotParamValueControl(param, title, desc, icon, 0, 99, tr(" mph"), std::map<float, QString>(), 1, true, 175);
|
||||
StarPilotParamValueControl *CESpeedLead = new StarPilotParamValueControl("CESpeedLead", tr("With Lead"), tr("<b>Switch to \"Experimental Mode\" when driving below this speed with a lead</b> to help openpilot handle low-speed situations more smoothly."), icon, 0, 99, tr(" mph"), std::map<float, QString>(), 1, true, 175);
|
||||
StarPilotDualParamValueControl *conditionalSpeeds = new StarPilotDualParamValueControl(CESpeed, CESpeedLead);
|
||||
longitudinalToggle = reinterpret_cast<AbstractControl*>(conditionalSpeeds);
|
||||
} else if (param == "CCMSpeed") {
|
||||
StarPilotParamValueControl *CCMSpeed = new StarPilotParamValueControl(param, title, desc, icon, 0, 99, tr(" mph"), std::map<float, QString>(), 1, true, 175);
|
||||
StarPilotParamValueControl *CCMSpeedLead = new StarPilotParamValueControl("CCMSpeedLead", tr("With Lead"), tr("<b>Switch to \"Chill Mode\" when a stable lead is being followed above this speed.</b>"), icon, 0, 99, tr(" mph"), std::map<float, QString>(), 1, true, 175);
|
||||
StarPilotDualParamValueControl *conditionalSpeeds = new StarPilotDualParamValueControl(CCMSpeed, CCMSpeedLead);
|
||||
longitudinalToggle = reinterpret_cast<AbstractControl*>(conditionalSpeeds);
|
||||
} else if (param == "CECurves") {
|
||||
std::vector<QString> curveToggles{"CECurvesLead"};
|
||||
std::vector<QString> curveToggleNames{tr("With Lead")};
|
||||
@@ -266,6 +286,8 @@ StarPilotLongitudinalPanel::StarPilotLongitudinalPanel(StarPilotSettingsWindow *
|
||||
std::vector<QString> leadToggles{"CESlowerLead", "CEStoppedLead"};
|
||||
std::vector<QString> leadToggleNames{tr("Slower Lead"), tr("Stopped Lead")};
|
||||
longitudinalToggle = new StarPilotButtonToggleControl(param, title, desc, icon, leadToggles, leadToggleNames);
|
||||
} else if (param == "CCMSetSpeedMargin") {
|
||||
longitudinalToggle = new StarPilotParamValueControl(param, title, desc, icon, 0, 15, tr(" mph"), std::map<float, QString>(), 1, true, 175);
|
||||
} else if (param == "CEModelStopTime") {
|
||||
std::map<float, QString> stopTimeLabels;
|
||||
for (int i = 0; i <= 10; ++i) {
|
||||
@@ -586,6 +608,8 @@ StarPilotLongitudinalPanel::StarPilotLongitudinalPanel(StarPilotSettingsWindow *
|
||||
advancedLongitudinalTuneList->addItem(longitudinalToggle);
|
||||
} else if (aggressivePersonalityKeys.contains(param)) {
|
||||
aggressivePersonalityList->addItem(longitudinalToggle);
|
||||
} else if (conditionalChillKeys.contains(param)) {
|
||||
conditionalChillList->addItem(longitudinalToggle);
|
||||
} else if (conditionalExperimentalKeys.contains(param)) {
|
||||
conditionalExperimentalList->addItem(longitudinalToggle);
|
||||
} else if (curveSpeedKeys.contains(param)) {
|
||||
@@ -658,6 +682,20 @@ StarPilotLongitudinalPanel::StarPilotLongitudinalPanel(StarPilotSettingsWindow *
|
||||
}
|
||||
updateToggles();
|
||||
});
|
||||
QObject::connect(static_cast<ToggleControl*>(toggles["ConditionalExperimental"]), &ToggleControl::toggleFlipped, this, [this]() {
|
||||
if (params.getBool("ConditionalExperimental")) {
|
||||
params.putBool("ConditionalChill", false);
|
||||
static_cast<ParamControl*>(toggles["ConditionalChill"])->refresh();
|
||||
}
|
||||
updateToggles();
|
||||
});
|
||||
QObject::connect(static_cast<ToggleControl*>(toggles["ConditionalChill"]), &ToggleControl::toggleFlipped, this, [this]() {
|
||||
if (params.getBool("ConditionalChill")) {
|
||||
params.putBool("ConditionalExperimental", false);
|
||||
static_cast<ParamControl*>(toggles["ConditionalExperimental"])->refresh();
|
||||
}
|
||||
updateToggles();
|
||||
});
|
||||
|
||||
StarPilotParamValueControl *trafficFollowToggle = static_cast<StarPilotParamValueControl*>(toggles["TrafficFollow"]);
|
||||
StarPilotParamValueControl *trafficAccelerationToggle = static_cast<StarPilotParamValueControl*>(toggles["TrafficJerkAcceleration"]);
|
||||
@@ -831,6 +869,9 @@ void StarPilotLongitudinalPanel::updateMetric(bool metric, bool bootRun) {
|
||||
params.putIntNonBlocking("IncreasedStoppedDistanceRainStorm", params.getInt("IncreasedStoppedDistanceRainStorm") * distanceConversion);
|
||||
params.putIntNonBlocking("IncreasedStoppedDistanceSnow", params.getInt("IncreasedStoppedDistanceSnow") * distanceConversion);
|
||||
|
||||
params.putIntNonBlocking("CCMSpeed", params.getInt("CCMSpeed") * speedConversion);
|
||||
params.putIntNonBlocking("CCMSpeedLead", params.getInt("CCMSpeedLead") * speedConversion);
|
||||
params.putIntNonBlocking("CCMSetSpeedMargin", params.getInt("CCMSetSpeedMargin") * speedConversion);
|
||||
params.putIntNonBlocking("CESignalSpeed", params.getInt("CESignalSpeed") * speedConversion);
|
||||
params.putIntNonBlocking("CESpeed", params.getInt("CESpeed") * speedConversion);
|
||||
params.putIntNonBlocking("CESpeedLead", params.getInt("CESpeedLead") * speedConversion);
|
||||
@@ -881,7 +922,9 @@ void StarPilotLongitudinalPanel::updateMetric(bool metric, bool bootRun) {
|
||||
labelsInitialized = true;
|
||||
}
|
||||
|
||||
StarPilotDualParamValueControl *ccmSpeedToggle = reinterpret_cast<StarPilotDualParamValueControl*>(toggles["CCMSpeed"]);
|
||||
StarPilotDualParamValueControl *ceSpeedToggle = reinterpret_cast<StarPilotDualParamValueControl*>(toggles["CESpeed"]);
|
||||
StarPilotParamValueControl *ccmSetSpeedMarginToggle = static_cast<StarPilotParamValueControl*>(toggles["CCMSetSpeedMargin"]);
|
||||
StarPilotParamValueButtonControl *ceSignal = static_cast<StarPilotParamValueButtonControl*>(toggles["CESignalSpeed"]);
|
||||
StarPilotParamValueControl *customCruiseToggle = static_cast<StarPilotParamValueControl*>(toggles["CustomCruise"]);
|
||||
StarPilotParamValueControl *customCruiseLongToggle = static_cast<StarPilotParamValueControl*>(toggles["CustomCruiseLong"]);
|
||||
@@ -922,6 +965,8 @@ void StarPilotLongitudinalPanel::updateMetric(bool metric, bool bootRun) {
|
||||
increasedStoppedDistanceRainStormToggle->updateControl(0, 3, metricDistanceLabels);
|
||||
increasedStoppedDistanceSnowToggle->updateControl(0, 3, metricDistanceLabels);
|
||||
|
||||
ccmSpeedToggle->updateControl(0, 150, metricSpeedLabels);
|
||||
ccmSetSpeedMarginToggle->updateControl(0, 25, metricSpeedLabels);
|
||||
ceSignal->updateControl(0, 150, metricSpeedLabels);
|
||||
ceSpeedToggle->updateControl(0, 150, metricSpeedLabels);
|
||||
customCruiseToggle->updateControl(1, 150, metricSpeedLabels);
|
||||
@@ -957,6 +1002,8 @@ void StarPilotLongitudinalPanel::updateMetric(bool metric, bool bootRun) {
|
||||
increasedStoppedDistanceRainStormToggle->updateControl(0, 10, imperialDistanceLabels);
|
||||
increasedStoppedDistanceSnowToggle->updateControl(0, 10, imperialDistanceLabels);
|
||||
|
||||
ccmSpeedToggle->updateControl(0, 99, imperialSpeedLabels);
|
||||
ccmSetSpeedMarginToggle->updateControl(0, 15, imperialSpeedLabels);
|
||||
ceSignal->updateControl(0, 99, imperialSpeedLabels);
|
||||
ceSpeedToggle->updateControl(0, 99, imperialSpeedLabels);
|
||||
customCruiseToggle->updateControl(1, 99, imperialSpeedLabels);
|
||||
@@ -1041,6 +1088,8 @@ void StarPilotLongitudinalPanel::updateToggles() {
|
||||
toggles["AdvancedLongitudinalTune"]->setVisible(true);
|
||||
} else if (aggressivePersonalityKeys.contains(key)) {
|
||||
toggles["AggressivePersonalityProfile"]->setVisible(true);
|
||||
} else if (conditionalChillKeys.contains(key)) {
|
||||
toggles["ConditionalChill"]->setVisible(true);
|
||||
} else if (conditionalExperimentalKeys.contains(key)) {
|
||||
toggles["ConditionalExperimental"]->setVisible(true);
|
||||
} else if (curveSpeedKeys.contains(key)) {
|
||||
|
||||
@@ -30,6 +30,7 @@ private:
|
||||
|
||||
QSet<QString> advancedLongitudinalTuneKeys = {"EVTuning", "TruckTuning", "LongitudinalActuatorDelay", "MaxDesiredAcceleration", "StartAccel", "StopAccel", "StoppingDecelRate", "VEgoStarting", "VEgoStopping"};
|
||||
QSet<QString> aggressivePersonalityKeys = {"AggressiveFollow", "AggressiveFollowHigh", "AggressiveJerkAcceleration", "AggressiveJerkDeceleration", "AggressiveJerkDanger", "AggressiveJerkSpeed", "AggressiveJerkSpeedDecrease", "ResetAggressivePersonality"};
|
||||
QSet<QString> conditionalChillKeys = {"PersistChillState", "CCMSpeed", "CCMSpeedLead", "CCMLead", "CCMSetSpeedMargin", "ShowCCMStatus"};
|
||||
QSet<QString> conditionalExperimentalKeys = {"PersistExperimentalState", "CESpeed", "CESpeedLead", "CECurves", "CELead", "CEModelStopTime", "CESignalSpeed", "CEStopLights", "ShowCEMStatus"};
|
||||
QSet<QString> curveSpeedKeys = {"CalibratedLateralAcceleration", "CalibrationProgress", "ResetCurveData", "ShowCSCStatus"};
|
||||
QSet<QString> customDrivingPersonalityKeys = {"AggressivePersonalityProfile", "RelaxedPersonalityProfile", "StandardPersonalityProfile", "TrafficPersonalityProfile"};
|
||||
|
||||
@@ -468,9 +468,14 @@ void StarPilotAnnotatedCameraWidget::paintCEMStatus(QPainter &p) {
|
||||
cemStatusPosition.setY(dmIconPosition.y() - widget_size / 2);
|
||||
|
||||
QRect cemWidget(cemStatusPosition, QSize(widget_size, widget_size));
|
||||
const bool conditionalExperimentalMode = starpilot_toggles.value("conditional_experimental_mode").toBool();
|
||||
const bool conditionalChillMode = starpilot_toggles.value("conditional_chill_mode").toBool();
|
||||
const bool manualOverride =
|
||||
(conditionalExperimentalMode && starpilot_scene.conditional_status == 1) ||
|
||||
(conditionalChillMode && (starpilot_scene.conditional_status == 1 || starpilot_scene.conditional_status == 2));
|
||||
|
||||
p.setBrush(blackColor(166));
|
||||
if (starpilot_scene.conditional_status == 1) {
|
||||
if (manualOverride) {
|
||||
p.setPen(QPen(QColor(bg_colors[STATUS_CEM_DISABLED]), 10));
|
||||
} else if (experimentalMode) {
|
||||
p.setPen(QPen(QColor(bg_colors[STATUS_EXPERIMENTAL_MODE_ENABLED]), 10));
|
||||
@@ -480,7 +485,19 @@ void StarPilotAnnotatedCameraWidget::paintCEMStatus(QPainter &p) {
|
||||
p.drawRoundedRect(cemWidget, 24, 24);
|
||||
|
||||
QSharedPointer<QMovie> icon = chillModeIcon;
|
||||
if (experimentalMode) {
|
||||
if (conditionalChillMode && !conditionalExperimentalMode) {
|
||||
if (starpilot_scene.conditional_status == 1) {
|
||||
icon = experimentalModeIcon;
|
||||
} else if (starpilot_scene.conditional_status == 2) {
|
||||
icon = chillModeIcon;
|
||||
} else if (starpilot_scene.conditional_status == 4) {
|
||||
icon = cemLeadIcon;
|
||||
} else if (starpilot_scene.conditional_status == 6) {
|
||||
icon = cemSpeedIcon;
|
||||
} else {
|
||||
icon = experimentalMode ? experimentalModeIcon : chillModeIcon;
|
||||
}
|
||||
} else if (experimentalMode) {
|
||||
if (starpilot_scene.conditional_status == 1) {
|
||||
icon = chillModeIcon;
|
||||
} else if (starpilot_scene.conditional_status == 2) {
|
||||
@@ -786,12 +803,17 @@ void StarPilotAnnotatedCameraWidget::paintPathEdges(QPainter &p, int height) {
|
||||
gradient.setColorAt(0.5f, QColor(baseColor.red(), baseColor.green(), baseColor.blue(), 255.0f * 0.35f));
|
||||
gradient.setColorAt(1.0f, QColor(baseColor.red(), baseColor.green(), baseColor.blue(), 255.0f * 0.0f));
|
||||
};
|
||||
const bool conditionalExperimentalMode = starpilot_toggles.value("conditional_experimental_mode").toBool();
|
||||
const bool conditionalChillMode = starpilot_toggles.value("conditional_chill_mode").toBool();
|
||||
const bool highlightOverride =
|
||||
(conditionalExperimentalMode && starpilot_scene.conditional_status == 1) ||
|
||||
(conditionalChillMode && (starpilot_scene.conditional_status == 1 || starpilot_scene.conditional_status == 2));
|
||||
|
||||
if (starpilot_scene.switchback_mode_enabled) {
|
||||
setPathEdgeColors(bg_colors[STATUS_SWITCHBACK_MODE_ENABLED]);
|
||||
} else if (starpilot_scene.always_on_lateral_active) {
|
||||
setPathEdgeColors(bg_colors[STATUS_ALWAYS_ON_LATERAL_ACTIVE]);
|
||||
} else if (starpilot_scene.conditional_status == 1) {
|
||||
} else if (highlightOverride) {
|
||||
setPathEdgeColors(bg_colors[STATUS_CEM_DISABLED]);
|
||||
} else if (experimentalMode) {
|
||||
setPathEdgeColors(bg_colors[STATUS_EXPERIMENTAL_MODE_ENABLED]);
|
||||
|
||||
@@ -128,7 +128,7 @@ void loadImage(const QString &basePath, QPixmap &pixmap, QSharedPointer<QMovie>
|
||||
void openDescriptions(bool forceOpenDescriptions, std::map<QString, AbstractControl*> toggles) {
|
||||
if (forceOpenDescriptions) {
|
||||
for (auto &[key, toggle] : toggles) {
|
||||
if (key != "CESpeed") {
|
||||
if (key != "CESpeed" && key != "CCMSpeed") {
|
||||
toggle->showDescription();
|
||||
}
|
||||
}
|
||||
|
||||
@@ -114,6 +114,11 @@ StarPilotUIState *starpilotUIState() {
|
||||
void StarPilotUIState::update() {
|
||||
update_state(this);
|
||||
|
||||
starpilot_scene.conditional_status = starpilot_scene.enabled ? params_memory.getInt("CEStatus") : 0;
|
||||
if (starpilot_scene.enabled && starpilot_scene.starpilot_toggles.value("conditional_chill_mode").toBool() &&
|
||||
!starpilot_scene.starpilot_toggles.value("conditional_experimental_mode").toBool()) {
|
||||
starpilot_scene.conditional_status = params_memory.getInt("CCStatus");
|
||||
} else {
|
||||
starpilot_scene.conditional_status = starpilot_scene.enabled ? params_memory.getInt("CEStatus") : 0;
|
||||
}
|
||||
starpilot_scene.driver_camera_timer = starpilot_scene.reverse && starpilot_scene.starpilot_toggles.value("driver_camera_in_reverse").toBool() ? starpilot_scene.driver_camera_timer + 1 : 0;
|
||||
}
|
||||
|
||||
Reference in New Issue
Block a user