Update long_mpc.py

common/params.cc

@@ -255,6 +255,7 @@ std::unordered_map<std::string, uint32_t> keys = {
     {"EndToEndLongToggle", PERSISTENT | BACKUP},
     {"EnforceTorqueLateral", PERSISTENT | BACKUP},
     {"EnhancedScc", PERSISTENT | BACKUP},
+    {"FastTakeOff", PERSISTENT | BACKUP},
     {"FeatureStatus", PERSISTENT | BACKUP},
     {"FleetManagerPin", PERSISTENT},
     {"ForceOffroad", CLEAR_ON_MANAGER_START},

selfdrive/car/toyota/interface.py

@@ -162,24 +162,21 @@ class CarInterface(CarInterfaceBase):
 
     # hand tuned (August 12, 2024)
     def custom_tss2_longitudinal_tuning():
-      ret.vEgoStopping = 0.25
-      ret.vEgoStarting = 0.25
-      ret.stoppingDecelRate = 0.0074
-
+      ret.vEgoStopping = 0.01
+      ret.vEgoStarting = 0.01
+      ret.stoppingDecelRate = 0.007
     def default_tss2_longitudinal_tuning():
       ret.vEgoStopping = 0.25
       ret.vEgoStarting = 0.25
       ret.stoppingDecelRate = 0.3  # reach stopping target smoothly
-
     def default_longitudinal_tuning():
       tune.kiBP = [0., 5., 35.]
       tune.kiV = [3.6, 2.4, 1.5]
-
     tune = ret.longitudinalTuning
     if candidate in TSS2_CAR or ret.enableGasInterceptorDEPRECATED:
       if sp_tss2_long_tune:
-        tune.kiBP = [0.,   3.,    8.,    12.,  20.,  27.,  36.,  50]
-        tune.kiV = [0.322, 0.244, 0.224, 0.202, 0.17, 0.12, 0.08, 0.06]
+        tune.kiBP = [ 0.,  5.,   12.,  20., 27., 40.]
+        tune.kiV =  [.35, .228,  .205, .195, .10, .01]
         custom_tss2_longitudinal_tuning()
       else:
         tune.kpV = [0.0]

selfdrive/controls/lib/longitudinal_mpc_lib/long_mpc.py

@@ -4,6 +4,7 @@ import time
 import numpy as np
 from cereal import custom
 from openpilot.common.numpy_fast import clip
+from openpilot.common.conversions import Conversions as CV
 from openpilot.common.realtime import DT_MDL
 from openpilot.common.swaglog import cloudlog
 # WARNING: imports outside of constants will not trigger a rebuild
@@ -63,9 +64,9 @@ def get_jerk_factor(personality=custom.LongitudinalPersonalitySP.standard):
   elif personality==custom.LongitudinalPersonalitySP.standard:
     return 1.0
   elif personality==custom.LongitudinalPersonalitySP.moderate:
-    return 0.85
+    return 0.95
   elif personality==custom.LongitudinalPersonalitySP.aggressive:
-    return 0.8
+    return 0.9
   elif personality==custom.LongitudinalPersonalitySP.overtake:
     return 0.1
   else:
@@ -93,13 +94,13 @@ def get_dynamic_personality(v_ego, personality=custom.LongitudinalPersonalitySP.
     y_dist = [1.0,  1.0,  1.75,  1.75,  1.83]
   elif personality==custom.LongitudinalPersonalitySP.standard:
     x_vel =  [0,    20.,   27.7]
-    y_dist = [1.75, 1.75,  1.70]
+    y_dist = [1.70, 1.70,  1.70]
   elif personality==custom.LongitudinalPersonalitySP.moderate:
     x_vel =  [0,    27.69, 27.7]
-    y_dist = [1.45, 1.45,  1.40]
+    y_dist = [1.40, 1.40,  1.45]
   elif personality==custom.LongitudinalPersonalitySP.aggressive:
-    x_vel =  [0,    20.0, 20.01, 27.69, 27.7]
-    y_dist = [1.07, 1.07, 1.12,  1.12,  1.20]
+    x_vel =  [0,    5.,    20.0, 20.01, 27.69, 27.7]
+    y_dist = [0.80, 1.10, 1.10, 1.12,  1.12,  1.20]
   else:
     raise NotImplementedError("Dynamic personality not supported")
   return np.interp(v_ego, x_vel, y_dist)
@@ -107,6 +108,21 @@ def get_dynamic_personality(v_ego, personality=custom.LongitudinalPersonalitySP.
 def get_stopped_equivalence_factor(v_lead):
   return (v_lead**2) / (2 * COMFORT_BRAKE)
 
+def get_stopped_equivalence_factor_krkeegen(v_lead, v_ego):
+  # KRKeegan this offset rapidly decreases the following distance when the lead pulls
+  # away, resulting in an early demand for acceleration.
+  v_diff_offset = 0
+  v_diff_offset_max = 12
+  speed_to_reach_max_v_diff_offset = 26 # in kp/h
+  speed_to_reach_max_v_diff_offset = speed_to_reach_max_v_diff_offset * CV.KPH_TO_MS
+  delta_speed = v_lead - v_ego
+  if np.all(delta_speed > 0):
+    v_diff_offset = delta_speed * 2
+    v_diff_offset = np.clip(v_diff_offset, 0, v_diff_offset_max)
+    # increase in a linear behavior
+    v_diff_offset = np.maximum(v_diff_offset * ((speed_to_reach_max_v_diff_offset - v_ego)/speed_to_reach_max_v_diff_offset), 0)
+  return (v_lead**2) / (2 * COMFORT_BRAKE) + v_diff_offset
+
 def get_safe_obstacle_distance(v_ego, t_follow):
   return (v_ego**2) / (2 * COMFORT_BRAKE) + t_follow * v_ego + STOP_DISTANCE
 
@@ -301,15 +317,24 @@ class LongitudinalMpc:
     for i in range(N):
       self.solver.cost_set(i, 'Zl', Zl)
 
-  def set_weights(self, prev_accel_constraint=True, personality=custom.LongitudinalPersonalitySP.standard):
+  def set_weights(self, prev_accel_constraint=True, personality=custom.LongitudinalPersonalitySP.standard, v_lead0=0, v_lead1=0):
     jerk_factor = get_jerk_factor(personality)
+    v_ego = self.x0[1]
+    v_ego_bps = [0, 10]
+    # KRKeegan adjustments to improve sluggish acceleration
+    # do not apply to deceleration
+    j_ego_v_ego = 1
+    a_change_v_ego = 1
+    if (v_lead0 - v_ego >= 0) and (v_lead1 - v_ego >= 0):
+      j_ego_v_ego = np.interp(v_ego, v_ego_bps, [.05, 1.])
+      a_change_v_ego = np.interp(v_ego, v_ego_bps, [.05, 1.])
     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]
+      cost_weights = [X_EGO_OBSTACLE_COST, X_EGO_COST, V_EGO_COST, A_EGO_COST, jerk_factor * a_change_cost * a_change_v_ego, jerk_factor * J_EGO_COST * j_ego_v_ego]
       constraint_cost_weights = [LIMIT_COST, LIMIT_COST, LIMIT_COST, DANGER_ZONE_COST]
     elif self.mode == 'blended':
       a_change_cost = 40.0 if prev_accel_constraint else 0
-      cost_weights = [0., 0.1, 0.2, 5.0, a_change_cost, 1.0]
+      cost_weights = [0., 0.1, 0.2, 5.0, a_change_cost * a_change_v_ego, 1.0]
       constraint_cost_weights = [LIMIT_COST, LIMIT_COST, LIMIT_COST, 50.0]
     else:
       raise NotImplementedError(f'Planner mode {self.mode} not recognized in planner cost set')
@@ -361,7 +386,7 @@ class LongitudinalMpc:
     self.max_a = max_a
 
   def update(self, radarstate, v_cruise, x, v, a, j, personality=custom.LongitudinalPersonalitySP.standard,
-             dynamic_personality=False, overtaking_acceleration_assist=False):
+             dynamic_personality=False, overtaking_acceleration_assist=False, fast_take_off = False):
     v_ego = self.x0[1]
     t_follow = get_dynamic_personality(v_ego, personality) if dynamic_personality else get_T_FOLLOW(personality)
     t_follow = get_T_FOLLOW(custom.LongitudinalPersonalitySP.overtake) if overtaking_acceleration_assist else t_follow
@@ -373,8 +398,12 @@ class LongitudinalMpc:
     # To estimate a safe distance from a moving lead, we calculate how much stopping
     # distance that lead needs as a minimum. We can add that to the current distance
     # and then treat that as a stopped car/obstacle at this new distance.
-    lead_0_obstacle = lead_xv_0[:,0] + get_stopped_equivalence_factor(lead_xv_0[:,1])
-    lead_1_obstacle = lead_xv_1[:,0] + get_stopped_equivalence_factor(lead_xv_1[:,1])
+    if fast_take_off:
+      lead_0_obstacle = lead_xv_0[:,0] + get_stopped_equivalence_factor_krkeegen(lead_xv_0[:,1], v_ego)
+      lead_1_obstacle = lead_xv_1[:,0] + get_stopped_equivalence_factor_krkeegen(lead_xv_1[:,1], v_ego)
+    else:
+      lead_0_obstacle = lead_xv_0[:,0] + get_stopped_equivalence_factor(lead_xv_0[:,1])
+      lead_1_obstacle = lead_xv_1[:,0] + get_stopped_equivalence_factor(lead_xv_1[:,1])
 
     cruise_target_e2ex = T_IDXS * np.clip(v_cruise, v_ego - 2.0, 1e3) + x[0]
     e2e_xforward = ((v[1:] + v[:-1]) / 2) * (T_IDXS[1:] - T_IDXS[:-1])

selfdrive/controls/lib/longitudinal_planner.py

@@ -103,6 +103,7 @@ class LongitudinalPlanner:
     self.turn_speed_controller = TurnSpeedController()
     self.dynamic_experimental_controller = DynamicExperimentalController()
     self.accel_controller = AccelController()
+    self.fast_take_off = False
 
   def read_param(self):
     try:
@@ -110,6 +111,7 @@ class LongitudinalPlanner:
     except AttributeError:
       self.dynamic_experimental_controller = DynamicExperimentalController()
       self.accel_controller = AccelController()
+      self.fast_take_off = self.params.get_bool("FastTakeOff")
 
   @staticmethod
   def parse_model(model_msg, model_error):
@@ -202,7 +204,7 @@ class LongitudinalPlanner:
     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,
-                    dynamic_personality=sm['controlsStateSP'].dynamicPersonality, overtaking_acceleration_assist=overtaking_accel_engaged)
+                    dynamic_personality=sm['controlsStateSP'].dynamicPersonality, overtaking_acceleration_assist=overtaking_accel_engaged, fast_take_off=self.fast_take_off)
 
     self.v_desired_trajectory = np.interp(CONTROL_N_T_IDX, T_IDXS_MPC, self.mpc.v_solution)
     self.a_desired_trajectory = np.interp(CONTROL_N_T_IDX, T_IDXS_MPC, self.mpc.a_solution)

