test

This reverts commit a0fe916e4b.

selfdrive/car/toyota/carstate.py

@@ -203,29 +203,37 @@ class CarState(CarStateBase):
 
     if self.toyota_drive_mode:
       # Determine sport signal based on car model
-      sport_signal = 'SPORT_ON_2' if self.CP.carFingerprint in (CAR.TOYOTA_RAV4_TSS2, CAR.LEXUS_ES_TSS2) else 'SPORT_ON'
+      sport_signal = 'SPORT_ON_2' if self.CP.carFingerprint in (CAR.TOYOTA_RAV4_TSS2, CAR.LEXUS_ES_TSS2, CAR.TOYOTA_HIGHLANDER_TSS2) else 'SPORT_ON'
 
       # Check signals once
       if not self.signals_checked:
         self.signals_checked = True
 
-        # Get sport and eco signals, using default values if not present
-        sport_mode = cp.vl["GEAR_PACKET"].get(sport_signal, 0)
-        eco_mode = cp.vl["GEAR_PACKET"].get('ECON_ON', 0)
+        # Try to detect sport mode signal, handle missing signal with a fallback
+        try:
+          sport_mode = cp.vl["GEAR_PACKET"][sport_signal]
+          self.sport_signal_seen = True
+        except KeyError:
+          sport_mode = 0
+          self.sport_signal_seen = False
 
-        # Track if signals were detected
-        self.sport_signal_seen = bool(sport_mode)
-        self.eco_signal_seen = bool(eco_mode)
+        # Try to detect eco mode signal, handle missing signal with a fallback
+        try:
+          eco_mode = cp.vl["GEAR_PACKET"]['ECON_ON']
+          self.eco_signal_seen = True
+        except KeyError:
+          eco_mode = 0
+          self.eco_signal_seen = False
       else:
-        # Use previously detected signals if they were seen
-        sport_mode = cp.vl["GEAR_PACKET"].get(sport_signal, 0) if self.sport_signal_seen else 0
-        eco_mode = cp.vl["GEAR_PACKET"].get('ECON_ON', 0) if self.eco_signal_seen else 0
+        # Always re-check the signals to account for mode changes
+        sport_mode = cp.vl["GEAR_PACKET"][sport_signal] if self.sport_signal_seen else 0
+        eco_mode = cp.vl["GEAR_PACKET"]['ECON_ON'] if self.eco_signal_seen else 0
 
-      # Set acceleration profile based on detected modes, prioritize eco over sport if both are detected
-      if eco_mode == 1:
-        self.accel_profile = AccelPersonality.eco
-      elif sport_mode == 1:
+      # Set acceleration profile based on detected modes, with sport mode having higher priority
+      if sport_mode == 1:
         self.accel_profile = AccelPersonality.sport
+      elif eco_mode == 1:
+        self.accel_profile = AccelPersonality.eco
       else:
         self.accel_profile = AccelPersonality.normal
 
@@ -235,9 +243,8 @@ class CarState(CarStateBase):
       if not self.accel_profile_init or self.accel_profile != self.prev_accel_profile:
         Params().put_nonblocking('AccelPersonality', str(self.accel_profile))
         self.accel_profile_init = True
-
-      # Update the previous profile
-      self.prev_accel_profile = self.accel_profile
+        # Update the previous profile to prevent unnecessary re-syncing
+        self.prev_accel_profile = self.accel_profile
 
     if self.CP.carFingerprint != CAR.TOYOTA_MIRAI:
       ret.engineRpm = cp.vl["ENGINE_RPM"]["RPM"]

selfdrive/car/toyota/interface.py

@@ -164,13 +164,12 @@ class CarInterface(CarInterfaceBase):
     def custom_tss2_longitudinal_tuning():
       ret.vEgoStopping = 0.25
       ret.vEgoStarting = 0.01
-      ret.stopAccel = -2.0
-      ret.stoppingDecelRate = 0.002  # reach stopping target smoothly
+      ret.stoppingDecelRate = 0.006
 
     def default_tss2_longitudinal_tuning():
       ret.vEgoStopping = 0.25
-      ret.vEgoStarting = 0.01
-      ret.stoppingDecelRate = 0.3  # reach stopping target smoothly
+      ret.vEgoStarting = 0.25
+      ret.stoppingDecelRate = 0.002  # reach stopping target smoothly
 
     def default_longitudinal_tuning():
       tune.kiBP = [0., 5., 35.]
