diff --git a/cereal/log.capnp b/cereal/log.capnp index 1bc9e05..528faf2 100644 --- 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)); diff --git a/common/params_keys.h b/common/params_keys.h index ccc1995..8fea6f7 100644 --- a/common/params_keys.h +++ b/common/params_keys.h @@ -196,6 +196,7 @@ inline static std::unordered_map keys = { {"TrafficLightDetectMode", PERSISTENT}, {"SteerActuatorDelay", PERSISTENT}, {"SteerSmoothSec", PERSISTENT}, + {"SteerLagGain", PERSISTENT }, {"CruiseOnDist", PERSISTENT}, {"CruiseMaxVals1", PERSISTENT}, {"CruiseMaxVals2", PERSISTENT}, diff --git a/opendbc_repo/opendbc/safety/safety.h b/opendbc_repo/opendbc/safety/safety.h index 71d5a81..fda1bff 100644 --- a/opendbc_repo/opendbc/safety/safety.h +++ b/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_inactive = desired_accel == limits.inactive_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); } diff --git a/scripts/add/events_ko.py b/scripts/add/events_ko.py index 88c7afd..d7e763b 100644 --- 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 diff --git a/selfdrive/car/cruise.py b/selfdrive/car/cruise.py index c87bbef..0737854 100644 --- 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]: - 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: - v_cruise_kph = self.v_ego_kph_set + elif 0 < self.d_rel < 20: + # 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: diff --git a/selfdrive/carrot_settings.json b/selfdrive/carrot_settings.json index 0e727b8..69aea47 100644 --- 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", diff --git a/selfdrive/controls/controlsd.py b/selfdrive/controls/controlsd.py index 6a4a493..bcf14d8 100644 --- 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: diff --git a/selfdrive/controls/lib/drive_helpers.py b/selfdrive/controls/lib/drive_helpers.py index cbc84f7..1a0b352 100644 --- 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 diff --git a/selfdrive/controls/lib/lane_planner_2.py b/selfdrive/controls/lib/lane_planner_2.py index ce86734..ec93907 100644 --- 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 - 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] + 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) diff --git a/selfdrive/controls/lib/lateral_planner.py b/selfdrive/controls/lib/lateral_planner.py index b50f200..911184a 100644 --- 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.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) diff --git a/selfdrive/controls/lib/longitudinal_mpc_lib/long_mpc.py b/selfdrive/controls/lib/longitudinal_mpc_lib/long_mpc.py index 7b5c6b9..8dad9d9 100644 --- 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 diff --git a/selfdrive/controls/radard.py b/selfdrive/controls/radard.py index 3850b2c..ed4fb1a 100644 --- 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) diff --git a/selfdrive/modeld/fill_model_msg.py b/selfdrive/modeld/fill_model_msg.py index a91c639..6a86fea 100644 --- 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) diff --git a/selfdrive/ui/qt/offroad/settings.cc b/selfdrive/ui/qt/offroad/settings.cc index 16f487f..aa59987 100644 --- 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); diff --git a/selfdrive/ui/qt/onroad/onroad_home.cc b/selfdrive/ui/qt/onroad/onroad_home.cc index d06fd08..71b5a2a 100644 --- 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 diff --git a/system/manager/manager.py b/system/manager/manager.py index 80c026b..4229357 100755 --- 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"), diff --git a/system/manager/process_config.py b/system/manager/process_config.py index 9787c2d..af0bd3b 100644 --- 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),