LCA - 2025-06-13

common/params_keys.h

@@ -122,4 +122,6 @@ inline static std::unordered_map<std::string, uint32_t> keys = {
     {"Version", PERSISTENT},
     {"dp_device_last_log", CLEAR_ON_ONROAD_TRANSITION},
     {"dp_device_reset_conf", CLEAR_ON_MANAGER_START},
+    {"dp_lat_lca_speed", PERSISTENT},
+    {"dp_lat_lca_auto_sec", PERSISTENT},
 };

selfdrive/controls/lib/desire_helper.py

@@ -1,6 +1,7 @@
 from cereal import log
 from openpilot.common.conversions import Conversions as CV
 from openpilot.common.realtime import DT_MDL
+import time
 
 LaneChangeState = log.LaneChangeState
 LaneChangeDirection = log.LaneChangeDirection
@@ -31,7 +32,7 @@ DESIRES = {
 
 
 class DesireHelper:
-  def __init__(self):
+  def __init__(self, dp_lat_lca_speed=LANE_CHANGE_SPEED_MIN, dp_lat_lca_auto_sec=0.):
     self.lane_change_state = LaneChangeState.off
     self.lane_change_direction = LaneChangeDirection.none
     self.lane_change_timer = 0.0
@@ -39,11 +40,14 @@ class DesireHelper:
     self.keep_pulse_timer = 0.0
     self.prev_one_blinker = False
     self.desire = log.Desire.none
+    self.dp_lat_lca_speed = float(dp_lat_lca_speed * CV.MPH_TO_MS)
+    self.dp_lat_lca_auto_sec = dp_lat_lca_auto_sec
+    self.dp_lat_lca_auto_sec_start = 0.
 
   def update(self, carstate, lateral_active, lane_change_prob):
     v_ego = carstate.vEgo
     one_blinker = carstate.leftBlinker != carstate.rightBlinker
-    below_lane_change_speed = v_ego < LANE_CHANGE_SPEED_MIN
+    below_lane_change_speed = True if self.dp_lat_lca_speed == 0. else v_ego < self.dp_lat_lca_speed
 
     if not lateral_active or self.lane_change_timer > LANE_CHANGE_TIME_MAX:
       self.lane_change_state = LaneChangeState.off
@@ -53,6 +57,8 @@ class DesireHelper:
       if self.lane_change_state == LaneChangeState.off and one_blinker and not self.prev_one_blinker and not below_lane_change_speed:
         self.lane_change_state = LaneChangeState.preLaneChange
         self.lane_change_ll_prob = 1.0
+        if self.dp_lat_lca_auto_sec > 0.:
+          self.dp_lat_lca_auto_sec_start = time.time()
 
       # LaneChangeState.preLaneChange
       elif self.lane_change_state == LaneChangeState.preLaneChange:
@@ -67,6 +73,14 @@ class DesireHelper:
         blindspot_detected = ((carstate.leftBlindspot and self.lane_change_direction == LaneChangeDirection.left) or
                               (carstate.rightBlindspot and self.lane_change_direction == LaneChangeDirection.right))
 
+        # reset timer
+        if self.dp_lat_lca_auto_sec > 0.:
+          if blindspot_detected:
+            self.dp_lat_lca_auto_sec_start = time.time()
+          else:
+            if (time.time() - self.dp_lat_lca_auto_sec_start) >= self.dp_lat_lca_auto_sec:
+              torque_applied = True
+
         if not one_blinker or below_lane_change_speed:
           self.lane_change_state = LaneChangeState.off
           self.lane_change_direction = LaneChangeDirection.none

selfdrive/modeld/modeld.py

@@ -259,7 +259,9 @@ def main(demo=False):
   long_delay = CP.longitudinalActuatorDelay + LONG_SMOOTH_SECONDS
   prev_action = log.ModelDataV2.Action()
 
-  DH = DesireHelper()
+  dp_lat_lca_speed = int(params.get("dp_lat_lca_speed"))
+  dp_lat_lca_auto_sec = float(params.get("dp_lat_lca_auto_sec"))
+  DH = DesireHelper(dp_lat_lca_speed=dp_lat_lca_speed, dp_lat_lca_auto_sec=dp_lat_lca_auto_sec)
 
   while True:
     # Keep receiving frames until we are at least 1 frame ahead of previous extra frame

selfdrive/ui/qt/offroad/dp_panel.cc

@@ -107,6 +107,8 @@ void DPPanel::add_lateral_toggles() {
       "",
     },
   };
+  auto lca_speed_toggle = new ParamSpinBoxControl("dp_lat_lca_speed", tr("LCA Speed:"), tr("Off = Disable LCA\n1 mph ≈ 1.2 km/h"), "", 0, 100, 5, tr(" mph"), tr("Off"));
+  lca_sec_toggle = new ParamDoubleSpinBoxControl("dp_lat_lca_auto_sec", QString::fromUtf8("　") + tr("Auto Lane Change after:"), tr("Off = Disable Auto Lane Change."), "", 0, 5.0, 0.5, tr(" sec"), tr("Off"));
 
   QWidget *label = nullptr;
   bool has_toggle = false;
@@ -115,6 +117,9 @@ void DPPanel::add_lateral_toggles() {
     if (param.isEmpty()) {
       label = new LabelControl(title, "");
       addItem(label);
+      addItem(lca_speed_toggle);
+      addItem(lca_sec_toggle);
+      has_toggle = true;
       continue;
     }
 
@@ -281,12 +286,14 @@ void DPPanel::showEvent(QShowEvent *event) {
 
 void DPPanel::updateStates() {
   // do fs_watch here
+  fs_watch->addParam("dp_lat_lca_speed");
 
   if (!isVisible()) {
     return;
   }
 
   // do state change logic here
+  lca_sec_toggle->setVisible(std::atoi(params.get("dp_lat_lca_speed").c_str()) > 0);
 
 }
 

selfdrive/ui/qt/offroad/dp_panel.h

@@ -28,4 +28,6 @@ private:
   void add_device_toggles();
   void updateStates();
   void showEvent(QShowEvent *event) override;
+
+  ParamDoubleSpinBoxControl* lca_sec_toggle;
 };

system/manager/manager.py

@@ -40,6 +40,8 @@ def manager_init() -> None:
     ("OpenpilotEnabledToggle", "1"),
     ("LongitudinalPersonality", str(log.LongitudinalPersonality.standard)),
     ("DisableLogging", "0"),
+    ("dp_lat_lca_speed", "20"),
+    ("dp_lat_lca_auto_sec", "0"),
   ]
 
   if params.get_bool("RecordFrontLock"):