selfdrive/controls/lib/sunnypilot/accel_controller.py

@@ -29,13 +29,13 @@ from openpilot.common.numpy_fast import interp
 AccelPersonality = custom.AccelerationPersonality
 
 # accel personality by @arne182 modified by cgw and kumar
-_DP_CRUISE_MIN_V =       [-0.031, -0.031, -0.080, -0.080, -0.19, -0.19, -0.59, -0.59, -0.79, -0.79, -1.0,  -1.0]
-_DP_CRUISE_MIN_V_ECO =   [-0.030, -0.030, -0.075, -0.075, -0.18, -0.18, -0.58, -0.58, -0.78, -0.78, -1.0,  -1.0]
-_DP_CRUISE_MIN_V_SPORT = [-0.102, -0.102, -0.085, -0.085, -0.20, -0.20, -0.60, -0.60, -0.80, -0.80, -1.0,  -1.0]
+_DP_CRUISE_MIN_V =       [-0.69, -0.69, -0.69, -0.69, -0.69, -0.69, -0.79, -0.79, -0.79, -0.79, -1.0,  -1.0]
+_DP_CRUISE_MIN_V_ECO =   [-0.68, -0.68, -0.68, -0.68, -0.68, -0.68, -0.78, -0.78, -0.88, -0.88, -1.0,  -1.0]
+_DP_CRUISE_MIN_V_SPORT = [-0.70, -0.70, -0.70, -0.70, -0.70, -0.70, -0.80, -0.80, -0.90, -0.90, -1.0,  -1.0]
 _DP_CRUISE_MIN_BP =      [0.,     3.0,    3.01,   10.,    10.01, 14.,   14.01, 18.,   18.01,  22.,  22.01, 30.]
 
