Update carcontroller.py

This reverts commit b4d070fbd58fe245be2ae7e1814589e0cfec1ad3.

cereal/log.capnp

@@ -1048,6 +1048,8 @@ struct ModelDataV2 {
 
   struct Action {
     desiredCurvature @0 :Float32;
+    desiredAcceleration @1 :Float32;
+    shouldStop @2 :Bool;
   }
 }
 

frogpilot/assets/model_manager.py

@@ -13,7 +13,7 @@ from openpilot.frogpilot.assets.download_functions import GITLAB_URL, download_f
 from openpilot.frogpilot.common.frogpilot_utilities import delete_file
 from openpilot.frogpilot.common.frogpilot_variables import DEFAULT_CLASSIC_MODEL, DEFAULT_MODEL, DEFAULT_TINYGRAD_MODEL, MODELS_PATH, params, params_default, params_memory
 
-VERSION = "v14"
+VERSION = "v15"
 
 CANCEL_DOWNLOAD_PARAM = "CancelModelDownload"
 DOWNLOAD_PROGRESS_PARAM = "ModelDownloadProgress"

frogpilot/common/frogpilot_utilities.py

@@ -19,7 +19,6 @@ from cereal import log
 from openpilot.common.realtime import DT_DMON, DT_HW
 from openpilot.selfdrive.car.toyota.carcontroller import LOCK_CMD
 from openpilot.system.hardware import HARDWARE
-from openpilot.system.manager.process_config import managed_processes
 from panda import Panda
 
 from openpilot.frogpilot.common.frogpilot_variables import EARTH_RADIUS, KONIK_PATH, MAPD_PATH, MAPS_PATH, params, params_memory
@@ -39,6 +38,10 @@ locks = {
   "update_openpilot": threading.Lock(),
 }
 
+def get_managed_processes():
+    from openpilot.system.manager.process_config import managed_processes
+    return managed_processes
+
 def run_thread_with_lock(name, target, args=(), report=True):
   if not running_threads.get(name, threading.Thread()).is_alive():
     with locks[name]:
@@ -170,7 +173,7 @@ def restart_processes(sm):
 
   if not any(ps.ignitionLine or ps.ignitionCan for ps in sm["pandaStates"] if ps.pandaType != log.PandaState.PandaType.unknown):
     for name in ["mapd", "ui"]:
-      managed_processes[name].stop(block=False, retry=False)
+      get_managed_processes()[name].stop(block=False, retry=False)
 
 def run_cmd(cmd, success_message, fail_message, report=True):
   try:

frogpilot/common/frogpilot_variables.py

@@ -58,8 +58,8 @@ DEFAULT_MODEL = "national-public-radio"
 DEFAULT_MODEL_NAME = "National Public Radio 👀📡"
 DEFAULT_MODEL_VERSION = "v6"
 
-DEFAULT_TINYGRAD_MODEL = "filet-o-fish"
-DEFAULT_TINYGRAD_MODEL_NAME = "Filet-O-Fish 👀📡"
+DEFAULT_TINYGRAD_MODEL = "tomb-raider"
+DEFAULT_TINYGRAD_MODEL_NAME = "Vikander 👀📡"
 DEFAULT_TINYGRAD_MODEL_VERSION = "v8"
 
 EXCLUDED_KEYS = {
@@ -373,12 +373,16 @@ 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)
     self.tuning_levels = {key: lvl for key, _, lvl in frogpilot_default_params + misc_tuning_levels}
 
-    self.short_branch = get_build_metadata().channel
+    self.short_branch = "FrogPilot-Staging"
     self.development_branch = self.short_branch == "FrogPilot-Development"
     self.release_branch = self.short_branch == "FrogPilot"
     self.staging_branch = self.short_branch == "FrogPilot-Staging"
@@ -388,7 +392,7 @@ class FrogPilotVariables:
     self.frogpilot_toggles.frogs_go_moo = Path("/persist/frogsgomoo.py").is_file()
     self.frogpilot_toggles.block_user = self.development_branch and not self.frogpilot_toggles.frogs_go_moo
 
-    self.not_vetted = Path("/data/openpilot/not_vetted").is_file()
+    self.not_vetted = False
 
     self.frogpilot_toggles.use_konik_server = params.get_bool("UseKonikServer")
     self.frogpilot_toggles.use_konik_server |= self.not_vetted

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">

frogpilot/tinygrad_modeld/fill_model_msg.py

@@ -56,7 +56,7 @@ def fill_lane_line_meta(builder, lane_lines, lane_line_probs):
   builder.rightProb = lane_line_probs[2]
 
 def fill_model_msg(base_msg: capnp._DynamicStructBuilder, extended_msg: capnp._DynamicStructBuilder,
-                   net_output_data: dict[str, np.ndarray], v_ego: float, delay: float,
+                   net_output_data: dict[str, np.ndarray], action: log.ModelDataV2.Action,
                    publish_state: PublishState, vipc_frame_id: int, vipc_frame_id_extra: int,
                    frame_id: int, frame_drop: float, timestamp_eof: int, model_execution_time: float,
                    valid: bool) -> None:
