From 2e79a63651109ce2b7a8f23f3c6f463ab6b64b04 Mon Sep 17 00:00:00 2001 From: Rick Lan Date: Sun, 23 Jun 2024 21:48:42 +0800 Subject: [PATCH] lane change assist controller --- common/params.cc | 2 ++ selfdrive/controls/controlsd.py | 22 ++++++++++++++++++++-- selfdrive/controls/lib/desire_helper.py | 15 ++++++++++++++- system/manager/manager.py | 3 +++ 4 files changed, 39 insertions(+), 3 deletions(-) diff --git a/common/params.cc b/common/params.cc index eaaba5475..7e8b30689 100644 --- a/common/params.cc +++ b/common/params.cc @@ -226,6 +226,8 @@ std::unordered_map 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}, diff --git a/selfdrive/controls/controlsd.py b/selfdrive/controls/controlsd.py index 317184943..9f82a5e0b 100755 --- a/selfdrive/controls/controlsd.py +++ b/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: diff --git a/selfdrive/controls/lib/desire_helper.py b/selfdrive/controls/lib/desire_helper.py index 90b685864..3a98043bf 100644 --- a/selfdrive/controls/lib/desire_helper.py +++ b/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: diff --git a/system/manager/manager.py b/system/manager/manager.py index ff2c8051e..4f8318b6b 100755 --- a/system/manager/manager.py +++ b/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')))