TorqueTune/Paddle

panda

frogpilot/common/frogpilot_variables.py

@@ -373,6 +373,10 @@ misc_tuning_levels: list[tuple[str, str | bytes, int]] = [
   ("WheelControls", "", 2)
 ]
 
+
+def scale_threshold(v_ego):
+  return 0.0 if v_ego > 31.3 else np.interp(v_ego, [0, 17.9, 26.8, 35.8, 44.7], [0.63, 0.63, 0.65, 0.95, 0.95])
+
 class FrogPilotVariables:
   def __init__(self):
     self.frogpilot_toggles = get_frogpilot_toggles(block=False)

frogpilot/controls/lib/conditional_experimental_mode.py

@@ -2,7 +2,7 @@
 from openpilot.common.filter_simple import FirstOrderFilter
 from openpilot.common.realtime import DT_MDL
 
-from openpilot.frogpilot.common.frogpilot_variables import CITY_SPEED_LIMIT, CRUISING_SPEED, THRESHOLD, params_memory
+from openpilot.frogpilot.common.frogpilot_variables import CITY_SPEED_LIMIT, CRUISING_SPEED, THRESHOLD, params_memory, scale_threshold
 
 class ConditionalExperimentalMode:
   def __init__(self, FrogPilotPlanner):
@@ -53,7 +53,7 @@ class ConditionalExperimentalMode:
       self.status_value = 8
       return True
 
-    if frogpilot_toggles.conditional_lead and self.slow_lead_detected:
+    if frogpilot_toggles.conditional_lead and self.slow_lead_detected and v_ego <= 29.1:
       self.status_value = 9 if self.frogpilot_planner.lead_one.vLead < 1 else 10
       return True
 
@@ -69,7 +69,7 @@ class ConditionalExperimentalMode:
 
   def update_conditions(self, frogpilotCarState, v_ego, frogpilot_toggles):
     self.curve_detection(v_ego, frogpilot_toggles)
-    self.slow_lead(frogpilot_toggles)
+    self.slow_lead(frogpilot_toggles, v_ego)
     self.stop_sign_and_light(frogpilotCarState, v_ego, frogpilot_toggles)
 
   def curve_detection(self, v_ego, frogpilot_toggles):
@@ -78,13 +78,14 @@ class ConditionalExperimentalMode:
     self.curvature_filter.update(self.frogpilot_planner.road_curvature_detected or curve_active)
     self.curve_detected = self.curvature_filter.x >= THRESHOLD and v_ego > CRUISING_SPEED
 
-  def slow_lead(self, frogpilot_toggles):
+  def slow_lead(self, frogpilot_toggles, v_ego):
+    v_lead = self.frogpilot_planner.lead_one.vLead
     if self.frogpilot_planner.tracking_lead:
       slower_lead = frogpilot_toggles.conditional_slower_lead and self.frogpilot_planner.frogpilot_following.slower_lead
-      stopped_lead = frogpilot_toggles.conditional_stopped_lead and self.frogpilot_planner.lead_one.vLead < 1
-
+      stopped_lead = frogpilot_toggles.conditional_stopped_lead and v_lead < 1
+      lead_threshold = scale_threshold(v_ego)
       self.slow_lead_filter.update(slower_lead or stopped_lead)
-      self.slow_lead_detected = self.slow_lead_filter.x >= THRESHOLD
+      self.slow_lead_detected = self.slow_lead_filter.x >= lead_threshold
     else:
       self.slow_lead_filter.x = 0
       self.slow_lead_detected = False

frogpilot/controls/lib/frogpilot_acceleration.py

@@ -1,6 +1,43 @@
 #!/usr/bin/env python3
 import numpy as np
 
+def cubic_interp(x, xp, fp):
+     """Cubic interpolation using NumPy's native operations for speed."""
+     # Boundary conditions
+     if x <= xp[0]:
+         return fp[0]
+     elif x >= xp[-1]:
+         return fp[-1]
+ 
+     # Find interval
+     i = np.searchsorted(xp, x) - 1
+     i = max(0, min(i, len(xp)-2))  # clamp the index
+ 
+     # Normalized position
+     t = (x - xp[i]) / float(xp[i+1] - xp[i])
+ 
+     # Hermite cubic formula
+     return fp[i]*(1 - 3*t**2 + 2*t**3) + fp[i+1]*(3*t**2 - 2*t**3)
+ 
+def akima_interp(x, xp, fp):
+     """Akima-inspired interpolation with reduced overshoot characteristics."""
+     if x <= xp[0]:
+         return fp[0]
+     elif x >= xp[-1]:
+         return fp[-1]
+ 
+     i = np.searchsorted(xp, x) - 1
+     i = max(0, min(i, len(xp)-2))  # clamp the index
+ 
+     t = (x - xp[i]) / float(xp[i+1] - xp[i])
+ 
+     # Quintic polynomial to reduce overshoot
+     t2 = t*t
+     t4 = t2*t2
+     t3 = t2*t
+     return (fp[i]*(1 - 10*t3 + 15*t4 - 6*t3*t2)
+             + fp[i+1]*(10*t3 - 15*t4 + 6*t3*t2))
+ 
 from openpilot.selfdrive.controls.lib.longitudinal_planner import A_CRUISE_MIN, get_max_accel
 
 from openpilot.frogpilot.common.frogpilot_variables import CITY_SPEED_LIMIT
@@ -11,26 +48,26 @@ A_CRUISE_MIN_SPORT = A_CRUISE_MIN * 2
                        # MPH = [0.0,  11,  22,  34,  45,  56,  89]
 A_CRUISE_MAX_BP_CUSTOM =       [0.0,  5., 10., 15., 20., 25., 40.]
 A_CRUISE_MAX_VALS_ECO =        [2.0, 1.5, 1.0, 0.8, 0.6, 0.4, 0.2]
-A_CRUISE_MAX_VALS_SPORT =      [3.0, 2.5, 2.0, 1.5, 1.0, 0.8, 0.6]
-A_CRUISE_MAX_VALS_SPORT_PLUS = [4.0, 3.5, 3.0, 2.5, 2.0, 1.5, 1.0]
+A_CRUISE_MAX_VALS_SPORT =      [1.5, 1.5, 1.25, 1.5, 1.5, 1.5, 2.0]
+A_CRUISE_MAX_VALS_SPORT_PLUS = [2.5, 2.5, 3.0, 2.5, 2.5, 2.5, 2.5]
 
 def get_max_accel_eco(v_ego):
