cleanup

Beta

selfdrive/car/toyota/carcontroller.py

@@ -5,7 +5,7 @@ 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
+                                    create_gas_interceptor_command, rate_limit
 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,7 +16,11 @@ from opendbc.can.packer import CANPacker
 GearShifter = car.CarState.GearShifter
 SteerControlType = car.CarParams.SteerControlType
 VisualAlert = car.CarControl.HUDControl.VisualAlert
-#LongCtrlState = car.CarControl.Actuators.LongControlState
+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
@@ -48,10 +52,10 @@ 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.pcm_accel_compensation = 0.0
+    self.permit_braking = 0.0
 
     self.packer = CANPacker(dbc_name)
     self.gas = 0
@@ -147,39 +151,37 @@ class CarController(CarControllerBase):
                                                           lta_active, self.frame // 2, torque_wind_down))
 
     # *** gas and brake ***
-    sp_tss2_long_tune = Params().get_bool("ToyotaTSS2Long")
+    # 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
 
-    # 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
+      # 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)
 
-      # 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
+      # prevent compensation windup
+      pcm_accel_compensation = clip(pcm_accel_compensation, actuators.accel - self.params.ACCEL_MAX,
+                                    actuators.accel - self.params.ACCEL_MIN)
 
-    #  if not CC.longActive:
-    #   self.pid.reset()
-    #    self.pcm_accel_comp = 0.0
-    #    pcm_accel_cmd = 0.0
+      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
 
-    #  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)
+      # 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:
@@ -197,7 +199,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
     if not (CC.enabled and CS.out.cruiseState.enabled) and CS.pcm_acc_status:
@@ -240,11 +242,14 @@ 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:
-        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))
+        # 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))
         self.accel = pcm_accel_cmd
       else:
-        can_sends.append(toyotacan.create_accel_command(self.packer, 0, pcm_cancel_cmd, False, lead, CS.acc_type, False, self.distance_button, reverse_acc))
+        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))
 
     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

@@ -57,11 +57,8 @@ 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.pcm_accel_net = 0.0
+    self.slope_angle = 0.0
 
     self.lkas_enabled = None
     self.prev_lkas_enabled = None
@@ -105,6 +102,24 @@ 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
@@ -131,14 +146,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:
@@ -203,37 +210,29 @@ class CarState(CarStateBase):
 
     if self.toyota_drive_mode:
       # Determine sport signal based on car model
-      sport_signal = 'SPORT_ON_2' if self.CP.carFingerprint in (CAR.TOYOTA_RAV4_TSS2, CAR.LEXUS_ES_TSS2, CAR.TOYOTA_HIGHLANDER_TSS2) else 'SPORT_ON'
+      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
 
-        # 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
+        # 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)
 
-        # 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
+        # Track if signals were detected
+        self.sport_signal_seen = sport_mode == 1
+        self.eco_signal_seen = eco_mode == 1
       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
+        # 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, with sport mode having higher priority
-      if sport_mode == 1:
-        self.accel_profile = AccelPersonality.sport
-      elif eco_mode == 1:
+      # 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
 
@@ -243,8 +242,9 @@ class CarState(CarStateBase):
       if not self.accel_profile_init or self.accel_profile != self.prev_accel_profile:
         Params().put_nonblocking('AccelPersonality', str(self.accel_profile))
         self.accel_profile_init = True
-        # Update the previous profile to prevent unnecessary re-syncing
-        self.prev_accel_profile = self.accel_profile
+
+      # 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,19 +463,19 @@ class CarState(CarStateBase):
       ("BODY_CONTROL_STATE", 3),
       ("BODY_CONTROL_STATE_2", 2),
       ("ESP_CONTROL", 3),
+      ("VSC1S07", 20),
       ("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.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.ACCEL_MIN, CarControllerParams.ACCEL_MAX
+    return CarControllerParams(CP).ACCEL_MIN, CarControllerParams(CP).ACCEL_MAX
 
   @staticmethod
   def _get_params(ret, candidate, fingerprint, car_fw, experimental_long, docs):
@@ -63,6 +63,9 @@ 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
@@ -162,14 +165,15 @@ class CarInterface(CarInterfaceBase):
 
     # hand tuned (August 12, 2024)
     def custom_tss2_longitudinal_tuning():
-      ret.vEgoStopping = 0.25
-      ret.vEgoStarting = 0.01
-      ret.stoppingDecelRate = 0.006
+      ret.vEgoStopping = 0.15
+      ret.vEgoStarting = 0.05
+      ret.stopAccel = -2.0
+      ret.stoppingDecelRate = 0.005  # reach stopping target smoothly
 
     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.]
