test

This reverts commit a0fe916e4b.

common/params.cc

@@ -333,6 +333,7 @@ std::unordered_map<std::string, uint32_t> keys = {
     {"ToyotaAutoHold", PERSISTENT | BACKUP},
     {"ToyotaAutoLockBySpeed", PERSISTENT | BACKUP},
     {"ToyotaAutoUnlockByShifter", PERSISTENT | BACKUP},
+    {"ToyotaDriveMode", PERSISTENT | BACKUP},
     {"ToyotaEnhancedBsm", PERSISTENT | BACKUP},
     {"ToyotaSnG", PERSISTENT | BACKUP},
     {"ToyotaTSS2Long", PERSISTENT | BACKUP},

selfdrive/car/toyota/carcontroller.py

@@ -1,4 +1,7 @@
 from cereal import car
+import math
+from openpilot.common.params import Params
+from openpilot.selfdrive.controls.lib.pid import PIDController
 from common.conversions import Conversions as CV
 from openpilot.common.numpy_fast import clip, interp
 from openpilot.selfdrive.car import apply_meas_steer_torque_limits, apply_std_steer_angle_limits, common_fault_avoidance, make_can_msg, make_tester_present_msg, \
@@ -13,6 +16,7 @@ from opendbc.can.packer import CANPacker
 GearShifter = car.CarState.GearShifter
 SteerControlType = car.CarParams.SteerControlType
 VisualAlert = car.CarControl.HUDControl.VisualAlert
+#LongCtrlState = car.CarControl.Actuators.LongControlState
 
 # LKA limits
 # EPS faults if you apply torque while the steering rate is above 100 deg/s for too long
@@ -44,8 +48,11 @@ class CarController(CarControllerBase):
     self.last_standstill = False
     self.standstill_req = False
     self.steer_rate_counter = 0
+    #self.pcm_accel_comp = 0
     self.distance_button = 0
 
+    #self.pid = PIDController(k_p=1.0, k_i=0.25, k_f=0)
+
     self.packer = CANPacker(dbc_name)
     self.gas = 0
     self.accel = 0
@@ -140,6 +147,41 @@ class CarController(CarControllerBase):
                                                           lta_active, self.frame // 2, torque_wind_down))
 
     # *** gas and brake ***
+    sp_tss2_long_tune = Params().get_bool("ToyotaTSS2Long")
+
+    # When sp_tss2_long_tune is True and CC.longActive
+    #if sp_tss2_long_tune:
+      # we will throw out PCM's compensations, but that may be a good thing. for example:
+      # we lose things like pitch compensation, gas to maintain speed, brake to compensate for creeping, etc.
+      # but also remove undesirable "snap to standstill" behavior when not requesting enough accel at low speeds,
+      # lag to start moving, lag to start braking, etc.
+      # PI should compensate for lack of the desirable behaviors, but might be worse than the PCM doing them
+
+      # FIXME? neutral force will only be positive under ~5 mph, which messes up stopping control considerably
+      # not sure why this isn't captured in the PCM accel net, maybe that just ignores creep force + high speed deceleration
+      # it also doesn't seem to capture slightly more braking on downhills (VSC1S07->ASLP (pitch, deg.) might have some clues)
+    #  offset = min(CS.pcm_neutral_force / self.CP.mass, 0.0)
+    #  pitch_offset = math.sin(math.radians(CS.vsc_slope_angle)) * 9.81  # downhill is negative
+      # TODO: these limits are too slow to prevent a jerk when engaging, ramp down on engage?
+      # self.pcm_accel_comp = clip(actuators.accel - CS.pcm_accel_net, self.pcm_accel_comp - 0.05, self.pcm_accel_comp + 0.05)
+    #  pcm_accel_comp = self.pid.update(actuators.accel - CS.pcm_calc_accel_net)
+    #  self.pcm_accel_comp = clip(pcm_accel_comp, self.pcm_accel_comp - 0.005, self.pcm_accel_comp + 0.005)
+    #  if CS.out.cruiseState.standstill or actuators.longControlState == LongCtrlState.stopping:
+    #    self.pcm_accel_comp = 0.0
+    #    self.pid.reset()
+    #  pcm_accel_cmd = actuators.accel + self.pcm_accel_comp # + offset
+      # pcm_accel_cmd = actuators.accel - pitch_offset
+
+    #  if not CC.longActive:
+    #   self.pid.reset()
+    #    self.pcm_accel_comp = 0.0
+    #    pcm_accel_cmd = 0.0
+
+    #  pcm_accel_cmd = clip(pcm_accel_cmd, self.params.ACCEL_MIN, self.params.ACCEL_MAX)
+    #else:
+    #  pcm_accel_cmd = clip(actuators.accel, self.params.ACCEL_MIN, self.params.ACCEL_MAX)
+
+
     if self.CP.enableGasInterceptorDEPRECATED and CC.longActive:
       MAX_INTERCEPTOR_GAS = 0.5
       # RAV4 has very sensitive gas pedal
@@ -156,7 +198,6 @@ class CarController(CarControllerBase):
     else:
       interceptor_gas_cmd = 0.
     pcm_accel_cmd = clip(actuators.accel, self.params.ACCEL_MIN, self.params.ACCEL_MAX)
-
     # TODO: probably can delete this. CS.pcm_acc_status uses a different signal
     # than CS.cruiseState.enabled. confirm they're not meaningfully different
     if not (CC.enabled and CS.out.cruiseState.enabled) and CS.pcm_acc_status:

selfdrive/car/toyota/carstate.py

@@ -1,18 +1,19 @@
 import copy
 
-from cereal import car
+from cereal import car, custom
 from openpilot.common.conversions import Conversions as CV
 from openpilot.common.numpy_fast import mean
 from openpilot.common.filter_simple import FirstOrderFilter
 from opendbc.can.can_define import CANDefine
 from opendbc.can.parser import CANParser
 from openpilot.selfdrive.car import DT_CTRL
+from openpilot.common.params import Params
 from openpilot.selfdrive.car.interfaces import CarStateBase
 from openpilot.selfdrive.car.toyota.values import ToyotaFlags, ToyotaFlagsSP, CAR, DBC, STEER_THRESHOLD, NO_STOP_TIMER_CAR, \
                                                   TSS2_CAR, RADAR_ACC_CAR, EPS_SCALE, UNSUPPORTED_DSU_CAR
 
 SteerControlType = car.CarParams.SteerControlType
-
+AccelPersonality = custom.AccelerationPersonality
 # These steering fault definitions seem to be common across LKA (torque) and LTA (angle):
 # - high steer rate fault: goes to 21 or 25 for 1 frame, then 9 for 2 seconds
 # - lka/lta msg drop out: goes to 9 then 11 for a combined total of 2 seconds, then 3.
@@ -56,6 +57,11 @@ class CarState(CarStateBase):
     self.low_speed_lockout = False
     self.acc_type = 1
     self.lkas_hud = {}
+    #self.pcm_accel_net = 0.0
+    #self.pcm_true_accel_net = 0.0
+    #self.pcm_calc_accel_net = 0.0
+    #self.pcm_neutral_force = 0.0
+    #self.vsc_slope_angle = 0.0
 
     self.lkas_enabled = None
     self.prev_lkas_enabled = None
@@ -79,6 +85,14 @@ class CarState(CarStateBase):
     self._right_blindspot_d2 = 0
     self._right_blindspot_counter = 0
 
+    self.signals_checked = False
+    self.sport_signal_seen = False
+    self.eco_signal_seen = False
+    self.accel_profile = None
+    self.prev_accel_profile = None
+    self.accel_profile_init = False
+    self.toyota_drive_mode = Params().get_bool('ToyotaDriveMode')
+
     if CP.spFlags & ToyotaFlagsSP.SP_AUTO_BRAKE_HOLD:
       self.pre_collision_2 = {}
 
@@ -117,6 +131,14 @@ class CarState(CarStateBase):
     ret.vEgo, ret.aEgo = self.update_speed_kf(ret.vEgoRaw)
     ret.vEgoCluster = ret.vEgo * 1.015  # minimum of all the cars
 
+    # thought to be the gas/brake as issued by the pcm (0=coasting)
+    #self.pcm_accel_net = cp.vl["PCM_CRUISE"]["ACCEL_NET"]  # this is only accurate for braking * 43
+    #self.pcm_true_accel_net = cp.vl["CLUTCH"]["TRUE_ACCEL_NET"]  # this is only accurate for acceleration * 78
+    #self.pcm_calc_accel_net = cp.vl["GEAR_PACKET_HYBRID"]["CAR_MOVEMENT"] / 78 - cp.vl["BRAKE"]["BRAKE_PEDAL"] / 43
+    #self.pcm_true_accel_net = cp.vl["CLUTCH"]["TRUE_ACCEL_NET"]
+    #self.pcm_neutral_force = cp.vl["PCM_CRUISE"]["NEUTRAL_FORCE"]
+    #self.vsc_slope_angle = cp.vl["VSC1S07"]["ASLP"]
+
     ret.standstill = abs(ret.vEgoRaw) < 1e-3
 
     if self.CP.carFingerprint != CAR.TOYOTA_PRIUS_V:
@@ -179,6 +201,51 @@ class CarState(CarStateBase):
     ret.leftBlinker = ret.leftBlinkerOn = cp.vl["BLINKERS_STATE"]["TURN_SIGNALS"] == 1
     ret.rightBlinker = ret.rightBlinkerOn = cp.vl["BLINKERS_STATE"]["TURN_SIGNALS"] == 2
 
+    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, CAR.TOYOTA_HIGHLANDER_TSS2) else 'SPORT_ON'
+
+      # Check signals once
+      if not self.signals_checked:
+        self.signals_checked = True
+
+        # 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
+
+        # 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:
+        # 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, 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
+
+      print(f"Accel profile set to: {self.accel_profile}")
+
+      # If not initialized, sync profile with the current mode on the car
+      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 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"]
 