@@ -179,12 +178,8 @@ class CarInterface(CarInterfaceBase):
     tune = ret.longitudinalTuning
     if candidate in TSS2_CAR or ret.enableGasInterceptorDEPRECATED:
       if sp_tss2_long_tune:
-        #tune.kiBP = [0.,  0.03,  5., 10., 15., 30.]
-        #tune.kiV = [0.1,  0.12, 0.08, 0.06, 0.5, 1.0]
-        #tune.kpBP = [0., 5., 20.]
-        #tune.kpV = [2.3, 1.0, 0.7]
-        tune.kiBP = [0.,  3.,   8.,  20.,  27.,  40.]
-        tune.kiV = [.35,  .235,  .20,  .17,  .10, .06]
+        tune.kiBP = [0.,  5.,   12.,  20.,  27.,  36.,  40.]
+        tune.kiV = [0.34, 0.234, 0.20, 0.17, 0.105, 0.09, 0.08]
         custom_tss2_longitudinal_tuning()
       else:
         tune.kpV = [0.0]

selfdrive/controls/lib/longitudinal_mpc_lib/long_mpc.py

@@ -3,7 +3,7 @@ import os
 import time
 import numpy as np
 from cereal import custom
-from openpilot.common.numpy_fast import clip
+from openpilot.common.numpy_fast import clip, interp
 from openpilot.common.realtime import DT_MDL
 from openpilot.common.swaglog import cloudlog
 # WARNING: imports outside of constants will not trigger a rebuild
@@ -55,7 +55,7 @@ T_IDXS = np.array(T_IDXS_LST)
 FCW_IDXS = T_IDXS < 5.0
 T_DIFFS = np.diff(T_IDXS, prepend=[0.])
 COMFORT_BRAKE = 2.5
-STOP_DISTANCE = 6.0
+STOP_DISTANCE = 5.0
 
 def get_jerk_factor(personality=custom.LongitudinalPersonalitySP.standard):
   if personality==custom.LongitudinalPersonalitySP.relaxed:
@@ -63,7 +63,7 @@ def get_jerk_factor(personality=custom.LongitudinalPersonalitySP.standard):
   elif personality==custom.LongitudinalPersonalitySP.standard:
     return 1.0
   elif personality==custom.LongitudinalPersonalitySP.moderate:
-    return 0.3
+    return 0.6
   elif personality==custom.LongitudinalPersonalitySP.aggressive:
     return 0.2
   elif personality==custom.LongitudinalPersonalitySP.overtake:
@@ -71,16 +71,85 @@ def get_jerk_factor(personality=custom.LongitudinalPersonalitySP.standard):
   else:
     raise NotImplementedError("Longitudinal personality not supported")
 
