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,11 +1,8 @@
 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, \
-                                    create_gas_interceptor_command, rate_limit
+                                    create_gas_interceptor_command
 from openpilot.selfdrive.car.interfaces import CarControllerBase
 from openpilot.selfdrive.car.toyota import toyotacan
 from openpilot.selfdrive.car.toyota.values import CAR, STATIC_DSU_MSGS, NO_STOP_TIMER_CAR, TSS2_CAR, \
@@ -16,11 +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
-
-ACCELERATION_DUE_TO_GRAVITY = 9.81  # m/s^2
-
-ACCEL_WINDUP_LIMIT = 0.5  # m/s^2 / frame
 
 # LKA limits
 # EPS faults if you apply torque while the steering rate is above 100 deg/s for too long
@@ -54,9 +46,6 @@ class CarController(CarControllerBase):
     self.steer_rate_counter = 0
     self.distance_button = 0
 
-    self.pcm_accel_compensation = 0.0
-    self.permit_braking = 0.0
-
     self.packer = CANPacker(dbc_name)
     self.gas = 0
     self.accel = 0
@@ -151,39 +140,6 @@ class CarController(CarControllerBase):
                                                           lta_active, self.frame // 2, torque_wind_down))
 
     # *** gas and brake ***
-    # For cars where we allow a higher max acceleration of 2.0 m/s^2, compensate for PCM request overshoot and imprecise braking
-    # TODO: sometimes when switching from brake to gas quickly, CLUTCH->ACCEL_NET shows a slow unwind. make it go to 0 immediately
-    if self.CP.flags & ToyotaFlags.RAISED_ACCEL_LIMIT and CC.longActive and not CS.out.cruiseState.standstill:
-      # calculate amount of acceleration PCM should apply to reach target, given pitch
-      accel_due_to_pitch = math.sin(CS.slope_angle) * ACCELERATION_DUE_TO_GRAVITY
-      net_acceleration_request = actuators.accel + accel_due_to_pitch
-
-      # let PCM handle stopping for now
-      pcm_accel_compensation = 0.0
-      if actuators.longControlState != LongCtrlState.stopping:
-        pcm_accel_compensation = 2.0 * (CS.pcm_accel_net - net_acceleration_request)
-
-      # prevent compensation windup
-      pcm_accel_compensation = clip(pcm_accel_compensation, actuators.accel - self.params.ACCEL_MAX,
-                                    actuators.accel - self.params.ACCEL_MIN)
-
-      self.pcm_accel_compensation = rate_limit(pcm_accel_compensation, self.pcm_accel_compensation, -0.01, 0.01)
-      pcm_accel_cmd = actuators.accel - self.pcm_accel_compensation
-
-      # Along with rate limiting positive jerk below, this greatly improves gas response time
-      # Consider the net acceleration request that the PCM should be applying (pitch included)
-      if net_acceleration_request < 0.1:
-        self.permit_braking = True
-      elif net_acceleration_request > 0.2:
-        self.permit_braking = False
-    else:
-      self.pcm_accel_compensation = 0.0
-      pcm_accel_cmd = actuators.accel
-      self.permit_braking = True
-
-    pcm_accel_cmd = clip(pcm_accel_cmd, 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
@@ -199,6 +155,7 @@ class CarController(CarControllerBase):
       interceptor_gas_cmd = clip(pedal_command, 0., MAX_INTERCEPTOR_GAS)
     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
@@ -242,14 +199,11 @@ class CarController(CarControllerBase):
       if pcm_cancel_cmd and self.CP.carFingerprint in UNSUPPORTED_DSU_CAR:
         can_sends.append(toyotacan.create_acc_cancel_command(self.packer))
       elif self.CP.openpilotLongitudinalControl:
-        # internal PCM gas command can get stuck unwinding from negative accel so we apply a generous rate limit
-        pcm_accel_cmd = min(pcm_accel_cmd, self.accel + ACCEL_WINDUP_LIMIT) if CC.longActive else 0.0
-
-        can_sends.append(toyotacan.create_accel_command(self.packer, pcm_accel_cmd, pcm_cancel_cmd, self.permit_braking, self.standstill_req, lead,
-                                                        CS.acc_type, fcw_alert, self.distance_button, reverse_acc))
+        can_sends.append(toyotacan.create_accel_command(self.packer, pcm_accel_cmd, pcm_cancel_cmd, self.standstill_req, lead, CS.acc_type, fcw_alert,
+                                                        self.distance_button, reverse_acc))
         self.accel = pcm_accel_cmd
       else:
-        can_sends.append(toyotacan.create_accel_command(self.packer, 0, pcm_cancel_cmd, True, False, lead, CS.acc_type, False, self.distance_button, reverse_acc))
+        can_sends.append(toyotacan.create_accel_command(self.packer, 0, pcm_cancel_cmd, False, lead, CS.acc_type, False, self.distance_button, reverse_acc))
 
     if self.frame % 2 == 0 and self.CP.enableGasInterceptorDEPRECATED and self.CP.openpilotLongitudinalControl:
       # send exactly zero if gas cmd is zero. Interceptor will send the max between read value and gas cmd.

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,8 +56,6 @@ class CarState(CarStateBase):
     self.low_speed_lockout = False
     self.acc_type = 1
     self.lkas_hud = {}
-    self.pcm_accel_net = 0.0
-    self.slope_angle = 0.0
 
     self.lkas_enabled = None
     self.prev_lkas_enabled = None
@@ -82,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 = {}
 
@@ -102,24 +91,6 @@ class CarState(CarStateBase):
     self.prev_lkas_enabled = self.lkas_enabled
     self.prev_lta_status = self.lta_status
 
-    # Describes the acceleration request from the PCM if on flat ground, may be higher or lower if pitched
-    # CLUTCH->ACCEL_NET is only accurate for gas, PCM_CRUISE->ACCEL_NET is only accurate for brake
-    # These signals only have meaning when ACC is active
-    if self.CP.flags & ToyotaFlags.RAISED_ACCEL_LIMIT:
-      self.pcm_accel_net = max(cp.vl["CLUTCH"]["ACCEL_NET"], 0.0)
-
-      # Sometimes ACC_BRAKING can be 1 while showing we're applying gas already
-      if cp.vl["PCM_CRUISE"]["ACC_BRAKING"]:
-        self.pcm_accel_net += min(cp.vl["PCM_CRUISE"]["ACCEL_NET"], 0.0)
-
-      # add creeping force at low speeds only for braking, CLUTCH->ACCEL_NET already shows this
-      neutral_accel = max(cp.vl["PCM_CRUISE"]["NEUTRAL_FORCE"] / self.CP.mass, 0.0)
-      if self.pcm_accel_net + neutral_accel < 0.0:
-        self.pcm_accel_net += neutral_accel
-
-    # filtered pitch estimate from the car, negative is a downward slope
-    self.slope_angle = cp.vl["VSC1S07"]["ASLP"] * CV.DEG_TO_RAD
-
     ret.doorOpen = any([cp.vl["BODY_CONTROL_STATE"]["DOOR_OPEN_FL"], cp.vl["BODY_CONTROL_STATE"]["DOOR_OPEN_FR"],
                         cp.vl["BODY_CONTROL_STATE"]["DOOR_OPEN_RL"], cp.vl["BODY_CONTROL_STATE"]["DOOR_OPEN_RR"]])
     ret.seatbeltUnlatched = cp.vl["BODY_CONTROL_STATE"]["SEATBELT_DRIVER_UNLATCHED"] != 0
@@ -208,44 +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) else 'SPORT_ON'
-
-      # Check signals once
-      if not self.signals_checked:
-        self.signals_checked = True
-
-        # Get sport and eco signals, handling missing signals
-        sport_mode = cp.vl["GEAR_PACKET"].get(sport_signal, 0)
-        eco_mode = cp.vl["GEAR_PACKET"].get('ECON_ON', 0)
-
-        # Track if signals were detected
-        self.sport_signal_seen = sport_mode == 1
-        self.eco_signal_seen = eco_mode == 1
-      else:
-        # Use previously detected signals if they were seen
-        sport_mode = 1 if self.sport_signal_seen else 0
-        eco_mode = 1 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:
-        self.accel_profile = AccelPersonality.sport
-      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
-      self.prev_accel_profile = self.accel_profile
-
     if self.CP.carFingerprint != CAR.TOYOTA_MIRAI:
       ret.engineRpm = cp.vl["ENGINE_RPM"]["RPM"]
 