-  return float(np.interp(v_ego, A_CRUISE_MAX_BP_CUSTOM, A_CRUISE_MAX_VALS_ECO))
+  return float(akima_interp(v_ego, A_CRUISE_MAX_BP_CUSTOM, A_CRUISE_MAX_VALS_ECO))
 
 def get_max_accel_sport(v_ego):
-  return float(np.interp(v_ego, A_CRUISE_MAX_BP_CUSTOM, A_CRUISE_MAX_VALS_SPORT))
+  return float(akima_interp(v_ego, A_CRUISE_MAX_BP_CUSTOM, A_CRUISE_MAX_VALS_SPORT))
 
 def get_max_accel_sport_plus(v_ego):
-  return float(np.interp(v_ego, A_CRUISE_MAX_BP_CUSTOM, A_CRUISE_MAX_VALS_SPORT_PLUS))
+  return float(akima_interp(v_ego, A_CRUISE_MAX_BP_CUSTOM, A_CRUISE_MAX_VALS_SPORT_PLUS))
 
 def get_max_accel_low_speeds(max_accel, v_cruise):
-  return float(np.interp(v_cruise, [0., CITY_SPEED_LIMIT / 2, CITY_SPEED_LIMIT], [max_accel / 4, max_accel / 2, max_accel]))
+  return float(akima_interp(v_cruise, [0., CITY_SPEED_LIMIT / 2, CITY_SPEED_LIMIT], [max_accel / 4, max_accel / 2, max_accel]))
 
 def get_max_accel_ramp_off(max_accel, v_cruise, v_ego):
-  return float(np.interp(v_cruise - v_ego, [0., 1., 5., 10.], [0., 0.5, 1.0, max_accel]))
+  return float(akima_interp(v_cruise - v_ego, [0., 1., 5., 10.], [0., 0.5, 1.0, max_accel]))
 
 def get_max_allowed_accel(v_ego):
-  return float(np.interp(v_ego, [0., 5., 20.], [4.0, 4.0, 2.0]))  # ISO 15622:2018
+  return float(akima_interp(v_ego, [0., 5., 20.], [4.0, 4.0, 2.0]))  # ISO 15622:2018
 
 class FrogPilotAcceleration:
   def __init__(self, FrogPilotPlanner):

frogpilot/system/fleetmanager/templates/layout.html

@@ -40,7 +40,7 @@
     text-align: center;
   }
 </style>
-  <title>FrogPilot: {% block title %}{% endblock %}</title>
+  <title>StarPilot: {% block title %}{% endblock %}</title>
 </head>
 <body>
   <nav class="navbar navbar-fixed-top navbar-expand-sm navbar-dark bg-dark">

opendbc/gm_global_a_powertrain_generated.dbc

@@ -82,6 +82,12 @@ VAL_TABLE_ HandsOffSWDetectionMode 2 "Failed" 1 "Enabled" 0 "Disabled" ;
 
 BO_ 189 EBCMRegenPaddle: 7 K17_EBCM
  SG_ RegenPaddle : 7|4@0+ (1,0) [0|0] ""  NEO
+ SG_ Byte1 : 8|8@1+ (1,0) [0|255] "" NEO
+ SG_ Byte2 : 16|8@1+ (1,0) [0|255] "" NEO
+ SG_ Byte3 : 24|8@1+ (1,0) [0|255] "" NEO
+ SG_ Byte4 : 32|8@1+ (1,0) [0|255] "" NEO
+ SG_ Byte5 : 40|8@1+ (1,0) [0|255] "" NEO
+ SG_ Byte6 : 48|8@1+ (1,0) [0|255] "" NEO
 
 BO_ 190 ECMAcceleratorPos: 6 K20_ECM
  SG_ BrakePedalPos : 15|8@0+ (1,0) [0|0] "sticky"  NEO
@@ -192,10 +198,15 @@ BO_ 500 SportMode: 6 XXX
  SG_ SportMode : 15|1@0+ (1,0) [0|1] "" XXX
 
 BO_ 501 ECMPRDNL2: 8 K20_ECM
- SG_ TransmissionState : 48|4@1+ (1,0) [0|7] "" NEO
+ SG_ Byte0 : 0|8@1+ (1,0) [0|255] "" NEO
+ SG_ Byte1 : 8|8@1+ (1,0) [0|255] "" NEO
+ SG_ Byte2 : 16|8@1+ (1,0) [0|255] "" NEO
  SG_ PRNDL2 : 27|4@0+ (1,0) [0|255] "" NEO
+ SG_ Byte4 : 32|8@1+ (1,0) [0|255] "" NEO
  SG_ ManualMode : 41|1@0+ (1,0) [0|1] "" NEO
-
+ SG_ TransmissionState : 48|4@1+ (1,0) [0|7] "" NEO
+ SG_ Byte7 : 56|8@1+ (1,0) [0|255] "" NEO
+ 
 BO_ 532 BRAKE_RELATED: 6 XXX
  SG_ UserBrakePressure : 0|9@0+ (1,0) [0|511] "" XXX
 
@@ -370,6 +381,6 @@ VAL_ 715 GasRegenCmdActive 1 "Active" 0 "Inactive" ;
 VAL_ 320 Intellibeam 1 "Active" 0 "Inactive" ;
 VAL_ 320 HighBeamsActive 1 "Active" 0 "Inactive" ;
 VAL_ 320 HighBeamsTemporary 1 "Active" 0 "Inactive" ;
-VAL_ 501 PRNDL2 6 "L" 4 "D" 3 "N" 2 "R" 1 "P" 0 "Shifting";
+VAL_ 501 PRNDL2 7 "L2" 6 "L" 5 "L3" 4 "D" 3 "N" 2 "R" 1 "P" 0 "Shifting";
 VAL_ 501 TransmissionState 11 "Shifting" 10 "Reverse" 9 "Forward" 8 "Disengaged";
 VAL_ 501 ManualMode 1 "Active" 0 "Inactive"

selfdrive/car/gm/carcontroller.py

