fix LaneMode, remov MapboxKey, fix autocruise speed (brake release) (#167)

* lanemode lag (#166) * fix.. laneline_t * check limit only, accel safety, * fix.. cruise speed.. * fix.. enable_dm * remove mapboxkey.. * test * fix.. * revert print
2025-05-13 16:59:19 +09:00 · 2025-05-13 16:59:19 +09:00 · b61696fd28
commit b61696fd28
parent 6a3f18dd51
17 changed files with 95 additions and 18 deletions
--- a/cereal/log.capnp
+++ b/cereal/log.capnp
@ -1375,6 +1375,7 @@ struct LateralPlan @0xe1e9318e2ae8b51e {
  latDebugText @34 :Text;

  position @35 :XYZTData;
+  distances @36 :List(Float32);

  struct SolverState {
    x @0 :List(List(Float32));
--- a/common/params_keys.h
+++ b/common/params_keys.h
@ -196,6 +196,7 @@ inline static std::unordered_map<std::string, uint32_t> keys = {
    {"TrafficLightDetectMode", PERSISTENT},
    {"SteerActuatorDelay", PERSISTENT},
    {"SteerSmoothSec", PERSISTENT},
+    {"SteerLagGain", PERSISTENT },
    {"CruiseOnDist", PERSISTENT},
    {"CruiseMaxVals1", PERSISTENT},
    {"CruiseMaxVals2", PERSISTENT},
--- a/opendbc_repo/opendbc/safety/safety.h
+++ b/opendbc_repo/opendbc/safety/safety.h
@ -618,7 +618,8 @@ bool longitudinal_accel_checks(int desired_accel, const LongitudinalLimits limit
      if(!controls_allowed) print("@@@@@@@@ longitudinal_accel_checks... auto controls_allowed enabled...\n");
      controls_allowed = true;
    }
-  bool accel_valid = get_longitudinal_allowed() && !max_limit_check(desired_accel, limits.max_accel, limits.min_accel);
+    //bool accel_valid = get_longitudinal_allowed() && !max_limit_check(desired_accel, limits.max_accel, limits.min_accel);
+    bool accel_valid = !max_limit_check(desired_accel, limits.max_accel, limits.min_accel);
    bool accel_inactive = desired_accel == limits.inactive_accel;
  return !(accel_valid || accel_inactive);
 }
--- a/scripts/add/events_ko.py
+++ b/scripts/add/events_ko.py
@ -571,7 +571,7 @@ EVENTS: dict[int, dict[str, Alert | AlertCallbackType]] = {
      "핸들을 잡아주세요",
      "회전이 조향 한도를 초과함",
      AlertStatus.userPrompt, AlertSize.mid,
-      Priority.LOW, VisualAlert.steerRequired, AudibleAlert.promptRepeat, 2.),
+      Priority.LOW, VisualAlert.none, AudibleAlert.none, 2.),
  },

  # Thrown when the fan is driven at >50% but is not rotating
--- a/selfdrive/car/cruise.py
+++ b/selfdrive/car/cruise.py
@ -648,10 +648,11 @@ class VCruiseCarrot:
        self._cruise_control(1, -1 if self.aTarget > 0.0 else 0, "Cruise on (speed)")
      elif abs(CS.steeringAngleDeg) < 20:
        if self.xState in [3, 5]:
+          if self.xState == 3:  # 감속중
            v_cruise_kph = self.v_ego_kph_set
          self._cruise_control(1, 0, "Cruise on (traffic sign)")
        elif 0 < self.d_rel < 20: 
-          v_cruise_kph = self.v_ego_kph_set
+          # v_cruise_kph = self.v_ego_kph_set # 전방에 차가 가까이 있을때, 기존속도 유지
          self._cruise_control(1, -1 if self.v_ego_kph_set < 1 else 0, "Cruise on (lead car)")

    elif not CC.enabled and self._brake_pressed_count < 0 and self._gas_pressed_count < 0:
--- a/selfdrive/carrot_settings.json
+++ b/selfdrive/carrot_settings.json
@ -235,6 +235,19 @@
      "default": 13,
      "unit": 1
    },
+    {
+      "group": "조향튜닝",
+      "name": "SteerLagGain",
+      "title": "조향래그게인x0.01(200)",
+      "descr": "레인모드 시험용",
+      "egroup": "LAT",
+      "etitle": "SteerLagGainx0.01(200)",
+      "edescr": "",
+      "min": 1,
+      "max": 300,
+      "default": 200,
+      "unit": 10
+    },
    {
      "group": "크루즈",
      "name": "CruiseOnDist",
--- a/selfdrive/controls/controlsd.py
+++ b/selfdrive/controls/controlsd.py
@ -135,6 +135,7 @@ class Controls:
                                  curve_speed_abs > self.params.get_int("UseLaneLineCurveSpeed"))
    lat_smooth_seconds = self.params.get_float("SteerSmoothSec") * 0.01
    steer_actuator_delay = self.params.get_float("SteerActuatorDelay") * 0.01
+    lag_gain = self.params.get_float("SteerLagGain") * 0.01
    if steer_actuator_delay == 0.0:
      steer_actuator_delay = self.sm['liveDelay'].lateralDelay 

@ -154,7 +155,7 @@ class Controls:
          alpha = 1 - np.exp(-DT_CTRL / tau) if tau > 0 else 1
          return alpha * val + (1 - alpha) * prev_val

-        curvature = get_lag_adjusted_curvature(self.CP, CS.vEgo, lat_plan.psis, lat_plan.curvatures, steer_actuator_delay + lat_smooth_seconds)
+        curvature = get_lag_adjusted_curvature(self.CP, CS.vEgo, lat_plan.psis, lat_plan.curvatures, steer_actuator_delay + lat_smooth_seconds, lat_plan.distances, lag_gain)

        new_desired_curvature = smooth_value(curvature, self.desired_curvature, lat_smooth_seconds)
    else:
--- a/selfdrive/controls/lib/drive_helpers.py
+++ b/selfdrive/controls/lib/drive_helpers.py
@ -26,10 +26,11 @@ def apply_deadzone(error, deadzone):
    error = 0.
  return error

-def get_lag_adjusted_curvature(CP, v_ego, psis, curvatures, steer_actuator_delay):
+def get_lag_adjusted_curvature(CP, v_ego, psis, curvatures, steer_actuator_delay, distances, lag_gain):
  if len(psis) != CONTROL_N:
    psis = [0.0]*CONTROL_N
    curvatures = [0.0]*CONTROL_N
+    distances = [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
@ -40,8 +41,11 @@ def get_lag_adjusted_curvature(CP, v_ego, psis, curvatures, steer_actuator_delay
  # psi to calculate a simple linearization of desired curvature
  current_curvature_desired = curvatures[0]
  psi = np.interp(delay, ModelConstants.T_IDXS[:CONTROL_N], psis)
-  average_curvature_desired = psi / (v_ego * delay)
-  desired_curvature = 2 * average_curvature_desired - current_curvature_desired
+  distance = max(np.interp(delay, ModelConstants.T_IDXS[:CONTROL_N], distances), 0.001)
+  #average_curvature_desired = psi / (v_ego * delay)
+  average_curvature_desired = psi / distance
+  #desired_curvature = 2 * average_curvature_desired - current_curvature_desired
+  desired_curvature = lag_gain * average_curvature_desired - current_curvature_desired

  # This is the "desired rate of the setpoint" not an actual desired rate
  max_curvature_rate = MAX_LATERAL_JERK / (v_ego**2) # inexact calculation, check https://github.com/commaai/openpilot/pull/24755
--- a/selfdrive/controls/lib/lane_planner_2.py
+++ b/selfdrive/controls/lib/lane_planner_2.py
@ -39,6 +39,7 @@ def max_abs(a, b):

 class LanePlanner:
  def __init__(self):
+    self.ll_t = np.zeros((TRAJECTORY_SIZE,))
    self.ll_x = np.zeros((TRAJECTORY_SIZE,))
    self.lll_y = np.zeros((TRAJECTORY_SIZE,))
    self.rll_y = np.zeros((TRAJECTORY_SIZE,))
@ -77,7 +78,8 @@ class LanePlanner:
    lane_lines = md.laneLines
    edges = md.roadEdges

-    if len(lane_lines) >= 4 and len(lane_lines[0].x) == TRAJECTORY_SIZE:
+    if len(lane_lines) >= 4 and len(lane_lines[0].t) == TRAJECTORY_SIZE:
+      self.ll_t = (np.array(lane_lines[1].t) + np.array(lane_lines[2].t))/2
      # left and right ll x is the same
      self.ll_x = lane_lines[1].x
      self.lll_y = np.array(lane_lines[1].y)
@ -231,8 +233,15 @@ class LanePlanner:
    laneline_active = False
    if self.lanefull_mode and self.d_prob > 0.3:
      laneline_active = True
+      use_dist_mode = False  ## 아무리생각해봐도.. 같은 방법인듯...
+      if use_dist_mode:
        lane_path_y_interp = np.interp(path_xyz[:,0] + v_ego * adjustLaneTime*0.01, self.ll_x, lane_path_y)
        path_xyz[:,1] = self.d_prob * lane_path_y_interp + (1.0 - self.d_prob) * path_xyz[:,1]
+      else:
+        safe_idxs = np.isfinite(self.ll_t)
+        if safe_idxs[0]:
+          lane_path_y_interp = np.interp(path_t * (1.0 + adjustLaneTime*0.01), self.ll_t[safe_idxs], lane_path_y[safe_idxs])
+          path_xyz[:,1] = self.d_prob * lane_path_y_interp + (1.0 - self.d_prob) * path_xyz[:,1]


    path_xyz[:, 1] += (CAMERA_OFFSET + self.lane_offset_filtered.x)
--- a/selfdrive/controls/lib/lateral_planner.py
+++ b/selfdrive/controls/lib/lateral_planner.py
@ -187,6 +187,8 @@ class LateralPlanner:
    else:
      self.solution_invalid_cnt = 0
  
+    self.x_sol = self.lat_mpc.x_sol
+
  def publish(self, sm, pm, carrot):
    plan_solution_valid = self.solution_invalid_cnt < 2
    plan_send = messaging.new_message('lateralPlan')
@ -202,8 +204,13 @@ class LateralPlanner:
    lateralPlan.modelMonoTime = sm.logMonoTime['modelV2']
    lateralPlan.dPathPoints = self.y_pts.tolist()
    lateralPlan.psis = self.lat_mpc.x_sol[0:CONTROL_N, 2].tolist()
+    lateralPlan.distances = self.lat_mpc.x_sol[0:CONTROL_N, 0].tolist()

+    if len(self.v_plan) == TRAJECTORY_SIZE:
+      lateralPlan.curvatures = (self.lat_mpc.x_sol[0:CONTROL_N, 3] / self.v_plan[0:CONTROL_N]).tolist()
+    else:
      lateralPlan.curvatures = (self.lat_mpc.x_sol[0:CONTROL_N, 3] / self.v_ego).tolist()
+
    lateralPlan.curvatureRates = [float(x.item() / self.v_ego) for x in self.lat_mpc.u_sol[0:CONTROL_N - 1]] + [0.0]

    lateralPlan.mpcSolutionValid = bool(plan_solution_valid)
--- a/selfdrive/controls/lib/longitudinal_mpc_lib/long_mpc.py
+++ b/selfdrive/controls/lib/longitudinal_mpc_lib/long_mpc.py
@ -346,6 +346,11 @@ class LongitudinalMpc:
    v_lead = np.clip(v_lead, 0.0, 1e8)
    a_lead = np.clip(a_lead, -10., 5.)

+    if a_lead < -2.0 and j_lead > 0.5:
+      a_lead = a_lead + j_lead
+      a_lead = min(a_lead, -0.5)
+      a_lead_tau = max(a_lead_tau, 1.5)
+
    lead_xv = self.extrapolate_lead(x_lead, v_lead, a_lead, a_lead_tau, j_lead)
    return lead_xv, v_lead

--- a/selfdrive/controls/radard.py
+++ b/selfdrive/controls/radard.py
@ -46,7 +46,7 @@ class Track:
    
    self.measured = measured   # measured or estimate

-    if abs(self.aLead) < 0.5 * self.radar_reaction_factor:
+    if abs(self.aLead) < 0.5 and abs(j_lead) < 0.5:
      self.aLeadTau.x = _LEAD_ACCEL_TAU * self.radar_reaction_factor
    else:
      self.aLeadTau.update(0.0)
--- a/selfdrive/modeld/fill_model_msg.py
+++ b/selfdrive/modeld/fill_model_msg.py
@ -103,7 +103,27 @@ def fill_model_msg(base_msg: capnp._DynamicStructBuilder, extended_msg: capnp._D
  modelV2.action = action

  # times at X_IDXS of edges and lines aren't used
-  LINE_T_IDXS: list[float] = []
+  # LINE_T_IDXS: list[float] = []
+
+  # times at X_IDXS according to model plan
+  LINE_T_IDXS = [np.nan] * ModelConstants.IDX_N
+  LINE_T_IDXS[0] = 0.0
+  plan_x = net_output_data['plan'][0, :, Plan.POSITION][:, 0].tolist()
+  for xidx in range(1, ModelConstants.IDX_N):
+    tidx = 0
+    # increment tidx until we find an element that's further away than the current xidx
+    while tidx < ModelConstants.IDX_N - 1 and plan_x[tidx + 1] < ModelConstants.X_IDXS[xidx]:
+      tidx += 1
+    if tidx == ModelConstants.IDX_N - 1:
+      # if the Plan doesn't extend far enough, set plan_t to the max value (10s), then break
+      LINE_T_IDXS[xidx] = ModelConstants.T_IDXS[ModelConstants.IDX_N - 1]
+      break
+    # interpolate to find `t` for the current xidx
+    current_x_val = plan_x[tidx]
+    next_x_val = plan_x[tidx + 1]
+    p = (ModelConstants.X_IDXS[xidx] - current_x_val) / (next_x_val - current_x_val) if abs(
+      next_x_val - current_x_val) > 1e-9 else float('nan')
+    LINE_T_IDXS[xidx] = p * ModelConstants.T_IDXS[tidx + 1] + (1 - p) * ModelConstants.T_IDXS[tidx]

  # lane lines
  modelV2.init('laneLines', 4)
--- a/selfdrive/ui/qt/offroad/settings.cc
+++ b/selfdrive/ui/qt/offroad/settings.cc
@ -242,6 +242,18 @@ DevicePanel::DevicePanel(SettingsWindow *parent) : ListWidget(parent) {
    }
    });

+  QPushButton* remove_mapbox_key_btn = new QPushButton(tr("Remove MapboxKey"));
+  remove_mapbox_key_btn->setObjectName("remove_mapbox_key_btn");
+  init_layout->addWidget(remove_mapbox_key_btn);
+  QObject::connect(remove_mapbox_key_btn, &QPushButton::clicked, [&]() {
+    if (ConfirmationDialog::confirm(tr("Remove Mapbox key?"), tr("Yes"), this)) {
+      QTimer::singleShot(1000, []() {
+        Params().put("MapboxPublicKey", "");
+        Params().put("MapboxSecretKey", "");
+        });
+    }
+    });
+
  setStyleSheet(R"(
    #reboot_btn { height: 120px; border-radius: 15px; background-color: #2CE22C; }
    #reboot_btn:pressed { background-color: #24FF24; }
@ -253,6 +265,8 @@ DevicePanel::DevicePanel(SettingsWindow *parent) : ListWidget(parent) {
    #init_btn:pressed { background-color: #2424FF; }
    #default_btn { height: 120px; border-radius: 15px; background-color: #BDBDBD; }
    #default_btn:pressed { background-color: #A9A9A9; }
+    #remove_mapbox_key_btn { height: 120px; border-radius: 15px; background-color: #BDBDBD; }
+    #remove_mapbox_key_btn:pressed { background-color: #A9A9A9; }
  )");
  addItem(init_layout);

--- a/selfdrive/ui/qt/onroad/onroad_home.cc
+++ b/selfdrive/ui/qt/onroad/onroad_home.cc
@ -172,7 +172,6 @@ void OnroadWindow::updateState(const UIState &s) {
      if (carrot_display == 5) ss->scene._current_carrot_display = (ss->scene._current_carrot_display % 3) + 1;
      else if(carrot_display > 0) ss->scene._current_carrot_display = carrot_display;
      if (map == nullptr && ss->scene._current_carrot_display > 2) ss->scene._current_carrot_display = 1;
-      //printf("_current_carrot_display2=%d\n", _current_carrot_display);
      //if (offroad) _current_carrot_display = 1;
      switch (ss->scene._current_carrot_display) {
      case 1: // default
--- a/system/manager/manager.py
+++ b/system/manager/manager.py
@ -146,6 +146,7 @@ def get_default_params():
    ("SpeedFromPCM", "2"),
    ("SteerActuatorDelay", "0"),
    ("SteerSmoothSec", "13"),
+    ("SteerLagGain", "200"),
    ("MaxTimeOffroadMin", "60"),
    ("DisableDM", "0"),
    ("RecordRoadCam", "0"),
--- a/system/manager/process_config.py
+++ b/system/manager/process_config.py
@ -87,7 +87,7 @@ procs = [
  # TODO: Make python process once TG allows opening QCOM from child pro
  # https://github.com/tinygrad/tinygrad/blob/ac9c96dae1656dc220ee4acc39cef4dd449aa850/tinygrad/device.py#L26
  NativeProcess("modeld", "selfdrive/modeld", ["./modeld.py"], only_onroad),
-  NativeProcess("dmonitoringmodeld", "selfdrive/modeld", ["./dmonitoringmodeld.py"], driverview, enabled=(WEBCAM or not PC)),
+  NativeProcess("dmonitoringmodeld", "selfdrive/modeld", ["./dmonitoringmodeld.py"], enable_dm, enabled=(WEBCAM or not PC)),
  #NativeProcess("mapsd", "selfdrive/navd", ["./mapsd"], only_onroad),
  #NativeProcess("mapsd", "selfdrive/navd", ["./mapsd"], always_run),
  #PythonProcess("navmodeld", "selfdrive.modeld.navmodeld", only_onroad),