lane change assist controller

common/params.cc

@@ -226,6 +226,8 @@ std::unordered_map<std::string, uint32_t> keys = {
     {"dp_long_alt_driving_personality_mode", PERSISTENT},
     {"dp_long_alt_driving_personality_speed", PERSISTENT},
     {"dp_lat_lane_change_assist_mode", PERSISTENT},
+    {"dp_lat_lane_change_assist_speed", PERSISTENT},
+    {"dp_lat_lane_change_assist_auto_timer", PERSISTENT},
 
     {"dp_nav_avoid_toll", PERSISTENT},
     {"dp_nav_avoid_highway", PERSISTENT},

selfdrive/controls/controlsd.py

@@ -7,7 +7,7 @@ from typing import SupportsFloat
 
 import cereal.messaging as messaging
 
-from cereal import car, log
+from cereal import car, log, custom
 from msgq.visionipc import VisionIpcClient, VisionStreamType
 
 
@@ -65,6 +65,8 @@ class Controls:
     # dp
     self._dp_alka = self.params.get_bool("dp_alka")
     self._dp_alka_active = True
+    self._dp_lat_lane_change_assist_mode = int(self.params.get("dp_lat_lane_change_assist_mode"))
+    self._dp_lat_lane_change_assist_mode_disable_active = False
 
     if CI is None:
       cloudlog.info("controlsd is waiting for CarParams")
@@ -262,7 +264,9 @@ class Controls:
         self.events.add(EventName.calibrationInvalid)
 
     # Handle lane change
-    if self.sm['modelV2'].meta.laneChangeState == LaneChangeState.preLaneChange:
+    if self._dp_lat_lane_change_assist_mode in [custom.LaneChangeAssistMode.disable, custom.LaneChangeAssistMode.hold]:
+      pass
+    elif self.sm['modelV2'].meta.laneChangeState == LaneChangeState.preLaneChange:
       direction = self.sm['modelV2'].meta.laneChangeDirection
       if (CS.leftBlindspot and direction == LaneChangeDirection.left) or \
          (CS.rightBlindspot and direction == LaneChangeDirection.right):
@@ -577,6 +581,20 @@ class Controls:
     if CS.leftBlinker or CS.rightBlinker:
       self.last_blinker_frame = self.sm.frame
 
+    # dp alc - disable
+    if self._dp_lat_lane_change_assist_mode == custom.LaneChangeAssistMode.disable:
+      # keep the state until blinker is off
+      if not (CS.leftBlinker and CS.rightBlinker):
+        self._dp_lat_lane_change_assist_mode_disable_active = False
+
+      if CS.steeringPressed and \
+              ((CS.steeringTorque > 0 and CS.leftBlinker) or
+               (CS.steeringTorque < 0 and CS.rightBlinker)):
+        self._dp_lat_lane_change_assist_mode_disable_active = True
+
+      if self._dp_lat_lane_change_assist_mode_disable_active:
+        CC.latActive = False
+
     # State specific actions
 
     if not CC.latActive:

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
+from dp_ext.selfdrive.controls.lib.lane_change_assist_controller import LaneChangeAssistController
 
 LaneChangeState = log.LaneChangeState
 LaneChangeDirection = log.LaneChangeDirection
@@ -40,10 +41,13 @@ class DesireHelper:
     self.prev_one_blinker = False
     self.desire = log.Desire.none
 
+    # dp
+    self.lca_controller = LaneChangeAssistController()
+
   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 = self.lca_controller.get_below_lane_change_speed(v_ego)
 
     if not lateral_active or self.lane_change_timer > LANE_CHANGE_TIME_MAX:
       self.lane_change_state = LaneChangeState.off
@@ -54,6 +58,8 @@ class DesireHelper:
         self.lane_change_state = LaneChangeState.preLaneChange
         self.lane_change_ll_prob = 1.0
 
+        self.lca_controller.update_off()
+
       # LaneChangeState.preLaneChange
       elif self.lane_change_state == LaneChangeState.preLaneChange:
         # Set lane change direction
@@ -67,6 +73,10 @@ class DesireHelper:
         blindspot_detected = ((carstate.leftBlindspot and self.lane_change_direction == LaneChangeDirection.left) or
                               (carstate.rightBlindspot and self.lane_change_direction == LaneChangeDirection.right))
 
+        # dp
+        self.lca_controller.update_pre_change(blindspot_detected)
+        torque_applied = self.lca_controller.get_torque_applied(torque_applied)
+
         if not one_blinker or below_lane_change_speed:
           self.lane_change_state = LaneChangeState.off
           self.lane_change_direction = LaneChangeDirection.none
@@ -94,6 +104,9 @@ class DesireHelper:
           else:
             self.lane_change_state = LaneChangeState.off
 
+        # dp
+        self.lca_controller.update_finishing()
+
     if self.lane_change_state in (LaneChangeState.off, LaneChangeState.preLaneChange):
       self.lane_change_timer = 0.0
     else:

system/manager/manager.py

@@ -64,6 +64,9 @@ def manager_init() -> None:
     ("dp_long_alt_driving_personality_mode", "0"),
     ("dp_long_alt_driving_personality_speed", "0"),
     ("dp_long_curve_speed_limiter", "0"),
+    ("dp_lat_lane_change_assist_mode", "0"),
+    ("dp_lat_lane_change_assist_speed", "32"),
+    ("dp_lat_lane_change_assist_auto_timer", "1.5"),
   ]
   if not PC:
     default_params.append(("LastUpdateTime", datetime.datetime.now(datetime.UTC).replace(tzinfo=None).isoformat().encode('utf8')))