@@ -1,3 +1,4 @@
+from typing import Tuple
 from cereal import car
 from openpilot.common.conversions import Conversions as CV
 from openpilot.common.filter_simple import FirstOrderFilter
@@ -7,7 +8,7 @@ from openpilot.common.params_pyx import Params
 from opendbc.can.packer import CANPacker
 from openpilot.selfdrive.car import apply_driver_steer_torque_limits, create_gas_interceptor_command
 from openpilot.selfdrive.car.gm import gmcan
-from openpilot.selfdrive.car.gm.values import DBC, CanBus, CarControllerParams, CruiseButtons, GMFlags, CC_ONLY_CAR, SDGM_CAR, EV_CAR, AccState
+from openpilot.selfdrive.car.gm.values import DBC, CanBus, CarControllerParams, CruiseButtons, GMFlags, CC_ONLY_CAR, SDGM_CAR, EV_CAR, AccState, CC_REGEN_PADDLE_CAR
 from openpilot.selfdrive.car.interfaces import CarControllerBase
 from openpilot.selfdrive.controls.lib.drive_helpers import apply_deadzone
 from openpilot.selfdrive.controls.lib.vehicle_model import ACCELERATION_DUE_TO_GRAVITY
@@ -55,23 +56,53 @@ class CarController(CarControllerBase):
     # FrogPilot variables
     self.pitch = FirstOrderFilter(0., 0.09 * 4, DT_CTRL * 4)  # runs at 25 Hz
     self.accel_g = 0.0
+    self.regen_paddle_pressed = False
+    self.aego = 0.0
 
-  @staticmethod
-  def calc_pedal_command(accel: float, long_active: bool) -> float:
-    if not long_active: return 0.
+  def calc_pedal_command(self, accel: float, long_active: bool, car_velocity) -> Tuple[float, bool]:
+    if not long_active:
+      return 0., False
 
-    zero = 0.15625  # 40/256
-    if accel > 0.:
-      # Scales the accel from 0-1 to 0.156-1
-      pedal_gas = clip(((1 - zero) * accel + zero), 0., 1.)
+    # Regen paddle hysteresis (200ms = 20 frames)
+    if not hasattr(self, 'regen_paddle_timer'):
+      self.regen_paddle_timer = 0
+
+    if self.aego < -0.7 and accel <= 0.0:
+      self.regen_paddle_timer += 1
     else:
-      # if accel is negative, -0.1 -> 0.015625
-      pedal_gas = clip(zero + accel, 0., zero)  # Make brake the same size as gas, but clip to regen
+      self.regen_paddle_timer = max(self.regen_paddle_timer - 1, 0)
 
-    return pedal_gas
+    self.regen_paddle_pressed = self.regen_paddle_timer >= 20
+
+    press_regen_paddle = self.regen_paddle_pressed
+
+    # Updated regen gain ratios from bin-averaged 60–0 deceleration sweep
+    speed_mps = [0.559, 1.678, 2.797, 3.916, 5.035, 6.154, 7.273, 8.392, 9.511, 10.63,
+                 11.749, 12.868, 13.987, 15.106, 16.225, 17.344, 18.463, 19.582, 20.701, 21.820,
+                 22.939, 24.058, 25.177, 26.296]
+    regen_gain_ratio = [1.289606, 1.227308, 1.200043, 1.274589, 1.332296, 1.345979, 1.369975,
+                         1.376302, 1.388052, 1.370367, 1.388498, 1.386030, 1.405950, 1.387555,
+                         1.390392, 1.394946, 1.414915, 1.428535, 1.439611, 1.440106, 1.441438,
+                         1.439395, 1.446909, 1.445738]
+
+    gain = interp(car_velocity, speed_mps, regen_gain_ratio)
+
+    pedaloffset = interp(car_velocity, [0., 3, 6, 30], [0.10, 0.175, 0.240, 0.240])
+    accel_cutoff = -0.5 * gain
+    
+    if press_regen_paddle:
+      pedal_gas = pedaloffset + (accel / gain) * 0.6
+      pedal_gas = max(pedal_gas, 0.01)
+    else:
+      pedal_gas = clip((pedaloffset + accel * 0.6), 0.0, 1.0)
+    pedal_gas = min(pedal_gas, 1.0)
+
+    return pedal_gas, press_regen_paddle
 
 
   def update(self, CC, CS, now_nanos, frogpilot_toggles):
+    self.CS = CS
+    self.aego = CS.out.aEgo
     actuators = CC.actuators
     accel = brake_accel = actuators.accel
     hud_control = CC.hudControl
@@ -83,6 +114,36 @@ class CarController(CarControllerBase):
     # Send CAN commands.
     can_sends = []
 
+    # Only send regen paddle and PRNDL2 commands at 40Hz when regen is active
+    regen_active = (
+      self.CP.carFingerprint in CC_REGEN_PADDLE_CAR and
+      self.CP.openpilotLongitudinalControl and
+      CC.longActive and
+      self.regen_paddle_pressed
+    )
+
+    # Time guard: PRNDL2 must be spaced out > 25ms (matches ~40Hz)
+    if regen_active:
+      current_time_ms = now_nanos * 1e-6
+      last_sent_time_ms = getattr(self, "last_prndl2_sent_time_ms", -1000)
+      frames_since_last = self.frame - getattr(self, "last_prndl2_frame", -4)
+      frame_wait = 3 if getattr(self, "wait_long_40hz", False) else 2
+
+      if (frames_since_last >= frame_wait) and (current_time_ms - last_sent_time_ms >= 25):
+        self.last_prndl2_frame = self.frame
+        self.last_prndl2_sent_time_ms = current_time_ms
+        self.wait_long_40hz = not getattr(self, "wait_long_40hz", False)
+
+        prndl2_value = 7
+        regen_paddle_value = 2
+        manual_mode = 1
+
+        can_sends.append(gmcan.create_prndl2_command(
+          self.packer_pt, CanBus.POWERTRAIN, prndl2_value, manual_mode
+        ))
+        can_sends.append(gmcan.create_regen_paddle_command(self.packer_pt, CanBus.POWERTRAIN, regen_paddle_value))
+
+
     # Steering (Active: 50Hz, inactive: 10Hz)
     steer_step = self.params.STEER_STEP if CC.latActive else self.params.INACTIVE_STEER_STEP
 
