diff --git a/common/params.cc b/common/params.cc index 5dec4b4..519dbda 100644 --- a/common/params.cc +++ b/common/params.cc @@ -272,7 +272,6 @@ std::unordered_map keys = { {"MyDrivingModeAuto", PERSISTENT}, {"TrafficLightDetectMode", PERSISTENT}, {"SteerActuatorDelay", PERSISTENT}, - {"ModelActuatorDelay", PERSISTENT}, {"CruiseOnDist", PERSISTENT}, {"CruiseMaxVals1", PERSISTENT}, {"CruiseMaxVals2", PERSISTENT}, @@ -310,7 +309,6 @@ std::unordered_map keys = { {"MaxAngleFrames", PERSISTENT}, {"SoftHoldMode", PERSISTENT}, {"CarrotLatControl", PERSISTENT}, - {"CarrotLatFilter", PERSISTENT}, {"LatMpcPathCost", PERSISTENT }, {"LatMpcMotionCost", PERSISTENT }, {"LatMpcMotionCost2", PERSISTENT }, diff --git a/selfdrive/carrot_settings.json b/selfdrive/carrot_settings.json index 0820a4c..bb5b510 100644 --- a/selfdrive/carrot_settings.json +++ b/selfdrive/carrot_settings.json @@ -66,19 +66,6 @@ "default": 1, "unit": 1 }, - { - "group": "조향튜닝", - "name": "CarrotLatFilter", - "title": "*당근조향제어필터(5)", - "descr": "필터링개수", - "egroup": "LAT", - "etitle": "*Carrot lateral filter(5)", - "edescr": "", - "min": 2, - "max": 30, - "default": 5, - "unit": 1 - }, { "group": "조향튜닝", "name": "LatMpcPathCost", @@ -222,19 +209,6 @@ "default": 40, "unit": 1 }, - { - "group": "조향튜닝", - "name": "ModelActuatorDelay", - "title": "*ModelActuatorDelay(80)x0.01", - "descr": "모델지연값, 커브진입시점로 조절", - "egroup": "LAT", - "etitle": "*ModelActuatorDelay(80)", - "edescr": "", - "min": 1, - "max": 300, - "default": 80, - "unit": 1 - }, { "group": "크루즈", "name": "CruiseOnDist", diff --git a/selfdrive/controls/controlsd.py b/selfdrive/controls/controlsd.py index e1bf2da..3e388e8 100644 --- a/selfdrive/controls/controlsd.py +++ b/selfdrive/controls/controlsd.py @@ -11,7 +11,7 @@ from openpilot.common.swaglog import cloudlog import numpy as np from opendbc.car.car_helpers import get_car_interface -from openpilot.selfdrive.controls.lib.drive_helpers import clip_curvature, get_lag_adjusted_curvature, get_lag_adjusted_curvature1 +from openpilot.selfdrive.controls.lib.drive_helpers import clip_curvature, get_lag_adjusted_curvature from openpilot.selfdrive.controls.lib.latcontrol import LatControl, MIN_LATERAL_CONTROL_SPEED from openpilot.selfdrive.controls.lib.latcontrol_pid import LatControlPID from openpilot.selfdrive.controls.lib.latcontrol_angle import LatControlAngle, STEER_ANGLE_SATURATION_THRESHOLD @@ -126,39 +126,20 @@ class Controls: self.lanefull_mode_enabled = (lat_plan.useLaneLines and self.params.get_int("UseLaneLineSpeedApply") > 0 and curve_speed_abs > self.params.get_int("UseLaneLineCurveSpeed")) - carrot_lat_control = self.params.get_int("CarrotLatControl") - if carrot_lat_control > 0: - model_delay = self.params.get_float("ModelActuatorDelay") * 0.01 - steer_actuator_delay = self.params.get_float("SteerActuatorDelay") * 0.01 - t_since_plan = (self.sm.frame - self.sm.recv_frame['lateralPlan']) * DT_CTRL - if carrot_lat_control == 1: - if len(lat_plan.curvatures) != CONTROL_N: - self.desired_curvature_next = self.desired_curvature = desired_curvature_ff = 0.0 - else: - curvature = np.interp(model_delay + t_since_plan, ModelConstants.T_IDXS[:CONTROL_N], lat_plan.curvatures) - desired_curvature_ff = np.interp(model_delay + steer_actuator_delay + t_since_plan, ModelConstants.T_IDXS[:CONTROL_N], lat_plan.curvatures) - self.desired_curvature = clip_curvature(CS.vEgo, self.desired_curvature, curvature) - elif carrot_lat_control == 2: - desired_curvature = get_lag_adjusted_curvature1(self.CP, CS.vEgo, lat_plan.psis, lat_plan.curvatures, steer_actuator_delay) - desired_curvature_ff = self.desired_curvature = clip_curvature(CS.vEgo, self.desired_curvature, desired_curvature) - else: - lat_filter = carrot_lat_control - desired_curvature_now, desired_curvature_ff = get_lag_adjusted_curvature(self.CP, CS.vEgo, lat_plan.psis, lat_plan.curvatures, self.desired_curvature, model_delay, steer_actuator_delay, t_since_plan, lat_filter) - - self.desired_curvature = clip_curvature(CS.vEgo, self.desired_curvature, desired_curvature_now) - - + steer_actuator_delay = self.params.get_float("SteerActuatorDelay") * 0.01 + if self.params.get_bool("CarrotLatControl"): + desired_curvature = get_lag_adjusted_curvature(self.CP, CS.vEgo, lat_plan.psis, lat_plan.curvatures, steer_actuator_delay) + self.desired_curvature = clip_curvature(CS.vEgo, self.desired_curvature, desired_curvature) else: - steer_actuator_delay = self.params.get_float("SteerActuatorDelay") * 0.01 if self.lanefull_mode_enabled: - desired_curvature = get_lag_adjusted_curvature1(self.CP, CS.vEgo, lat_plan.psis, lat_plan.curvatures, steer_actuator_delay) - desired_curvature_ff = self.desired_curvature = clip_curvature(CS.vEgo, self.desired_curvature, desired_curvature) + desired_curvature = get_lag_adjusted_curvature(self.CP, CS.vEgo, lat_plan.psis, lat_plan.curvatures, steer_actuator_delay) + self.desired_curvature = clip_curvature(CS.vEgo, self.desired_curvature, desired_curvature) else: - desired_curvature_ff = self.desired_curvature = clip_curvature(CS.vEgo, self.desired_curvature, model_v2.action.desiredCurvature) + self.desired_curvature = clip_curvature(CS.vEgo, self.desired_curvature, model_v2.action.desiredCurvature) actuators.curvature = float(self.desired_curvature) steer, steeringAngleDeg, lac_log = self.LaC.update(CC.latActive, CS, self.VM, lp, - self.steer_limited, desired_curvature_ff, self.desired_curvature, + self.steer_limited, self.desired_curvature, self.sm['liveLocationKalman']) # TODO what if not available actuators.steer = float(steer) actuators.steeringAngleDeg = float(steeringAngleDeg) diff --git a/selfdrive/controls/lib/drive_helpers.py b/selfdrive/controls/lib/drive_helpers.py index 4c945b3..6957a0c 100644 --- a/selfdrive/controls/lib/drive_helpers.py +++ b/selfdrive/controls/lib/drive_helpers.py @@ -23,52 +23,7 @@ def apply_deadzone(error, deadzone): error = 0. return error -def get_lag_adjusted_curvature(CP, v_ego, psis, curvatures, desired_curvature_last, model_delay, steer_actuator_delay, t_since_plan, lat_filter): - if len(psis) != CONTROL_N: - psis = [0.0]*CONTROL_N - curvatures = [0.0]*CONTROL_N - - v_ego = max(MIN_SPEED, v_ego) - - delay = max(0.01, steer_actuator_delay) - - current_curvature_desired = desired_curvature_last - - window_size = lat_filter - if window_size > 0: - #def moving_average(data, window_size): - # kernel = np.ones(window_size) / window_size - # return np.convolve(data, kernel, mode='same') - def moving_average(data, window_size): - data = np.array(data, dtype=float) - - kernel = np.ones(window_size) / window_size - smoothed = np.convolve(data, kernel, mode='same') - - half_window = window_size // 2 - smoothed[:half_window] = ( - np.cumsum(data[:window_size])[:half_window] / np.arange(1, half_window + 1) - ) - smoothed[-half_window:] = ( - np.cumsum(data[-window_size:][::-1])[:half_window][::-1] / np.arange(1, half_window + 1) - ) - return smoothed.tolist() - - curvatures = moving_average(curvatures, window_size) - - desired_curvature = np.interp(model_delay + t_since_plan, ModelConstants.T_IDXS[:CONTROL_N], curvatures) - desired_curvature_ff = np.interp(model_delay + steer_actuator_delay + t_since_plan, ModelConstants.T_IDXS[:CONTROL_N], curvatures) - - max_curvature_rate = MAX_LATERAL_JERK / (v_ego**2) # inexact calculation, check https://github.com/commaai/openpilot/pull/24755 - safe_desired_curvature = np.clip(desired_curvature, - current_curvature_desired - max_curvature_rate * DT_MDL, - current_curvature_desired + max_curvature_rate * DT_MDL) - #safe_desired_curvature_ff = clip(desired_curvature_ff, - # current_curvature_desired - max_curvature_rate * DT_MDL, - # current_curvature_desired + max_curvature_rate * DT_MDL) - return safe_desired_curvature, desired_curvature_ff - -def get_lag_adjusted_curvature1(CP, v_ego, psis, curvatures, steer_actuator_delay): +def get_lag_adjusted_curvature(CP, v_ego, psis, curvatures, steer_actuator_delay): if len(psis) != CONTROL_N: psis = [0.0]*CONTROL_N curvatures = [0.0]*CONTROL_N diff --git a/selfdrive/controls/lib/latcontrol.py b/selfdrive/controls/lib/latcontrol.py index 39da6df..cf284df 100644 --- a/selfdrive/controls/lib/latcontrol.py +++ b/selfdrive/controls/lib/latcontrol.py @@ -17,7 +17,7 @@ class LatControl(ABC): self.steer_max = 1.0 @abstractmethod - def update(self, active, CS, VM, params, steer_limited, desired_curvature, desired_curvature_now, llk): + def update(self, active, CS, VM, params, steer_limited, desired_curvature, llk): pass def reset(self): diff --git a/selfdrive/controls/lib/latcontrol_angle.py b/selfdrive/controls/lib/latcontrol_angle.py index 37b6769..0cacb97 100644 --- a/selfdrive/controls/lib/latcontrol_angle.py +++ b/selfdrive/controls/lib/latcontrol_angle.py @@ -11,7 +11,7 @@ class LatControlAngle(LatControl): super().__init__(CP, CI) self.sat_check_min_speed = 5. - def update(self, active, CS, VM, params, steer_limited, desired_curvature, desired_curvature_now, llk): + def update(self, active, CS, VM, params, steer_limited, desired_curvature, llk): angle_log = log.ControlsState.LateralAngleState.new_message() if not active: diff --git a/selfdrive/controls/lib/latcontrol_pid.py b/selfdrive/controls/lib/latcontrol_pid.py index a7b6964..1e5b062 100644 --- a/selfdrive/controls/lib/latcontrol_pid.py +++ b/selfdrive/controls/lib/latcontrol_pid.py @@ -17,7 +17,7 @@ class LatControlPID(LatControl): super().reset() self.pid.reset() - def update(self, active, CS, VM, params, steer_limited, desired_curvature, desired_curvature_now, llk): + def update(self, active, CS, VM, params, steer_limited, desired_curvature, llk): pid_log = log.ControlsState.LateralPIDState.new_message() pid_log.steeringAngleDeg = float(CS.steeringAngleDeg) pid_log.steeringRateDeg = float(CS.steeringRateDeg) diff --git a/selfdrive/controls/lib/latcontrol_torque.py b/selfdrive/controls/lib/latcontrol_torque.py index 32e7f74..2fb2ce6 100644 --- a/selfdrive/controls/lib/latcontrol_torque.py +++ b/selfdrive/controls/lib/latcontrol_torque.py @@ -52,7 +52,7 @@ class LatControlTorque(LatControl): self.torque_params.latAccelOffset = latAccelOffset self.torque_params.friction = friction - def update(self, active, CS, VM, params, steer_limited, desired_curvature, desired_curvature_now, llk): + def update(self, active, CS, VM, params, steer_limited, desired_curvature, llk): self.frame += 1 if self.frame % 10 == 0: lateralTorqueCustom = self.params.get_int("LateralTorqueCustom") @@ -96,8 +96,7 @@ class LatControlTorque(LatControl): lateral_accel_deadzone = curvature_deadzone * CS.vEgo ** 2 low_speed_factor = np.interp(CS.vEgo, LOW_SPEED_X, LOW_SPEED_Y)**2 - desired_lateral_accel_now = desired_curvature_now * CS.vEgo ** 2 - setpoint = desired_lateral_accel_now + low_speed_factor * desired_curvature_now + setpoint = desired_lateral_accel + low_speed_factor * desired_curvature measurement = actual_lateral_accel + low_speed_factor * actual_curvature gravity_adjusted_lateral_accel = desired_lateral_accel - roll_compensation torque_from_setpoint = self.torque_from_lateral_accel(LatControlInputs(setpoint, roll_compensation, CS.vEgo, CS.aEgo), self.torque_params, diff --git a/selfdrive/controls/lib/lateral_planner.py b/selfdrive/controls/lib/lateral_planner.py index 27de8de..6da9549 100644 --- a/selfdrive/controls/lib/lateral_planner.py +++ b/selfdrive/controls/lib/lateral_planner.py @@ -73,13 +73,6 @@ class LateralPlanner: self.reset_mpc(np.zeros(4)) self.curve_speed = 0 - self.prev_path_xyz = None - self.carrot_lat_control = 0 - self.carrot_lat_filter = 5 - self.path_history = deque(maxlen=self.carrot_lat_filter) - - self.lateralMotionCost = LATERAL_MOTION_COST - self.lateralMotionCost2 = LATERAL_MOTION_COST def reset_mpc(self, x0=None): if x0 is None: @@ -100,11 +93,6 @@ class LateralPlanner: LATERAL_ACCEL_COST = self.params.get_float("LatMpcAccelCost") * 0.01 LATERAL_JERK_COST = self.params.get_float("LatMpcJerkCost") * 0.01 STEERING_RATE_COST = self.params.get_float("LatMpcSteeringRateCost") - self.carrot_lat_control = self.params.get_int("CarrotLatControl") - carrot_lat_filter = self.params.get_int("CarrotLatFilter") - if carrot_lat_filter != self.carrot_lat_filter: - self.carrot_lat_filter = carrot_lat_filter - self.path_history = deque(maxlen=self.carrot_lat_filter) # clip speed , lateral planning is not possible at 0 speed measured_curvature = sm['controlsState'].curvature @@ -150,12 +138,15 @@ class LateralPlanner: self.LP.lane_change_multiplier = 1.0 lateral_motion_cost = self.lateralMotionCost + path_cost = PATH_COST atc_type = sm['carrotMan'].atcType if atc_activate: - if atc_type == "turn left" and md.meta.desireState[1] > 0.1: + if atc_type == "turn left" and (md.orientationRate.z[-1] > 0.1 or md.meta.desireState[1] > 0.1): lateral_motion_cost = self.lateralMotionCost2 - elif atc_type == "turn right" and md.meta.desireState[2] > 0.1: + #path_cost *= 2 + elif atc_type == "turn right" and (md.orientationRate.z[-1] < -0.1 or md.meta.desireState[2] > 0.1): lateral_motion_cost = self.lateralMotionCost2 + #path_cost *= 2 # lanelines calculation? self.LP.lanefull_mode = self.useLaneLineMode @@ -170,22 +161,7 @@ class LateralPlanner: self.path_xyz[:, 1] += self.pathOffset - """ - # Smooth path - self.alpha = 0.2 - if self.prev_path_xyz is None: - self.prev_path_xyz = self.path_xyz.copy() - self.path_xyz = self.alpha * self.path_xyz + (1 - self.alpha) * self.prev_path_xyz - self.prev_path_xyz = self.path_xyz - """ - if self.carrot_lat_control in [1,2]: - if False: #self.plan_a[0] < -1.0: - self.path_history.clear() - - self.path_history.append(self.path_xyz) - self.path_xyz = np.mean(np.array(self.path_history), axis=0) - - self.lat_mpc.set_weights(PATH_COST, lateral_motion_cost, + self.lat_mpc.set_weights(path_cost, lateral_motion_cost, LATERAL_ACCEL_COST, LATERAL_JERK_COST, STEERING_RATE_COST) diff --git a/system/manager/manager.py b/system/manager/manager.py index bdeeadb..78340bc 100755 --- a/system/manager/manager.py +++ b/system/manager/manager.py @@ -124,7 +124,6 @@ def get_default_params(): ("LaneChangeNeedTorque", "0"), ("MaxAngleFrames", "89"), ("CarrotLatControl", "0"), - ("CarrotLatFilter", "5"), ("DampingFactor", "0"), ("LateralTorqueCustom", "0"), ("LateralTorqueAccelFactor", "2500"), @@ -141,7 +140,6 @@ def get_default_params(): ("CustomSteerDeltaDown", "0"), ("SpeedFromPCM", "2"), ("SteerActuatorDelay", "30"), - ("ModelActuatorDelay", "20"), ("MaxTimeOffroadMin", "60"), ("DisableDM", "0"), ("RecordRoadCam", "0"),