-_DP_CRUISE_MAX_V =       [2.0, 2.0, 2.0, 1.70, 1.11, .70,  .54,  .38,  .17]
-_DP_CRUISE_MAX_V_ECO =   [2.0, 2.0, 1.8, 1.40, 0.90, .53,  .43,  .32,  .09]
+_DP_CRUISE_MAX_V =       [2.0, 2.0, 2.0, 1.80, 1.11, .70,  .54,  .38,  .17]
+_DP_CRUISE_MAX_V_ECO =   [2.0, 2.0, 2.0, 1.60, 0.90, .53,  .43,  .32,  .09]
 _DP_CRUISE_MAX_V_SPORT = [2.0, 2.0, 2.0, 2.00, 1.40, .90,  .70,  .50,  .30]
 _DP_CRUISE_MAX_BP =      [0.,  4.,  6.,  8.,   11.,  20.,  25.,  30.,  40.]
 

selfdrive/ui/sunnypilot/qt/offroad/settings/sunnypilot_settings.cc

@@ -43,6 +43,12 @@ SunnypilotPanel::SunnypilotPanel(QWidget *parent) : QFrame(parent) {
       tr("Enable the beloved M.A.D.S. feature. Disable toggle to revert back to stock openpilot engagement/disengagement."),
       "../assets/offroad/icon_blank.png",
     },
