Conditional Chill

starpilot/common/experimental_state.py

@@ -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

starpilot/common/safe_mode.py

@@ -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,
 }
 

starpilot/common/starpilot_variables.py

@@ -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

starpilot/common/tests/test_experimental_state.py

@@ -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"]