Driving Personality: sunnypilot changes (commaai#176)

* make it callable

* ui: disable longitudinal personality when GAC is enabled

* different profiles between stock and gac

* not this class

* update profiles in stock

* only checks once on init

* Match GAC names

* update description

selfdrive/car/interfaces.py

@@ -563,7 +563,6 @@ def sp_update_params(self, CS):
     self._frame += 1
     if self._frame % 300 == 0:
       self._frame = 0
-      self.gac = self.param_s.get_bool("GapAdjustCruise")
       self.gac_mode = round(float(self.param_s.get("GapAdjustCruiseMode", encoding="utf8")))
       self.reverse_dm_cam = self.param_s.get_bool("ReverseDmCam")
     return CS

selfdrive/controls/lib/longitudinal_mpc_lib/long_mpc.py

@@ -57,26 +57,54 @@
 COMFORT_BRAKE = 2.5
 STOP_DISTANCE = 6.0
 
-def get_jerk_factor(personality=log.LongitudinalPersonality.standard):
-  if personality == log.LongitudinalPersonality.standard:
-    return 1.0
-  elif personality == log.LongitudinalPersonality.moderate:
-    return 0.5
-  elif personality == log.LongitudinalPersonality.aggressive:
-    return 0.222
+def get_jerk_factor(personality=log.LongitudinalPersonality.standard, personality_mode="stock"):
+  if personality_mode == "stock":
+    if personality==log.LongitudinalPersonality.relaxed:
+      return 1.0
+    elif personality==log.LongitudinalPersonality.standard:
+      return 1.0
+    elif personality==log.LongitudinalPersonality.moderate:
+      return 0.5
+    elif personality==log.LongitudinalPersonality.aggressive:
+      return 0.222
+    else:
+      raise NotImplementedError("Longitudinal personality not supported")
+  elif personality_mode == "gac":
+    if personality == log.LongitudinalPersonality.standard:
+      return 1.0
+    elif personality == log.LongitudinalPersonality.moderate:
+      return 0.5
+    elif personality == log.LongitudinalPersonality.aggressive:
+      return 0.222
+    else:
+      raise NotImplementedError("Longitudinal personality not supported")
   else:
-    raise NotImplementedError("Longitudinal personality not supported")
-
-
-def get_T_FOLLOW(personality=log.LongitudinalPersonality.standard):
-  if personality == log.LongitudinalPersonality.standard:
-    return 1.45
-  elif personality == log.LongitudinalPersonality.moderate:
-    return 1.25
-  elif personality == log.LongitudinalPersonality.aggressive:
-    return 1.0
+    raise NotImplementedError("Longitudinal personality mode not supported")
+
+
+def get_T_FOLLOW(personality=log.LongitudinalPersonality.standard, personality_mode="stock"):
+  if personality_mode == "stock":
+    if personality==log.LongitudinalPersonality.relaxed:
+      return 1.75
+    elif personality==log.LongitudinalPersonality.standard:
+      return 1.45
+    elif personality==log.LongitudinalPersonality.moderate:
+      return 1.25
+    elif personality==log.LongitudinalPersonality.aggressive:
+      return 1.0
+    else:
+      raise NotImplementedError("Longitudinal personality not supported")
+  elif personality_mode == "gac":
+    if personality == log.LongitudinalPersonality.standard:
+      return 1.45
+    elif personality == log.LongitudinalPersonality.moderate:
+      return 1.25
+    elif personality == log.LongitudinalPersonality.aggressive:
+      return 1.0
+    else:
+      raise NotImplementedError("Longitudinal personality not supported")
   else:
-    raise NotImplementedError("Longitudinal personality not supported")
+    raise NotImplementedError("Longitudinal personality mode not supported")
 
 def get_stopped_equivalence_factor(v_lead):
   return (v_lead**2) / (2 * COMFORT_BRAKE)
@@ -273,8 +301,8 @@ def set_cost_weights(self, cost_weights, constraint_cost_weights):
     for i in range(N):
       self.solver.cost_set(i, 'Zl', Zl)
 
-  def set_weights(self, prev_accel_constraint=True, personality=log.LongitudinalPersonality.standard):
-    jerk_factor = get_jerk_factor(personality)
+  def set_weights(self, prev_accel_constraint=True, personality=log.LongitudinalPersonality.standard, personality_mode="stock"):
+    jerk_factor = get_jerk_factor(personality, personality_mode=personality_mode)
     if self.mode == 'acc':
       a_change_cost = A_CHANGE_COST if prev_accel_constraint else 0
       cost_weights = [X_EGO_OBSTACLE_COST, X_EGO_COST, V_EGO_COST, A_EGO_COST, jerk_factor * a_change_cost, jerk_factor * J_EGO_COST]
@@ -332,8 +360,8 @@ def set_accel_limits(self, min_a, max_a):
     self.cruise_min_a = min_a
     self.max_a = max_a
 
-  def update(self, radarstate, v_cruise, x, v, a, j, personality=log.LongitudinalPersonality.standard):
-    self.desired_TF = get_T_FOLLOW(personality)
+  def update(self, radarstate, v_cruise, x, v, a, j, personality=log.LongitudinalPersonality.standard, personality_mode="stock"):
+    self.desired_TF = get_T_FOLLOW(personality, personality_mode)
     v_ego = self.x0[1]
     self.status = radarstate.leadOne.status or radarstate.leadTwo.status
 

selfdrive/controls/lib/longitudinal_planner.py

@@ -76,6 +76,7 @@ def __init__(self, CP, init_v=0.0, init_a=0.0):
     self.events = Events()
     self.turn_speed_controller = TurnSpeedController()
 
+    self.gap_adjust_cruise = "gac" if self.params.get_bool("GapAdjustCruise") else "stock"
     self.read_gac(gac_tr=3)
 
   def read_param(self):
@@ -105,10 +106,12 @@ def parse_model(model_msg, model_error):
     return x, v, a, j
 
   def update(self, sm):
-    if self.param_read_counter % 50 == 0:
-      self.read_param()
-    self.param_read_counter += 1
-    self.read_gac(gac_tr=sm['carState'].gapAdjustCruiseTr)
+    if self.gap_adjust_cruise == "stock":
+      if self.param_read_counter % 50 == 0:
+        self.read_param()
+      self.param_read_counter += 1
+    elif self.gap_adjust_cruise == "gac":
+      self.read_gac(gac_tr=sm['carState'].gapAdjustCruiseTr)
     self.mpc.mode = 'blended' if sm['controlsState'].experimentalMode else 'acc'
 
     v_ego = sm['carState'].vEgo
@@ -154,11 +157,11 @@ def update(self, sm):
     accel_limits_turns[0] = min(accel_limits_turns[0], self.a_desired + 0.05, a_min_sol)
     accel_limits_turns[1] = max(accel_limits_turns[1], self.a_desired - 0.05)
 
-    self.mpc.set_weights(prev_accel_constraint, personality=self.personality)
+    self.mpc.set_weights(prev_accel_constraint, personality=self.personality, personality_mode=self.gap_adjust_cruise)
     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_sol, x, v, a, j, personality=self.personality)