@@ -178,12 +182,20 @@ class CarInterface(CarInterfaceBase):
     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.,  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]
         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, standstill_req, lead, acc_type, fcw_alert, distance, reverse_acc):
+def create_accel_command(packer, accel, pcm_cancel, permit_braking, 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": 1,
+    "PERMIT_BRAKING": permit_braking,
     "RELEASE_STANDSTILL": not standstill_req,
     "CANCEL_REQ": pcm_cancel,
     "ALLOW_LONG_PRESS": reverse_acc,

selfdrive/car/toyota/values.py

@@ -16,9 +16,6 @@ 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
@@ -33,6 +30,12 @@ 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)
@@ -58,6 +61,8 @@ 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

@@ -3,7 +3,7 @@ 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.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 = 5.0
+STOP_DISTANCE = 6.0
 
 def get_jerk_factor(personality=custom.LongitudinalPersonalitySP.standard):
   if personality==custom.LongitudinalPersonalitySP.relaxed:
@@ -63,7 +63,7 @@ def get_jerk_factor(personality=custom.LongitudinalPersonalitySP.standard):
   elif personality==custom.LongitudinalPersonalitySP.standard:
     return 1.0
   elif personality==custom.LongitudinalPersonalitySP.moderate:
-    return 0.6
+    return 0.5
   elif personality==custom.LongitudinalPersonalitySP.aggressive:
     return 0.2
   elif personality==custom.LongitudinalPersonalitySP.overtake:
@@ -71,85 +71,16 @@ def get_jerk_factor(personality=custom.LongitudinalPersonalitySP.standard):
   else:
     raise NotImplementedError("Longitudinal personality not supported")
 
