diff --git a/cereal/custom.capnp b/cereal/custom.capnp index cd6dcae50..9a8bb373d 100644 --- a/cereal/custom.capnp +++ b/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 { diff --git a/common/params.cc b/common/params.cc index 9695ebfe8..ee3206ac1 100644 --- a/common/params.cc +++ b/common/params.cc @@ -229,6 +229,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}, }; } // namespace diff --git a/selfdrive/controls/lib/longitudinal_planner.py b/selfdrive/controls/lib/longitudinal_planner.py index 2157a584f..5adaf9c66 100755 --- a/selfdrive/controls/lib/longitudinal_planner.py +++ b/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) diff --git a/system/manager/manager.py b/system/manager/manager.py index 940965f09..7f1906eb6 100755 --- a/system/manager/manager.py +++ b/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')))