diff --git a/common/params.cc b/common/params.cc index e282262..b1bd9d8 100644 --- a/common/params.cc +++ b/common/params.cc @@ -272,6 +272,7 @@ std::unordered_map keys = { {"MyDrivingModeAuto", PERSISTENT}, {"TrafficLightDetectMode", PERSISTENT}, {"SteerActuatorDelay", PERSISTENT}, + {"ModelTurnDelay", PERSISTENT}, {"CruiseOnDist", PERSISTENT}, {"CruiseMaxVals1", PERSISTENT}, {"CruiseMaxVals2", PERSISTENT}, diff --git a/selfdrive/carrot_settings.json b/selfdrive/carrot_settings.json index bb5b510..4efcf52 100644 --- a/selfdrive/carrot_settings.json +++ b/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", diff --git a/selfdrive/controls/controlsd.py b/selfdrive/controls/controlsd.py index 39975da..ec98dcc 100644 --- a/selfdrive/controls/controlsd.py +++ b/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: diff --git a/selfdrive/controls/lib/lane_planner_2.py b/selfdrive/controls/lib/lane_planner_2.py index 57ee69f..bbbbd6c 100644 --- a/selfdrive/controls/lib/lane_planner_2.py +++ b/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] diff --git a/selfdrive/modeld/modeld.py b/selfdrive/modeld/modeld.py index 5ae38d6..4514bc5 100755 --- a/selfdrive/modeld/modeld.py +++ b/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) diff --git a/system/manager/manager.py b/system/manager/manager.py index cd14a05..9a56e4e 100755 --- a/system/manager/manager.py +++ b/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"),