@@ -397,12 +464,16 @@ class CarState(CarStateBase):
       ("BODY_CONTROL_STATE_2", 2),
       ("ESP_CONTROL", 3),
       ("EPS_STATUS", 25),
+      #("GEAR_PACKET_HYBRID", 60),
+      #("BRAKE", 80),
       ("BRAKE_MODULE", 40),
       ("WHEEL_SPEEDS", 80),
       ("STEER_ANGLE_SENSOR", 80),
       ("PCM_CRUISE", 33),
       ("PCM_CRUISE_SM", 1),
+      #("VSC1S07", 20),
       ("STEER_TORQUE_SENSOR", 50),
+      #("CLUTCH", 16),
     ]
 
     if CP.carFingerprint != CAR.TOYOTA_MIRAI:

selfdrive/car/toyota/interface.py

@@ -163,13 +163,13 @@ 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.vEgoStarting = 0.01
+      ret.stoppingDecelRate = 0.006
 
     def default_tss2_longitudinal_tuning():
       ret.vEgoStopping = 0.25
       ret.vEgoStarting = 0.25
-      ret.stoppingDecelRate = 0.3  # reach stopping target smoothly
+      ret.stoppingDecelRate = 0.002  # reach stopping target smoothly
 
     def default_longitudinal_tuning():
       tune.kiBP = [0., 5., 35.]