+    {
+      "FastTakeOff",
+      tr("Very fast prius"),
+      tr("When prius goes faster then sunnys car :) vroom"),
+      "../assets/offroad/icon_blank.png",
+    },
     {
       "VisionCurveLaneless",
       tr("Laneless for Curves in \"Auto\" Mode"),

selfdrive/ui/sunnypilot/qt/onroad/annotated_camera.cc

@@ -216,6 +216,7 @@ void AnnotatedCameraWidgetSP::updateState(const UIStateSP &s) {
   latAccelFactorFiltered = ltp.getLatAccelFactorFiltered();
   frictionCoefficientFiltered = ltp.getFrictionCoefficientFiltered();
   liveValid = ltp.getLiveValid();
+  ecoMode = vEgo > 0 && car_state.getEngineRpm() == 0;
   // ############################## DEV UI END ##############################
 
   btnPerc = s.scene.sleep_btn_opacity * 0.05;
@@ -523,10 +524,32 @@ void AnnotatedCameraWidgetSP::drawHud(QPainter &p) {
 
   // current speed
   if (!hideVEgoUi) {
+    // Set up the font for the speed text
     p.setFont(InterFont(176, QFont::Bold));
-    drawColoredText(p, rect().center().x(), 210, speedStr, brakeLights ? QColor(0xff, 0, 0, 255) : QColor(0xff, 0xff, 0xff, 255));
+
+    // Define text coordinates
+    int centerX = rect().center().x();
+    int centerY = 210;
+
+    // Draw a red border if brakeLights is active
+    if (brakeLights) {
+      for (int offsetX = -6; offsetX <= 6; offsetX++) {
+        for (int offsetY = -6; offsetY <= 6; offsetY++) {
+          // Avoid drawing at the original text position
+          if (offsetX != 0 || offsetY != 0) {
+            drawColoredText(p, centerX + offsetX, centerY + offsetY, speedStr, QColor(255, 0, 0, 255)); // Red border
+          }
+        }
+      }
+    }
+
+    // Draw the main speed text: green if ecoMode is on, otherwise white
+    QColor speedTextColor = ecoMode ? QColor(0, 245, 0) : QColor(255, 255, 255, 255);
+    drawColoredText(p, centerX, centerY, speedStr, speedTextColor);
+
+    // Draw the speed unit below the main text
     p.setFont(InterFont(66));
-    drawText(p, rect().center().x(), 290, speedUnit, 200);
+    drawText(p, centerX, 290, speedUnit, 200);
   }
 
   if (!reversing) {

selfdrive/ui/sunnypilot/qt/onroad/annotated_camera.h

@@ -176,6 +176,7 @@ private:
   float latAccelFactorFiltered;
   float frictionCoefficientFiltered;
   bool liveValid;
+  bool ecoMode;
   // ############################## DEV UI END ##############################
 
   float btnPerc;

system/manager/manager.py

@@ -124,6 +124,7 @@ def manager_init() -> None:
     ("LastSunnylinkPingTime", "0"),
     ("EnableGitlabRunner", "0"),
     ("EnableSunnylinkUploader", "0"),
+    ("FastTakeOff", "0"),
   ]
   if not PC:
     default_params.append(("LastUpdateTime", datetime.datetime.now(datetime.UTC).replace(tzinfo=None).isoformat().encode('utf8')))