Conditional Chill

selfdrive/controls/tests/test_conditional_chill_mode.py

@@ -0,0 +1,226 @@
+from types import SimpleNamespace
+
+from openpilot.common.constants import CV
+from openpilot.starpilot.common.experimental_state import CCStatus
+from openpilot.starpilot.controls.lib.conditional_chill_mode import ConditionalChillMode
+
+
+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)
+
+
+class FakeDetector:
+  def __init__(self):
+    self.curve_detected = False
+    self.slow_lead_detected = False
+    self.stop_light_detected = False
+    self.stop_light_model_detected = False
+
+  def curve_detection(self, *_args, **_kwargs):
+    return None
+
+  def slow_lead(self, *_args, **_kwargs):
+    return None
+
+  def stop_sign_and_light(self, *_args, **_kwargs):
+    return None
+
+
+def make_sm():
+  return {
+    "carState": SimpleNamespace(standstill=False, leftBlinker=False, rightBlinker=False),
+    "starpilotCarState": SimpleNamespace(trafficModeEnabled=False),
+    "starpilotRadarState": SimpleNamespace(
+      leadLeft=SimpleNamespace(status=False, dRel=float("inf"), vLead=0.0),
+      leadRight=SimpleNamespace(status=False, dRel=float("inf"), vLead=0.0),
+    ),
+  }
+
+
+def make_toggles():
+  return SimpleNamespace(
+    conditional_chill_speed=45 * CV.MPH_TO_MS,
+    conditional_chill_speed_lead=35 * CV.MPH_TO_MS,
+    conditional_chill_speed_margin=3 * CV.MPH_TO_MS,
+    conditional_chill_lead=True,
+  )
+
+
+def make_ccm():
+  planner = SimpleNamespace(
+    params=FakeParams(),
+    params_memory=FakeParams(),
+    starpilot_vcruise=SimpleNamespace(
+      stop_sign_confirmed=False,
+      forcing_stop=False,
+      slc=SimpleNamespace(experimental_mode=False),
+    ),
+    raw_model_stopped=False,
+    model_stopped=False,
+    tracking_lead=False,
+    lead_one=SimpleNamespace(
+      status=False,
+      dRel=float("inf"),
+      vLead=0.0,
+      aLeadK=0.0,
+      modelProb=0.0,
+      radar=False,
+    ),
+  )
+  detector = FakeDetector()
+  return planner, detector, ConditionalChillMode(planner, detector)
+
+
+def test_ccm_stays_experimental_when_no_chill_condition_matches(monkeypatch):
+  planner, _detector, ccm = make_ccm()
+  sm = make_sm()
+  toggles = make_toggles()
+
+  monkeypatch.setattr("openpilot.starpilot.controls.lib.conditional_chill_mode.time.monotonic", lambda: 1.0)
+  ccm.update(20 * CV.MPH_TO_MS, 21 * CV.MPH_TO_MS, sm, toggles)
+
+  assert ccm.experimental_mode
+  assert ccm.status_value == CCStatus["OFF"]
+
+
+def test_ccm_enters_chill_for_open_road_speed_recovery(monkeypatch):
+  planner, _detector, ccm = make_ccm()
+  sm = make_sm()
+  toggles = make_toggles()
+  monotonic_values = iter([10.0, 10.5])
+  monkeypatch.setattr("openpilot.starpilot.controls.lib.conditional_chill_mode.time.monotonic", lambda: next(monotonic_values))
+
+  v_ego = 55 * CV.MPH_TO_MS
+  v_cruise = v_ego + 5 * CV.MPH_TO_MS
+  ccm.update(v_ego, v_cruise, sm, toggles)
+  ccm.update(v_ego, v_cruise, sm, toggles)
+
+  assert not ccm.experimental_mode
+  assert ccm.status_value == CCStatus["SPEED"]
+  assert planner.params_memory.get_int("CCStatus") == CCStatus["SPEED"]
+
+
+def test_ccm_enters_chill_for_stable_lead_cruising(monkeypatch):
+  planner, _detector, ccm = make_ccm()
+  sm = make_sm()
+  toggles = make_toggles()
+  monotonic_values = iter([10.0, 10.5])
+  monkeypatch.setattr("openpilot.starpilot.controls.lib.conditional_chill_mode.time.monotonic", lambda: next(monotonic_values))
+
+  planner.tracking_lead = True
+  planner.lead_one.status = True
+  planner.lead_one.dRel = 45.0
+  planner.lead_one.vLead = 24.8
+  planner.lead_one.radar = True
+
+  v_ego = 58 * CV.MPH_TO_MS
+  ccm.update(v_ego, v_ego, sm, toggles)
+  ccm.update(v_ego, v_ego, sm, toggles)
+
+  assert not ccm.experimental_mode
+  assert ccm.status_value == CCStatus["LEAD"]
+
+
+def test_ccm_hard_vetoes_force_experimental(monkeypatch):
+  planner, detector, ccm = make_ccm()
+  sm = make_sm()
+  toggles = make_toggles()
+  v_ego = 60 * CV.MPH_TO_MS
+  v_cruise = v_ego + 5 * CV.MPH_TO_MS
+
+  veto_scenes = []
+
+  detector.slow_lead_detected = True
+  veto_scenes.append(("slow_lead", make_sm()))
+  detector.slow_lead_detected = False
+
+  traffic_sm = make_sm()
+  traffic_sm["starpilotCarState"].trafficModeEnabled = True
+  veto_scenes.append(("traffic_mode", traffic_sm))
+
+  adjacent_sm = make_sm()
+  adjacent_sm["starpilotRadarState"].leadLeft = SimpleNamespace(status=True, dRel=25.0, vLead=12.0)
+  veto_scenes.append(("adjacent_lead", adjacent_sm))
+
+  for index, (_name, scene_sm) in enumerate(veto_scenes, start=1):
+    monkeypatch.setattr("openpilot.starpilot.controls.lib.conditional_chill_mode.time.monotonic", lambda idx=index: float(idx))
+    if _name == "slow_lead":
+      detector.slow_lead_detected = True
+    else:
+      detector.slow_lead_detected = False
+    ccm.update(v_ego, v_cruise, scene_sm, toggles)
+    assert ccm.experimental_mode
+    assert ccm.status_value == CCStatus["OFF"]
+
+  planner.starpilot_vcruise.slc.experimental_mode = True
+  monkeypatch.setattr("openpilot.starpilot.controls.lib.conditional_chill_mode.time.monotonic", lambda: 10.0)
+  ccm.update(v_ego, v_cruise, sm, toggles)
+  assert ccm.experimental_mode
+  assert ccm.status_value == CCStatus["OFF"]
+
+
+def test_ccm_immediately_exits_chill_when_scene_turns_into_slow_lead(monkeypatch):
+  planner, detector, ccm = make_ccm()
+  sm = make_sm()
+  toggles = make_toggles()
+  monotonic_values = iter([10.0, 10.5, 10.6])
+  monkeypatch.setattr("openpilot.starpilot.controls.lib.conditional_chill_mode.time.monotonic", lambda: next(monotonic_values))
+
+  planner.tracking_lead = True
+  planner.lead_one.status = True
+  planner.lead_one.dRel = 40.0
+  planner.lead_one.vLead = 24.9
+  planner.lead_one.radar = True
+
+  v_ego = 58 * CV.MPH_TO_MS
+  ccm.update(v_ego, v_ego, sm, toggles)
+  ccm.update(v_ego, v_ego, sm, toggles)
+  assert not ccm.experimental_mode
+
+  detector.slow_lead_detected = True
+  ccm.update(v_ego, v_ego, sm, toggles)
+
+  assert ccm.experimental_mode
+  assert ccm.status_value == CCStatus["OFF"]
+
+
+def test_ccm_respects_manual_chill_override(monkeypatch):
+  planner, _detector, ccm = make_ccm()
+  sm = make_sm()
+  toggles = make_toggles()
+  planner.params_memory.put_int("CCStatus", CCStatus["USER_CHILL"])
+
+  monkeypatch.setattr("openpilot.starpilot.controls.lib.conditional_chill_mode.time.monotonic", lambda: 1.0)
+  ccm.update(55 * CV.MPH_TO_MS, 60 * CV.MPH_TO_MS, sm, toggles)
+
+  assert not ccm.experimental_mode
+  assert ccm.status_value == CCStatus["USER_CHILL"]
+
+
+def test_ccm_restores_persisted_manual_experimental_override(monkeypatch):
+  planner, _detector, ccm = make_ccm()
+  sm = make_sm()
+  toggles = make_toggles()
+  planner.params.put_bool("PersistChillState", True)
+  planner.params.put_int("PersistedCCStatus", CCStatus["USER_EXPERIMENTAL"])
+
+  monkeypatch.setattr("openpilot.starpilot.controls.lib.conditional_chill_mode.time.monotonic", lambda: 1.0)
+  ccm.update(55 * CV.MPH_TO_MS, 60 * CV.MPH_TO_MS, sm, toggles)
+
+  assert ccm.experimental_mode
+  assert ccm.status_value == CCStatus["USER_EXPERIMENTAL"]
+  assert planner.params_memory.get_int("CCStatus") == CCStatus["USER_EXPERIMENTAL"]

