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},
@@ -333,7 +334,6 @@ 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,7 +1,4 @@
 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, \
@@ -16,7 +13,6 @@ 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
@@ -48,11 +44,8 @@ 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
@@ -147,41 +140,6 @@ 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
@@ -198,6 +156,7 @@ 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,19 +1,18 @@
 import copy
 
-from cereal import car, custom
+from cereal import car
 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.
@@ -57,11 +56,6 @@ 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
@@ -85,14 +79,6 @@ 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 = {}
 
@@ -131,14 +117,6 @@ 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:
@@ -201,51 +179,6 @@ 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"]
 
@@ -464,16 +397,12 @@ 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

@@ -162,24 +162,21 @@ class CarInterface(CarInterfaceBase):
 
     # hand tuned (August 12, 2024)
     def custom_tss2_longitudinal_tuning():
-      ret.vEgoStopping = 0.25
+      ret.vEgoStopping = 0.01
       ret.vEgoStarting = 0.01
-      ret.stoppingDecelRate = 0.006
-
+      ret.stoppingDecelRate = 0.007
     def default_tss2_longitudinal_tuning():
       ret.vEgoStopping = 0.25
       ret.vEgoStarting = 0.25
-      ret.stoppingDecelRate = 0.002  # reach stopping target smoothly
-
+      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.,  5.,   12.,  20.,  27.,  36.,  40.]
-        tune.kiV = [0.34, 0.234, 0.20, 0.17, 0.105, 0.09, 0.08]
+        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

@@ -3,7 +3,8 @@ import os
 import time
 import numpy as np
 from cereal import custom
-from openpilot.common.numpy_fast import clip, interp
+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
@@ -55,7 +56,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 = 5.0
+STOP_DISTANCE = 6.0
 
 def get_jerk_factor(personality=custom.LongitudinalPersonalitySP.standard):
   if personality==custom.LongitudinalPersonalitySP.relaxed:
@@ -63,93 +64,24 @@ def get_jerk_factor(personality=custom.LongitudinalPersonalitySP.standard):
   elif personality==custom.LongitudinalPersonalitySP.standard:
     return 1.0
   elif personality==custom.LongitudinalPersonalitySP.moderate:
-    return 0.6
+    return 0.95
   elif personality==custom.LongitudinalPersonalitySP.aggressive:
-    return 0.2
+    return 0.9
   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.65
-  elif personality==custom.LongitudinalPersonalitySP.moderate:
     return 1.45
-  elif personality==custom.LongitudinalPersonalitySP.aggressive:
+  elif personality==custom.LongitudinalPersonalitySP.moderate:
     return 1.25
+  elif personality==custom.LongitudinalPersonalitySP.aggressive:
+    return 1.0
   elif personality==custom.LongitudinalPersonalitySP.overtake:
     return 0.25
   else:
@@ -158,17 +90,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.,   22.,   22.01,  36.1]
-    y_dist = [1.70, 1.70,  1.80,   1.80]
+    x_vel =  [0,    5.,   5.01,  20.,   27.7]
+    y_dist = [1.0,  1.0,  1.75,  1.75,  1.83]
   elif personality==custom.LongitudinalPersonalitySP.standard:
-    x_vel =  [0.,   22.,   22.01,  36.1]
-    y_dist = [1.65, 1.65,  1.75,   1.75]
+    x_vel =  [0,    20.,   27.7]
+    y_dist = [1.70, 1.70,  1.70]
   elif personality==custom.LongitudinalPersonalitySP.moderate:
-    x_vel =  [0.,   22.,   22.01,  36.1]
-    y_dist = [1.45, 1.45,  1.55,   1.55]
+    x_vel =  [0,    27.69, 27.7]
+    y_dist = [1.40, 1.40,  1.45]
   elif personality==custom.LongitudinalPersonalitySP.aggressive:
-    x_vel =  [0.,   19.7,  19.71,  36.1]
-    y_dist = [1.00, 1.00,  1.25,   1.25]
+    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)
@@ -176,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
 
@@ -370,18 +317,24 @@ class LongitudinalMpc:
     for i in range(N):
       self.solver.cost_set(i, 'Zl', Zl)
 
-  def set_weights(self, prev_accel_constraint=True, v_lead0 = 0., v_lead1 = 0., personality=custom.LongitudinalPersonalitySP.standard):
-    v_ego = self.x0[1]
+  def set_weights(self, prev_accel_constraint=True, personality=custom.LongitudinalPersonalitySP.standard, v_lead0=0, v_lead1=0):
     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)
+    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, 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]
+      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')
@@ -432,8 +385,8 @@ class LongitudinalMpc:
     self.cruise_min_a = min_a
     self.max_a = max_a
 
-  def update(self, radarstate, v_cruise, prev_accel_constraint, x, v, a, j, personality=custom.LongitudinalPersonalitySP.standard,
-             dynamic_personality=False, overtaking_acceleration_assist=False):
+  def update(self, radarstate, v_cruise, x, v, a, j, personality=custom.LongitudinalPersonalitySP.standard,
+             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
@@ -442,13 +395,15 @@ 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.
-    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])
@@ -571,4 +526,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

@@ -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):
@@ -201,8 +203,8 @@ 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, prev_accel_constraint, x, v, a, j, personality=sm['controlsStateSP'].personality,
-                    dynamic_personality=sm['controlsStateSP'].dynamicPersonality, overtaking_acceleration_assist=overtaking_accel_engaged)
+    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, 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,15 +29,16 @@ from openpilot.common.numpy_fast import interp
 AccelPersonality = custom.AccelerationPersonality
 
 # accel personality by @arne182 modified by cgw and kumar
-_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.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.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.]
 
-_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 = 4
+SLOW_DOWN_WINDOW_SIZE = 5
 SLOW_DOWN_PROB = 0.6
 
 SLOW_DOWN_BP = [0., 10., 20., 30., 40., 50., 55., 60.]
-SLOW_DOWN_DIST = [25., 38., 55., 75., 95., 115., 130., 150.]
+SLOW_DOWN_DIST = [20, 30., 50., 70., 80., 90., 105., 120.]
 
 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, 3, window_size)  # Linear weights, adjust as needed
+    self.weights = np.linspace(1, 2, window_size)  # Linear weights, adjust as needed
 
   def add_data(self, value):
     if len(self.data) == self.window_size:
@@ -153,21 +153,17 @@ 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.03 * np.log(1 + len(self._slow_down_gmac.data)))
+    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)))
 
-  def _anomaly_detection(self, recent_data, threshold=2.0, context_check=True):
+  def _anomaly_detection(self, recent_data, threshold=2.0):
     """
     Basic anomaly detection using standard deviation.
     """
-    if len(recent_data) < 5:
+    if len(recent_data) < 3:
       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/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/offroad/settings/vehicle_settings.cc

@@ -203,14 +203,6 @@ 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(

system/manager/manager.py

@@ -107,7 +107,6 @@ def manager_init() -> None:
     ("ToyotaAutoHold", "0"),
     ("ToyotaAutoLockBySpeed", "0"),
     ("ToyotaAutoUnlockByShifter", "0"),
-    ("ToyotaDriveMode", "0"),
     ("ToyotaEnhancedBsm", "0"),
     ("TrueVEgoUi", "0"),
     ("TurnSpeedControl", "0"),
@@ -125,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')))