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...

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;

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

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
     },
     {

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)

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
 

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

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