selfdrive/selfdrived/selfdrived.py

@@ -593,7 +593,7 @@ class SelfdriveD:
 
     self.starpilot_events.add_from_msg(self.sm['starpilotPlan'].starpilotEvents)
 
-    if self.starpilot_toggles.conditional_experimental_mode:
+    if self.starpilot_toggles.conditional_experimental_mode or getattr(self.starpilot_toggles, "conditional_chill_mode", False):
       self.experimental_mode = self.sm['starpilotPlan'].experimentalMode
     else:
       self.experimental_mode |= self.sm['starpilotPlan'].experimentalMode

selfdrive/ui/qt/offroad/experimental_mode.cc

@@ -12,8 +12,13 @@ ExperimentalModeButton::ExperimentalModeButton(QWidget *parent) : QPushButton(pa
   chill_pixmap = QPixmap("../assets/icons/couch.svg").scaledToWidth(img_width, Qt::SmoothTransformation);
   experimental_pixmap = QPixmap("../assets/icons/experimental_grey.svg").scaledToWidth(img_width, Qt::SmoothTransformation);
 
-  // go to toggles and expand experimental mode description
-  connect(this, &QPushButton::clicked, [=]() { emit openSettings(2, "ExperimentalMode"); });
+  // go to toggles and expand whichever mode control is actually active
+  connect(this, &QPushButton::clicked, [=]() {
+    const QString toggle = params.getBool("ConditionalExperimental") ? "ConditionalExperimental" :
+                           params.getBool("ConditionalChill") ? "ConditionalChill" :
+                           "ExperimentalMode";
+    emit openSettings(2, toggle);
+  });
 
   setFixedHeight(125);
   QHBoxLayout *main_layout = new QHBoxLayout;
@@ -76,6 +81,12 @@ void ExperimentalModeButton::showEvent(QShowEvent *event) {
       status = params.getInt("PersistedCEStatus");
     }
     experimental_mode = !params.getBool("SafeMode") && status == 2;
+  } else if (params.getBool("ConditionalChill")) {
+    int status = params_memory.getInt("CCStatus");
+    if ((status != 1 && status != 2) && params.getBool("PersistChillState")) {
+      status = params.getInt("PersistedCCStatus");
+    }
+    experimental_mode = !params.getBool("SafeMode") && (status == 0 || status == 1);
   } else {
     experimental_mode = params.getBool("ExperimentalMode") && !params.getBool("SafeMode");
   }

