dynamic personality

common/params.cc

@@ -243,6 +243,7 @@ std::unordered_map<std::string, uint32_t> keys = {
     {"DrivingModelUrl", PERSISTENT},
     {"DynamicExperimentalControl", PERSISTENT | BACKUP},
     {"DynamicLaneProfile", PERSISTENT | BACKUP},
+    {"DynamicPersonality", PERSISTENT | BACKUP},
     {"EnableAmap", PERSISTENT | BACKUP},
     {"EnableGmap", PERSISTENT | BACKUP},
     {"EnableMads", PERSISTENT | BACKUP},

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.75
   elif personality==custom.LongitudinalPersonalitySP.aggressive:
-    return 0.222
+    return 0.5
   else:
     raise NotImplementedError("Longitudinal personality not supported")
 
@@ -82,6 +82,24 @@ 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.25,  1.25,  1.12,  1.12,  1.12,  1.105, 1.105, 1.15, 1.15, 1.18, 1.20,  1.23]
+  else:
+    raise NotImplementedError("Dynamic Follow 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 +356,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_T_FOLLOW(personality) if not dynamic_personality else get_dynamic_personality(v_ego, personality)
     self.status = radarstate.leadOne.status or radarstate.leadTwo.status
 
     lead_xv_0 = self.process_lead(radarstate.leadOne)

selfdrive/controls/lib/longitudinal_planner.py

@@ -101,11 +101,14 @@ class LongitudinalPlanner:
     self.turn_speed_controller = TurnSpeedController()
     self.dynamic_experimental_controller = DynamicExperimentalController()
 
+    self.dynamic_personality = False
+
   def read_param(self):
     try:
       self.dynamic_experimental_controller.set_enabled(self.params.get_bool("DynamicExperimentalControl"))
     except AttributeError:
       self.dynamic_experimental_controller = DynamicExperimentalController()
+    self.dynamic_personality = self.params.get_bool("DynamicPersonality")
 
   @staticmethod
   def parse_model(model_msg, model_error):
@@ -178,7 +181,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['controlsState'].personality)
+    self.mpc.update(sm['radarState'], v_cruise, x, v, a, j, personality=sm['controlsState'].personality, dynamic_personality=self.dynamic_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)

selfdrive/ui/qt/offroad/settings.cc

@@ -55,6 +55,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 toggle to allow sunnypilot will dynamically adjust following distance based on your \"Driving Personality\" setting."),
+      "../assets/offroad/icon_blank.png",
+    },
     {
       "DisengageOnAccelerator",
       tr("Disengage on Accelerator Pedal"),
@@ -108,7 +114,7 @@ TogglesPanel::TogglesPanel(SettingsWindow *parent) : ListWidget(parent) {
   };
 
 
-  std::vector<QString> longi_button_texts{tr("Maniac"), tr("Aggressive"), tr("Stock"), tr("Relaxed")};
+  std::vector<QString> 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. "
                                              "In relaxed mode openpilot will stay further away from lead cars. On supported cars, you can cycle through these personalities with "

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" };
 };
 

system/manager/manager.py

@@ -64,6 +64,7 @@ def manager_init() -> None:
     ("DisengageLateralOnBrake", "0"),
     ("DrivingModelGeneration", "0"),
     ("DynamicLaneProfile", "1"),
+    ("DynamicFollow", "0"),
     ("EnableMads", "1"),
     ("EnhancedScc", "0"),
     ("FeatureStatus", "1"),