@@ -71,7 +71,8 @@ def fill_model_msg(base_msg: capnp._DynamicStructBuilder, extended_msg: capnp._D
   driving_model_data.frameIdExtra = vipc_frame_id_extra
   driving_model_data.frameDropPerc = frame_drop_perc
   driving_model_data.modelExecutionTime = model_execution_time
-  driving_model_data.action.desiredCurvature = float(net_output_data['desired_curvature'][0,0])
+
+  driving_model_data.action = action
 
   modelV2 = extended_msg.modelV2
   modelV2.frameId = vipc_frame_id
@@ -89,17 +90,17 @@ def fill_model_msg(base_msg: capnp._DynamicStructBuilder, extended_msg: capnp._D
   fill_xyzt(modelV2.orientationRate, ModelConstants.T_IDXS, *net_output_data['plan'][0,:,Plan.ORIENTATION_RATE].T)
 
   # temporal pose
-  temporal_pose = modelV2.temporalPose
-  temporal_pose.trans = net_output_data['sim_pose'][0,:ModelConstants.POSE_WIDTH//2].tolist()
-  temporal_pose.transStd = net_output_data['sim_pose_stds'][0,:ModelConstants.POSE_WIDTH//2].tolist()
-  temporal_pose.rot = net_output_data['sim_pose'][0,ModelConstants.POSE_WIDTH//2:].tolist()
-  temporal_pose.rotStd = net_output_data['sim_pose_stds'][0,ModelConstants.POSE_WIDTH//2:].tolist()
+  #temporal_pose = modelV2.temporalPose
+  #temporal_pose.trans = net_output_data['sim_pose'][0,:ModelConstants.POSE_WIDTH//2].tolist()
+  #temporal_pose.transStd = net_output_data['sim_pose_stds'][0,:ModelConstants.POSE_WIDTH//2].tolist()
+  #temporal_pose.rot = net_output_data['sim_pose'][0,ModelConstants.POSE_WIDTH//2:].tolist()
+  #temporal_pose.rotStd = net_output_data['sim_pose_stds'][0,ModelConstants.POSE_WIDTH//2:].tolist()
 
   # poly path
   fill_xyz_poly(driving_model_data.path, ModelConstants.POLY_PATH_DEGREE, *net_output_data['plan'][0,:,Plan.POSITION].T)
 
-  # lateral planning
-  modelV2.action.desiredCurvature = float(net_output_data['desired_curvature'][0,0])
+  # action
+  modelV2.action = action
 
   # times at X_IDXS of edges and lines aren't used
   LINE_T_IDXS: list[float] = []

frogpilot/tinygrad_modeld/parse_model_outputs.py

@@ -88,6 +88,12 @@ class Parser:
     self.parse_mdn('pose', outs, in_N=0, out_N=0, out_shape=(ModelConstants.POSE_WIDTH,))
     self.parse_mdn('wide_from_device_euler', outs, in_N=0, out_N=0, out_shape=(ModelConstants.WIDE_FROM_DEVICE_WIDTH,))
     self.parse_mdn('road_transform', outs, in_N=0, out_N=0, out_shape=(ModelConstants.POSE_WIDTH,))
+    self.parse_mdn('lane_lines', outs, in_N=0, out_N=0, out_shape=(ModelConstants.NUM_LANE_LINES,ModelConstants.IDX_N,ModelConstants.LANE_LINES_WIDTH))
+    self.parse_mdn('road_edges', outs, in_N=0, out_N=0, out_shape=(ModelConstants.NUM_ROAD_EDGES,ModelConstants.IDX_N,ModelConstants.LANE_LINES_WIDTH))
+    self.parse_mdn('lead', outs, in_N=ModelConstants.LEAD_MHP_N, out_N=ModelConstants.LEAD_MHP_SELECTION,
+                   out_shape=(ModelConstants.LEAD_TRAJ_LEN,ModelConstants.LEAD_WIDTH))
+    for k in ['lead_prob', 'lane_lines_prob']:
+      self.parse_binary_crossentropy(k, outs)
     self.parse_categorical_crossentropy('desire_pred', outs, out_shape=(ModelConstants.DESIRE_PRED_LEN,ModelConstants.DESIRE_PRED_WIDTH))
     self.parse_binary_crossentropy('meta', outs)
     return outs
@@ -95,17 +101,10 @@ class Parser:
   def parse_policy_outputs(self, outs: dict[str, np.ndarray]) -> dict[str, np.ndarray]:
     self.parse_mdn('plan', outs, in_N=ModelConstants.PLAN_MHP_N, out_N=ModelConstants.PLAN_MHP_SELECTION,
                    out_shape=(ModelConstants.IDX_N,ModelConstants.PLAN_WIDTH))
-    self.parse_mdn('lane_lines', outs, in_N=0, out_N=0, out_shape=(ModelConstants.NUM_LANE_LINES,ModelConstants.IDX_N,ModelConstants.LANE_LINES_WIDTH))
-    self.parse_mdn('road_edges', outs, in_N=0, out_N=0, out_shape=(ModelConstants.NUM_ROAD_EDGES,ModelConstants.IDX_N,ModelConstants.LANE_LINES_WIDTH))
-    self.parse_mdn('sim_pose', outs, in_N=0, out_N=0, out_shape=(ModelConstants.POSE_WIDTH,))
-    self.parse_mdn('lead', outs, in_N=ModelConstants.LEAD_MHP_N, out_N=ModelConstants.LEAD_MHP_SELECTION,
-                   out_shape=(ModelConstants.LEAD_TRAJ_LEN,ModelConstants.LEAD_WIDTH))
     if 'lat_planner_solution' in outs:
       self.parse_mdn('lat_planner_solution', outs, in_N=0, out_N=0, out_shape=(ModelConstants.IDX_N,ModelConstants.LAT_PLANNER_SOLUTION_WIDTH))
     if 'desired_curvature' in outs:
       self.parse_mdn('desired_curvature', outs, in_N=0, out_N=0, out_shape=(ModelConstants.DESIRED_CURV_WIDTH,))
-    for k in ['lead_prob', 'lane_lines_prob']:
-      self.parse_binary_crossentropy(k, outs)
     self.parse_categorical_crossentropy('desire_state', outs, out_shape=(ModelConstants.DESIRE_PRED_WIDTH,))
     return outs
 

frogpilot/tinygrad_modeld/tinygrad_modeld.py

@@ -20,19 +20,21 @@ from msgq.visionipc import VisionIpcClient, VisionStreamType, VisionBuf
 from openpilot.common.swaglog import cloudlog
 from openpilot.common.params import Params
 from openpilot.common.filter_simple import FirstOrderFilter
-from openpilot.common.realtime import config_realtime_process
+from openpilot.common.realtime import config_realtime_process, DT_MDL
 from openpilot.common.transformations.camera import DEVICE_CAMERAS
 from openpilot.common.transformations.model import get_warp_matrix
 from openpilot.system import sentry
 from openpilot.selfdrive.car.car_helpers import get_demo_car_params
 from openpilot.selfdrive.controls.lib.desire_helper import DesireHelper
+from openpilot.selfdrive.controls.lib.drive_helpers import get_accel_from_plan_tomb_raider, smooth_value, get_curvature_from_plan
 from openpilot.frogpilot.tinygrad_modeld.parse_model_outputs import Parser
 from openpilot.frogpilot.tinygrad_modeld.fill_model_msg import fill_model_msg, fill_pose_msg, PublishState
-from openpilot.frogpilot.tinygrad_modeld.constants import ModelConstants
+from openpilot.frogpilot.tinygrad_modeld.constants import ModelConstants, Plan
 from openpilot.frogpilot.tinygrad_modeld.models.commonmodel_pyx import DrivingModelFrame, CLContext
 
 from openpilot.frogpilot.common.frogpilot_variables import get_frogpilot_toggles
 
+
 PROCESS_NAME = "frogpilot.tinygrad_modeld.tinygrad_modeld"
 SEND_RAW_PRED = os.getenv('SEND_RAW_PRED')
 
@@ -41,6 +43,30 @@ POLICY_PKL_PATH = Path(__file__).parent / 'models/driving_policy_tinygrad.pkl'
 VISION_METADATA_PATH = Path(__file__).parent / 'models/driving_vision_metadata.pkl'
 POLICY_METADATA_PATH = Path(__file__).parent / 'models/driving_policy_metadata.pkl'
 
+LAT_SMOOTH_SECONDS = 0.2
+LONG_SMOOTH_SECONDS = 0.2
+MIN_LAT_CONTROL_SPEED = 0.3
+
+
+def get_action_from_model(model_output: dict[str, np.ndarray], prev_action: log.ModelDataV2.Action,
+                          lat_action_t: float, long_action_t: float, v_ego: float) -> log.ModelDataV2.Action:
+    plan = model_output['plan'][0]
+    desired_accel, should_stop = get_accel_from_plan_tomb_raider(plan[:,Plan.VELOCITY][:,0],
+                                                                 plan[:,Plan.ACCELERATION][:,0],
+                                                                 ModelConstants.T_IDXS,
+                                                                 action_t=long_action_t)
+    desired_accel = smooth_value(desired_accel, prev_action.desiredAcceleration, LONG_SMOOTH_SECONDS)
+
+    desired_curvature = model_output['desired_curvature'][0, 0]
+    if v_ego > MIN_LAT_CONTROL_SPEED:
+      desired_curvature = smooth_value(desired_curvature, prev_action.desiredCurvature, LAT_SMOOTH_SECONDS)
+    else:
+      desired_curvature = prev_action.desiredCurvature
+
+    return log.ModelDataV2.Action(desiredCurvature=float(desired_curvature),
+                                  desiredAcceleration=float(desired_accel),
+                                  shouldStop=bool(should_stop))
+
 class FrameMeta:
   frame_id: int = 0
   timestamp_sof: int = 0
@@ -148,7 +174,7 @@ class ModelState:
     # TODO model only uses last value now
     self.full_prev_desired_curv[0,:-1] = self.full_prev_desired_curv[0,1:]
     self.full_prev_desired_curv[0,-1,:] = policy_outputs_dict['desired_curvature'][0, :]
-    self.numpy_inputs['prev_desired_curv'][:] = self.full_prev_desired_curv[0, self.temporal_idxs]
+    self.numpy_inputs['prev_desired_curv'][:] = 0*self.full_prev_desired_curv[0, self.temporal_idxs]
 
     combined_outputs_dict = {**vision_outputs_dict, **policy_outputs_dict}
     if SEND_RAW_PRED:
@@ -223,7 +249,10 @@ def main(demo=False):
   cloudlog.info("tinygrad_modeld got CarParams: %s", CP.carName)
 
   # TODO this needs more thought, use .2s extra for now to estimate other delays
-  steer_delay = CP.steerActuatorDelay + .2
+  # TODO Move smooth seconds to action function
+  lat_delay = CP.steerActuatorDelay + .2 + LAT_SMOOTH_SECONDS
+  long_delay = CP.longitudinalActuatorDelay + LONG_SMOOTH_SECONDS
+  prev_action = log.ModelDataV2.Action()
 
   DH = DesireHelper()
 
@@ -268,7 +297,7 @@ def main(demo=False):
     is_rhd = sm["driverMonitoringState"].isRHD
     frame_id = sm["roadCameraState"].frameId
     v_ego = max(sm["carState"].vEgo, 0.)
-    lateral_control_params = np.array([v_ego, steer_delay], dtype=np.float32)
+    lateral_control_params = np.array([v_ego, lat_delay], dtype=np.float32)
     if sm.updated["liveCalibration"] and sm.seen['roadCameraState'] and sm.seen['deviceState']:
       device_from_calib_euler = np.array(sm["liveCalibration"].rpyCalib, dtype=np.float32)
       dc = DEVICE_CAMERAS[(str(sm['deviceState'].deviceType), str(sm['roadCameraState'].sensor))]
@@ -311,7 +340,10 @@ def main(demo=False):
       modelv2_send = messaging.new_message('modelV2')
       drivingdata_send = messaging.new_message('drivingModelData')
       posenet_send = messaging.new_message('cameraOdometry')
-      fill_model_msg(drivingdata_send, modelv2_send, model_output, v_ego, steer_delay,
+
+      action = get_action_from_model(model_output, prev_action, lat_delay + DT_MDL, long_delay + DT_MDL, v_ego)
+      prev_action = action
+      fill_model_msg(drivingdata_send, modelv2_send, model_output, action,
                      publish_state, meta_main.frame_id, meta_extra.frame_id, frame_id,
                      frame_drop_ratio, meta_main.timestamp_eof, model_execution_time, live_calib_seen)
 

launch_env.sh

@@ -7,7 +7,7 @@ export OPENBLAS_NUM_THREADS=1
 export VECLIB_MAXIMUM_THREADS=1
 
 if [ -z "$AGNOS_VERSION" ]; then
-  export AGNOS_VERSION="10.1"
+  export AGNOS_VERSION="10.1.1"
 fi
 
 export STAGING_ROOT="/data/safe_staging"

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/car_helpers.py

@@ -209,7 +209,8 @@ def get_car(logcan, sendcan, experimental_long_allowed, params, num_pandas=1, fr
   CP.fingerprintSource = source
   CP.fuzzyFingerprint = not exact_match
 
-  return get_car_interface(CP, FPCP), CP, FPCP
+  interface_instance = get_car_interface(CP, None)
+  return interface_instance, CP, FPCP
 
 def write_car_param(platform=MOCK.MOCK):
   params = Params()

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
@@ -22,7 +23,6 @@ TransmissionType = car.CarParams.TransmissionType
 CAMERA_CANCEL_DELAY_FRAMES = 10
 # Enforce a minimum interval between steering messages to avoid a fault
 MIN_STEER_MSG_INTERVAL_MS = 15
-
 # Constants for pitch compensation
 PITCH_DEADZONE = 0.01  # [radians] 0.01 ≈ 1% grade
 BRAKE_PITCH_FACTOR_BP = [5., 10.]  # [m/s] smoothly revert to planned accel at low speeds
@@ -38,6 +38,10 @@ class CarController(CarControllerBase):
     self.apply_speed = 0
     self.frame = 0
     self.last_steer_frame = 0
+    self.last_steer_ts_ns = 0
+    self.last_regen_active = False
+    self.prev_steer_ts_ns = 0
+    self.last_spoof_ts_ns = 0
     self.last_button_frame = 0
     self.cancel_counter = 0
     self.pedal_steady = 0.
@@ -55,25 +59,64 @@ 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
+    self.regen_paddle_timer = 0
 
-  @staticmethod
-  def calc_pedal_command(accel: float, long_active: bool) -> float:
-    if not long_active: return 0.
+    # Midpoint + overflow spoof accumulator and flags
+    self.spoof_accum = 0.0
+    self.spoof_mid_sent = False
+    self.spoof_over_sent = False
+    self.last_interval_ns = 0
 
-    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.)
-    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
+  def calc_pedal_command(self, accel: float, long_active: bool, car_velocity) -> Tuple[float, bool]:
+    if not long_active:
+      return 0., False
 
-    return pedal_gas
+    # Regen paddle hysteresis (frame-based): hold 30 frames, with decrement dead-zone
+    if not hasattr(self, 'regen_paddle_timer'):
+      self.regen_paddle_timer = 0  # frames
+
+    # Regen paddle hysteresis (frame‑based): count frames when decelerating hard, decrement only when truly released
+    if self.aego < -0.7:
+      self.regen_paddle_timer += 1
+    elif self.aego > -0.3:
+      self.regen_paddle_timer = max(self.regen_paddle_timer - 1, 0)
+    # else: hold timer between -0.7 and -0.3
+
+    # Base paddle press hysteresis
+    self.regen_paddle_pressed = self.regen_paddle_timer >= 20  # 30 frames
+    press_regen_paddle = self.regen_paddle_pressed
+
+    # Regen gain ratios from bin-averaged 60–0 deceleration sweep; Calculates stronger decel from paddle
+    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.01, 1.01, 1.02, 1.05, 1.08, 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])
+
+    # Compute raw pedal gas
+    raw_pedal_gas = clip((pedaloffset + (accel / gain) * 0.6), 0.0, 1.0) if press_regen_paddle else clip((pedaloffset + accel * 0.6), 0.0, 1.0)
+
+    # --- Immediate application of raw pedal gas, no blending ---
+    pedal_gas = raw_pedal_gas
+    # Safety cap: ramp from 22% at 0 m/s to 37.25% at 10 mph (4.47 m/s), then allow full throttleAdd commentMore actions
+    pedal_gas_max = interp(car_velocity, [0.0, 4.47, 4.48], [0.22, 0.3725, 1.0])
+    pedal_gas = clip(pedal_gas, 0.0, pedal_gas_max)
+    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
+    press_regen_paddle = False
     hud_control = CC.hudControl
     hud_alert = hud_control.visualAlert
     hud_v_cruise = hud_control.setSpeed
@@ -82,6 +125,98 @@ class CarController(CarControllerBase):
 
     # Send CAN commands.
     can_sends = []
+    paddle_sends = []
+
+    raw_regen_active = (
+      self.CP.carFingerprint in CC_REGEN_PADDLE_CAR and
+      self.CP.openpilotLongitudinalControl and
+      CC.longActive and
+      self.CP.enableGasInterceptor and
+      self.regen_paddle_timer >= 20  # raw hysteresis-only
+    )
+    regen_active = raw_regen_active
+
+    # === Spoof scheduling: midpoint + overflow (~40Hz) ===
+    # Implement 1s hold-off after CAN reconnect
+    # Detect reconnect: loopback_lka_steering_cmd_ts_nanos == 0 and self.last_steer_ts_ns != 0
+    if CS.loopback_lka_steering_cmd_ts_nanos == 0 and self.last_steer_ts_ns != 0:
+      self.loopback_holdoff_until = now_nanos + 1_000_000_000
+    # Remove holdoff if expired
+    if hasattr(self, "loopback_holdoff_until") and now_nanos >= self.loopback_holdoff_until:
+      del self.loopback_holdoff_until
+
+    # Rising-edge reset on regen start
+    if raw_regen_active and not self.last_regen_active:
+      self.prev_steer_ts_ns = self.last_steer_ts_ns
+      self.last_spoof_ts_ns = 0
+      self.spoof_accum = 0.0
+      self.spoof_mid_sent = False
+      self.spoof_over_sent = False
+
+    if raw_regen_active:
+      # Interval between last two bus-0 steer sends
+      interval_ns = self.last_steer_ts_ns - self.prev_steer_ts_ns
+
+      # New steer interval? clear per-interval flags
+      if interval_ns != self.last_interval_ns:
+        self.spoof_mid_sent = False
+        self.spoof_over_sent = False
+        self.last_interval_ns = interval_ns
+
+      # Accumulate extra spoofs needed above 33Hz base to reach 40Hz
+      self.spoof_accum += (40.0/33.0 - 1.0)
+
+      # 500ms holdoff after reconnect: skip spoof scheduling if active
+      skip_spoof = hasattr(self, "loopback_holdoff_until") and now_nanos < self.loopback_holdoff_until
+
+      # Midpoint spoof: one per interval; avoid sending if loopback stale
+      if not skip_spoof and not self.spoof_mid_sent and interval_ns > 0:
+        midpoint_ns = self.prev_steer_ts_ns + interval_ns // 2
+        if (CS.loopback_lka_steering_cmd_ts_nanos != 0 and
+            now_nanos - CS.loopback_lka_steering_cmd_ts_nanos < 10_000_000 and
+            now_nanos >= midpoint_ns and
+            now_nanos - self.last_steer_ts_ns >= 26_000_000):
+          paddle_sends.append(gmcan.create_prndl2_command(self.packer_pt, CanBus.POWERTRAIN, True))
+          paddle_sends.append(gmcan.create_regen_paddle_command(self.packer_pt, CanBus.POWERTRAIN, True))
+          self.last_spoof_ts_ns = now_nanos
+          self.spoof_mid_sent = True
+
+      # Overflow spoof: insert extra when accumulator allows
+      if not skip_spoof and self.spoof_accum >= 0.5 and not self.spoof_over_sent and interval_ns > 0:
+        slot2_ns = self.prev_steer_ts_ns + (interval_ns * 2) // 3
+        if (CS.loopback_lka_steering_cmd_ts_nanos != 0 and
+            now_nanos - CS.loopback_lka_steering_cmd_ts_nanos < 10_000_000 and
+            now_nanos >= slot2_ns and
+            now_nanos - self.last_steer_ts_ns >= 28_000_000):
+          paddle_sends.append(gmcan.create_prndl2_command(self.packer_pt, CanBus.POWERTRAIN, True))
+          paddle_sends.append(gmcan.create_regen_paddle_command(self.packer_pt, CanBus.POWERTRAIN, True))
+          self.last_spoof_ts_ns = now_nanos
+          self.spoof_over_sent = True
+          self.spoof_accum -= 0.5
+    # === End Spoof scheduling ===
+
+    # === Off-pulse scheduling on regen release ===
+    if not raw_regen_active and self.last_regen_active:
+      # schedule two off-slots at 1/3 and 2/3 of the last steer interval
+      if self.prev_steer_ts_ns and self.last_steer_ts_ns:
+        intv = self.last_steer_ts_ns - self.prev_steer_ts_ns
+        self.off_schedule_ns = [
+          self.prev_steer_ts_ns + intv // 3,
+          self.prev_steer_ts_ns + (2 * intv) // 3
+        ]
+        self.off_sent = [False, False]
+
+    if hasattr(self, "off_schedule_ns"):
+      for i, t_ns in enumerate(self.off_schedule_ns):
+        if not self.off_sent[i] and now_nanos >= t_ns and now_nanos - self.last_steer_ts_ns >= 18_000_000:
+          paddle_sends.append(gmcan.create_prndl2_command(self.packer_pt, CanBus.POWERTRAIN, False))
+          paddle_sends.append(gmcan.create_regen_paddle_command(self.packer_pt, CanBus.POWERTRAIN, False))
+          self.off_sent[i] = True
+      # clean up once both off pulses are sent
+      if hasattr(self, "off_sent") and all(self.off_sent):
+        del self.off_schedule_ns
+        del self.off_sent
+    # === End off-pulse scheduling ===
 
     # Steering (Active: 50Hz, inactive: 10Hz)
     steer_step = self.params.STEER_STEP if CC.latActive else self.params.INACTIVE_STEER_STEP
@@ -111,11 +246,24 @@ class CarController(CarControllerBase):
       else:
         apply_steer = 0
 
+      # shift previous steer timestamp
+      self.prev_steer_ts_ns = self.last_steer_ts_ns
+      self.last_steer_ts_ns = now_nanos
       self.last_steer_frame = self.frame
       self.apply_steer_last = apply_steer
       idx = self.lka_steering_cmd_counter % 4
       can_sends.append(gmcan.create_steering_control(self.packer_pt, CanBus.POWERTRAIN, apply_steer, idx, CC.latActive))
 
+    # Update regen_active state and last_regen_paddle_pressed for next loop
+    self.last_regen_active = regen_active
+    self.last_regen_paddle_pressed = self.regen_paddle_pressed
+
+    # Merge paddle spoof CAN frames, time-guarded only
+    if paddle_sends:
+      # wait at least 15 ms after the last bus0 steer send
+      if now_nanos - self.last_steer_ts_ns >= 15_000_000:
+        can_sends.extend(paddle_sends)
+
     if self.CP.openpilotLongitudinalControl:
       # Gas/regen, brakes, and UI commands - all at 25Hz
       if self.frame % 4 == 0:
@@ -142,12 +290,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 +307,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,33 +344,30 @@ 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
-          can_sends.append(gmcan.create_acc_dashboard_command(self.packer_pt, CanBus.POWERTRAIN, CC.enabled,
+          if self.frame % 10 == 5:
+            can_sends.append(gmcan.create_acc_dashboard_command(self.packer_pt, CanBus.POWERTRAIN, CC.enabled,
                                                               hud_v_cruise * CV.MS_TO_KPH, hud_control, send_fcw))
       else:
         # to keep accel steady for logs when not sending gas
         accel += self.accel_g
 
       # Radar needs to know current speed and yaw rate (50hz),
-      # and that ADAS is alive (10hz)
+      # and that ADAS is alive (5hz, previously 10hz)
       if not self.CP.radarUnavailable:
         tt = self.frame * DT_CTRL
-        time_and_headlights_step = 10
+        time_and_headlights_step = 20
         if self.frame % time_and_headlights_step == 0:
           idx = (self.frame // time_and_headlights_step) % 4
           can_sends.append(gmcan.create_adas_time_status(CanBus.OBSTACLE, int((tt - self.start_time) * 60), idx))
           can_sends.append(gmcan.create_adas_headlights_status(self.packer_obj, CanBus.OBSTACLE))
-
-        speed_and_accelerometer_step = 2
-        if self.frame % speed_and_accelerometer_step == 0:
-          idx = (self.frame // speed_and_accelerometer_step) % 4
           can_sends.append(gmcan.create_adas_steering_status(CanBus.OBSTACLE, idx))
           can_sends.append(gmcan.create_adas_accelerometer_speed_status(CanBus.OBSTACLE, CS.out.vEgo, idx))
 
-      if self.CP.networkLocation == NetworkLocation.gateway and self.frame % self.params.ADAS_KEEPALIVE_STEP == 0:
+      if self.CP.networkLocation == NetworkLocation.gateway and self.frame % (self.params.ADAS_KEEPALIVE_STEP * 2) == 0:
         can_sends += gmcan.create_adas_keepalive(CanBus.POWERTRAIN)
 
       # TODO: integrate this with the code block below?
@@ -250,7 +395,7 @@ class CarController(CarControllerBase):
 
     if self.CP.networkLocation == NetworkLocation.fwdCamera:
       # Silence "Take Steering" alert sent by camera, forward PSCMStatus with HandsOffSWlDetectionStatus=1
-      if self.frame % 10 == 0:
+      if self.frame % 20 == 0:
         can_sends.append(gmcan.create_pscm_status(self.packer_pt, CanBus.CAMERA, CS.pscm_status))
 
     new_actuators = actuators.as_builder()

selfdrive/car/gm/carstate.py

@@ -56,6 +56,10 @@ class CarState(CarStateBase):
     self.loopback_lka_steering_cmd_updated = len(loopback_cp.vl_all["ASCMLKASteeringCmd"]["RollingCounter"]) > 0
     if self.loopback_lka_steering_cmd_updated:
       self.loopback_lka_steering_cmd_ts_nanos = loopback_cp.ts_nanos["ASCMLKASteeringCmd"]["RollingCounter"]
+
+    # Track timestamps for OEM PRNDL2 and Regen Paddle messages (used to sync spoofing timing)
+    self.prndl2_ts_nanos = pt_cp.ts_nanos["ECMPRDNL2"]["PRNDL2"]
+    self.regen_paddle_ts_nanos = pt_cp.ts_nanos["EBCMRegenPaddle"]["RegenPaddle"]
     if self.CP.networkLocation == NetworkLocation.fwdCamera and not self.CP.flags & GMFlags.NO_CAMERA.value:
       self.pt_lka_steering_cmd_counter = pt_cp.vl["ASCMLKASteeringCmd"]["RollingCounter"]
       self.cam_lka_steering_cmd_counter = cam_cp.vl["ASCMLKASteeringCmd"]["RollingCounter"]
@@ -92,11 +96,16 @@ class CarState(CarStateBase):
     # Regen braking is braking
     if self.CP.transmissionType == TransmissionType.direct:
       ret.regenBraking = pt_cp.vl["EBCMRegenPaddle"]["RegenPaddle"] != 0
-      self.single_pedal_mode = ret.gearShifter == GearShifter.low or pt_cp.vl["EVDriveMode"]["SinglePedalModeActive"] == 1 or (ret.regenBraking and GearShifter.manumatic)
+      self.single_pedal_mode = (
+        ret.gearShifter == GearShifter.low
+        or pt_cp.vl["EVDriveMode"]["SinglePedalModeActive"] == 1
+        or pt_cp.vl["ECMPRDNL2"]["TransmissionState"] == 21
+        or (ret.regenBraking and GearShifter.manumatic)
+      )
 
     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 +178,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"]
@@ -230,7 +239,7 @@ class CarState(CarStateBase):
       ]
     else:
       messages += [
-        ("ECMPRDNL2", 10),
+        ("ECMPRDNL2", 40),
         ("AcceleratorPedal2", 33),
         ("ECMEngineStatus", 100),
         ("BCMTurnSignals", 1),
@@ -252,7 +261,7 @@ class CarState(CarStateBase):
 
     if CP.transmissionType == TransmissionType.direct:
       messages += [
-        ("EBCMRegenPaddle", 50),
+        ("EBCMRegenPaddle", 40),
         ("EVDriveMode", 0),
       ]
 

selfdrive/car/gm/gmcan.py

@@ -177,6 +177,33 @@ def create_lka_icon_command(bus, active, critical, steer):
     dat = b"\x00\x00\x00"
   return make_can_msg(0x104c006c, dat, bus)
 
+def create_prndl2_command(packer, bus, press_regen_paddle):
+  prndl2_value = 7 if press_regen_paddle else 6
+  manual_mode = 1 if press_regen_paddle else 0
+  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_regen_paddle_command(packer, bus, press_regen_paddle):
+  regen_paddle_value = 2 if press_regen_paddle else 0
+  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_gm_cc_spam_command(packer, controller, CS, actuators):
   if controller.params_.get_bool("IsMetric"):

selfdrive/car/gm/interface.py

@@ -12,8 +12,8 @@ from openpilot.selfdrive.car.gm.values import CAR, CruiseButtons, CarControllerP
 from openpilot.selfdrive.car.interfaces import CarInterfaceBase, TorqueFromLateralAccelCallbackType, FRICTION_THRESHOLD, LatControlInputs, NanoFFModel
 from openpilot.selfdrive.controls.lib.drive_helpers import get_friction
 
-from openpilot.frogpilot.common.frogpilot_variables import params
-
+from openpilot.common.params import Params
+params = Params()
 ButtonType = car.CarState.ButtonEvent.Type
 FrogPilotButtonType = custom.FrogPilotCarState.ButtonEvent.Type
 EventName = car.CarEvent.EventName
@@ -29,10 +29,10 @@ 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.GMC_ACADIA: [4.78003305, 1.0, 0.3122, 0.05591772],
-  CAR.CHEVROLET_SILVERADO: [3.29974374, 1.0, 0.25571356, 0.0465122]
+  CAR.CHEVROLET_BOLT_EUV: {"left": [2.15, 1.0, 0.17, -0.04], "right": [2.15, 1.0, 0.21, -0.04]},
+  CAR.CHEVROLET_BOLT_CC: {"left": [2.15, 1.0, 0.17, -0.04], "right": [2.15, 1.0, 0.21, -0.04]},
+  CAR.GMC_ACADIA: {"left": [4.78, 1.0, 0.31, 0.05], "right": [4.78, 1.0, 0.31, 0.05]},
+  CAR.CHEVROLET_SILVERADO: {"left": [3.30, 1.0, 0.25, 0.05], "right": [3.30, 1.0, 0.25, 0.05]}
 }
 
 NEURAL_PARAMS_PATH = os.path.join(BASEDIR, 'selfdrive/car/torque_data/neural_ff_weights.json')
@@ -74,8 +74,12 @@ 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)
+    if latcontrol_inputs.lateral_acceleration >= 0:
+      params = non_linear_torque_params["right"]
+    else:
+      params = non_linear_torque_params["left"]
+    a, b, c, d = 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 +91,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):
@@ -104,13 +102,15 @@ class CarInterface(CarInterfaceBase):
     if PEDAL_MSG in fingerprint[0]:
       ret.enableGasInterceptor = True
       ret.safetyConfigs[0].safetyParam |= Panda.FLAG_GM_GAS_INTERCEPTOR
+      # When a pedal interceptor is present, always use normal longitudinal (block stock cruise)
+      experimental_long = False
 
     if candidate in EV_CAR:
       ret.transmissionType = TransmissionType.direct
     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
@@ -146,7 +146,7 @@ class CarInterface(CarInterfaceBase):
       ret.safetyConfigs[0].safetyParam |= Panda.FLAG_GM_HW_SDGM
 
     else:  # ASCM, OBD-II harness
-      ret.openpilotLongitudinalControl = not frogpilot_toggles.disable_openpilot_long
+      ret.openpilotLongitudinalControl = not params.get_bool("DisableOpenpilotLongitudinal")
       ret.networkLocation = NetworkLocation.gateway
       ret.radarUnavailable = RADAR_HEADER_MSG not in fingerprint[CanBus.OBSTACLE] and not docs
       ret.pcmCruise = False  # stock non-adaptive cruise control is kept off
@@ -206,6 +206,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.4
 
       if ret.enableGasInterceptor:
         # ACC Bolts use pedal for full longitudinal control, not just sng
@@ -267,7 +268,7 @@ class CarInterface(CarInterfaceBase):
       ret.safetyConfigs[0].safetyParam |= Panda.FLAG_GM_HW_CAM
       ret.minEnableSpeed = -1
       ret.pcmCruise = False
-      ret.openpilotLongitudinalControl = not frogpilot_toggles.disable_openpilot_long
+      ret.openpilotLongitudinalControl = not params.get_bool("DisableOpenpilotLongitudinal")
       ret.stoppingControl = True
       ret.autoResumeSng = True
 
@@ -275,9 +276,9 @@ class CarInterface(CarInterfaceBase):
         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
@@ -291,19 +292,18 @@ class CarInterface(CarInterfaceBase):
       ret.radarUnavailable = True
       ret.experimentalLongitudinalAvailable = False
       ret.minEnableSpeed = 24 * CV.MPH_TO_MS
-      ret.openpilotLongitudinalControl = not frogpilot_toggles.disable_openpilot_long
+      ret.openpilotLongitudinalControl = not params.get_bool("DisableOpenpilotLongitudinal")
       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:
+        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

@@ -11,13 +11,13 @@ Ecu = car.CarParams.Ecu
 
 
 class CarControllerParams:
-  STEER_MAX = 300  # GM limit is 3Nm. Used by carcontroller to generate LKA output
+  STEER_MAX = 450  # GM limit is 3Nm. Used by carcontroller to generate LKA output
   STEER_STEP = 3  # Active control frames per command (~33hz)
   INACTIVE_STEER_STEP = 10  # Inactive control frames per command (10hz)
-  STEER_DELTA_UP = 10  # Delta rates require review due to observed EPS weakness
-  STEER_DELTA_DOWN = 15
-  STEER_DRIVER_ALLOWANCE = 65
-  STEER_DRIVER_MULTIPLIER = 4
+  STEER_DELTA_UP = 15  # Delta rates require review due to observed EPS weakness
+  STEER_DELTA_DOWN = 34
+  STEER_DRIVER_ALLOWANCE = 78
+  STEER_DRIVER_MULTIPLIER = 6
   STEER_DRIVER_FACTOR = 100
   NEAR_STOP_BRAKE_PHASE = 0.25  # m/s
   SNG_INTERCEPTOR_GAS = 18. / 255.
@@ -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

@@ -218,9 +218,10 @@ class CarInterfaceBase(ABC):
     self.silent_steer_warning = True
     self.v_ego_cluster_seen = False
 
-    self.CS = CarState(CP, FPCP)
-    self.cp = self.CS.get_can_parser(CP, FPCP)
-    self.cp_cam = self.CS.get_cam_can_parser(CP, FPCP)
+    self.CS = CarState(CP, None)
+    fp = FPCP if FPCP is not None else getattr(self, "FPCP", None)
+    self.cp = self.CS.get_can_parser(CP, fp)
+    self.cp_cam = self.CS.get_cam_can_parser(CP, fp)
     self.cp_adas = self.CS.get_adas_can_parser(CP)
     self.cp_body = self.CS.get_body_can_parser(CP)
     self.cp_loopback = self.CS.get_loopback_can_parser(CP)
@@ -411,7 +412,7 @@ class CarInterfaceBase(ABC):
 
     tune.init('torque')
     tune.torque.useSteeringAngle = use_steering_angle
-    tune.torque.kp = 1.0
+    tune.torque.kp = 0.4
     tune.torque.kf = 1.0
     tune.torque.ki = 0.1
     tune.torque.friction = params['FRICTION']

selfdrive/car/torque_data/override.toml

@@ -43,7 +43,7 @@ legend = ["LAT_ACCEL_FACTOR", "MAX_LAT_ACCEL_MEASURED", "FRICTION"]
 "CADILLAC_ESCALADE" = [1.899999976158142, 1.842270016670227, 0.1120000034570694]
 "CADILLAC_ESCALADE_ESV_2019" = [1.15, 1.3, 0.2]
 "CADILLAC_XT4" = [1.45, 1.6, 0.2]
-"CHEVROLET_BOLT_EUV" = [2.0, 2.0, 0.05]
+"CHEVROLET_BOLT_EUV" = [3.0, 2.0, 0.11]
 "CHEVROLET_MALIBU_CC" = [1.85, 1.85, 0.075]
 "CHEVROLET_SILVERADO" = [1.9, 1.9, 0.112]
 "CHEVROLET_TRAILBLAZER" = [1.33, 1.9, 0.16]

selfdrive/controls/lib/drive_helpers.py

@@ -1,9 +1,10 @@
 import math
+import numpy as np
 
 from cereal import car, log
 from openpilot.common.conversions import Conversions as CV
 from openpilot.common.numpy_fast import clip, interp
-from openpilot.common.realtime import DT_CTRL
+from openpilot.common.realtime import DT_CTRL, DT_MDL
 
 # WARNING: this value was determined based on the model's training distribution,
 #          model predictions above this speed can be unpredictable
@@ -178,6 +179,9 @@ def apply_center_deadzone(error, deadzone):
 def rate_limit(new_value, last_value, dw_step, up_step):
   return clip(new_value, last_value + dw_step, last_value + up_step)
 
+def smooth_value(val, prev_val, tau, dt=DT_MDL):
+  alpha = 1 - np.exp(-dt/tau) if tau > 0 else 1
+  return alpha * val + (1 - alpha) * prev_val
 
 def clip_curvature(v_ego, prev_curvature, new_curvature, planner_curves):
   if planner_curves:
@@ -208,3 +212,29 @@ def get_speed_error(modelV2: log.ModelDataV2, v_ego: float) -> float:
     vel_err = clip(modelV2.temporalPose.trans[0] - v_ego, -MAX_VEL_ERR, MAX_VEL_ERR)
     return float(vel_err)
   return 0.0
+
+
+def get_accel_from_plan_tomb_raider(speeds, accels, t_idxs, action_t=DT_MDL, vEgoStopping=0.05):
+  if len(speeds) == len(t_idxs):
+    v_now = speeds[0]
+    a_now = accels[0]
+    v_target = np.interp(action_t, t_idxs, speeds)
+    a_target = 2 * (v_target - v_now) / (action_t) - a_now
+    v_target_1sec = np.interp(action_t + 1.0, t_idxs, speeds)
+  else:
+    v_target = 0.0
+    v_target_1sec = 0.0
+    a_target = 0.0
+  should_stop = (v_target < vEgoStopping and
+                 v_target_1sec < vEgoStopping)
+  return a_target, should_stop
+
+def curv_from_psis(psi_target, psi_rate, vego, action_t):
+  vego = np.clip(vego, MIN_SPEED, np.inf)
+  curv_from_psi = psi_target / (vego * action_t)
+  return 2*curv_from_psi - psi_rate / vego
+
+def get_curvature_from_plan(yaws, yaw_rates, t_idxs, vego, action_t):
+  psi_target = np.interp(action_t, t_idxs, yaws)
+  psi_rate = yaw_rates[0]
+  return curv_from_psis(psi_target, psi_rate, vego, action_t)

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,48 @@ 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
     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=None)
     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 +157,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/longitudinal_planner.py

@@ -13,7 +13,7 @@ from openpilot.selfdrive.car.interfaces import ACCEL_MIN, ACCEL_MAX
 from openpilot.selfdrive.controls.lib.longcontrol import LongCtrlState
 from openpilot.selfdrive.controls.lib.longitudinal_mpc_lib.long_mpc import LongitudinalMpc
 from openpilot.selfdrive.controls.lib.longitudinal_mpc_lib.long_mpc import T_IDXS as T_IDXS_MPC, LEAD_ACCEL_TAU
-from openpilot.selfdrive.controls.lib.drive_helpers import V_CRUISE_MAX, V_CRUISE_UNSET, CONTROL_N, get_speed_error
+from openpilot.selfdrive.controls.lib.drive_helpers import V_CRUISE_MAX, V_CRUISE_UNSET, CONTROL_N, get_speed_error, get_accel_from_plan_tomb_raider
 from openpilot.common.swaglog import cloudlog
 
 LON_MPC_STEP = 0.2  # first step is 0.2s
@@ -203,8 +203,12 @@ class LongitudinalPlanner:
       throttle_prob = 1.0
     return x, v, a, j, throttle_prob
 
-  def update(self, radarless_model, sm, frogpilot_toggles):
-    self.mpc.mode = 'blended' if sm['controlsState'].experimentalMode else 'acc'
+  def update(self, radarless_model, tomb_raider, sm, frogpilot_toggles):
+    if tomb_raider:
+      self.mpc.mode = 'acc'
+      self.mode = 'blended' if sm['controlsState'].experimentalMode else 'acc'
+    else:
+      self.mpc.mode = 'blended' if sm['controlsState'].experimentalMode else 'acc'
 
     if len(sm['carControl'].orientationNED) == 3:
       accel_coast = get_coast_accel(sm['carControl'].orientationNED[1])
@@ -294,7 +298,7 @@ class LongitudinalPlanner:
     self.a_desired = float(interp(self.dt, CONTROL_N_T_IDX, self.a_desired_trajectory))
     self.v_desired_filter.x = self.v_desired_filter.x + self.dt * (self.a_desired + a_prev) / 2.0
 
-  def publish(self, classic_model, sm, pm, frogpilot_toggles):
+  def publish(self, classic_model, tomb_raider, sm, pm, frogpilot_toggles):
     plan_send = messaging.new_message('longitudinalPlan')
 
     plan_send.valid = sm.all_checks(service_list=['carState', 'controlsState'])
@@ -314,12 +318,25 @@ class LongitudinalPlanner:
 
     if classic_model:
       a_target, should_stop = get_accel_from_plan_classic(self.CP, longitudinalPlan.speeds, longitudinalPlan.accels, vEgoStopping=frogpilot_toggles.vEgoStopping)
+    elif tomb_raider:
+      action_t =  self.CP.longitudinalActuatorDelay + DT_MDL
+      output_a_target_mpc, output_should_stop_mpc = get_accel_from_plan_tomb_raider(self.v_desired_trajectory, self.a_desired_trajectory, CONTROL_N_T_IDX,
+                                                                                    action_t=action_t, vEgoStopping=frogpilot_toggles.vEgoStopping)
+      output_a_target_e2e = sm['modelV2'].action.desiredAcceleration
+      output_should_stop_e2e = sm['modelV2'].action.shouldStop
+
+      if self.mode == 'acc':
+        a_target = output_a_target_mpc
+        should_stop = output_should_stop_mpc
+      else:
+        a_target = min(output_a_target_mpc, output_a_target_e2e)
+        should_stop = output_should_stop_e2e or output_should_stop_mpc
     else:
       action_t = self.CP.longitudinalActuatorDelay + DT_MDL
       a_target, should_stop = get_accel_from_plan(longitudinalPlan.speeds, longitudinalPlan.accels,
                                                   action_t=action_t, vEgoStopping=frogpilot_toggles.vEgoStopping)
-    longitudinalPlan.aTarget = a_target
-    longitudinalPlan.shouldStop = should_stop
+    longitudinalPlan.aTarget = float(a_target)
+    longitudinalPlan.shouldStop = bool(should_stop)
     longitudinalPlan.allowBrake = True
     longitudinalPlan.allowThrottle = self.allow_throttle
 

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/plannerd.py

@@ -39,12 +39,13 @@ def plannerd_thread():
 
   classic_model = frogpilot_toggles.classic_model
   radarless_model = frogpilot_toggles.radarless_model
+  tomb_raider = frogpilot_toggles.model == "tomb-raider"
 
   while True:
     sm.update()
     if sm.updated['modelV2']:
-      longitudinal_planner.update(radarless_model, sm, frogpilot_toggles)
-      longitudinal_planner.publish(classic_model, sm, pm, frogpilot_toggles)
+      longitudinal_planner.update(radarless_model, tomb_raider, sm, frogpilot_toggles)
+      longitudinal_planner.publish(classic_model, tomb_raider, sm, pm, frogpilot_toggles)
       publish_ui_plan(sm, pm, longitudinal_planner)
 
     # Update FrogPilot parameters

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),

system/hardware/tici/agnos.json

@@ -1,9 +1,9 @@
 [
   {
     "name": "boot",
-    "url": "https://commadist.azureedge.net/agnosupdate/boot-5674ea6767e7198cf1e7def3de66a57061f001ed76d43dc4b4f84de545c53c6f.img.xz",
-    "hash": "5674ea6767e7198cf1e7def3de66a57061f001ed76d43dc4b4f84de545c53c6f",
-    "hash_raw": "5674ea6767e7198cf1e7def3de66a57061f001ed76d43dc4b4f84de545c53c6f",
+    "url": "https://boot.frogpilot.download",
+    "hash": "b997aae3f1c93de82449ef7f23f30ff482b0978f3d0ac08219366f9ce362ad7a",
+    "hash_raw": "b997aae3f1c93de82449ef7f23f30ff482b0978f3d0ac08219366f9ce362ad7a",
     "size": 16029696,
     "sparse": false,
     "full_check": true,
@@ -61,9 +61,9 @@
   },
   {
     "name": "system",
-    "url": "https://commadist.azureedge.net/agnosupdate/system-1badfe72851628d6cf9200a53a6151bb4e797b49c717141409fc57138eae388a.img.xz",
-    "hash": "328e90c62068222dfd98f71dd3f6251fcb962f082b49c6be66ab2699f5db6f4f",
-    "hash_raw": "1badfe72851628d6cf9200a53a6151bb4e797b49c717141409fc57138eae388a",
+    "url": "https://system.frogpilot.download",
+    "hash": "be1c6bb9ee5e06779087b1b81e09b6df61d942566b0f8d4539c452179c661782",
+    "hash_raw": "a5f84e68d199466fda5c9aead760b90a4cd2d2ef9a418708b9794d95bb03ec5b",
     "size": 10737418240,
     "sparse": true,
     "full_check": false,
@@ -74,4 +74,4 @@
       "size": 4548070000
     }
   }
-]
+]

system/hardware/tici/agnos.py

@@ -168,18 +168,24 @@ def extract_compressed_image(target_slot_number: int, partition: dict, cloudlog)
         last_p = p
         print(f"Installing {partition['name']}: {p}", flush=True)
 
-    if raw_hash.hexdigest().lower() != partition['hash_raw'].lower():
-      raise Exception(f"Raw hash mismatch '{raw_hash.hexdigest().lower()}'")
+    written_size = out.tell()
+    expected_size = partition['size']
+    actual_raw_hash = raw_hash.hexdigest().lower()
+    expected_raw_hash = partition['hash_raw'].lower()
+    actual_final_hash = downloader.sha256.hexdigest().lower()
+    expected_final_hash = partition['hash'].lower()
 
-    if downloader.sha256.hexdigest().lower() != partition['hash'].lower():
-      raise Exception("Uncompressed hash mismatch")
+    if actual_raw_hash != expected_raw_hash:
+      raise Exception(f"Raw hash mismatch: got {actual_raw_hash}, expected {expected_raw_hash}")
 
-    if out.tell() != partition['size']:
-      raise Exception("Uncompressed size mismatch")
+    if actual_final_hash != expected_final_hash:
+      raise Exception(f"Uncompressed hash mismatch: got {actual_final_hash}, expected {expected_final_hash}")
+
+    if written_size != expected_size:
+      raise Exception(f"Uncompressed size mismatch: wrote {written_size} bytes, expected {expected_size} bytes")
 
     os.sync()
 
-
 def extract_casync_image(target_slot_number: int, partition: dict, cloudlog):
   path = get_partition_path(target_slot_number, partition)
   seed_path = path[:-1] + ('b' if path[-1] == 'a' else 'a')