@@ -178,8 +178,8 @@ class CarInterface(CarInterfaceBase):
     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.,  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,24 +63,93 @@ 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.6
   elif personality==custom.LongitudinalPersonalitySP.aggressive:
-    return 0.8
+    return 0.2
   elif personality==custom.LongitudinalPersonalitySP.overtake:
     return 0.1
   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.45
+    return 1.65
   elif personality==custom.LongitudinalPersonalitySP.moderate:
-    return 1.25
+    return 1.45
   elif personality==custom.LongitudinalPersonalitySP.aggressive:
-    return 1.0
+    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,  20.,   27.7]
-    y_dist = [1.0,  1.0,  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,    20.,   27.7]
-    y_dist = [1.75, 1.75,  1.70]
+    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,    27.69, 27.7]
-    y_dist = [1.45, 1.45,  1.40]
+    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,    20.0, 20.01, 27.69, 27.7]
-    y_dist = [1.07, 1.07, 1.12,  1.12,  1.20]
+    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,16 +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.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_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_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.]
+_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_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:

selfdrive/controls/lib/sunnypilot/dynamic_experimental_controller.py

@@ -31,11 +31,11 @@ TRAJECTORY_SIZE = 33
 LEAD_WINDOW_SIZE = 4
 LEAD_PROB = 0.6
 
