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
This commit is contained in:
carrot
GitHub
parent
6a3f18dd51
commit
b61696fd28
@ -1375,6 +1375,7 @@ struct LateralPlan @0xe1e9318e2ae8b51e {
|
||||
latDebugText @34 :Text;
|
||||
|
||||
position @35 :XYZTData;
|
||||
distances @36 :List(Float32);
|
||||
|
||||
struct SolverState {
|
||||
x @0 :List(List(Float32));
|
||||
|
||||
@ -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},
|
||||
|
||||
@ -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);
|
||||
}
|
||||
|
||||
|
||||
@ -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
|
||||
|
||||
@ -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
|
||||
# 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:
|
||||
|
||||
@ -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",
|
||||
|
||||
@ -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:
|
||||
|
||||
@ -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
|
||||
|
||||
@ -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)
|
||||
|
||||
@ -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)
|
||||
|
||||
@ -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
|
||||
|
||||
|
||||
@ -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)
|
||||
|
||||
@ -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)
|
||||
|
||||
@ -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);
|
||||
|
||||
|
||||
@ -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
|
||||
|
||||
@ -146,6 +146,7 @@ def get_default_params():
|
||||
("SpeedFromPCM", "2"),
|
||||
("SteerActuatorDelay", "0"),
|
||||
("SteerSmoothSec", "13"),
|
||||
("SteerLagGain", "200"),
|
||||
("MaxTimeOffroadMin", "60"),
|
||||
("DisableDM", "0"),
|
||||
("RecordRoadCam", "0"),
|
||||
|
||||
@ -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),
|
||||
|
||||
Loading…
x
Reference in New Issue
Block a user