diff --git a/common/params.cc b/common/params.cc index ecefe31..42102f6 100644 --- a/common/params.cc +++ b/common/params.cc @@ -311,8 +311,9 @@ std::unordered_map keys = { {"SoftHoldMode", PERSISTENT}, {"CarrotLatControl", PERSISTENT}, {"LatMpcPathCost", PERSISTENT}, + {"LatMpcPathCostTurn", PERSISTENT }, {"LatMpcMotionCost", PERSISTENT}, - {"LatMpcMotionCost2", PERSISTENT}, + {"LatMpcMotionCostTurn", PERSISTENT}, {"LatMpcAccelCost", PERSISTENT}, {"LatMpcJerkCost", PERSISTENT}, {"LatMpcSteeringRateCost", PERSISTENT}, diff --git a/selfdrive/carrot_settings.json b/selfdrive/carrot_settings.json index 261e11f..a045975 100644 --- a/selfdrive/carrot_settings.json +++ b/selfdrive/carrot_settings.json @@ -131,6 +131,19 @@ "default": 1, "unit": 10 }, + { + "group": "조향튜닝", + "name": "LatMpcPathCostTurn", + "title": "*LATMPC.PathCostTurn(200)", + "descr": "경로추종가중치, 높으면 경로우선시함.", + "egroup": "LAT", + "etitle": "*LATMPC.PathCostTurn(200)", + "edescr": "", + "min": 0, + "max": 500, + "default": 1, + "unit": 10 + }, { "group": "조향튜닝", "name": "LatMpcMotionCost", @@ -146,11 +159,11 @@ }, { "group": "조향튜닝", - "name": "LatMpcMotionCost2", - "title": "*LATMPC.MotionCost2(11)", + "name": "LatMpcMotionCostTurn", + "title": "*LATMPC.MotionCostTurn(1)", "descr": "헤딩가중치", "egroup": "LAT", - "etitle": "*LATMPC.MotionCost2(11)", + "etitle": "*LATMPC.MotionCostTurn(1)", "edescr": "", "min": 0, "max": 500, diff --git a/selfdrive/controls/lib/lateral_planner.py b/selfdrive/controls/lib/lateral_planner.py index edf62cf..2f4dcbe 100644 --- a/selfdrive/controls/lib/lateral_planner.py +++ b/selfdrive/controls/lib/lateral_planner.py @@ -81,15 +81,16 @@ class LateralPlanner: self.lat_mpc.reset(x0=self.x0) def update(self, sm): - global PATH_COST, LATERAL_ACCEL_COST, LATERAL_JERK_COST, STEERING_RATE_COST + global LATERAL_ACCEL_COST, LATERAL_JERK_COST, STEERING_RATE_COST self.readParams -= 1 if self.readParams <= 0: self.readParams = 100 self.useLaneLineSpeedApply = self.params.get_int("UseLaneLineSpeedApply") self.pathOffset = float(self.params.get_int("PathOffset")) * 0.01 - PATH_COST = self.params.get_float("LatMpcPathCost") * 0.01 + self.lateralPathCost = self.params.get_float("LatMpcPathCost") * 0.01 + self.lateralPathCostTurn = self.params.get_float("LatMpcPathCostTurn") * 0.01 self.lateralMotionCost = self.params.get_float("LatMpcMotionCost") * 0.01 - self.lateralMotionCost2 = self.params.get_float("LatMpcMotionCost2") * 0.01 + self.lateralMotionCostTurn = self.params.get_float("LatMpcMotionCostTurn") * 0.01 LATERAL_ACCEL_COST = self.params.get_float("LatMpcAccelCost") * 0.01 LATERAL_JERK_COST = self.params.get_float("LatMpcJerkCost") * 0.01 STEERING_RATE_COST = self.params.get_float("LatMpcSteeringRateCost") @@ -138,15 +139,15 @@ class LateralPlanner: self.LP.lane_change_multiplier = 1.0 lateral_motion_cost = self.lateralMotionCost - path_cost = PATH_COST + path_cost = self.lateralPathCost atc_type = sm['carrotMan'].atcType if atc_activate: if atc_type == "turn left" and (md.orientationRate.z[-1] > 0.1 or md.meta.desireState[1] > 0.1): - lateral_motion_cost = self.lateralMotionCost2 - #path_cost *= 2 + lateral_motion_cost = self.lateralMotionCostTurn + path_cost = self.lateralPathCostTurn elif atc_type == "turn right" and (md.orientationRate.z[-1] < -0.1 or md.meta.desireState[2] > 0.1): - lateral_motion_cost = self.lateralMotionCost2 - #path_cost *= 2 + lateral_motion_cost = self.lateralMotionCostTurn + path_cost = self.lateralPathCostTurn # lanelines calculation? self.LP.lanefull_mode = self.useLaneLineMode diff --git a/system/manager/manager.py b/system/manager/manager.py index 04aa508..8beb64c 100755 --- a/system/manager/manager.py +++ b/system/manager/manager.py @@ -134,8 +134,9 @@ def get_default_params(): ("LateralTorqueKf", "100"), ("LateralTorqueKd", "0"), ("LatMpcPathCost", "100"), + ("LatMpcPathCostTurn", "200"), ("LatMpcMotionCost", "11"), - ("LatMpcMotionCost2", "11"), + ("LatMpcMotionCostTurn", "1"), ("LatMpcAccelCost", "0"), ("LatMpcJerkCost", "4"), ("LatMpcSteeringRateCost", "700"),