+    self.mpc.update(sm['radarState'], v_cruise_sol, x, v, a, j, personality=self.personality, personality_mode=self.gap_adjust_cruise)
 
     self.v_desired_trajectory_full = np.interp(T_IDXS, T_IDXS_MPC, self.mpc.v_solution)
     self.a_desired_trajectory_full = np.interp(T_IDXS, T_IDXS_MPC, self.mpc.a_solution)

selfdrive/ui/qt/offroad/settings.cc

@@ -100,9 +100,9 @@ TogglesPanel::TogglesPanel(SettingsWindow *parent) : ListWidget(parent) {
   };
 
 
-  std::vector<QString> longi_button_texts{tr("Aggressive"), tr("Standard"), tr("Relaxed")};
+  std::vector<QString> longi_button_texts{tr("Maniac"), tr("Aggro"), tr("Stock"), tr("Relaxed")};
   long_personality_setting = new ButtonParamControl("LongitudinalPersonality", tr("Driving Personality"),
-                                          tr("Standard is recommended. In aggressive 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."),
+                                          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."),
                                           "../assets/offroad/icon_speed_limit.png",
                                           longi_button_texts);
   for (auto &[param, title, desc, icon] : toggle_defs) {
@@ -129,6 +129,8 @@ TogglesPanel::TogglesPanel(SettingsWindow *parent) : ListWidget(parent) {
   connect(toggles["ExperimentalLongitudinalEnabled"], &ToggleControl::toggleFlipped, [=]() {
     updateToggles();
   });
+
+  connect(new SPControlsPanel(), &SPControlsPanel::updateStockToggles, this, &TogglesPanel::updateToggles);
 }
 
 void TogglesPanel::expandToggleDescription(const QString &param) {
@@ -179,7 +181,7 @@ void TogglesPanel::updateToggles() {
       // normal description and toggle
       e2e_toggle->setEnabled(true);
       e2e_toggle->setDescription(e2e_description);
-      long_personality_setting->setEnabled(true);
+      long_personality_setting->setEnabled(!params.getBool("GapAdjustCruise"));
       custom_stock_long_toggle->setEnabled(false);
       params.remove("CustomStockLong");
     } else {

selfdrive/ui/qt/offroad/settings.h

@@ -68,13 +68,12 @@ class TogglesPanel : public ListWidget {
 
 public slots:
   void expandToggleDescription(const QString &param);
+  void updateToggles();
 
 private:
   Params params;
   std::map<std::string, ParamControl*> toggles;
   ButtonParamControl *long_personality_setting;
-
-  void updateToggles();
 };
 
 class SoftwarePanel : public ListWidget {

selfdrive/ui/qt/offroad/sunnypilot_settings.cc

@@ -271,7 +271,7 @@ SPControlsPanel::SPControlsPanel(QWidget *parent) : ListWidget(parent) {
   // toggle names to trigger updateToggles() when toggleFlipped and display ConfirmationDialog::alert
   std::vector<std::string> toggleOffroad{
     "EnableMads", "DisengageLateralOnBrake", "AccMadsCombo", "MadsCruiseMain", "BelowSpeedPause", "EnforceTorqueLateral",
-    "CustomTorqueLateral", "LiveTorque"
+    "CustomTorqueLateral", "LiveTorque", "GapAdjustCruise"
   };
 
   // Controls: Camera Offset (cm)
@@ -361,6 +361,7 @@ SPControlsPanel::SPControlsPanel(QWidget *parent) : ListWidget(parent) {
 
   connect(auto_lane_change_timer, &AutoLaneChangeTimer::toggleUpdated, this, &SPControlsPanel::updateToggles);
   connect(slo_type, &SpeedLimitOffsetType::offsetTypeUpdated, this, &SPControlsPanel::updateToggles);
+  connect(toggles["GapAdjustCruise"], &ToggleControl::toggleFlipped, [=]() { emit updateStockToggles(); });
 
   toggles["EnableMads"]->setConfirmation(true, false);
   toggles["DisengageLateralOnBrake"]->setConfirmation(true, false);

selfdrive/ui/qt/offroad/sunnypilot_settings.h

@@ -289,6 +289,9 @@ class SPControlsPanel : public ListWidget {
   explicit SPControlsPanel(QWidget *parent = nullptr);
   void showEvent(QShowEvent *event) override;
 
+signals:
+  void updateStockToggles();
+
 public slots:
   void updateToggles();