Lane Priority Mode

CHANGELOGS.md

@@ -7,8 +7,11 @@ 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
+  * [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)
 
 
 ===============

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'])

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;

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())}

common/params.cc

@@ -252,6 +252,9 @@ std::unordered_map<std::string, uint32_t> 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

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
 

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,

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

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"),

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, [&]() {

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<float>(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<float>(scene.lane_line_probs[i], 0.0, 0.7)));
     painter.drawPolygon(scene.lane_line_vertices[i]);
   }
 

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;

selfdrive/ui/ui.cc

@@ -254,6 +254,8 @@ UIState::UIState(QObject *parent) : QObject(parent) {
     "longitudinalPlanExt",
     // TeToo
     "teToo",
+    // lane priority mode
+    "lateralPlan",
   });
 
   Params params;

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')))

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}