selfdrive/ui/qt/onroad/buttons.cc

@@ -4,6 +4,16 @@
 
 #include "selfdrive/ui/qt/util.h"
 
+namespace {
+bool ccmManualOverride(int status) {
+  return status == 1 || status == 2;
+}
+
+bool cemManualOverride(int status) {
+  return status == 1 || status == 2;
+}
+}  // namespace
+
 void drawIcon(QPainter &p, const QPoint &center, const QPixmap &img, const QBrush &bg, float opacity, const int &angle) {
   p.setRenderHint(QPainter::Antialiasing);
   p.setOpacity(1.0);  // bg dictates opacity of ellipse
@@ -38,9 +48,13 @@ void ExperimentalButton::changeMode() {
   bool can_change = hasLongitudinalControl(cp) && params.getBool("ExperimentalModeConfirmed");
   if (can_change) {
     if (starpilot_toggles.value("conditional_experimental_mode").toBool()) {
-      int override_value = (starpilot_scene.conditional_status == 1 || starpilot_scene.conditional_status == 2) ? 0 : experimental_mode ? 1 : 2;
+      int override_value = cemManualOverride(starpilot_scene.conditional_status) ? 0 : experimental_mode ? 1 : 2;
       params_memory.putInt("CEStatus", override_value);
       params.putInt("PersistedCEStatus", params.getBool("PersistExperimentalState") ? override_value : 0);
+    } else if (starpilot_toggles.value("conditional_chill_mode").toBool()) {
+      int override_value = ccmManualOverride(starpilot_scene.conditional_status) ? 0 : experimental_mode ? 2 : 1;
+      params_memory.putInt("CCStatus", override_value);
+      params.putInt("PersistedCCStatus", params.getBool("PersistChillState") ? override_value : 0);
     } else {
       params.putBool("ExperimentalMode", !experimental_mode);
     }
@@ -97,6 +111,11 @@ void ExperimentalButton::showEvent(QShowEvent *event) {
 }
 
 void ExperimentalButton::updateBackgroundColor() {
+  const bool conditional_experimental_mode = starpilot_toggles.value("conditional_experimental_mode").toBool();
+  const bool conditional_chill_mode = starpilot_toggles.value("conditional_chill_mode").toBool();
+  const bool highlight_override =
+    (conditional_experimental_mode && starpilot_scene.conditional_status == 1) ||
+    (conditional_chill_mode && ccmManualOverride(starpilot_scene.conditional_status));
   if (starpilot_toggles.value("simple_mode").toBool()) {
     background_color = QColor(0, 0, 0, 166);
   } else if (isDown() || !engageable) {
@@ -105,7 +124,7 @@ void ExperimentalButton::updateBackgroundColor() {
     background_color = bg_colors[STATUS_SWITCHBACK_MODE_ENABLED];
   } else if (starpilot_scene.always_on_lateral_active) {
     background_color = bg_colors[STATUS_ALWAYS_ON_LATERAL_ACTIVE];
-  } else if (starpilot_scene.conditional_status == 1) {
+  } else if (highlight_override) {
     background_color = bg_colors[STATUS_CEM_DISABLED];
   } else if (experimental_mode) {
     background_color = bg_colors[STATUS_EXPERIMENTAL_MODE_ENABLED];

selfdrive/ui/qt/onroad/onroad_home.cc

@@ -59,13 +59,19 @@ void OnroadWindow::updateState(const UIState &s, const StarPilotUIState &fs) {
   nvg->updateState(s, fs);
 
   const StarPilotUIScene &starpilot_scene = fs.starpilot_scene;
+  const QJsonObject &starpilot_toggles = starpilot_scene.starpilot_toggles;
   const auto selfdriveState = (*s.sm)["selfdriveState"].getSelfdriveState();
   QColor bgColor = bg_colors[s.status];
+  const bool conditional_experimental_mode = starpilot_toggles.value("conditional_experimental_mode").toBool();
+  const bool conditional_chill_mode = starpilot_toggles.value("conditional_chill_mode").toBool();
+  const bool highlight_override =
+    (conditional_experimental_mode && starpilot_scene.conditional_status == 1) ||
+    (conditional_chill_mode && (starpilot_scene.conditional_status == 1 || starpilot_scene.conditional_status == 2));
   if (starpilot_scene.switchback_mode_enabled && (selfdriveState.getEnabled() || starpilot_scene.always_on_lateral_active)) {
     bgColor = bg_colors[STATUS_SWITCHBACK_MODE_ENABLED];
   } else if (starpilot_scene.always_on_lateral_active) {
     bgColor = bg_colors[STATUS_ALWAYS_ON_LATERAL_ACTIVE];
-  } else if (starpilot_scene.conditional_status == 1) {
+  } else if (highlight_override) {
     bgColor = bg_colors[STATUS_CEM_DISABLED];
   } else if (selfdriveState.getExperimentalMode()) {
     bgColor = bg_colors[STATUS_EXPERIMENTAL_MODE_ENABLED];
@@ -78,9 +84,6 @@ void OnroadWindow::updateState(const UIState &s, const StarPilotUIState &fs) {
     bg = bgColor;
     update();
   }
-
-  const QJsonObject &starpilot_toggles = starpilot_scene.starpilot_toggles;
-
   starpilot_nvg->alertHeight = alerts->alertHeight;
 
   starpilot_onroad->bg = bg;

selfdrive/ui/qt/widgets/controls.cc

@@ -139,6 +139,11 @@ void ParamControl::toggleClicked(bool state) {
   if (!confirm || confirmed || !state || do_confirm()) {
     if (store_confirm && state) params.putBool(key + "Confirmed", true);
     params.putBool(key, state);
+    if (state && key == "ConditionalExperimental") {
+      params.putBool("ConditionalChill", false);
+    } else if (state && key == "ConditionalChill") {
+      params.putBool("ConditionalExperimental", false);
+    }
     if (key == "PersistExperimentalState") {
       static Params params_memory{"", true};
       int persisted_status = 0;
@@ -147,6 +152,14 @@ void ParamControl::toggleClicked(bool state) {
         persisted_status = (current_status == 1 || current_status == 2) ? current_status : 0;
       }
       params.putInt("PersistedCEStatus", persisted_status);
+    } else if (key == "PersistChillState") {
+      static Params params_memory{"", true};
+      int persisted_status = 0;
+      if (state) {
+        int current_status = params_memory.getInt("CCStatus");
+        persisted_status = (current_status == 1 || current_status == 2) ? current_status : 0;
+      }
+      params.putInt("PersistedCCStatus", persisted_status);
     }
     setIcon(state);
   } else {