-SLOW_DOWN_WINDOW_SIZE = 5
+SLOW_DOWN_WINDOW_SIZE = 4
 SLOW_DOWN_PROB = 0.6
 
 SLOW_DOWN_BP = [0., 10., 20., 30., 40., 50., 55., 60.]
-SLOW_DOWN_DIST = [20, 30., 50., 70., 80., 90., 105., 120.]
+SLOW_DOWN_DIST = [25., 38., 55., 75., 95., 115., 130., 150.]
 
 SLOWNESS_WINDOW_SIZE = 12
 SLOWNESS_PROB = 0.5
@@ -87,7 +87,7 @@ class WeightedMovingAverageCalculator:
   def __init__(self, window_size):
     self.window_size = window_size
     self.data = []
-    self.weights = np.linspace(1, 2, window_size)  # Linear weights, adjust as needed
+    self.weights = np.linspace(1, 3, window_size)  # Linear weights, adjust as needed
 
   def add_data(self, value):
     if len(self.data) == self.window_size:
@@ -153,17 +153,21 @@ class DynamicExperimentalController:
     """
     Adapts the slow down threshold based on vehicle speed and recent behavior.
     """
-    return interp(self._v_ego_kph, SLOW_DOWN_BP, SLOW_DOWN_DIST) * (1.0 + 0.05 * np.log(1 + len(self._slow_down_gmac.data)))
+    return interp(self._v_ego_kph, SLOW_DOWN_BP, SLOW_DOWN_DIST) * (1.0 + 0.03 * np.log(1 + len(self._slow_down_gmac.data)))
 
-  def _anomaly_detection(self, recent_data, threshold=2.0):
+  def _anomaly_detection(self, recent_data, threshold=2.0, context_check=True):
     """
     Basic anomaly detection using standard deviation.
     """
-    if len(recent_data) < 3:
+    if len(recent_data) < 5:
       return False
     mean = np.mean(recent_data)
     std_dev = np.std(recent_data)
     anomaly = recent_data[-1] > mean + threshold * std_dev
+
+    # Context check to ensure repeated anomaly
+    if context_check:
+      return np.count_nonzero(np.array(recent_data) > mean + threshold * std_dev) > 1
     return anomaly
 
   def _smoothed_lead_detection(self, lead_prob, smoothing_factor=0.2):

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

@@ -203,6 +203,14 @@ SPVehiclesTogglesPanel::SPVehiclesTogglesPanel(VehiclePanel *parent) : ListWidge
   toyotaAutoUnlock->setConfirmation(true, false);
   addItem(toyotaAutoUnlock);
 
+  auto toyotaDriveMode = new ParamControlSP(
+    "ToyotaDriveMode",
+    tr("Enable Toyota Drive Mode Button"),
+    tr("Sunnypilot will link the Acceleration Personality to the car's physical drive mode selector.\nReboot Required."),
+    "../assets/offroad/icon_blank.png");
+  toyotaDriveMode->setConfirmation(true, false);
+  addItem(toyotaDriveMode);
+
   // Volkswagen
   addItem(new LabelControlSP(tr("Volkswagen")));
   auto volkswagenCCOnly = new ParamControlSP(

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

@@ -107,6 +107,7 @@ def manager_init() -> None:
     ("ToyotaAutoHold", "0"),
     ("ToyotaAutoLockBySpeed", "0"),
     ("ToyotaAutoUnlockByShifter", "0"),
+    ("ToyotaDriveMode", "0"),
     ("ToyotaEnhancedBsm", "0"),
     ("TrueVEgoUi", "0"),
     ("TurnSpeedControl", "0"),