ModelTurnDelay

common/params.cc

@@ -272,6 +272,7 @@ std::unordered_map<std::string, uint32_t> keys = {
     {"MyDrivingModeAuto", PERSISTENT},
     {"TrafficLightDetectMode", PERSISTENT},
     {"SteerActuatorDelay", PERSISTENT},
+    {"ModelTurnDelay", PERSISTENT},
     {"CruiseOnDist", PERSISTENT},
     {"CruiseMaxVals1", PERSISTENT},
     {"CruiseMaxVals2", PERSISTENT},

selfdrive/carrot_settings.json

@@ -209,6 +209,19 @@
       "default": 40,
       "unit": 1
     },
+    {
+      "group": "조향튜닝",
+      "name": "ModelTurnDelay",
+      "title": "*ModelTurnDelay(90)",
+      "descr": "",
+      "egroup": "LAT",
+      "etitle": "ModelTurnDelay(90)",
+      "edescr": "",
+      "min": 1,
+      "max": 300,
+      "default": 40,
+      "unit": 10
+    },
     {
       "group": "크루즈",
       "name": "CruiseOnDist",

selfdrive/controls/controlsd.py

@@ -128,7 +128,10 @@ class Controls:
     
     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)
+      model_turn_delay = self.params.get_float("ModelTurnDelay") * 0.01
+      desire_state =  self.sm['modelV2'].meta.desireState
+      turn_state = desire_state[1] + desire_state[2]
+      desired_curvature = get_lag_adjusted_curvature(self.CP, CS.vEgo, lat_plan.psis, lat_plan.curvatures, steer_actuator_delay if turn_state < 0.1 else model_turn_delay)
       self.desired_curvature = clip_curvature(CS.vEgo, self.desired_curvature, desired_curvature)
     else:
       if self.lanefull_mode_enabled:

selfdrive/controls/lib/lane_planner_2.py

@@ -248,6 +248,7 @@ class LanePlanner:
 
     return path_xyz
 
+
   def calculate_plan_yaw_and_yaw_rate(self, path_xyz):
     x = path_xyz[:, 0]
     y = path_xyz[:, 1]

selfdrive/modeld/modeld.py

@@ -204,8 +204,9 @@ def main(demo=False):
 
   DH = DesireHelper()
 
+  turn_state = False
   while True:
-    steer_delay = params.get_float("SteerActuatorDelay") * 0.01 #CP.steerActuatorDelay + .2
+    steer_delay = params.get_float("SteerActuatorDelay") * 0.01 if not turn_state else params.get_float("ModelTurnDelay") * 0.01
 
     # Keep receiving frames until we are at least 1 frame ahead of previous extra frame
     while meta_main.timestamp_sof < meta_extra.timestamp_sof + 25000000:
@@ -302,6 +303,8 @@ def main(demo=False):
       drivingdata_send.drivingModelData.meta.laneChangeState = DH.lane_change_state
       drivingdata_send.drivingModelData.meta.laneChangeDirection = DH.lane_change_direction
 
+      turn_state = (desire_state[1] + desire_state[2]) > 0.1
+
       modelv2_send.modelV2.meta.laneWidthLeft = float(DH.lane_width_left)
       modelv2_send.modelV2.meta.laneWidthRight = float(DH.lane_width_right)
       modelv2_send.modelV2.meta.distanceToRoadEdgeLeft = float(DH.distance_to_road_edge_left)

system/manager/manager.py

@@ -140,6 +140,7 @@ def get_default_params():
     ("CustomSteerDeltaDown", "0"),
     ("SpeedFromPCM", "2"),
     ("SteerActuatorDelay", "30"),
+    ("ModelTurnDelay", "90"),
     ("MaxTimeOffroadMin", "60"),
     ("DisableDM", "0"),
     ("RecordRoadCam", "0"),