@@ -463,7 +396,6 @@ class CarState(CarStateBase):
       ("BODY_CONTROL_STATE", 3),
       ("BODY_CONTROL_STATE_2", 2),
       ("ESP_CONTROL", 3),
-      ("VSC1S07", 20),
       ("EPS_STATUS", 25),
       ("BRAKE_MODULE", 40),
       ("WHEEL_SPEEDS", 80),
@@ -473,9 +405,6 @@ class CarState(CarStateBase):
       ("STEER_TORQUE_SENSOR", 50),
     ]
 
-    if CP.flags & ToyotaFlags.RAISED_ACCEL_LIMIT:
-      messages.append(("CLUTCH", 15))
-
     if CP.carFingerprint != CAR.TOYOTA_MIRAI:
       messages.append(("ENGINE_RPM", 42))
 

selfdrive/car/toyota/interface.py

@@ -17,7 +17,7 @@ GearShifter = car.CarState.GearShifter
 class CarInterface(CarInterfaceBase):
   @staticmethod
   def get_pid_accel_limits(CP, current_speed, cruise_speed):
-    return CarControllerParams(CP).ACCEL_MIN, CarControllerParams(CP).ACCEL_MAX
+    return CarControllerParams.ACCEL_MIN, CarControllerParams.ACCEL_MAX
 
   @staticmethod
   def _get_params(ret, candidate, fingerprint, car_fw, experimental_long, docs):