+def get_a_change_factor(v_ego, v_lead0, v_lead1, personality=custom.LongitudinalPersonalitySP.standard):
+  # Set cost multipliers based on driving personality (relaxed, standard, moderate, aggressive).
+  # These values adjust the sensitivity of acceleration change.
+  # Higher value = more cautious (slower reaction), smaller value = quicker response (more aggressive driving)
+  if personality==custom.LongitudinalPersonalitySP.relaxed:
+    a_change_cost_multiplier_follow = 1.2  # Highest cost for changing acceleration, meaning more gradual transitions
+    a_change_cost_high_speed_factor = 2.0  # No extra penalty for high-speed changes (more cautious)
+  elif personality==custom.LongitudinalPersonalitySP.standard:
+    a_change_cost_multiplier_follow = 0.6  # Moderate cost for changing acceleration (quicker transitions compared to relaxed)
+    a_change_cost_high_speed_factor = 2.5  # Higher penalty for changes at higher speeds (more cautious)
+  elif personality==custom.LongitudinalPersonalitySP.moderate:
+    a_change_cost_multiplier_follow = 0.4  # Similar to standard (quicker transitions compared to relaxed)
+    a_change_cost_high_speed_factor = 3.0  # Similar to standard (higher penalty for high speeds)
+  elif personality==custom.LongitudinalPersonalitySP.aggressive:
+    a_change_cost_multiplier_follow = 0.2  # Very low cost for changing acceleration, meaning quicker reactions (less cautious)
+    a_change_cost_high_speed_factor = 5.0  # Much higher penalty for abrupt changes at high speeds (very cautious at high speeds)
+  elif personality==custom.LongitudinalPersonalitySP.overtake:
+    a_change_cost_multiplier_follow = 0.1  # Very low cost for changing acceleration, meaning quicker reactions (less cautious)
+    a_change_cost_high_speed_factor = 5.0  # Much higher penalty for abrupt changes at high speeds (very cautious at high speeds)
+  else:
+    raise NotImplementedError("Longitudinal personality not supported")
+
+  # Variables to modify the acceleration change based on speed and lead vehicle conditions.
+  # LEAD_AUGMENTATION_BP_MAX defines the vEgo threshold for rapid acceleration.
+  LEAD_AUGMENTATION_BP_MAX = 5.  # Maximum speed (5 m/s ~ 18 km/h) where rapid acceleration adjustments are allowed
+
+  # LEAD_AUGMENTATION_BP: breakpoints for ego vehicle speed (vEgo) in m/s
+  # LEAD_AUGMENTATION_V: multiplier values for ego vehicle speed interpolation
+  LEAD_AUGMENTATION_BP = [0., LEAD_AUGMENTATION_BP_MAX]  # vEgo breakpoints: [0 m/s, 5 m/s (~18 km/h)]
+  LEAD_AUGMENTATION_V = [.05, 1.]  # acceleration multipliers: At 0 m/s, allow very small changes (.05), at 5 m/s allow faster acceleration (1.0)
+
+  # SPEED_AUGMENTATION_BP: breakpoints for speed adjustment to reduce abrupt braking at higher speeds
+  # SPEED_AUGMENTATION_V: interpolation values for scaling acceleration cost based on speed
+  # Higher = more cautious (penalizes abrupt braking), smaller = more aggressive (less penalty)
+  SPEED_AUGMENTATION_BP = [0., LEAD_AUGMENTATION_BP_MAX, 10.]  # Speed breakpoints: [0 m/s, 5 m/s, 10 m/s (~36 km/h)]
+  SPEED_AUGMENTATION_V = [1., 1., a_change_cost_high_speed_factor]  # Multiplier: between 0-5 m/s, no change (1.0), after 5 m/s, scale by a_change_cost_high_speed_factor (e.g., 1.5 in standard mode)
+
+  # Calculate a cost for acceleration changes when lead vehicles are pulling away and ego speed is below the threshold.
+  lead_augmented_a_change_cost = 1.0  # Default cost factor
+  if (v_lead0 - v_ego > 1e-3) and (v_lead1 - v_ego > 1e-3):
+    # Interpolate for the acceleration change cost when lead vehicles are increasing speed, based on vEgo
+    lead_augmented_a_change_cost = interp(v_ego, LEAD_AUGMENTATION_BP, LEAD_AUGMENTATION_V)
+
+  # Multiply the lead-based cost with speed-based cost to get a final cost factor, scaling with vehicle speed
+  speed_augmented_a_change_cost = a_change_cost_multiplier_follow * interp(v_ego, SPEED_AUGMENTATION_BP, SPEED_AUGMENTATION_V)
+
+  # Choose the smaller factor between the lead-based cost and the speed-based cost
+  a_change_factor = lead_augmented_a_change_cost if v_ego <= LEAD_AUGMENTATION_BP_MAX else speed_augmented_a_change_cost
+
+  # Return the final acceleration change factor to be applied
+  return a_change_factor
+
+# Function to return a multiplier for a danger zone cost based on driving personality
+def get_danger_zone_factor(personality=custom.LongitudinalPersonalitySP.standard):
+  # Higher values mean more cautious driving in dangerous situations, scaling the cost accordingly
+  if personality==custom.LongitudinalPersonalitySP.relaxed:
+    return 1.8  # Higher danger zone cost for relaxed personality (more cautious)
+  elif personality==custom.LongitudinalPersonalitySP.standard:
+    return 1.5  # Medium danger zone cost for standard personality
+  elif personality==custom.LongitudinalPersonalitySP.moderate:
+    return 1.2  # Medium danger zone cost for moderate personality (similar to standard)
+  elif personality==custom.LongitudinalPersonalitySP.aggressive:
+    return 1.0  # Lowest danger zone cost for aggressive personality (less cautious)
+  elif personality==custom.LongitudinalPersonalitySP.overtake:
+    return 1.0  # Lowest danger zone cost for aggressive personality (less cautious)
+  else:
+    raise NotImplementedError("Longitudinal personality not supported")
+
+
 
 def get_T_FOLLOW(personality=custom.LongitudinalPersonalitySP.standard):
   if personality==custom.LongitudinalPersonalitySP.relaxed:
     return 1.75
   elif personality==custom.LongitudinalPersonalitySP.standard:
