Low Speed Aggressive Mode

cereal/custom.capnp

@@ -16,6 +16,7 @@ struct ControlsStateExt @0x81c2f05a394cf4af {
 struct LongitudinalPlanExt @0xaedffd8f31e7b55d {
   de2eIsBlended @0 :Bool;
   de2eIsEnabled @1 :Bool;
+  lowSpeedAggressiveModeActive @2 :Bool;
 }
 
 struct CustomReserved2 @0xf35cc4560bbf6ec2 {

common/params.cc

@@ -229,6 +229,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},
 };
 
 } // namespace

selfdrive/controls/lib/longitudinal_planner.py

@@ -18,6 +18,7 @@ from openpilot.common.swaglog import cloudlog
 # dp
 from openpilot.common.params import Params
 from openpilot.dp_ext.selfdrive.controls.lib.dynamic_endtoend_controller import DynamicEndtoEndController
+from cereal import log
 
 LON_MPC_STEP = 0.2  # first step is 0.2s
 A_CRUISE_MIN = -1.2
@@ -88,6 +89,8 @@ class LongitudinalPlanner:
     self.params = Params()
     self._frame = 0
     self._dynamic_endtoend_controller = DynamicEndtoEndController()
+    self._dp_long_low_speed_aggressive_mode = self.params.get_bool("dp_long_low_speed_aggressive_mode")
+    self._dp_long_low_speed_aggressive_mode_active = False
 
   @staticmethod
   def parse_model(model_msg, model_error):
@@ -158,11 +161,16 @@ class LongitudinalPlanner:
     accel_limits_turns[0] = min(accel_limits_turns[0], self.a_desired + 0.05)
     accel_limits_turns[1] = max(accel_limits_turns[1], self.a_desired - 0.05)
 
-    self.mpc.set_weights(prev_accel_constraint, personality=sm['controlsState'].personality)
+    # dp
+    self._dp_long_low_speed_aggressive_mode_active = self._dp_long_low_speed_aggressive_mode and v_ego < 10
+    personality = log.LongitudinalPersonality.aggressive if self._dp_long_low_speed_aggressive_mode_active else sm['controlsState'].personality
+
+    self.mpc.set_weights(prev_accel_constraint, personality=personality)
     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)
+    self.mpc.update(sm['radarState'], v_cruise, x, v, a, j, personality=personality)
 
     self.v_desired_trajectory_full = np.interp(ModelConstants.T_IDXS, T_IDXS_MPC, self.mpc.v_solution)
     self.a_desired_trajectory_full = np.interp(ModelConstants.T_IDXS, T_IDXS_MPC, self.mpc.a_solution)
@@ -220,6 +228,7 @@ class LongitudinalPlanner:
     # longitudinalPlanExt.visionTurnSpeed = float(self.vision_turn_controller.v_turn)
     longitudinalPlanExt.de2eIsBlended = self.mpc.mode == 'blended'
     longitudinalPlanExt.de2eIsEnabled = self._dynamic_endtoend_controller.is_enabled()
+    longitudinalPlanExt.lowSpeedAggressiveModeActive = self._dp_long_low_speed_aggressive_mode_active
     # longitudinalPlanExt.longitudinalPlanExtSource = self.mpc.source if self.mpc.source != 'cruise' else self.cruise_source
 
     pm.send('longitudinalPlanExt', plan_ext_send)

system/manager/manager.py

@@ -61,6 +61,7 @@ def manager_init() -> None:
     ("dp_device_disable_onroad_uploads", "0"),
     ("dp_toyota_zss", "0"),
     ("dp_hkg_canfd_low_speed_turn_enhancer", "0"),
+    ("dp_long_low_speed_aggressive_mode", "0"),
   ]
   if not PC:
     default_params.append(("LastUpdateTime", datetime.datetime.now(datetime.UTC).replace(tzinfo=None).isoformat().encode('utf8')))