@@ -63,9 +63,6 @@ class CarInterface(CarInterfaceBase):
     ret.enableDsu = len(found_ecus) > 0 and Ecu.dsu not in found_ecus and candidate not in (NO_DSU_CAR | UNSUPPORTED_DSU_CAR) \
                                         and not (ret.flags & ToyotaFlags.SMART_DSU)
 
-    if True: # candidate in (CAR.LEXUS_ES_TSS2, CAR.TOYOTA_COROLLA_TSS2, CAR.TOYOTA_PRIUS_TSS2, CAR.LEXUS_RX_TSS2) and Ecu.hybrid not in found_ecus:
-      ret.flags |= ToyotaFlags.RAISED_ACCEL_LIMIT.value
-
     if candidate == CAR.TOYOTA_PRIUS:
       zss = ret.spFlags & ToyotaFlagsSP.SP_ZSS
       stop_and_go = True
@@ -165,37 +162,25 @@ class CarInterface(CarInterfaceBase):
 
     # hand tuned (August 12, 2024)
     def custom_tss2_longitudinal_tuning():
-      ret.vEgoStopping = 0.15
-      ret.vEgoStarting = 0.05
-      ret.stopAccel = -2.0
-      ret.stoppingDecelRate = 0.005  # reach stopping target smoothly
-
+      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.,  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,  .24,  .20,  .17,  .10, .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]
         tune.kiV = [0.5]
-        # Since we compensate for imprecise acceleration in carcontroller, we can be less aggressive with tuning
-        # This also prevents unnecessary request windup due to internal car jerk limits
-        if ret.flags & ToyotaFlags.RAISED_ACCEL_LIMIT:
-          tune.kiV = [0.25]
       if candidate in TSS2_CAR:
         default_tss2_longitudinal_tuning()
     else:

selfdrive/car/toyota/toyotacan.py