@@ -142,12 +203,11 @@ class CarController(CarControllerBase):
         else:
           # Normal operation
           if self.CP.carFingerprint in EV_CAR:
-            self.params.update_ev_gas_brake_threshold(CS.out.vEgo)
             if frogpilot_toggles.sport_plus:
-              self.apply_gas = int(round(interp(accel, self.params.EV_GAS_LOOKUP_BP_PLUS, self.params.GAS_LOOKUP_V_PLUS)))
+              self.apply_gas = int(round(interp(accel, self.params.GAS_LOOKUP_BP_PLUS, self.params.GAS_LOOKUP_V_PLUS)))
             else:
-              self.apply_gas = int(round(interp(accel, self.params.EV_GAS_LOOKUP_BP, self.params.GAS_LOOKUP_V)))
-            self.apply_brake = int(round(interp(brake_accel, self.params.EV_BRAKE_LOOKUP_BP, self.params.BRAKE_LOOKUP_V)))
+              self.apply_gas = int(round(interp(accel, self.params.GAS_LOOKUP_BP, self.params.GAS_LOOKUP_V)))
+            self.apply_brake = int(round(interp(brake_accel, self.params.BRAKE_LOOKUP_BP, self.params.BRAKE_LOOKUP_V)))
           else:
             if frogpilot_toggles.sport_plus:
               self.apply_gas = int(round(interp(accel, self.params.GAS_LOOKUP_BP_PLUS, self.params.GAS_LOOKUP_V_PLUS)))
@@ -160,12 +220,13 @@ class CarController(CarControllerBase):
             self.apply_gas = self.params.INACTIVE_REGEN
           if self.CP.carFingerprint in CC_ONLY_CAR:
             # gas interceptor only used for full long control on cars without ACC
-            interceptor_gas_cmd = self.calc_pedal_command(actuators.accel, CC.longActive)
+            interceptor_gas_cmd, press_regen_paddle = self.calc_pedal_command(actuators.accel, CC.longActive, CS.out.vEgo)
 
         if self.CP.enableGasInterceptor and self.apply_gas > self.params.INACTIVE_REGEN and CS.out.cruiseState.standstill:
           # "Tap" the accelerator pedal to re-engage ACC
           interceptor_gas_cmd = self.params.SNG_INTERCEPTOR_GAS
           self.apply_brake = 0
+          press_regen_paddle = False
           self.apply_gas = self.params.INACTIVE_REGEN
 
         idx = (self.frame // 4) % 4
@@ -196,7 +257,7 @@ class CarController(CarControllerBase):
           # GasRegenCmdActive needs to be 1 to avoid cruise faults. It describes the ACC state, not actuation
           can_sends.append(gmcan.create_gas_regen_command(self.packer_pt, CanBus.POWERTRAIN, self.apply_gas, idx, acc_engaged, at_full_stop))
           can_sends.append(gmcan.create_friction_brake_command(self.packer_ch, friction_brake_bus, self.apply_brake,
-                                                             idx, CC.enabled, near_stop, at_full_stop, self.CP))
+                                                               idx, CC.enabled, near_stop, at_full_stop, self.CP))
 
           # Send dashboard UI commands (ACC status)
           send_fcw = hud_alert == VisualAlert.fcw

selfdrive/car/gm/carstate.py

@@ -96,7 +96,7 @@ class CarState(CarStateBase):
 
     if self.CP.enableGasInterceptor:
       ret.gas = (pt_cp.vl["GAS_SENSOR"]["INTERCEPTOR_GAS"] + pt_cp.vl["GAS_SENSOR"]["INTERCEPTOR_GAS2"]) / 2.
-      threshold = 10 if self.CP.carFingerprint in CAMERA_ACC_CAR else 4 # Panda 515 threshold = 10.88. Set lower to avoid panda blocking messages and GasInterceptor faulting.
+      threshold = 12 if self.CP.carFingerprint in CAMERA_ACC_CAR else 4 # Panda 595 threshold = 10.88. Set lower to avoid panda blocking messages and GasInterceptor faulting.
       ret.gasPressed = ret.gas > threshold
     else:
       ret.gas = pt_cp.vl["AcceleratorPedal2"]["AcceleratorPedal2"] / 254.
@@ -169,7 +169,7 @@ class CarState(CarStateBase):
         ret.leftBlindspot = cam_cp.vl["BCMBlindSpotMonitor"]["LeftBSM"] == 1
         ret.rightBlindspot = cam_cp.vl["BCMBlindSpotMonitor"]["RightBSM"] == 1
 
-    # FrogPilot CarState functions
+
     self.lkas_previously_enabled = self.lkas_enabled
     if self.CP.carFingerprint in SDGM_CAR:
       self.lkas_enabled = cam_cp.vl["ASCMSteeringButton"]["LKAButton"]
