diff --git a/opendbc_repo/opendbc/car/car.capnp b/opendbc_repo/opendbc/car/car.capnp index d11b1e4..419522d 100644 --- a/opendbc_repo/opendbc/car/car.capnp +++ b/opendbc_repo/opendbc/car/car.capnp @@ -405,6 +405,7 @@ struct CarControl { jerk @9: Float32; # m/s^3 aTargetNow @10: Float32; # m/s^2 + yStd @11: Float32; enum LongControlState @0xe40f3a917d908282{ off @0; diff --git a/opendbc_repo/opendbc/car/hyundai/carcontroller.py b/opendbc_repo/opendbc/car/hyundai/carcontroller.py index 81dc11a..a8d0f5e 100644 --- a/opendbc_repo/opendbc/car/hyundai/carcontroller.py +++ b/opendbc_repo/opendbc/car/hyundai/carcontroller.py @@ -150,15 +150,16 @@ class CarController(CarControllerBase): base_max_torque = self.angle_max_torque else: curv = abs(actuators.curvature) - curve_scale = np.clip((curv - 0.0) / (0.006 - 0.0), 0.0, 1.0) + y_std = actuators.yStd + curvature_threshold = np.interp(y_std, [0.0, 0.25], [0.5, 0.006]) + + curve_scale = np.clip(curv / curvature_threshold, 0.0, 1.0) torque_pts = [ (1 - curve_scale) * self.angle_max_torque + curve_scale * 25, (1 - curve_scale) * self.angle_max_torque + curve_scale * 50, - self.angle_max_torque # 고속(30km/h 이상)은 항상 최대 토크 허용 - ] + self.angle_max_torque + ] base_max_torque = np.interp(CS.out.vEgo * CV.MS_TO_KPH, [0, 20, 30], torque_pts) - #base_max_torque = np.interp(CS.out.vEgo * CV.MS_TO_KPH, [0, 20, 30], [25, 50, self.angle_max_torque]) - #base_max_torque = np.interp(CS.out.vEgo * CV.MS_TO_KPH, [0, 20], [25, self.angle_max_torque]) target_torque = np.interp(abs(actuators.curvature), [0.0, 0.003, 0.006], [0.5 * base_max_torque, 0.75 * base_max_torque, base_max_torque]) @@ -504,7 +505,7 @@ class HyundaiJerk: else: jerk = actuators.jerk if actuators.longControlState == LongCtrlState.pid else 0.0 #a_error = actuators.aTargetNow - CS.out.aEgo - self.jerk = jerk #+ a_error + self.jerk = jerk# + a_error jerk_max_l = 5.0 jerk_max_u = jerk_max_l diff --git a/selfdrive/carrot_settings.json b/selfdrive/carrot_settings.json index 3787aa3..e981ee9 100644 --- a/selfdrive/carrot_settings.json +++ b/selfdrive/carrot_settings.json @@ -394,14 +394,14 @@ { "group": "주행튜닝", "name": "LongTuningKpV", - "title": "Long KpV(0)x0.01", + "title": "Long KpV(100)x0.01", "descr": "HKG: AccelPid:0, VelocityPid:100", "egroup": "LONG", - "etitle": "Long KpV(0)x0.01", + "etitle": "Long KpV(100)x0.01", "edescr": "HKG: AccelPid:0, VelocityPid:100", "min": 0, "max": 200, - "default": 0, + "default": 100, "unit": 5 }, { @@ -446,14 +446,14 @@ { "group": "주행튜닝", "name": "LongVelocityControl", - "title": "LongVelocityControl(0)", + "title": "LongVelocityControl(1)", "descr": "0: Accel PID, 1: Velocity PID", "egroup": "LONG", - "etitle": "LongVelocityControl(0)", + "etitle": "LongVelocityControl(1)", "edescr": "0: Accel PID, 1: Velocity PID", "min": 0, "max": 1, - "default": 0, + "default": 1, "unit": 1 }, { diff --git a/selfdrive/controls/controlsd.py b/selfdrive/controls/controlsd.py index b5b4a6c..3a57eae 100644 --- a/selfdrive/controls/controlsd.py +++ b/selfdrive/controls/controlsd.py @@ -55,6 +55,7 @@ class Controls: self.steer_limited_by_controls = False self.curvature = 0.0 self.desired_curvature = 0.0 + self.yStd = 0.0 self.pose_calibrator = PoseCalibrator() self.calibrated_pose: Pose | None = None @@ -144,6 +145,12 @@ class Controls: curve_speed_abs > self.params.get_int("UseLaneLineCurveSpeed")) steer_actuator_delay = self.params.get_float("SteerActuatorDelay") * 0.01 + LAT_SMOOTH_SECONDS + + if len(model_v2.position.yStd) > 0: + yStd = np.interp(steer_actuator_delay + LAT_SMOOTH_SECONDS, ModelConstants.T_IDXS, model_v2.position.yStd) + self.yStd = yStd * 0.1 + self.yStd * 0.9 + else: + self.yStd = 0.0 if not CC.latActive: new_desired_curvature = self.curvature @@ -169,6 +176,7 @@ class Controls: self.calibrated_pose, curvature_limited) # TODO what if not available actuators.torque = float(steer) actuators.steeringAngleDeg = float(steeringAngleDeg) + actuators.yStd = float(self.yStd) # Ensure no NaNs/Infs for p in ACTUATOR_FIELDS: attr = getattr(actuators, p) diff --git a/selfdrive/controls/lib/longcontrol.py b/selfdrive/controls/lib/longcontrol.py index e9a8ff8..fc158d5 100644 --- a/selfdrive/controls/lib/longcontrol.py +++ b/selfdrive/controls/lib/longcontrol.py @@ -12,7 +12,7 @@ LongCtrlState = car.CarControl.Actuators.LongControlState def long_control_state_trans(CP, active, long_control_state, v_ego, - should_stop, brake_pressed, cruise_standstill, a_ego, stopping_accel): + should_stop, brake_pressed, cruise_standstill, a_ego, stopping_accel, radarState): stopping_condition = should_stop starting_condition = (not should_stop and not cruise_standstill and @@ -41,7 +41,9 @@ def long_control_state_trans(CP, active, long_control_state, v_ego, elif long_control_state in [LongCtrlState.starting, LongCtrlState.pid]: if stopping_condition: stopping_accel = stopping_accel if stopping_accel < 0.0 else -0.5 - if a_ego > stopping_accel and v_ego < 1.0: + leadOne = radarState.leadOne + fcw_stop = leadOne.status and leadOne.dRel < 4.0 + if a_ego > stopping_accel or fcw_stop: # and v_ego < 1.0: long_control_state = LongCtrlState.stopping if long_control_state == LongCtrlState.starting: long_control_state = LongCtrlState.stopping @@ -62,6 +64,7 @@ class LongControl: self.params = Params() self.readParamCount = 0 self.stopping_accel = 0 + self.j_lead = 0.0 def reset(self): self.pid.reset() @@ -77,15 +80,16 @@ class LongControl: j_lead_factor = (1 - self.params.get_float("JLeadFactor2") * 0.01) * 2.0 # 최대 2 sec 미래를 봄.. # JLeadFactor2가 0에 가까우면 더 민감함. 100이면 안함. if j_lead_factor > 0.0 and radarState.leadOne.status and velocity_pid == 0: j_lead = np.clip(radarState.leadOne.jLead, -2.0, 2.0) - plan_alpha = np.interp(abs(j_lead), [0.0, 2.0], [0.0, j_lead_factor]) + self.j_lead = j_lead * 0.1 + self.j_lead * 0.9 + plan_alpha = np.interp(abs(self.j_lead), [0.0, 2.0], [0.0, j_lead_factor]) else: - j_lead = 0.0 + self.j_lead = 0.0 plan_alpha = 0.0 speeds = long_plan.speeds if len(speeds) == CONTROL_N: j_target_now = long_plan.jerks[0] #np.interp(long_delay, ModelConstants.T_IDXS[:CONTROL_N], long_plan.jerks) - if j_target_now < 0.2 and j_lead > 0.0: + if j_target_now < 0.2 and self.j_lead > 0.0: plan_alpha = 0.0 v_target_now = np.interp(long_delay + plan_alpha, ModelConstants.T_IDXS[:CONTROL_N], long_plan.speeds) a_target_now = np.interp(long_delay + plan_alpha, ModelConstants.T_IDXS[:CONTROL_N], long_plan.accels) @@ -111,7 +115,7 @@ class LongControl: self.long_control_state = long_control_state_trans(self.CP, active, self.long_control_state, CS.vEgo, should_stop, CS.brakePressed, - CS.cruiseState.standstill, CS.aEgo, self.stopping_accel) + CS.cruiseState.standstill, CS.aEgo, self.stopping_accel, radarState) if active and soft_hold_active: self.long_control_state = LongCtrlState.stopping diff --git a/selfdrive/ui/qt/offroad/settings.cc b/selfdrive/ui/qt/offroad/settings.cc index 2f3d1fd..44cd741 100644 --- a/selfdrive/ui/qt/offroad/settings.cc +++ b/selfdrive/ui/qt/offroad/settings.cc @@ -671,10 +671,10 @@ CarrotPanel::CarrotPanel(QWidget* parent) : QWidget(parent) { //latLongToggles->addItem(new CValueControl("JerkStartLimit", "LONG: JERK START(10)x0.1", "Starting Jerk.", "../assets/offroad/icon_road.png", 1, 50, 1)); //latLongToggles->addItem(new CValueControl("LongitudinalTuningApi", "LONG: ControlType", "0:velocity pid, 1:accel pid, 2:accel pid(comma)", "../assets/offroad/icon_road.png", 0, 2, 1)); latLongToggles->addItem(new CValueControl("LongTuningKpV", "LONG: P Gain(100)", "", "../assets/offroad/icon_logic.png", 0, 150, 5)); - latLongToggles->addItem(new CValueControl("LongTuningKiV", "LONG: I Gain(200)", "", "../assets/offroad/icon_logic.png", 0, 2000, 5)); + latLongToggles->addItem(new CValueControl("LongTuningKiV", "LONG: I Gain(0)", "", "../assets/offroad/icon_logic.png", 0, 2000, 5)); latLongToggles->addItem(new CValueControl("LongTuningKf", "LONG: FF Gain(100)", "", "../assets/offroad/icon_logic.png", 0, 200, 5)); latLongToggles->addItem(new CValueControl("LongActuatorDelay", "LONG: ActuatorDelay(20)", "", "../assets/offroad/icon_logic.png", 0, 200, 5)); - latLongToggles->addItem(new CValueControl("LongVelocityControl", "LONG: VelocityControl(0)", "", "../assets/offroad/icon_logic.png", 0, 1, 1)); + latLongToggles->addItem(new CValueControl("LongVelocityControl", "LONG: VelocityControl(1)", "0:AccelPID, 1:VelocityPID", "../assets/offroad/icon_logic.png", 0, 1, 1)); latLongToggles->addItem(new CValueControl("VEgoStopping", "LONG: VEgoStopping(50)", "Stopping factor", "../assets/offroad/icon_logic.png", 1, 100, 5)); latLongToggles->addItem(new CValueControl("RadarReactionFactor", "LONG: Radar reaction factor(10)", "", "../assets/offroad/icon_logic.png", 0, 200, 10)); //latLongToggles->addItem(new CValueControl("StartAccelApply", "LONG: StartingAccel 2.0x(0)%", "정지->출발시 가속도의 가속율을 지정합니다 0: 사용안함.", "../assets/offroad/icon_road.png", 0, 100, 10)); diff --git a/system/manager/manager.py b/system/manager/manager.py index 4bbf155..1d42ae3 100755 --- a/system/manager/manager.py +++ b/system/manager/manager.py @@ -100,11 +100,11 @@ def get_default_params(): ("CruiseMaxVals4", "110"), ("CruiseMaxVals5", "95"), ("CruiseMaxVals6", "80"), - ("LongTuningKpV", "0"), + ("LongTuningKpV", "100"), ("LongTuningKiV", "0"), ("LongTuningKf", "100"), ("LongActuatorDelay", "20"), - ("LongVelocityControl", "0"), + ("LongVelocityControl", "1"), ("VEgoStopping", "50"), ("RadarReactionFactor", "10"), ("EnableRadarTracks", "0"),