-def get_a_change_factor(v_ego, v_lead0, v_lead1, personality=custom.LongitudinalPersonalitySP.standard):
-  # Set cost multipliers based on driving personality (relaxed, standard, moderate, aggressive).
-  # These values adjust the sensitivity of acceleration change.
-  # Higher value = more cautious (slower reaction), smaller value = quicker response (more aggressive driving)
-  if personality==custom.LongitudinalPersonalitySP.relaxed:
-    a_change_cost_multiplier_follow = 1.2  # Highest cost for changing acceleration, meaning more gradual transitions
-    a_change_cost_high_speed_factor = 2.0  # No extra penalty for high-speed changes (more cautious)
-  elif personality==custom.LongitudinalPersonalitySP.standard:
-    a_change_cost_multiplier_follow = 0.6  # Moderate cost for changing acceleration (quicker transitions compared to relaxed)
-    a_change_cost_high_speed_factor = 2.5  # Higher penalty for changes at higher speeds (more cautious)
-  elif personality==custom.LongitudinalPersonalitySP.moderate:
-    a_change_cost_multiplier_follow = 0.4  # Similar to standard (quicker transitions compared to relaxed)
-    a_change_cost_high_speed_factor = 3.0  # Similar to standard (higher penalty for high speeds)
-  elif personality==custom.LongitudinalPersonalitySP.aggressive:
-    a_change_cost_multiplier_follow = 0.2  # Very low cost for changing acceleration, meaning quicker reactions (less cautious)
-    a_change_cost_high_speed_factor = 5.0  # Much higher penalty for abrupt changes at high speeds (very cautious at high speeds)
-  elif personality==custom.LongitudinalPersonalitySP.overtake:
-    a_change_cost_multiplier_follow = 0.1  # Very low cost for changing acceleration, meaning quicker reactions (less cautious)
-    a_change_cost_high_speed_factor = 5.0  # Much higher penalty for abrupt changes at high speeds (very cautious at high speeds)
-  else:
-    raise NotImplementedError("Longitudinal personality not supported")
-
-  # Variables to modify the acceleration change based on speed and lead vehicle conditions.
-  # LEAD_AUGMENTATION_BP_MAX defines the vEgo threshold for rapid acceleration.
-  LEAD_AUGMENTATION_BP_MAX = 5.  # Maximum speed (5 m/s ~ 18 km/h) where rapid acceleration adjustments are allowed
-
-  # LEAD_AUGMENTATION_BP: breakpoints for ego vehicle speed (vEgo) in m/s
-  # LEAD_AUGMENTATION_V: multiplier values for ego vehicle speed interpolation
-  LEAD_AUGMENTATION_BP = [0., LEAD_AUGMENTATION_BP_MAX]  # vEgo breakpoints: [0 m/s, 5 m/s (~18 km/h)]
-  LEAD_AUGMENTATION_V = [.05, 1.]  # acceleration multipliers: At 0 m/s, allow very small changes (.05), at 5 m/s allow faster acceleration (1.0)
-
-  # SPEED_AUGMENTATION_BP: breakpoints for speed adjustment to reduce abrupt braking at higher speeds
-  # SPEED_AUGMENTATION_V: interpolation values for scaling acceleration cost based on speed
-  # Higher = more cautious (penalizes abrupt braking), smaller = more aggressive (less penalty)
-  SPEED_AUGMENTATION_BP = [0., LEAD_AUGMENTATION_BP_MAX, 10.]  # Speed breakpoints: [0 m/s, 5 m/s, 10 m/s (~36 km/h)]
-  SPEED_AUGMENTATION_V = [1., 1., a_change_cost_high_speed_factor]  # Multiplier: between 0-5 m/s, no change (1.0), after 5 m/s, scale by a_change_cost_high_speed_factor (e.g., 1.5 in standard mode)
-
-  # Calculate a cost for acceleration changes when lead vehicles are pulling away and ego speed is below the threshold.
-  lead_augmented_a_change_cost = 1.0  # Default cost factor
-  if (v_lead0 - v_ego > 1e-3) and (v_lead1 - v_ego > 1e-3):
-    # Interpolate for the acceleration change cost when lead vehicles are increasing speed, based on vEgo
-    lead_augmented_a_change_cost = interp(v_ego, LEAD_AUGMENTATION_BP, LEAD_AUGMENTATION_V)
-
-  # Multiply the lead-based cost with speed-based cost to get a final cost factor, scaling with vehicle speed
-  speed_augmented_a_change_cost = a_change_cost_multiplier_follow * interp(v_ego, SPEED_AUGMENTATION_BP, SPEED_AUGMENTATION_V)
-
-  # Choose the smaller factor between the lead-based cost and the speed-based cost
-  a_change_factor = lead_augmented_a_change_cost if v_ego <= LEAD_AUGMENTATION_BP_MAX else speed_augmented_a_change_cost
-
-  # Return the final acceleration change factor to be applied
-  return a_change_factor
-
-# Function to return a multiplier for a danger zone cost based on driving personality
-def get_danger_zone_factor(personality=custom.LongitudinalPersonalitySP.standard):
-  # Higher values mean more cautious driving in dangerous situations, scaling the cost accordingly
-  if personality==custom.LongitudinalPersonalitySP.relaxed:
-    return 1.8  # Higher danger zone cost for relaxed personality (more cautious)
-  elif personality==custom.LongitudinalPersonalitySP.standard:
-    return 1.5  # Medium danger zone cost for standard personality
-  elif personality==custom.LongitudinalPersonalitySP.moderate:
-    return 1.2  # Medium danger zone cost for moderate personality (similar to standard)
-  elif personality==custom.LongitudinalPersonalitySP.aggressive:
-    return 1.0  # Lowest danger zone cost for aggressive personality (less cautious)
-  elif personality==custom.LongitudinalPersonalitySP.overtake:
-    return 1.0  # Lowest danger zone cost for aggressive personality (less cautious)
-  else:
-    raise NotImplementedError("Longitudinal personality not supported")
-
-
 
 def get_T_FOLLOW(personality=custom.LongitudinalPersonalitySP.standard):
   if personality==custom.LongitudinalPersonalitySP.relaxed:
-    return 1.75
+    return 1.80
   elif personality==custom.LongitudinalPersonalitySP.standard:
-    return 1.65
+    return 1.55
   elif personality==custom.LongitudinalPersonalitySP.moderate:
-    return 1.45
+    return 1.35
   elif personality==custom.LongitudinalPersonalitySP.aggressive:
-    return 1.25
+    return 1.20
   elif personality==custom.LongitudinalPersonalitySP.overtake:
     return 0.25
   else:
@@ -158,17 +89,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, 8.33,  8.34,  27.69, 27.7]
+    y_dist = [0.0,  1.2,  1.7,  1.7,   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,    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]
   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,    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]
   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.,   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]
   else:
     raise NotImplementedError("Dynamic personality not supported")
   return np.interp(v_ego, x_vel, y_dist)
@@ -370,15 +301,12 @@ 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):
     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, 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, jerk_factor * J_EGO_COST]
+      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]
@@ -432,7 +360,7 @@ 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,
+  def update(self, radarstate, v_cruise, 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)
@@ -442,8 +370,6 @@ 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.
@@ -571,4 +497,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, prev_accel_constraint, x, v, a, j, personality=sm['controlsStateSP'].personality,
+    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)
 
     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,15 +29,22 @@ 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.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 =       [-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.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: