Curve Speed Limiter

common/params.cc

@@ -222,7 +222,7 @@ std::unordered_map<std::string, uint32_t> 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},
 };

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)

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