@@ -34,14 +34,14 @@ def create_lta_steer_command(packer, steer_control_type, steer_angle, steer_req,
   return packer.make_can_msg("STEERING_LTA", 0, values)
 
 
-def create_accel_command(packer, accel, pcm_cancel, permit_braking, standstill_req, lead, acc_type, fcw_alert, distance, reverse_acc):
+def create_accel_command(packer, accel, pcm_cancel, standstill_req, lead, acc_type, fcw_alert, distance, reverse_acc):
   # TODO: find the exact canceling bit that does not create a chime
   values = {
     "ACCEL_CMD": 0 if pcm_cancel else accel,
     "ACC_TYPE": acc_type,
     "DISTANCE": distance,
     "MINI_CAR": lead,
-    "PERMIT_BRAKING": permit_braking,
+    "PERMIT_BRAKING": 1,
     "RELEASE_STANDSTILL": not standstill_req,
     "CANCEL_REQ": pcm_cancel,
     "ALLOW_LONG_PRESS": reverse_acc,

selfdrive/car/toyota/values.py

@@ -16,6 +16,9 @@ PEDAL_TRANSITION = 10. * CV.MPH_TO_MS
 
 
 class CarControllerParams:
+  ACCEL_MAX = 2.0  # m/s2, lower than allowed 2.0 m/s2 for tuning reasons
+  ACCEL_MIN = -3.5  # m/s2
+
   STEER_STEP = 1
   STEER_MAX = 1500
   STEER_ERROR_MAX = 350     # max delta between torque cmd and torque motor
@@ -30,12 +33,6 @@ class CarControllerParams:
   ANGLE_RATE_LIMIT_DOWN = AngleRateLimit(speed_bp=[5, 25], angle_v=[0.36, 0.26])
 
   def __init__(self, CP):
-    if CP.flags & ToyotaFlags.RAISED_ACCEL_LIMIT:
-      self.ACCEL_MAX = 2.0
-    else:
-      self.ACCEL_MAX = 1.5  # m/s2, lower than allowed 2.0 m/s^2 for tuning reasons
-    self.ACCEL_MIN = -3.5  # m/s2
-
     if CP.lateralTuning.which == 'torque':
       self.STEER_DELTA_UP = 15       # 1.0s time to peak torque
       self.STEER_DELTA_DOWN = 25     # always lower than 45 otherwise the Rav4 faults (Prius seems ok with 50)
@@ -61,8 +58,6 @@ class ToyotaFlags(IntFlag):
   NO_STOP_TIMER = 256
   # these cars are speculated to allow stop and go when the DSU is unplugged or disabled with sDSU
   SNG_WITHOUT_DSU = 512
-  # these cars can utilize 2.0 m/s^2
-  RAISED_ACCEL_LIMIT = 1024
 
 
 class ToyotaFlagsSP(IntFlag):

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.5
+    return 0.95
   elif personality==custom.LongitudinalPersonalitySP.aggressive:
-    return 0.2
+    return 0.9
   elif personality==custom.LongitudinalPersonalitySP.overtake:
     return 0.1
   else:
@@ -74,13 +75,13 @@ def get_jerk_factor(personality=custom.LongitudinalPersonalitySP.standard):
 
 def get_T_FOLLOW(personality=custom.LongitudinalPersonalitySP.standard):
   if personality==custom.LongitudinalPersonalitySP.relaxed:
-    return 1.80
+    return 1.75
   elif personality==custom.LongitudinalPersonalitySP.standard:
-    return 1.55
+    return 1.45
   elif personality==custom.LongitudinalPersonalitySP.moderate:
-    return 1.35
+    return 1.25
   elif personality==custom.LongitudinalPersonalitySP.aggressive:
-    return 1.20
+    return 1.0
   elif personality==custom.LongitudinalPersonalitySP.overtake:
     return 0.25
   else:
@@ -89,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,    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,    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,    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,    20.,   27.7]
+    y_dist = [1.70, 1.70,  1.70]
   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,    27.69, 27.7]
+    y_dist = [1.40, 1.40,  1.45]
   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,    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,21 +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.12, -0.12, -0.08, -0.08, -0.14, -0.14, -0.16, -0.16, -0.24, -0.24, -1.0,  -1.0,  -1.15]
-_DP_CRUISE_MIN_V_ECO =   [-0.11, -0.11, -0.07, -0.07, -0.13, -0.13, -0.15, -0.15, -0.23, -0.23, -1.0,  -1.0,  -1.10]
-_DP_CRUISE_MIN_V_SPORT = [-0.13, -0.13, -0.09, -0.09, -0.15, -0.15, -0.17, -0.17, -0.25, -0.25, -1.0,  -1.0,  -1.20]
-_DP_CRUISE_MIN_BP =      [0.,     0.08,  0.09, 2.77,  2.78,  8.33,  8.34,  13.88, 13.89, 19.44, 25.01, 30.55, 30.56]
-#_DP_CRUISE_MIN_BP in kph[0.,     0.3,   0.3,  10,    10,    30,    30,    50,    50,    70,    90,    110,   >110]
+_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_MIN_V =       [-1.0, -0.88]
-#_DP_CRUISE_MIN_V_ECO =   [-1.0, -0.76]
-#_DP_CRUISE_MIN_V_SPORT = [-1.0, -1.0]
-#_DP_CRUISE_MIN_BP =      [0., 40.]
-
-_DP_CRUISE_MAX_V =       [2.0, 2.0,  1.75, 0.96, .65,  .53,  .38,  .17]
-_DP_CRUISE_MAX_V_ECO =   [2.0, 1.98, 1.67, 0.92, .58,  .47,  .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.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/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')))