diff --git a/cereal/custom.capnp b/cereal/custom.capnp index b557ef659b..2e2361d027 100644 --- a/cereal/custom.capnp +++ b/cereal/custom.capnp @@ -28,7 +28,8 @@ enum AccelerationProfile { struct ControlsStateSP @0x81c2f05a394cf4af { lateralState @0 :Text; personality @8 :LongitudinalPersonalitySP; - accelProfile @9 :AccelerationProfile; + dynamicPersonality @9 :Bool; + accelProfile @10 :AccelerationProfile; lateralControlState :union { indiState @1 :LateralINDIState; diff --git a/common/params.cc b/common/params.cc index 172f99b2e5..aaeb286aaa 100644 --- a/common/params.cc +++ b/common/params.cc @@ -245,6 +245,7 @@ std::unordered_map keys = { {"DrivingModelUrl", PERSISTENT}, {"DynamicExperimentalControl", PERSISTENT | BACKUP}, {"DynamicLaneProfile", PERSISTENT | BACKUP}, + {"DynamicPersonality", PERSISTENT | BACKUP}, {"EnableAmap", PERSISTENT | BACKUP}, {"EnableGmap", PERSISTENT | BACKUP}, {"EnableMads", PERSISTENT | BACKUP}, diff --git a/selfdrive/controls/controlsd.py b/selfdrive/controls/controlsd.py index 96178ccdd7..301add6cf1 100755 --- a/selfdrive/controls/controlsd.py +++ b/selfdrive/controls/controlsd.py @@ -188,6 +188,8 @@ class Controls: model_capabilities = ModelCapabilities.get_by_gen(self.model_gen) self.model_use_lateral_planner = self.custom_model and model_capabilities & ModelCapabilities.LateralPlannerSolution + self.dynamic_personality = self.params.get_bool("DynamicPersonality") + self.accel_profile = self.read_accel_profile_param() self.can_log_mono_time = 0 @@ -861,6 +863,7 @@ class Controls: controlsStateSP.lateralState = lat_tuning controlsStateSP.personality = self.personality + controlsStateSP.dynamicPersonality = self.dynamic_personality controlsStateSP.accelProfile = self.accel_profile if self.enable_nnff and lat_tuning == 'torque': @@ -922,6 +925,7 @@ class Controls: self.experimental_mode = self.params.get_bool("ExperimentalMode") and (self.CP.openpilotLongitudinalControl or (not self.CP.pcmCruiseSpeed and self.custom_stock_planner_speed)) self.personality = self.read_personality_param() + self.dynamic_personality = self.params.get_bool("DynamicPersonality") self.accel_profile = self.read_accel_profile_param() if self.CP.notCar: self.joystick_mode = self.params.get_bool("JoystickDebugMode") diff --git a/selfdrive/controls/lib/longitudinal_mpc_lib/long_mpc.py b/selfdrive/controls/lib/longitudinal_mpc_lib/long_mpc.py index cba398bcca..a2df527a55 100755 --- a/selfdrive/controls/lib/longitudinal_mpc_lib/long_mpc.py +++ b/selfdrive/controls/lib/longitudinal_mpc_lib/long_mpc.py @@ -63,9 +63,9 @@ def get_jerk_factor(personality=custom.LongitudinalPersonalitySP.standard): elif personality==custom.LongitudinalPersonalitySP.standard: return 1.0 elif personality==custom.LongitudinalPersonalitySP.moderate: - return 0.5 + return 0.8 elif personality==custom.LongitudinalPersonalitySP.aggressive: - return 0.222 + return 0.6 else: raise NotImplementedError("Longitudinal personality not supported") @@ -82,6 +82,26 @@ def get_T_FOLLOW(personality=custom.LongitudinalPersonalitySP.standard): else: raise NotImplementedError("Longitudinal personality not supported") + +def get_dynamic_personality(v_ego, personality=custom.LongitudinalPersonalitySP.standard): + if personality==custom.LongitudinalPersonalitySP.relaxed: + x_vel = [0, 11, 14.5, 15, 20, 20.01, 25, 25.01, 36, 36.01] + y_dist = [1.5, 1.5, 1.5, 1.6, 1.76, 1.76, 1.78, 1.78, 1.8, 1.8] + elif personality==custom.LongitudinalPersonalitySP.standard: + x_vel = [0, 11, 14.5, 15, 20, 20.01, 25, 25.01, 36, 36.01] + y_dist = [1.40, 1.40, 1.40, 1.50, 1.60, 1.76, 1.76, 1.78, 1.8, 1.8] + elif personality==custom.LongitudinalPersonalitySP.moderate: + x_vel = [0, 11, 14.5, 15, 20, 20.01, 25, 25.01, 36, 36.01] + y_dist = [1.3, 1.3, 1.3, 1.35, 1.35, 1.385, 1.385, 1.4, 1.4, 1.45] + elif personality==custom.LongitudinalPersonalitySP.aggressive: + x_vel = [0, 5, 5.01, 11, 14.5, 15, 20, 20.01, 25, 25.01, 36, 36.01] + y_dist = [1.12, 1.12, 1.12, 1.12, 1.12, 1.105, 1.105, 1.15, 1.15, 1.18, 1.20, 1.23] + else: + raise NotImplementedError("Dynamic personality not supported") + + return np.interp(v_ego, x_vel, y_dist) + + def get_stopped_equivalence_factor(v_lead): return (v_lead**2) / (2 * COMFORT_BRAKE) @@ -338,9 +358,9 @@ class LongitudinalMpc: self.cruise_min_a = min_a self.max_a = max_a - def update(self, radarstate, v_cruise, x, v, a, j, personality=custom.LongitudinalPersonalitySP.standard): - t_follow = get_T_FOLLOW(personality) + def update(self, radarstate, v_cruise, x, v, a, j, personality=custom.LongitudinalPersonalitySP.standard, dynamic_personality=False): v_ego = self.x0[1] + t_follow = get_dynamic_personality(v_ego, personality) if dynamic_personality else get_T_FOLLOW(personality) self.status = radarstate.leadOne.status or radarstate.leadTwo.status lead_xv_0 = self.process_lead(radarstate.leadOne) diff --git a/selfdrive/controls/lib/longitudinal_planner.py b/selfdrive/controls/lib/longitudinal_planner.py index 9e554865f0..4b2d530f08 100755 --- a/selfdrive/controls/lib/longitudinal_planner.py +++ b/selfdrive/controls/lib/longitudinal_planner.py @@ -195,7 +195,7 @@ 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['controlsStateSP'].personality) + self.mpc.update(sm['radarState'], v_cruise, x, v, a, j, personality=sm['controlsStateSP'].personality, dynamic_personality=sm['controlsStateSP'].dynamicPersonality) 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) diff --git a/selfdrive/ui/qt/offroad/settings.cc b/selfdrive/ui/qt/offroad/settings.cc index d846ebe999..882aa5d1b6 100644 --- a/selfdrive/ui/qt/offroad/settings.cc +++ b/selfdrive/ui/qt/offroad/settings.cc @@ -61,6 +61,12 @@ TogglesPanel::TogglesPanel(SettingsWindow *parent) : ListWidget(parent) { tr("Enable toggle to allow the model to determine when to use openpilot ACC or openpilot End to End Longitudinal."), "../assets/offroad/icon_blank.png", }, + { + "DynamicPersonality", + tr("Enable Dynamic Personality"), + tr("Enable this to allow sunnypilot to dynamically adjust following distance and reaction based on your \"Driving Personality\" setting."), + "../assets/offroad/icon_blank.png", + }, { "DisengageOnAccelerator", tr("Disengage on Accelerator Pedal"), @@ -114,9 +120,9 @@ TogglesPanel::TogglesPanel(SettingsWindow *parent) : ListWidget(parent) { }; - std::vector longi_button_texts{tr("Maniac"), tr("Aggressive"), tr("Stock"), tr("Relaxed")}; + std::vector longi_button_texts{tr("Aggressive"), tr("Moderate"), tr("Standard"), tr("Relaxed")}; long_personality_setting = new ButtonParamControl("LongitudinalPersonality", tr("Driving Personality"), - tr("Stock is recommended. In aggressive/maniac mode, openpilot will follow lead cars closer and be more aggressive with the gas and brake. " + tr("Standard is recommended. In aggressive/moderate mode, openpilot will follow lead cars closer and be more aggressive with the gas and brake. " "In relaxed mode openpilot will stay further away from lead cars. On supported cars, you can cycle through these personalities with " "your steering wheel distance button."), "../assets/offroad/icon_blank.png", diff --git a/selfdrive/ui/ui.h b/selfdrive/ui/ui.h index ed9996f54d..a43887a100 100644 --- a/selfdrive/ui/ui.h +++ b/selfdrive/ui/ui.h @@ -27,7 +27,7 @@ const int UI_ROAD_NAME_MARGIN_X = 14; struct FeatureStatusText { const QStringList dlp_list_text = { "Laneful", "Laneless", "Auto" }; - const QStringList gac_list_text = { "Maniac", "Aggressive", "Standard", "Relaxed" }; + const QStringList gac_list_text = { "Aggressive", "Moderate", "Standard", "Relaxed" }; const QStringList slc_list_text = { "Inactive", "Temp Off", "Adapting", "Active", "Pre Active" }; }; diff --git a/system/manager/manager.py b/system/manager/manager.py index 973eb4091d..2010595f63 100755 --- a/system/manager/manager.py +++ b/system/manager/manager.py @@ -64,6 +64,7 @@ def manager_init() -> None: ("DisengageLateralOnBrake", "0"), ("DrivingModelGeneration", "0"), ("DynamicLaneProfile", "1"), + ("DynamicPersonality", "0"), ("EnableMads", "1"), ("EnhancedScc", "0"), ("FeatureStatus", "1"),