@@ -224,13 +224,13 @@ class CarState(CarStateBase):
 
     if CP.carFingerprint in SDGM_CAR:
       messages += [
-        ("ECMPRDNL2", 40),
+        ("ECMPRDNL2", 25),
         ("AcceleratorPedal2", 40),
         ("ECMEngineStatus", 80),
       ]
     else:
       messages += [
-        ("ECMPRDNL2", 10),
+        ("ECMPRDNL2", 25),
         ("AcceleratorPedal2", 33),
         ("ECMEngineStatus", 100),
         ("BCMTurnSignals", 1),
@@ -252,7 +252,7 @@ class CarState(CarStateBase):
 
     if CP.transmissionType == TransmissionType.direct:
       messages += [
-        ("EBCMRegenPaddle", 50),
+        ("EBCMRegenPaddle", 25),
         ("EVDriveMode", 0),
       ]
 

selfdrive/car/gm/gmcan.py

@@ -177,6 +177,30 @@ def create_lka_icon_command(bus, active, critical, steer):
     dat = b"\x00\x00\x00"
   return make_can_msg(0x104c006c, dat, bus)
 
+def create_regen_paddle_command(packer, bus, regen_paddle_value):
+   values = {
+     "RegenPaddle": regen_paddle_value,
+     "Byte1": 0,
+     "Byte2": 0,
+     "Byte3": 0,
+     "Byte4": 0,
+     "Byte5": 0,
+     "Byte6": 0
+   }
+   return packer.make_can_msg("EBCMRegenPaddle", bus, values)
+
+def create_prndl2_command(packer, bus, prndl2_value, manual_mode):
+  values = {
+    "Byte0": 0x0C,
+    "Byte1": 0x0C,
+    "Byte2": 0x00,
+    "PRNDL2": prndl2_value,
+    "Byte4": 0x00,
+    "ManualMode": manual_mode,
+    "TransmissionState": 1,
+    "Byte7": 0x00
+  }
+  return packer.make_can_msg("ECMPRDNL2", bus, values)
 
 def create_gm_cc_spam_command(packer, controller, CS, actuators):
   if controller.params_.get_bool("IsMetric"):

selfdrive/car/gm/interface.py

@@ -29,8 +29,8 @@ CAM_MSG = 0x320  # AEBCmd
 ACCELERATOR_POS_MSG = 0xbe
 
 NON_LINEAR_TORQUE_PARAMS = {
-  CAR.CHEVROLET_BOLT_EUV: [2.6531724862969748, 1.0, 0.1919764879840985, 0.009054123646805178],
-  CAR.CHEVROLET_BOLT_CC: [2.6531724862969748, 1.0, 0.1919764879840985, 0.009054123646805178],
+  CAR.CHEVROLET_BOLT_EUV: [1.8, 1.1, 0.290, -0.045],
+  CAR.CHEVROLET_BOLT_CC: [1.8, 1.1, 0.290, -0.045],
   CAR.GMC_ACADIA: [4.78003305, 1.0, 0.3122, 0.05591772],
   CAR.CHEVROLET_SILVERADO: [3.29974374, 1.0, 0.25571356, 0.0465122]
 }
@@ -74,8 +74,8 @@ class CarInterface(CarInterfaceBase):
     # ToDo: To generalize to other GMs, explore tanh function as the nonlinear
     non_linear_torque_params = NON_LINEAR_TORQUE_PARAMS.get(self.CP.carFingerprint)
     assert non_linear_torque_params, "The params are not defined"
-    a, b, c, _ = non_linear_torque_params
-    steer_torque = (sig(latcontrol_inputs.lateral_acceleration * a) * b) + (latcontrol_inputs.lateral_acceleration * c)
+    a, b, c, d = non_linear_torque_params
+    steer_torque = (sig(latcontrol_inputs.lateral_acceleration * a) * b) + (latcontrol_inputs.lateral_acceleration * c) + d
     return float(steer_torque) + friction
 
   def torque_from_lateral_accel_neural(self, latcontrol_inputs: LatControlInputs, torque_params: car.CarParams.LateralTorqueTuning, lateral_accel_error: float,
@@ -87,13 +87,7 @@ class CarInterface(CarInterfaceBase):
     return float(self.neural_ff_model.predict(inputs)) + friction
 
   def torque_from_lateral_accel(self) -> TorqueFromLateralAccelCallbackType:
-    if self.CP.carFingerprint in (CAR.CHEVROLET_BOLT_EUV, CAR.CHEVROLET_BOLT_CC):
-      self.neural_ff_model = NanoFFModel(NEURAL_PARAMS_PATH, self.CP.carFingerprint)
-      return self.torque_from_lateral_accel_neural
-    elif self.CP.carFingerprint in NON_LINEAR_TORQUE_PARAMS:
       return self.torque_from_lateral_accel_siglin
-    else:
-      return self.torque_from_lateral_accel_linear
 
   @staticmethod
   def _get_params(ret, candidate, fingerprint, car_fw, experimental_long, docs, frogpilot_toggles):
@@ -110,7 +104,7 @@ class CarInterface(CarInterfaceBase):
     else:
       ret.transmissionType = TransmissionType.automatic
 
-    ret.longitudinalTuning.kiBP = [5., 35.]
+    ret.longitudinalTuning.kiBP = [5., 35., 60.]
 
     if candidate in CAMERA_ACC_CAR:
       ret.experimentalLongitudinalAvailable = candidate not in CC_ONLY_CAR
@@ -122,13 +116,14 @@ class CarInterface(CarInterfaceBase):
       ret.minSteerSpeed = 10 * CV.KPH_TO_MS
 
       # Tuning for experimental long
-      ret.longitudinalTuning.kiV = [2.0, 1.5]
+      ret.longitudinalTuning.kiV = [0.5, 0.5, 0.5]
       ret.vEgoStopping = 0.1
       ret.vEgoStarting = 0.1
 
-      ret.stoppingDecelRate = 2.0  # reach brake quickly after enabling
+      ret.stoppingDecelRate = 1.0  # reach brake quickly after enabling
       ret.vEgoStopping = 0.25
       ret.vEgoStarting = 0.25
+      ret.stopAccel = -0.25
 
       if experimental_long:
         ret.pcmCruise = False
@@ -136,7 +131,7 @@ class CarInterface(CarInterfaceBase):
         ret.safetyConfigs[0].safetyParam |= Panda.FLAG_GM_HW_CAM_LONG
 
     elif candidate in SDGM_CAR:
-      ret.longitudinalTuning.kiV = [0., 0.]  # TODO: tuning
+      ret.longitudinalTuning.kiV = [0., 0., 0.]  # TODO: tuning
       ret.experimentalLongitudinalAvailable = False
       ret.networkLocation = NetworkLocation.fwdCamera
       ret.pcmCruise = True
@@ -155,7 +150,7 @@ class CarInterface(CarInterfaceBase):
       ret.minSteerSpeed = 7 * CV.MPH_TO_MS
 
       # Tuning
-      ret.longitudinalTuning.kiV = [2.4, 1.5]
+      ret.longitudinalTuning.kiV = [0.5, 0.5, 0.5]
 
       if ret.enableGasInterceptor:
         # Need to set ASCM long limits when using pedal interceptor, instead of camera ACC long limits
@@ -206,6 +201,7 @@ class CarInterface(CarInterfaceBase):
     elif candidate in (CAR.CHEVROLET_BOLT_EUV, CAR.CHEVROLET_BOLT_CC):
       ret.steerActuatorDelay = 0.2
       CarInterfaceBase.configure_torque_tune(candidate, ret.lateralTuning)
+      ret.lateralTuning.torque.kp = 0.6
 
       if ret.enableGasInterceptor:
         # ACC Bolts use pedal for full longitudinal control, not just sng
@@ -271,13 +267,13 @@ class CarInterface(CarInterfaceBase):
       ret.stoppingControl = True
       ret.autoResumeSng = True
 
-      if candidate in CC_ONLY_CAR:
+      if candidate in CC_ONLY_CAR: #pedal interceptor tuning
         ret.flags |= GMFlags.PEDAL_LONG.value
         ret.safetyConfigs[0].safetyParam |= Panda.FLAG_GM_PEDAL_LONG
         # Note: Low speed, stop and go not tested. Should be fairly smooth on highway
-        ret.longitudinalTuning.kiBP = [0.0, 5., 35.]
-        ret.longitudinalTuning.kiV = [0.0, 0.35, 0.5]
-        ret.longitudinalTuning.kf = 0.15
+        ret.longitudinalTuning.kiBP = [0., 3., 6., 35.]
+        ret.longitudinalTuning.kiV = [0.125, 0.175, 0.225, 0.33]
+        ret.longitudinalTuning.kf = 0.25
         ret.stoppingDecelRate = 0.8
       else:  # Pedal used for SNG, ACC for longitudinal control otherwise
         ret.safetyConfigs[0].safetyParam |= Panda.FLAG_GM_HW_CAM_LONG
@@ -294,16 +290,15 @@ class CarInterface(CarInterfaceBase):
       ret.openpilotLongitudinalControl = not frogpilot_toggles.disable_openpilot_long
       ret.pcmCruise = False
 
-      ret.stoppingDecelRate = 11.18  # == 25 mph/s (.04 rate)
-
-      ret.longitudinalTuning.deadzoneBP = [0.]
-      ret.longitudinalTuning.deadzoneV = [0.56]  # == 2 km/h/s, 1.25 mph/s
-      ret.longitudinalActuatorDelay = 1.  # TODO: measure this
-
-      ret.longitudinalTuning.kpBP = [10.7, 10.8, 28.]  # 10.7 m/s == 24 mph
-      ret.longitudinalTuning.kpV = [0., 20., 20.]  # set lower end to 0 since we can't drive below that speed
-      ret.longitudinalTuning.kiBP = [0.]
-      ret.longitudinalTuning.kiV = [0.1]
+      if not ret.enableGasInterceptor and candidate in CC_ONLY_CAR: #redneck tuning
+        ret.longitudinalTuning.kpBP = [10.7, 10.8, 28.]  # 10.7 m/s == 24 mph
+        ret.longitudinalTuning.kpV = [0., 20., 20.]  # set lower end to 0 since we can't drive below that speed
+        ret.longitudinalTuning.deadzoneBP = [0.]
+        ret.longitudinalTuning.deadzoneV = [0.56]  # == 2 km/h/s, 1.25 mph/s
+        ret.longitudinalActuatorDelay = 1.  # TODO: measure this
+        ret.longitudinalTuning.kiBP = [0.]
+        ret.longitudinalTuning.kiV = [0.1]
+        ret.stoppingDecelRate = 11.18  # == 25 mph/s (.04 rate)
 
     if candidate in CC_ONLY_CAR:
       ret.safetyConfigs[0].safetyParam |= Panda.FLAG_GM_NO_ACC

selfdrive/car/gm/values.py

@@ -41,7 +41,7 @@ class CarControllerParams:
     self.ZERO_GAS = 6144  # Coasting
     self.MAX_BRAKE = 400  # ~ -4.0 m/s^2 with regen
 
-    if CP.carFingerprint in CAMERA_ACC_CAR and CP.carFingerprint not in CC_ONLY_CAR:
+    if CP.carFingerprint in CAMERA_ACC_CAR and CP.carFingerprint not in CC_ONLY_CAR and CP.carFingerprint != CAR.CHEVROLET_BOLT_EUV:
       self.MAX_GAS = 7496
       self.MAX_GAS_PLUS = 8848
       self.MAX_ACC_REGEN = 5610
@@ -53,18 +53,18 @@ class CarControllerParams:
     elif CP.carFingerprint in SDGM_CAR:
       self.MAX_GAS = 7496
       self.MAX_GAS_PLUS = 7496
-      self.MAX_ACC_REGEN = 5610
+      self.MAX_ACC_REGEN = 7110
       self.INACTIVE_REGEN = 5650
       self.max_regen_acceleration = 0.
 
     else:
       self.MAX_GAS = 7168  # Safety limit, not ACC max. Stock ACC >8192 from standstill.
       self.MAX_GAS_PLUS = 8191 # 8292 uses new bit, possible but not tested. Matches Twilsonco tw-main max
-      self.MAX_ACC_REGEN = 5500  # Max ACC regen is slightly less than max paddle regen
+      self.MAX_ACC_REGEN = 7110  # Increased for stronger regen braking
       self.INACTIVE_REGEN = 5500
       # ICE has much less engine braking force compared to regen in EVs,
       # lower threshold removes some braking deadzone
-      self.max_regen_acceleration = -1. if CP.carFingerprint in EV_CAR else -0.1
+      self.max_regen_acceleration = -3. if CP.carFingerprint in EV_CAR else -0.1  # More aggressive regen for EVs
 
     self.GAS_LOOKUP_BP = [self.max_regen_acceleration, 0., self.ACCEL_MAX]
     self.GAS_LOOKUP_BP_PLUS = [self.max_regen_acceleration, 0., self.ACCEL_MAX_PLUS]
@@ -74,18 +74,7 @@ class CarControllerParams:
     self.BRAKE_LOOKUP_BP = [self.ACCEL_MIN, self.max_regen_acceleration]
     self.BRAKE_LOOKUP_V = [self.MAX_BRAKE, 0.]
 
-  # determined by letting Volt regen to a stop in L gear from 89mph,
-  # and by letting off gas and allowing car to creep, for determining
-  # the positive threshold values at very low speed
-  EV_GAS_BRAKE_THRESHOLD_BP = [1.29, 1.52, 1.55, 1.6, 1.7, 1.8, 2.0, 2.2, 2.5, 5.52, 9.6, 20.5, 23.5, 35.0] # [m/s]
-  EV_GAS_BRAKE_THRESHOLD_V = [0.0, -0.14, -0.16, -0.18, -0.215, -0.255, -0.32, -0.41, -0.5, -0.72, -0.895, -1.125, -1.145, -1.16] # [m/s^s]
 
-  def update_ev_gas_brake_threshold(self, v_ego):
-    gas_brake_threshold = interp(v_ego, self.EV_GAS_BRAKE_THRESHOLD_BP, self.EV_GAS_BRAKE_THRESHOLD_V)
-    self.GAS_LOOKUP_BP_PLUS = [self.max_regen_acceleration, 0., self.ACCEL_MAX_PLUS]
-    self.EV_GAS_LOOKUP_BP = [gas_brake_threshold, max(0., gas_brake_threshold), self.ACCEL_MAX]
-    self.EV_GAS_LOOKUP_BP_PLUS = [gas_brake_threshold, max(0., gas_brake_threshold), self.ACCEL_MAX_PLUS]
-    self.EV_BRAKE_LOOKUP_BP = [self.ACCEL_MIN, gas_brake_threshold]
 
 @dataclass
 class GMCarDocs(CarDocs):
@@ -205,15 +194,15 @@ class CAR(Platforms):
     CHEVROLET_SUBURBAN.specs,
   )
   GMC_YUKON_CC = GMPlatformConfig(
-    [GMCarDocs("GMC Yukon - No-ACC")],
+    [GMCarDocs("GMC Yukon No ACC")],
     CarSpecs(mass=2541, wheelbase=2.95, steerRatio=16.3, centerToFrontRatio=0.4),
   )
   CADILLAC_CT6_CC = GMPlatformConfig(
-    [GMCarDocs("Cadillac CT6 - No-ACC")],
+    [GMCarDocs("Cadillac CT6 No ACC")],
     CarSpecs(mass=2358, wheelbase=3.11, steerRatio=17.7, centerToFrontRatio=0.4),
   )
   CHEVROLET_TRAILBLAZER_CC = GMPlatformConfig(
-    [GMCarDocs("Chevrolet Trailblazer 2021-22 - No-ACC")],
+    [GMCarDocs("Chevrolet Trailblazer 2021-22")],
     CHEVROLET_TRAILBLAZER.specs,
   )
   CADILLAC_XT4 = GMPlatformConfig(
@@ -221,7 +210,7 @@ class CAR(Platforms):
     CarSpecs(mass=1660, wheelbase=2.78, steerRatio=14.4, centerToFrontRatio=0.4),
   )
   CADILLAC_XT5_CC = GMPlatformConfig(
-    [GMCarDocs("Cadillac XT5 - No-ACC")],
+    [GMCarDocs("Cadillac XT5 No ACC")],
     CarSpecs(mass=1810, wheelbase=2.86, steerRatio=16.34, centerToFrontRatio=0.5),
   )
   CHEVROLET_TRAVERSE = GMPlatformConfig(
@@ -233,7 +222,7 @@ class CAR(Platforms):
     CarSpecs(mass=2050, wheelbase=2.86, steerRatio=16.0, centerToFrontRatio=0.5),
   )
   CHEVROLET_MALIBU_CC = GMPlatformConfig(
-    [GMCarDocs("Chevrolet Malibu 2023 - No-ACC")],
+    [GMCarDocs("Chevrolet Malibu 2023 No ACC")],
     CarSpecs(mass=1450, wheelbase=2.8, steerRatio=15.8, centerToFrontRatio=0.4),
   )
   CHEVROLET_TRAX = GMPlatformConfig(
@@ -322,6 +311,7 @@ FW_QUERY_CONFIG = FwQueryConfig(
 
 EV_CAR = {CAR.CHEVROLET_VOLT, CAR.CHEVROLET_BOLT_EUV, CAR.CHEVROLET_VOLT_CC, CAR.CHEVROLET_BOLT_CC}
 CC_ONLY_CAR = {CAR.CHEVROLET_VOLT_CC, CAR.CHEVROLET_BOLT_CC, CAR.CHEVROLET_EQUINOX_CC, CAR.CHEVROLET_SUBURBAN_CC, CAR.GMC_YUKON_CC, CAR.CADILLAC_CT6_CC, CAR.CHEVROLET_TRAILBLAZER_CC, CAR.CADILLAC_XT5_CC, CAR.CHEVROLET_MALIBU_CC}
+CC_REGEN_PADDLE_CAR = {CAR.CHEVROLET_BOLT_CC, CAR.CHEVROLET_BOLT_EUV}
 # CC_ONLY_CAR = set(c for c in CAR if str(c).endswith('_CC'))
 
 # We're integrated at the Safety Data Gateway Module on these cars

selfdrive/car/interfaces.py

@@ -411,7 +411,7 @@ class CarInterfaceBase(ABC):
 
     tune.init('torque')
     tune.torque.useSteeringAngle = use_steering_angle
-    tune.torque.kp = 1.0
+    tune.torque.kp = 0.6
     tune.torque.kf = 1.0
     tune.torque.ki = 0.1
     tune.torque.friction = params['FRICTION']

selfdrive/controls/lib/longcontrol.py

@@ -4,6 +4,8 @@ from openpilot.common.realtime import DT_CTRL
 from openpilot.selfdrive.controls.lib.drive_helpers import CONTROL_N, apply_deadzone
 from openpilot.selfdrive.controls.lib.pid import PIDController
 from openpilot.selfdrive.modeld.constants import ModelConstants
+from openpilot.common.filter_simple import FirstOrderFilter
+from openpilot.selfdrive.car.gm.values import CarControllerParams
 
 CONTROL_N_T_IDX = ModelConstants.T_IDXS[:CONTROL_N]
 
@@ -85,17 +87,49 @@ def long_control_state_trans_old_long(CP, active, long_control_state, v_ego, v_t
 
   return long_control_state
 
-
 class LongControl:
   def __init__(self, CP):
     self.CP = CP
     self.long_control_state = LongCtrlState.off
+    self.experimental_mode = False
+    pos_p_limit = 0.0
     self.pid = PIDController((CP.longitudinalTuning.kpBP, CP.longitudinalTuning.kpV),
                              (CP.longitudinalTuning.kiBP, CP.longitudinalTuning.kiV),
-                             k_f=CP.longitudinalTuning.kf, rate=1 / DT_CTRL)
+                             k_f=CP.longitudinalTuning.kf, rate=1 / DT_CTRL,
+                             pos_p_limit=pos_p_limit)
     self.v_pid = 0.0
+    self._mode_setup()
     self.last_output_accel = 0.0
 
+
+
+  def update_mpc_mode(self, experimental_mode):
+    new_mode = 'blended' if experimental_mode else 'acc'
+   
+    if self.transitioning and self.prev_mode == 'blended' and self.current_mode == 'acc':
+      self.mode_transition_timer = 0.0
+
+    if new_mode != self.current_mode:
+      self.prev_mode = self.current_mode
+      self.transitioning = True
+      self.mode_transition_timer = 0.0
+      self.mode_transition_filter.x = self.last_output_accel
+
+      self.current_mode = new_mode
+
+    if self.transitioning:
+      self.mode_transition_timer += DT_CTRL
+      if self.mode_transition_timer >= self.mode_transition_duration:
+        self.transitioning = False
+
+  def _mode_setup(self):
+    self.prev_mode = 'acc'
+    self.current_mode = 'acc'
+    self.mode_transition_filter = FirstOrderFilter(0.0, 0.5, DT_CTRL)
+    self.mode_transition_timer = 0.0
+    self.mode_transition_duration = 1.0
+    self.transitioning = False
+
   def reset(self):
     self.pid.reset()
 
@@ -124,8 +158,19 @@ class LongControl:
 
     else:  # LongCtrlState.pid
       error = a_target - CS.aEgo
-      output_accel = self.pid.update(error, speed=CS.vEgo,
-                                     feedforward=a_target)
+      self.update_mpc_mode(self.experimental_mode)
+      raw_output_accel = self.pid.update(error, speed=CS.vEgo, feedforward=a_target)
+
+     
+      if self.transitioning and self.prev_mode == 'acc' and self.current_mode == 'blended':
+        if raw_output_accel < 0 and raw_output_accel < self.last_output_accel:
+          progress = min(1.0, self.mode_transition_timer / self.mode_transition_duration)
+          blend_factor = 1.0 - (1.0 - progress) * (1.0 - abs(raw_output_accel / CarControllerParams.ACCEL_MIN))
+          output_accel = self.last_output_accel + (raw_output_accel - self.last_output_accel) * blend_factor
+        else:
+          output_accel = raw_output_accel 
+      else:
+        output_accel = raw_output_accel 
 
     self.last_output_accel = clip(output_accel, accel_limits[0], accel_limits[1])
     return self.last_output_accel

selfdrive/controls/lib/pid.py

@@ -5,7 +5,9 @@ from openpilot.common.numpy_fast import clip, interp
 
 
 class PIDController:
-  def __init__(self, k_p, k_i, k_f=0., k_d=0., pos_limit=1e308, neg_limit=-1e308, rate=100):
+  def __init__(self, k_p, k_i, k_f=0., k_d=0.,
+              pos_limit=1e308, neg_limit=-1e308, rate=100,
+              pos_p_limit=None, neg_p_limit=None):
     self._k_p = k_p
     self._k_i = k_i
     self._k_d = k_d
@@ -20,6 +22,9 @@ class PIDController:
     self.pos_limit = pos_limit
     self.neg_limit = neg_limit
 
+    self.pos_p_limit = pos_p_limit
+    self.neg_p_limit = neg_p_limit
+
     self.i_unwind_rate = 0.3 / rate
     self.i_rate = 1.0 / rate
     self.speed = 0.0
@@ -53,6 +58,10 @@ class PIDController:
     self.speed = speed
 
     self.p = float(error) * self.k_p
+    if self.pos_p_limit is not None and self.p > self.pos_p_limit:
+      self.p = self.pos_p_limit
+    elif self.neg_p_limit is not None and self.p < self.neg_p_limit:
+      self.p = self.neg_p_limit
     self.f = feedforward * self.k_f
     self.d = error_rate * self.k_d
 

selfdrive/controls/radard.py

@@ -18,7 +18,7 @@ from openpilot.common.simple_kalman import KF1D
 from openpilot.frogpilot.common.frogpilot_variables import get_frogpilot_toggles
 
 # Default lead acceleration decay set to 50% at 1s
-_LEAD_ACCEL_TAU = 1.5
+_LEAD_ACCEL_TAU = 0.6
 
 # radar tracks
 SPEED, ACCEL = 0, 1     # Kalman filter states enum
@@ -79,7 +79,7 @@ class Track:
 
     # Learn if constant acceleration
     if abs(self.aLeadK) < 0.5:
-      self.aLeadTau.x = _LEAD_ACCEL_TAU
+      self.aLeadTau.x = min(max(self.aLeadTau, 1e-2) * 1.1, _LEAD_ACCEL_TAU)
     else:
       self.aLeadTau.update(0.0)
 
@@ -163,14 +163,16 @@ def match_vision_to_track(v_ego: float, lead: capnp._DynamicStructReader, tracks
 
 
 def get_RadarState_from_vision(lead_msg: capnp._DynamicStructReader, v_ego: float, model_v_ego: float):
-  lead_v_rel_pred = lead_msg.v[0] - model_v_ego
+  prev_aLeadK = getattr(get_RadarState_from_vision, "prev_aLeadK", 0.0)
+  blended_aLeadK = 0.8 * float(lead_msg.a[0]) + 0.2 * prev_aLeadK
+  get_RadarState_from_vision.prev_aLeadK = blended_aLeadK
   return {
     "dRel": float(lead_msg.x[0] - RADAR_TO_CAMERA),
     "yRel": float(-lead_msg.y[0]),
-    "vRel": float(lead_v_rel_pred),
-    "vLead": float(v_ego + lead_v_rel_pred),
-    "vLeadK": float(v_ego + lead_v_rel_pred),
-    "aLeadK": float(lead_msg.a[0]),
+    "vRel": float(lead_msg.v[0] - model_v_ego),
+    "vLead": float(v_ego + (lead_msg.v[0] - model_v_ego)),
+    "vLeadK": float(v_ego + (lead_msg.v[0] - model_v_ego)),
+    "aLeadK": blended_aLeadK,
     "aLeadTau": 0.3,
     "fcw": False,
     "modelProb": float(lead_msg.prob),