-    return 1.50
+    return 1.65
   elif personality==custom.LongitudinalPersonalitySP.moderate:
-    return 1.35
+    return 1.45
   elif personality==custom.LongitudinalPersonalitySP.aggressive:
-    return 1.20
+    return 1.25
   elif personality==custom.LongitudinalPersonalitySP.overtake:
     return 0.25
   else:
@@ -89,17 +158,17 @@ def get_T_FOLLOW(personality=custom.LongitudinalPersonalitySP.standard):
 
 def get_dynamic_personality(v_ego, personality=custom.LongitudinalPersonalitySP.standard):
   if personality==custom.LongitudinalPersonalitySP.relaxed:
-    x_vel =  [0,    5.,   5.01, 8.33,  8.34,  27.69, 27.7]
-    y_dist = [0.0,  1.2,  1.7,  1.7,   1.75,  1.75,  1.83]
+    x_vel =  [0.,   22.,   22.01,  36.1]
+    y_dist = [1.70, 1.70,  1.80,   1.80]
   elif personality==custom.LongitudinalPersonalitySP.standard:
-    x_vel =  [0,    5.0,  5.01, 8.33,  8.34,  27.69, 27.7]
-    y_dist = [0.0,  1.6,  1.6,  1.6,   1.75,  1.75,  1.80]
+    x_vel =  [0.,   22.,   22.01,  36.1]
+    y_dist = [1.65, 1.65,  1.75,   1.75]
   elif personality==custom.LongitudinalPersonalitySP.moderate:
-    x_vel =  [0,    5.,   5.01, 8.33,  8.34,  27.69, 27.7]
-    y_dist = [0.0,  1.4,  1.4,  1.4,   1.45,  1.45,  1.50]
+    x_vel =  [0.,   22.,   22.01,  36.1]
+    y_dist = [1.45, 1.45,  1.55,   1.55]
   elif personality==custom.LongitudinalPersonalitySP.aggressive:
-    x_vel =  [0,    5.,   5.01,  17.0, 17.01, 27.69, 27.7]
-    y_dist = [0.0,  1.2,  1.2,   1.2,  1.25,  1.25,  1.3]
+    x_vel =  [0.,   19.7,  19.71,  36.1]
+    y_dist = [1.00, 1.00,  1.25,   1.25]
   else:
     raise NotImplementedError("Dynamic personality not supported")
   return np.interp(v_ego, x_vel, y_dist)
@@ -301,12 +370,15 @@ 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, v_lead0 = 0., v_lead1 = 0., personality=custom.LongitudinalPersonalitySP.standard):
+    v_ego = self.x0[1]
     jerk_factor = get_jerk_factor(personality)
+    a_change_factor = get_a_change_factor(v_ego, v_lead0, v_lead1, personality)
+    danger_zone_factor = get_danger_zone_factor(personality)
     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]
-      constraint_cost_weights = [LIMIT_COST, LIMIT_COST, LIMIT_COST, DANGER_ZONE_COST]
+      cost_weights = [X_EGO_OBSTACLE_COST, X_EGO_COST, V_EGO_COST, A_EGO_COST, a_change_factor * a_change_cost, jerk_factor * J_EGO_COST]
+      constraint_cost_weights = [LIMIT_COST, LIMIT_COST, LIMIT_COST, DANGER_ZONE_COST * danger_zone_factor]
     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]
@@ -360,7 +432,7 @@ 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,
+  def update(self, radarstate, v_cruise, prev_accel_constraint, x, v, a, j, personality=custom.LongitudinalPersonalitySP.standard,
              dynamic_personality=False, overtaking_acceleration_assist=False):
     v_ego = self.x0[1]
     t_follow = get_dynamic_personality(v_ego, personality) if dynamic_personality else get_T_FOLLOW(personality)
