From 84a7b7b4e1c3f8b9ff493c5016998dc812ee48bb Mon Sep 17 00:00:00 2001 From: Rick Lan Date: Sat, 20 Jul 2024 09:06:48 +0800 Subject: [PATCH 1/6] better brake hold --- selfdrive/car/toyota/interface.py | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/selfdrive/car/toyota/interface.py b/selfdrive/car/toyota/interface.py index a8a68f1e01cf4d..a7cf87bab82ed4 100644 --- a/selfdrive/car/toyota/interface.py +++ b/selfdrive/car/toyota/interface.py @@ -153,7 +153,7 @@ def _get_params(ret, candidate, fingerprint, car_fw, experimental_long, docs): if dp_toyota_pcm_compensation: # on stock Toyota this is -2.5 if candidate in TSS2_CAR: - ret.stopAccel = -1.2 + ret.stopAccel = -1.6 else: ret.stopAccel = -2.0 From 6234b40f768204c8aa81d2df8208d9db974269c8 Mon Sep 17 00:00:00 2001 From: Rick Lan Date: Sat, 20 Jul 2024 09:41:09 +0800 Subject: [PATCH 2/6] moved to device --- CHANGELOGS.md | 2 +- selfdrive/ui/qt/offroad/settings.cc | 11 +++++++++++ selfdrive/ui/qt/offroad/software_settings.cc | 11 ----------- 3 files changed, 12 insertions(+), 12 deletions(-) diff --git a/CHANGELOGS.md b/CHANGELOGS.md index 251bc53708439a..416b2592a7a01a 100644 --- a/CHANGELOGS.md +++ b/CHANGELOGS.md @@ -7,7 +7,7 @@ dragonpilot [branch] [latest] * [NEW] [TESTING] Personalized Accel Learner (PAL) - Learn and apply accel habits from driver. * Ability to set a launch boost for initial acceleration * Ability to freeze / reset learned parameters - * [NEW] Delete logs button (Software Panel) + * [NEW] Delete logs button (Device Panel) * [NEW] [VAG] Patch for Modified PQ Platform 　　 　　 diff --git a/selfdrive/ui/qt/offroad/settings.cc b/selfdrive/ui/qt/offroad/settings.cc index 7d5cd6c1405a00..14bc00e5b5fbc5 100644 --- a/selfdrive/ui/qt/offroad/settings.cc +++ b/selfdrive/ui/qt/offroad/settings.cc @@ -208,6 +208,17 @@ DevicePanel::DevicePanel(SettingsWindow *parent) : ListWidget(parent) { }); addItem(pair_device); + // dp delete logs btn + auto dp_delete_logs_btn = new ButtonControl(tr("Delete All Logs"), tr("DELETE")); + connect(dp_delete_logs_btn, &ButtonControl::clicked, [&]() { + if (ConfirmationDialog::confirm(tr("Are you sure you want to delete all logs?"), tr("DELETE"), this)) { + #ifdef QCOM2 + std::system("rm -fr /data/media/0/realdata/*"); + #endif + } + }); + addItem(dp_delete_logs_btn); + // offroad-only buttons if (!params.getBool("dp_device_dm_unavailable")) { auto dcamBtn = new ButtonControl(tr("Driver Camera"), tr("PREVIEW"), diff --git a/selfdrive/ui/qt/offroad/software_settings.cc b/selfdrive/ui/qt/offroad/software_settings.cc index da18c8d9c2c803..e999ee3c2fe654 100644 --- a/selfdrive/ui/qt/offroad/software_settings.cc +++ b/selfdrive/ui/qt/offroad/software_settings.cc @@ -29,17 +29,6 @@ SoftwarePanel::SoftwarePanel(QWidget* parent) : ListWidget(parent) { versionLbl = new LabelControl(tr("Current Version"), ""); addItem(versionLbl); - // dp delete logs btn - auto dp_delete_logs_btn = new ButtonControl(tr("Delete All Logs"), tr("DELETE")); - connect(dp_delete_logs_btn, &ButtonControl::clicked, [&]() { - if (ConfirmationDialog::confirm(tr("Are you sure you want to delete all logs?"), tr("DELETE"), this)) { - #ifdef QCOM2 - std::system("rm -fr /data/media/0/realdata/*"); - #endif - } - }); - addItem(dp_delete_logs_btn); - // dp on/off btn onOffBtn = new ButtonControl(tr("Onroad/Offroad Mode"), tr("Go Offroad")); connect(onOffBtn, &ButtonControl::clicked, [&]() { From 52523732750aede6ef61859f64f3bc2c43a13af8 Mon Sep 17 00:00:00 2001 From: Rick Lan Date: Mon, 22 Jul 2024 10:29:37 +0800 Subject: [PATCH 3/6] lane priority mode --- CHANGELOGS.md | 3 ++ SConstruct | 1 + cereal/log.capnp | 2 +- cereal/services.py | 1 + common/params.cc | 3 ++ selfdrive/controls/controlsd.py | 21 ++++++++++++-- selfdrive/controls/lib/drive_helpers.py | 33 ++++++++++++++++++++++ selfdrive/ui/qt/onroad/annotated_camera.cc | 7 ++++- selfdrive/ui/qt/onroad/annotated_camera.h | 1 + selfdrive/ui/ui.cc | 2 ++ system/manager/manager.py | 3 ++ system/manager/process_config.py | 4 +++ 12 files changed, 76 insertions(+), 5 deletions(-) diff --git a/CHANGELOGS.md b/CHANGELOGS.md index 416b2592a7a01a..b6a3e7fab5e601 100644 --- a/CHANGELOGS.md +++ b/CHANGELOGS.md @@ -9,6 +9,9 @@ dragonpilot [branch] [latest] * Ability to freeze / reset learned parameters * [NEW] Delete logs button (Device Panel) * [NEW] [VAG] Patch for Modified PQ Platform + * [NEW] Lane Priority Mode + * Ability to use Lane Planner for lane centering instead of end to end (when lane probs are high, logic by sunnyhaibin). + * Ability to set speed limit and only activate at high speed (e.g. highway use only) 　　 　　 =============== diff --git a/SConstruct b/SConstruct index 7994c1a51e2418..82bffaae7c0252 100644 --- a/SConstruct +++ b/SConstruct @@ -394,6 +394,7 @@ if arch == "larch64": SConscript(['third_party/SConscript']) SConscript(['selfdrive/SConscript']) +SConscript(['dp_ext/selfdrive/controls/lib/lateral_mpc_lib/SConscript']) if Dir('#tools/cabana/').exists() and GetOption('extras'): SConscript(['tools/replay/SConscript']) diff --git a/cereal/log.capnp b/cereal/log.capnp index ee047f00e8f3ae..e4f7e0017b4bca 100644 --- a/cereal/log.capnp +++ b/cereal/log.capnp @@ -2352,7 +2352,7 @@ struct Event { controlsStateExt @107 :Custom.ControlsStateExt; longitudinalPlanExt @108 :Custom.LongitudinalPlanExt; teToo @109 :Custom.TeToo; - customReserved3 @110 :Custom.CustomReserved3; + lateralPlan @110 :LateralPlan; customReserved4 @111 :Custom.CustomReserved4; customReserved5 @112 :Custom.CustomReserved5; customReserved6 @113 :Custom.CustomReserved6; diff --git a/cereal/services.py b/cereal/services.py index 98925bdcd60bac..b34c7294c54bb4 100755 --- a/cereal/services.py +++ b/cereal/services.py @@ -92,6 +92,7 @@ def __init__(self, should_log: bool, frequency: float, decimation: Optional[int] "controlsStateExt": (False, 100., 10), "longitudinalPlanExt": (False, 20., 5), "teToo": (False, 5), + "lateralPlan": (False, 20., 5), } SERVICE_LIST = {name: Service(*vals) for idx, (name, vals) in enumerate(_services.items())} diff --git a/common/params.cc b/common/params.cc index 47c8b4f8fa60e4..ec9bd6da365f6d 100644 --- a/common/params.cc +++ b/common/params.cc @@ -252,6 +252,9 @@ std::unordered_map keys = { {"dp_long_pal_reset", CLEAR_ON_MANAGER_START}, {"dp_long_pal_launch_boost", PERSISTENT}, {"dp_vag_pq_steering_patch", PERSISTENT}, + {"dp_lat_lane_priority_mode", PERSISTENT}, + {"dp_lat_lane_priority_mode_speed", PERSISTENT}, + {"dp_lat_lane_priority_mode_camera_offset", PERSISTENT}, }; } // namespace diff --git a/selfdrive/controls/controlsd.py b/selfdrive/controls/controlsd.py index 99771dda4391ec..6853b667155d23 100755 --- a/selfdrive/controls/controlsd.py +++ b/selfdrive/controls/controlsd.py @@ -20,7 +20,7 @@ from openpilot.selfdrive.car.car_helpers import get_car_interface, get_startup_event from openpilot.selfdrive.controls.lib.alertmanager import AlertManager, set_offroad_alert -from openpilot.selfdrive.controls.lib.drive_helpers import VCruiseHelper, clip_curvature +from openpilot.selfdrive.controls.lib.drive_helpers import VCruiseHelper, clip_curvature, get_lag_adjusted_curvature from openpilot.selfdrive.controls.lib.events import Events, ET from openpilot.selfdrive.controls.lib.latcontrol import LatControl, MIN_LATERAL_CONTROL_SPEED from openpilot.selfdrive.controls.lib.latcontrol_pid import LatControlPID @@ -77,6 +77,12 @@ def __init__(self, CI=None): self._dp_device_dm_unavailable = self.params.get_bool("dp_device_dm_unavailable") except: self._dp_device_dm_unavailable = False + + try: + self._dp_lat_lane_priority_mode = self.params.get_bool("dp_lat_lane_priority_mode") + except (ValueError, TypeError): + self._dp_lat_lane_priority_mode = False + self._dp_device_dm_unavailable_active = True if self._dp_device_dm_unavailable else False if CI is None: @@ -112,9 +118,14 @@ def __init__(self, CI=None): if REPLAY: # no vipc in replay will make them ignored anyways ignore += ['roadCameraState', 'wideRoadCameraState'] + # dp + if not self._dp_lat_lane_priority_mode: + ignore += ['lateralPlan'] + self.sm = messaging.SubMaster(['deviceState', 'pandaStates', 'peripheralState', 'modelV2', 'liveCalibration', 'carOutput', 'driverMonitoringState', 'longitudinalPlan', 'liveLocationKalman', 'managerState', 'liveParameters', 'radarState', 'liveTorqueParameters', + "lateralPlan", # dp - lane priority mode 'testJoystick'] + self.camera_packets + self.sensor_packets, ignore_alive=ignore, ignore_avg_freq=ignore+['radarState', 'testJoystick'], ignore_valid=['testJoystick', ], frequency=int(1/DT_CTRL)) @@ -626,8 +637,12 @@ def state_control(self, CS): pid_accel_limits = self.CI.get_pid_accel_limits(self.CP, CS.vEgo, self.v_cruise_helper.v_cruise_kph * CV.KPH_TO_MS) actuators.accel = self.LoC.update(CC.longActive, CS, long_plan.aTarget, long_plan.shouldStop, pid_accel_limits) - # Steering PID loop and lateral MPC - self.desired_curvature = clip_curvature(CS.vEgo, self.desired_curvature, model_v2.action.desiredCurvature) + # dp - lane line priority mode + if self._dp_lat_lane_priority_mode and self.sm['lateralPlan'].useLaneLines: + self.desired_curvature, _ = get_lag_adjusted_curvature(self.CP, CS.vEgo, self.sm['lateralPlan'].psis, self.sm['lateralPlan'].curvatures) + else: + # Steering PID loop and lateral MPC + self.desired_curvature = clip_curvature(CS.vEgo, self.desired_curvature, model_v2.action.desiredCurvature) actuators.curvature = self.desired_curvature actuators.steer, actuators.steeringAngleDeg, lac_log = self.LaC.update(CC.latActive, CS, self.VM, lp, self.steer_limited, self.desired_curvature, diff --git a/selfdrive/controls/lib/drive_helpers.py b/selfdrive/controls/lib/drive_helpers.py index df9be8d7d4923d..624a7e718bd015 100644 --- a/selfdrive/controls/lib/drive_helpers.py +++ b/selfdrive/controls/lib/drive_helpers.py @@ -178,3 +178,36 @@ def get_speed_error(modelV2: log.ModelDataV2, v_ego: float) -> float: vel_err = clip(modelV2.temporalPose.trans[0] - v_ego, -MAX_VEL_ERR, MAX_VEL_ERR) return float(vel_err) return 0.0 + +# dp - for lat priority mode +from openpilot.selfdrive.modeld.constants import ModelConstants +from openpilot.common.realtime import DT_MDL +def get_lag_adjusted_curvature(CP, v_ego, psis, curvatures): #, curvature_rates): + if len(psis) != CONTROL_N: + psis = [0.0]*CONTROL_N + curvatures = [0.0]*CONTROL_N + # curvature_rates = [0.0]*CONTROL_N + v_ego = max(MIN_SPEED, v_ego) + + # TODO this needs more thought, use .2s extra for now to estimate other delays + delay = CP.steerActuatorDelay + .2 + + # MPC can plan to turn the wheel and turn back before t_delay. This means + # in high delay cases some corrections never even get commanded. So just use + # psi to calculate a simple linearization of desired curvature + current_curvature_desired = curvatures[0] + psi = interp(delay, ModelConstants.T_IDXS[:CONTROL_N], psis) + average_curvature_desired = psi / (v_ego * delay) + desired_curvature = 2 * average_curvature_desired - current_curvature_desired + + # This is the "desired rate of the setpoint" not an actual desired rate + # desired_curvature_rate = curvature_rates[0] + max_curvature_rate = MAX_LATERAL_JERK / (v_ego**2) # inexact calculation, check https://github.com/commaai/openpilot/pull/24755 + # safe_desired_curvature_rate = clip(desired_curvature_rate, + # -max_curvature_rate, + # max_curvature_rate) + safe_desired_curvature = clip(desired_curvature, + current_curvature_desired - max_curvature_rate * DT_MDL, + current_curvature_desired + max_curvature_rate * DT_MDL) + + return safe_desired_curvature#, safe_desired_curvature_rate \ No newline at end of file diff --git a/selfdrive/ui/qt/onroad/annotated_camera.cc b/selfdrive/ui/qt/onroad/annotated_camera.cc index 1ba55270244112..d2c1ab273687d5 100644 --- a/selfdrive/ui/qt/onroad/annotated_camera.cc +++ b/selfdrive/ui/qt/onroad/annotated_camera.cc @@ -98,6 +98,10 @@ void AnnotatedCameraWidget::updateState(const UIState &s) { chevron_ext->update_states(s, is_metric); tt_indicator->update_states(s); #endif + if (sm.updated("lateralPlan")) { + auto lat_plan = sm["lateralPlan"].getLateralPlan(); + dp_lat_lane_priority_mode_active = lat_plan.getUseLaneLines(); + } } void AnnotatedCameraWidget::drawHud(QPainter &p) { @@ -201,7 +205,8 @@ void AnnotatedCameraWidget::drawLaneLines(QPainter &painter, const UIState *s) { // lanelines for (int i = 0; i < std::size(scene.lane_line_vertices); ++i) { - painter.setBrush(QColor::fromRgbF(1.0, 1.0, 1.0, std::clamp(scene.lane_line_probs[i], 0.0, 0.7))); + bool green = dp_lat_lane_priority_mode_active && (i == 1 || i == 2); + painter.setBrush(QColor::fromRgbF((green? 0.0 : 1.0), 1.0, (green? 0.0 : 1.0), std::clamp(scene.lane_line_probs[i], 0.0, 0.7))); painter.drawPolygon(scene.lane_line_vertices[i]); } diff --git a/selfdrive/ui/qt/onroad/annotated_camera.h b/selfdrive/ui/qt/onroad/annotated_camera.h index 921848caf8c625..668e605e13184d 100644 --- a/selfdrive/ui/qt/onroad/annotated_camera.h +++ b/selfdrive/ui/qt/onroad/annotated_camera.h @@ -47,6 +47,7 @@ class AnnotatedCameraWidget : public CameraWidget { ChevronExt *chevron_ext; TeTooIndicator *tt_indicator; #endif + bool dp_lat_lane_priority_mode_active = false; protected: void paintGL() override; diff --git a/selfdrive/ui/ui.cc b/selfdrive/ui/ui.cc index 8673ab9fa9fb43..1732b72c414167 100644 --- a/selfdrive/ui/ui.cc +++ b/selfdrive/ui/ui.cc @@ -254,6 +254,8 @@ UIState::UIState(QObject *parent) : QObject(parent) { "longitudinalPlanExt", // TeToo "teToo", + // lane priority mode + "lateralPlan", }); Params params; diff --git a/system/manager/manager.py b/system/manager/manager.py index 41f3b8e515c27b..930c05a0bc3566 100755 --- a/system/manager/manager.py +++ b/system/manager/manager.py @@ -88,6 +88,9 @@ def manager_init() -> None: ("dp_long_pal_freeze", "0"), ("dp_long_pal_launch_boost", "0"), ("dp_vag_pq_steering_patch", "0"), + ("dp_lat_lane_priority_mode", "0"), + ("dp_lat_lane_priority_mode_speed", "0"), + ("dp_lat_lane_priority_mode_camera_offset", "4"), ] if not PC: default_params.append(("LastUpdateTime", datetime.datetime.now(datetime.UTC).replace(tzinfo=None).isoformat().encode('utf8'))) diff --git a/system/manager/process_config.py b/system/manager/process_config.py index 3336f2a21f3aa8..5be7d9ca441f59 100644 --- a/system/manager/process_config.py +++ b/system/manager/process_config.py @@ -56,6 +56,9 @@ def dpdmonitoringd(started, params, CP: car.CarParams) -> bool: def tetood(started, params, CP: car.CarParams) -> bool: return started and (params.get_bool("dp_tetoo") or params.get_bool("dp_tetoo_speed_camera_taiwan")) +def latpland(started, params, CP: car.CarParams) -> bool: + return started and params.get_bool("dp_lat_lane_priority_mode") + procs = [ DaemonProcess("manage_athenad", "system.athena.manage_athenad", "AthenadPid"), @@ -104,6 +107,7 @@ def tetood(started, params, CP: car.CarParams) -> bool: #dp PythonProcess("dpdmonitoringd", "dp_ext.selfdrive.monitoring.dmonitoringd", dpdmonitoringd, enabled=not PC), NativeProcess("tetood", "dp_ext/selfdrive/tetood", ["./tetood"], tetood), + PythonProcess("latpland", "dp_ext.selfdrive.controls.latpland", latpland), ] managed_processes = {p.name: p for p in procs} From bbcb87f4b9be8f908093d59a7e2fec18b4576e5f Mon Sep 17 00:00:00 2001 From: Rick Lan Date: Mon, 22 Jul 2024 10:38:36 +0800 Subject: [PATCH 4/6] update ref --- dp_ext | 2 +- dp_priv | 2 +- 2 files changed, 2 insertions(+), 2 deletions(-) diff --git a/dp_ext b/dp_ext index a7c7eb8f1e8326..f157795ace4edd 160000 --- a/dp_ext +++ b/dp_ext @@ -1 +1 @@ -Subproject commit a7c7eb8f1e8326f07e6ff20340b2cec47994f19b +Subproject commit f157795ace4eddef22199778ffc85c2e5f49a4ec diff --git a/dp_priv b/dp_priv index bae5f90f961dbc..0416f9aeb05281 160000 --- a/dp_priv +++ b/dp_priv @@ -1 +1 @@ -Subproject commit bae5f90f961dbc72c784a01ebc368f13e52f3b63 +Subproject commit 0416f9aeb05281a4f5b24432e4ec7c2d038e1d33 From af242805fd27ad93c81ca42f346e6acc7ea46cf9 Mon Sep 17 00:00:00 2001 From: Rick Lan Date: Mon, 22 Jul 2024 15:30:36 +0800 Subject: [PATCH 5/6] only return desired curvature --- selfdrive/controls/controlsd.py | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/selfdrive/controls/controlsd.py b/selfdrive/controls/controlsd.py index 6853b667155d23..f8ce7b4fd4ded4 100755 --- a/selfdrive/controls/controlsd.py +++ b/selfdrive/controls/controlsd.py @@ -639,7 +639,7 @@ def state_control(self, CS): # dp - lane line priority mode if self._dp_lat_lane_priority_mode and self.sm['lateralPlan'].useLaneLines: - self.desired_curvature, _ = get_lag_adjusted_curvature(self.CP, CS.vEgo, self.sm['lateralPlan'].psis, self.sm['lateralPlan'].curvatures) + self.desired_curvature = get_lag_adjusted_curvature(self.CP, CS.vEgo, self.sm['lateralPlan'].psis, self.sm['lateralPlan'].curvatures) else: # Steering PID loop and lateral MPC self.desired_curvature = clip_curvature(CS.vEgo, self.desired_curvature, model_v2.action.desiredCurvature) From cf9e6123152fba1459c6bfdf83d9fae20db206bd Mon Sep 17 00:00:00 2001 From: Rick Lan Date: Tue, 23 Jul 2024 12:12:18 +0800 Subject: [PATCH 6/6] update ref --- dp_ext | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/dp_ext b/dp_ext index f157795ace4edd..e4d911f816dab4 160000 --- a/dp_ext +++ b/dp_ext @@ -1 +1 @@ -Subproject commit f157795ace4eddef22199778ffc85c2e5f49a4ec +Subproject commit e4d911f816dab4d55e2a8f2e665f211479e2d391