diff --git a/common/params.cc b/common/params.cc index 2c796d531..09e6b991f 100644 --- a/common/params.cc +++ b/common/params.cc @@ -222,7 +222,7 @@ std::unordered_map keys = { {"dp_device_disable_onroad_uploads", PERSISTENT}, {"dp_toyota_zss", PERSISTENT}, {"dp_hkg_canfd_low_speed_turn_enhancer", PERSISTENT}, - {"dp_long_low_speed_aggressive_mode", PERSISTENT}, + {"dp_long_curve_speed_limiter", PERSISTENT}, {"dp_long_alt_personality_mode", PERSISTENT}, {"dp_long_alt_personality_speed", PERSISTENT}, }; diff --git a/selfdrive/controls/lib/longitudinal_planner.py b/selfdrive/controls/lib/longitudinal_planner.py index e4ba15bae..dc20b20aa 100755 --- a/selfdrive/controls/lib/longitudinal_planner.py +++ b/selfdrive/controls/lib/longitudinal_planner.py @@ -19,6 +19,7 @@ from openpilot.common.swaglog import cloudlog from openpilot.common.params import Params from openpilot.dp_ext.selfdrive.controls.lib.dynamic_endtoend_controller import DynamicEndtoEndController from openpilot.dp_ext.selfdrive.controls.lib.alt_driving_personality_controller import AlternativeDrivingPersonalityController +from openpilot.dp_ext.selfdrive.controls.lib.curve_speed_limiter import CurveSpeedLimiter LON_MPC_STEP = 0.2 # first step is 0.2s A_CRUISE_MIN = -1.2 @@ -89,6 +90,7 @@ class LongitudinalPlanner: self._frame = 0 self._dynamic_endtoend_controller = DynamicEndtoEndController() self._adp_controller = AlternativeDrivingPersonalityController() + self._curve_speed_limiter = CurveSpeedLimiter() @staticmethod def parse_model(model_msg, model_error): @@ -167,7 +169,10 @@ class LongitudinalPlanner: self.mpc.set_accel_limits(accel_limits_turns[0], accel_limits_turns[1]) self.mpc.set_cur_state(self.v_desired_filter.x, self.a_desired) x, v, a, j = self.parse_model(sm['modelV2'], self.v_model_error) - self.mpc.update(sm['radarState'], v_cruise, x, v, a, j, personality=sm['controlsState'].personality) + # dp + self._curve_speed_limiter.update(v_ego, sm['modelV2'].orientationRate.z, v) + v = self._curve_speed_limiter.get_v(v) + self.mpc.update(sm['radarState'], v_cruise, x, v, a, j, personality=personality) self.v_desired_trajectory = np.interp(CONTROL_N_T_IDX, T_IDXS_MPC, self.mpc.v_solution) diff --git a/system/manager/manager.py b/system/manager/manager.py index aba402453..2a962b056 100755 --- a/system/manager/manager.py +++ b/system/manager/manager.py @@ -63,6 +63,7 @@ def manager_init() -> None: ("dp_hkg_canfd_low_speed_turn_enhancer", "0"), ("dp_long_alt_personality_mode", "0"), ("dp_long_alt_personality_speed", "0"), + ("dp_long_curve_speed_limiter", "0"), ] if not PC: default_params.append(("LastUpdateTime", datetime.datetime.now(datetime.UTC).replace(tzinfo=None).isoformat().encode('utf8')))