fix angle steering & long (#156)

* test... angle torque using yStd.. * fix.. * fix.. * fix.. * fix.. * apply more jerk (using a_error) * fix.. stopping.. * fix.. * change.. stopping condition 3->4M * fix.. default * fix.. * fix... jerk.. * fix.. angle steer... threshold. * fix.. j_lead adjust...
2025-04-26 13:30:45 +09:00 · 2025-04-26 13:30:45 +09:00 · 99a555579b
commit 99a555579b
parent ee5076f581
7 changed files with 36 additions and 22 deletions
--- 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;
--- 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])

--- 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
    },
    {
--- 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
@ -145,6 +146,12 @@ class Controls:

    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
    elif self.lanefull_mode_enabled:
@ -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)
--- 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

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