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

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

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},

opendbc_repo/opendbc/safety/safety.h

@@ -618,8 +618,9 @@ 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_inactive = desired_accel == limits.inactive_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);
 }
 

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

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]:
-          v_cruise_kph = self.v_ego_kph_set
+          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:
+        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:

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

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:

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)
+  distance = max(np.interp(delay, ModelConstants.T_IDXS[:CONTROL_N], distances), 0.001)
-  desired_curvature = 2 * average_curvature_desired - current_curvature_desired
+  #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

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
-      lane_path_y_interp = np.interp(path_xyz[:,0] + v_ego * adjustLaneTime*0.01, self.ll_x, lane_path_y)
+      use_dist_mode = False  ## 아무리생각해봐도.. 같은 방법인듯...
-      path_xyz[:,1] = self.d_prob * lane_path_y_interp + (1.0 - self.d_prob) * path_xyz[:,1]
+      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)

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

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
 

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)

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)

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

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

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

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