@@ -370,6 +442,8 @@ class LongitudinalMpc:
     lead_xv_0 = self.process_lead(radarstate.leadOne)
     lead_xv_1 = self.process_lead(radarstate.leadTwo)
 
+    self.set_weights(prev_accel_constraint=prev_accel_constraint, v_lead0=lead_xv_0[0, 1], v_lead1=lead_xv_1[0, 1], personality=personality)
+
     # 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.
@@ -497,4 +571,4 @@ class LongitudinalMpc:
 if __name__ == "__main__":
   ocp = gen_long_ocp()
   AcadosOcpSolver.generate(ocp, json_file=JSON_FILE)
-  # AcadosOcpSolver.build(ocp.code_export_directory, with_cython=True)
+  # AcadosOcpSolver.build(ocp.code_export_directory, with_cython=True)

selfdrive/controls/lib/longitudinal_planner.py

@@ -201,7 +201,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, prev_accel_constraint, x, v, a, j, personality=sm['controlsStateSP'].personality,
                     dynamic_personality=sm['controlsStateSP'].dynamicPersonality, overtaking_acceleration_assist=overtaking_accel_engaged)
 
     self.v_desired_trajectory = np.interp(CONTROL_N_T_IDX, T_IDXS_MPC, self.mpc.v_solution)

selfdrive/controls/lib/sunnypilot/accel_controller.py

@@ -29,20 +29,15 @@ from openpilot.common.numpy_fast import interp
 AccelPersonality = custom.AccelerationPersonality
 
 # accel personality by @arne182 modified by cgw and kumar
-_DP_CRUISE_MIN_V =       [-0.01, -0.01, -0.07, -0.07, -0.16, -0.16, -0.46, -1.2]
-_DP_CRUISE_MIN_V_ECO =   [-0.01, -0.01, -0.06, -0.06, -0.15, -0.15, -0.45, -1.2]
-_DP_CRUISE_MIN_V_SPORT = [-0.01, -0.01, -0.08, -0.08, -0.17, -0.17, -0.47, -1.2]
-_DP_CRUISE_MIN_BP =      [0.,     0.05,   0.06, 8.0,  8.01,  12.,   18.,   30.]
+_DP_CRUISE_MIN_V =       [-1.0,  -1.0]
+_DP_CRUISE_MIN_V_ECO =   [-1.0,  -1.0]
+_DP_CRUISE_MIN_V_SPORT = [-1.0,  -1.0]
+_DP_CRUISE_MIN_BP =      [0., 20.]
 
-#_DP_CRUISE_MIN_V =       [-0.020, -0.020, -0.23, -0.23, -0.38, -0.38, -1.00]
-#_DP_CRUISE_MIN_V_ECO =   [-0.019, -0.019, -0.21, -0.21, -0.36, -0.36, -1.00]
-#_DP_CRUISE_MIN_V_SPORT = [-0.022, -0.022, -0.25, -0.25, -0.40, -0.40, -1.00]
-#_DP_CRUISE_MIN_BP =      [0.,     0.05,   3.12,  10.,   10.01, 20.,   30.]
-
-_DP_CRUISE_MAX_V =       [2.0, 2.0, 1.75, 0.96, .65,  .54,  .38,  .17]
-_DP_CRUISE_MAX_V_ECO =   [2.0, 1.9, 1.65, 0.90, .57,  .45,  .32,  .09]
-_DP_CRUISE_MAX_V_SPORT = [2.0, 2.0, 1.95, 1.15, .84,  .70,  .50,  .30]
-_DP_CRUISE_MAX_BP =      [0.,  6.0,  8.,   11.,  20.,  25.,  30.,  40.]
+_DP_CRUISE_MAX_V =       [2.0, 2.0, 2.0, 1.80, 1.03, .62,  .47,  .36,  .11]
+_DP_CRUISE_MAX_V_ECO =   [2.0, 2.0, 2.0, 1.65, 0.92, .532, .432, .32,  .095]
+_DP_CRUISE_MAX_V_SPORT = [2.0, 2.0, 2.0, 2.00, 1.25, .71,  .54,  .46,  .2]
+_DP_CRUISE_MAX_BP =      [0.,  1.,  6.,  8.,   11.,  20.,  25.,  30.,  55.]
 
 
 class AccelController: