From 84f39f316c023f323fb6b3bbc2f448f0a80cd7ab Mon Sep 17 00:00:00 2001 From: Rick Lan Date: Mon, 12 May 2025 13:12:53 +0800 Subject: [PATCH] LCA - 2025-06-13 --- common/params_keys.h | 2 ++ selfdrive/controls/lib/desire_helper.py | 18 ++++++++++++++++-- selfdrive/modeld/modeld.py | 4 +++- selfdrive/ui/qt/offroad/dp_panel.cc | 7 +++++++ selfdrive/ui/qt/offroad/dp_panel.h | 2 ++ system/manager/manager.py | 2 ++ 6 files changed, 32 insertions(+), 3 deletions(-) diff --git a/common/params_keys.h b/common/params_keys.h index 92853f92f..872364869 100644 --- a/common/params_keys.h +++ b/common/params_keys.h @@ -122,4 +122,6 @@ inline static std::unordered_map 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}, }; diff --git a/selfdrive/controls/lib/desire_helper.py b/selfdrive/controls/lib/desire_helper.py index 90b685864..5075be1d6 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 +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 diff --git a/selfdrive/modeld/modeld.py b/selfdrive/modeld/modeld.py index 1e3d1782e..25e0492d4 100755 --- a/selfdrive/modeld/modeld.py +++ b/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 diff --git a/selfdrive/ui/qt/offroad/dp_panel.cc b/selfdrive/ui/qt/offroad/dp_panel.cc index 548b02be5..bc6550e07 100644 --- a/selfdrive/ui/qt/offroad/dp_panel.cc +++ b/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); } diff --git a/selfdrive/ui/qt/offroad/dp_panel.h b/selfdrive/ui/qt/offroad/dp_panel.h index 87095c73b..2e5889fe1 100644 --- a/selfdrive/ui/qt/offroad/dp_panel.h +++ b/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; }; diff --git a/system/manager/manager.py b/system/manager/manager.py index 5dc08c6d3..9a97104bc 100755 --- a/system/manager/manager.py +++ b/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"):