IQ.Pilot Release Commit @ bec7652

iqpilot/selfdrive/controls/.gitignore

@@ -0,0 +1,2 @@
+calibration_param
+traces

iqpilot/selfdrive/controls/controlsd.py

@@ -0,0 +1,443 @@
+#!/usr/bin/env python3
+import math
+import time
+from numbers import Number
+
+from iqpilot.cereal import car, log
+import iqpilot.cereal.messaging as messaging
+from iqpilot.common.constants import CV
+from iqpilot.common.iq_perf import PerfSample, PerfTraceEmitter, PerfTraceRing
+from iqpilot.common.params import Params
+from iqpilot.common.realtime import config_realtime_process, lock_memory, DT_CTRL, Priority, Ratekeeper
+from iqpilot.common.swaglog import cloudlog
+
+from iqdbc.car.car_helpers import interfaces
+from iqdbc.car.vehicle_model import VehicleModel
+from iqpilot.common.steer_delay import lateral_action_delay
+from iqpilot.selfdrive.controls.lib.drive_helpers import clip_curvature
+from iqpilot.selfdrive.controls.lib.curvature_lookahead import get_lookahead_curvature
+from iqpilot.selfdrive.controls.lib.latcontrol import LatControl
+from iqpilot.selfdrive.controls.lib.latcontrol_pid import LatControlPID
+from iqpilot.selfdrive.controls.lib.latcontrol_angle import LatControlAngle, STEER_ANGLE_SATURATION_THRESHOLD
+from iqpilot.selfdrive.controls.lib.latcontrol_torque import LatControlTorque
+from iqpilot.selfdrive.controls.lib.latcontrol_torque_pq import LatControlTorquePQ
+from iqpilot.selfdrive.controls.lib.longcontrol import LongControl
+from iqpilot.system.proprietary_runtime._verified_import import import_verified_module
+from iqpilot.selfdrive.locationd.helpers import PoseCalibrator, Pose
+
+from iqpilot.selfdrive.controls.iq_controls_layer import IQControlsLayer
+
+NavTurnSignalController = import_verified_module(
+  "iqpilot_navd_private", "iqpilot_private.navd.nav_turn_signals"
+).NavTurnSignalController
+
+State = log.SelfdriveState.OpenpilotState
+EventName = log.OnroadEvent.EventName
+LaneChangeState = log.LaneChangeState
+LaneChangeDirection = log.LaneChangeDirection
+
+ACTUATOR_FIELDS = tuple(car.CarControl.Actuators.schema.fields.keys())
+CTRL_ESSENTIAL_SERVICES = ("carState", "modelV2", "longitudinalPlan", "vehicleParameters", "selfdriveState")
+CTRL_LOOP_WARN_US = 15_000
+CTRL_LOOP_BAD_US = 25_000
+CTRL_LOOP_SEVERE_US = 50_000
+CTRL_PHASE_WARN_US = 8_000
+CTRL_TAIL_WARN_US = 8_000
+CTRL_FLAG_MISSING_INPUTS = 1 << 0
+CTRL_FLAG_RK_OVERRUN = 1 << 1
+LAT_SMOOTH_SECONDS = 0.0
+
+
+class Controls(IQControlsLayer):
+  def __init__(self) -> None:
+    self.params = Params()
+    cloudlog.info("controlsd is waiting for CarParams")
+    self.CP = messaging.log_from_bytes(self.params.get("CarParams", block=True), car.CarParams)
+    cloudlog.info("controlsd got CarParams")
+
+    # Initialize iqpilot controlsd extension and base model state
+    IQControlsLayer.__init__(self, self.CP, self.params)
+
+    self.CI = interfaces[self.CP.carFingerprint](self.CP, self.CP_IQ)
+
+    self.sm = messaging.SubMaster(['lateralDelay', 'vehicleParameters', 'lateralTorqueParameters', 'modelV2', 'selfdriveState',
+                                   'extrinsicsCalibration', 'deviceMotion', 'longitudinalPlan', 'lateralManeuverPlan',
+                                   'carState', 'carOutput', 'driverMonitoringState', 'onroadEvents',
+                                   'driverAssistance', 'lateralDelay'] + self.iq_sub_services,
+                                  poll='selfdriveState')
+    self.pm = messaging.PubMaster(['carControl', 'controlsState', 'iqPerfTrace'] + self.iq_pub_services)
+
+    self.steer_limited_by_safety = False
+    self.curvature = 0.0
+    self.desired_curvature = 0.0
+    self.roll_compensation = 0.0
+
+    self._perf = PerfTraceEmitter("controlsd", pubmaster=self.pm)
+    self._perf_ring = PerfTraceRing()
+
+    self._param_update_time = 0.0
+    self.enable_curvature_controller = False
+    self.enable_speed_limit_control = False
+    self.enable_speed_limit_predicative = False
+    self.enable_pred_react_to_speed_limits = False
+    self.enable_pred_react_to_curves = False
+    self.enable_long_comfort_mode = False
+    self.force_rhd_for_bsm = False
+    self.navigation_enabled = False
+    self.nav_exit_lane_change = False
+    self._update_params()
+
+    self.nav_turn_signal_controller = NavTurnSignalController(self.CP)
+
+    self.pose_calibrator = PoseCalibrator()
+    self.calibrated_pose: Pose | None = None
+
+    self.LoC = LongControl(self.CP, self.CP_IQ)
+    self.VM = VehicleModel(self.CP)
+    self.LaC: LatControl
+    if self.CP.steerControlType in (car.CarParams.SteerControlType.angle, car.CarParams.SteerControlType.curvatureDEPRECATED):
+      self.LaC = LatControlAngle(self.CP, self.CP_IQ, self.CI, DT_CTRL)
+    elif self.CP.lateralTuning.which() == 'pid':
+      self.LaC = LatControlPID(self.CP, self.CP_IQ, self.CI, DT_CTRL)
+    elif self.CP.lateralTuning.which() == 'torque':
+      self.LaC = LatControlTorque(self.CP, self.CP_IQ, self.CI, DT_CTRL)
+      if self._use_pq_torque():
+        try:
+          self.LaC = LatControlTorquePQ(self.CP, self.CP_IQ, self.CI, DT_CTRL)
+        except Exception:
+          cloudlog.exception("LatControlTorquePQ init failed; using generic torque")
+
+  def _use_pq_torque(self) -> bool:
+    try:
+      if self.CP.brand != 'volkswagen':
+        return False
+      from iqdbc.car.volkswagen.values import VolkswagenFlags
+      return bool(self.CP.flags & VolkswagenFlags.PQ)
+    except Exception:
+      cloudlog.exception("pq torque selection failed; using generic torque")
+      return False
+
+  def _update_params(self) -> None:
+    self.enable_curvature_controller = self.params.get_bool("EnableCurvatureController")
+    self.enable_speed_limit_control = self.params.get_bool("EnableSpeedLimitControl")
+    self.enable_speed_limit_predicative = self.params.get_bool("EnableSpeedLimitPredicative")
+    self.enable_pred_react_to_speed_limits = self.params.get_bool("EnableSLPredReactToSL")
+    self.enable_pred_react_to_curves = self.params.get_bool("EnableSLPredReactToCurves")
+    self.enable_long_comfort_mode = self.params.get_bool("EnableLongComfortMode")
+    self.force_rhd_for_bsm = self.params.get_bool("ForceRHDForBSM")
+    self.navigation_enabled = self.params.get_bool("NavigationEnabled")
+    self.nav_exit_lane_change = self.params.get_bool("NavExitLaneChange")
+
+  def update(self):
+    self.sm.update(15)
+    if self.sm.updated["extrinsicsCalibration"]:
+      self.pose_calibrator.feed_live_calib(self.sm['extrinsicsCalibration'])
+    if self.sm.updated["deviceMotion"]:
+      device_pose = Pose.from_live_pose(self.sm['deviceMotion'])
+      self.calibrated_pose = self.pose_calibrator.build_calibrated_pose(device_pose)
+    if time.monotonic() - self._param_update_time > 3:
+      self._update_params()
+      self._param_update_time = time.monotonic()
+
+  def state_control(self):
+    CS = self.sm['carState']
+
+    # Update VehicleModel
+    lp = self.sm['vehicleParameters']
+    x = max(lp.stiffnessFactor, 0.1)
+    sr = max(lp.steerRatio, 0.1)
+    self.VM.update_params(x, sr)
+
+    steer_angle_without_offset = math.radians(CS.steeringAngleDeg - lp.angleOffsetDeg)
+    self.curvature = -self.VM.calc_curvature(steer_angle_without_offset, CS.vEgo, lp.roll)
+    self.roll_compensation = -self.VM.roll_compensation(lp.roll, CS.vEgo)
+
+    lac_ext = getattr(self.LaC, "extension", None)
+    if self.CP.lateralTuning.which() == 'torque' and hasattr(self.LaC, "update_live_torque_params"):
+      torque_params = self.sm['lateralTorqueParameters']
+      if self.sm.all_checks(['lateralTorqueParameters']) and torque_params.useParams:
+        self.LaC.update_live_torque_params(torque_params.latAccelFactorFiltered, torque_params.latAccelOffsetFiltered,
+                                           torque_params.frictionCoefficientFiltered)
+
+        if lac_ext is not None:
+          lac_ext.update_limits()
+
+      if lac_ext is not None:
+        lac_ext.update_model_v2(self.sm['modelV2'])
+
+      if lac_ext is not None:
+        lac_ext.update_lateral_lag(self.CP.steerActuatorDelay)
+
+    long_plan = self.sm['longitudinalPlan']
+    model_v2 = self.sm['modelV2']
+
+    CC = car.CarControl.new_message()
+    CC.enabled = self.sm['selfdriveState'].enabled
+
+    # Check which actuators can be enabled
+    standstill = abs(CS.vEgo) <= max(self.CP.minSteerSpeed, 0.3) or CS.standstill
+
+    # Get which state to use for active lateral control
+    _lat_active = self.iq_lateral_allowed(self.sm)
+
+    CC.latActive = _lat_active and not CS.steerFaultTemporary and not CS.steerFaultPermanent and \
+                   (not standstill or self.CP.steerAtStandstill)
+    # long control may stay active through a gas override on platforms that opt in
+    override_longitudinal = any(e.overrideLongitudinal for e in self.sm['onroadEvents'])
+    long_through_override = self.CP_IQ.longActiveWithGasOverride and self.CP.openpilotLongitudinalControl
+    CC.longActive = CC.enabled and not getattr(CS, 'cruiseFaultLateralMode', False) and \
+                    (not override_longitudinal or long_through_override) and \
+                    (self.CP.openpilotLongitudinalControl or not self.CP_IQ.pcmCruiseSpeed)
+
+    CC.leftBlinker, CC.rightBlinker = self.nav_turn_signal_controller.update(
+      self.sm['iqNavState'] if self.sm.alive['iqNavState'] else None,
+      self.navigation_enabled,
+      self.nav_exit_lane_change,
+      CC.enabled and CC.latActive,
+      CS.leftBlinker,
+      CS.rightBlinker,
+      CS.vEgo,
+      CS.yawRate,
+      CS.steeringAngleDeg,
+      self.sm.alive['iqNavState'] and self.sm.valid['iqNavState'],
+    )
+
+    actuators = CC.actuators
+    actuators.longControlState = self.LoC.long_control_state
+
+    if not CC.latActive:
+      self.LaC.reset()
+
+    if not CC.longActive:
+      self.LoC.reset()
+
+    # accel PID loop
+    pid_accel_limits = self.CI.get_pid_accel_limits(self.CP, self.CP_IQ, CS.vEgo, CS.vCruise * CV.KPH_TO_MS)
+    actuators.accel = float(self.LoC.update(CC.longActive, CS, long_plan.aTarget, long_plan.shouldStop, pid_accel_limits,
+                                            long_plan.leadDistance, long_plan.hasLead, gas_override=override_longitudinal))
+
+    # Steering PID loop and lateral MPC
+    # Reset desired curvature to current to avoid violating the limits on engage
+    if not CC.latActive:
+      new_desired_curvature = self.curvature
+    elif self.sm.valid['lateralManeuverPlan']:
+      new_desired_curvature = self.sm['lateralManeuverPlan'].desiredCurvature
+    else:
+      new_desired_curvature = model_v2.action.desiredCurvature
+
+    lat_accel_override = bool(CS.gasPressed) or bool(self.sm['iqState'].aol.active)
+    self.desired_curvature, curvature_limited = clip_curvature(CS.vEgo, self.desired_curvature, new_desired_curvature, lp.roll, lat_accel_override)
+    lat_delay = lateral_action_delay(self.params, self.CP, self.sm["lateralDelay"].lateralDelay) + LAT_SMOOTH_SECONDS
+    lookahead_curvature = None
+    if not self.sm.valid['lateralManeuverPlan']:
+      lookahead_curvature = get_lookahead_curvature(model_v2, CS.vEgo, lat_delay)
+
+    actuators.curvature = self.desired_curvature
+    steer, steeringAngleDeg, lac_log = self.LaC.update(CC.latActive, CS, self.VM, lp,
+                                                       self.steer_limited_by_safety, self.desired_curvature,
+                                                       self.calibrated_pose, curvature_limited, lat_delay, lookahead_curvature)
+    if self.CP.steerControlType in (car.CarParams.SteerControlType.angle, car.CarParams.SteerControlType.curvatureDEPRECATED):
+      actuators.torque = 0.0
+      actuators.steeringAngleDeg = float(steeringAngleDeg)
+    else:
+      actuators.torque = float(steer)
+      actuators.steeringAngleDeg = float(steeringAngleDeg)
+    # Ensure no NaNs/Infs
+    for p in ACTUATOR_FIELDS:
+      attr = getattr(actuators, p)
+      if not isinstance(attr, Number):
+        continue
+
+      if not math.isfinite(attr):
+        cloudlog.error(f"actuators.{p} not finite {actuators.to_dict()}")
+        setattr(actuators, p, 0.0)
+
+    return CC, lac_log
+
+  def publish(self, CC, lac_log):
+    CS = self.sm['carState']
+
+    CC.curvatureControllerActive = self.enable_curvature_controller
+    CC.steerLimited = self.steer_limited_by_safety
+    CC.forceRHDForBSM = self.force_rhd_for_bsm
+    CC.longComfortMode = self.enable_long_comfort_mode
+
+    # Orientation and angle rates can be useful for carcontroller
+    # Only calibrated (car) frame is relevant for the carcontroller
+    CC.currentCurvature = self.curvature
+    CC.rollCompensation = self.roll_compensation
+    if self.calibrated_pose is not None:
+      CC.orientationNED = self.calibrated_pose.orientation.xyz.tolist()
+      CC.angularVelocity = self.calibrated_pose.angular_velocity.xyz.tolist()
+
+    CC.cruiseControl.override = CC.enabled and not CC.longActive and (self.CP.openpilotLongitudinalControl or not self.CP_IQ.pcmCruiseSpeed)
+    dm_lockout = self.CP.pcmCruise and any(e.name == EventName.tooDistracted for e in self.sm['onroadEvents'])
+    CC.cruiseControl.cancel = CS.cruiseState.enabled and (not CC.enabled or not self.CP.pcmCruise) and not dm_lockout
+    CC.cruiseControl.resume = CC.enabled and CS.cruiseState.standstill and not self.sm['longitudinalPlan'].shouldStop
+    CC.cruiseControl.speedLimit = self.enable_speed_limit_control
+    CC.cruiseControl.speedLimitPredicative = self.enable_speed_limit_predicative
+    CC.cruiseControl.speedLimitPredReactToSL = self.enable_pred_react_to_speed_limits
+    CC.cruiseControl.speedLimitPredReactToCurves = self.enable_pred_react_to_curves
+
+    hudControl = CC.hudControl
+    hudControl.setSpeed = float(CS.vCruiseCluster * CV.KPH_TO_MS)
+    hudControl.speedVisible = CC.enabled
+    hudControl.lanesVisible = CC.enabled
+    hudControl.leadVisible = self.sm['longitudinalPlan'].hasLead
+    hudControl.leadDistance = self.sm['longitudinalPlan'].leadDistance
+    hudControl.leadDistanceBars = self.sm['selfdriveState'].personality.raw + 1
+    if self.sm['selfdriveState'].personality.raw == log.LongitudinalPersonality.relaxed:
+      hudControl.leadFollowTime = 1.75
+    elif self.sm['selfdriveState'].personality.raw == log.LongitudinalPersonality.aggressive:
+      hudControl.leadFollowTime = 1.25
+    else:
+      hudControl.leadFollowTime = 1.45
+    hudControl.visualAlert = self.sm['selfdriveState'].alertHudVisual
+    hudControl.audibleAlert = self.sm['selfdriveState'].alertSound
+    hudControl.driverUnresponsive = self.sm['selfdriveState'].alertType.split('/', 1)[0] == 'driverUnresponsive'
+
+    hudControl.rightLaneVisible = True
+    hudControl.leftLaneVisible = True
+    if self.sm.valid['driverAssistance']:
+      hudControl.leftLaneDepart = self.sm['driverAssistance'].leftLaneDeparture
+      hudControl.rightLaneDepart = self.sm['driverAssistance'].rightLaneDeparture
+
+    if self.sm['selfdriveState'].active:
+      CO = self.sm['carOutput']
+      if self.CP.steerControlType in (car.CarParams.SteerControlType.angle, car.CarParams.SteerControlType.curvatureDEPRECATED):
+        self.steer_limited_by_safety = abs(CC.actuators.steeringAngleDeg - CO.actuatorsOutput.steeringAngleDeg) > \
+                                              STEER_ANGLE_SATURATION_THRESHOLD
+      else:
+        self.steer_limited_by_safety = abs(CC.actuators.torque - CO.actuatorsOutput.torque) > 1e-2
+
+    # TODO: both controlsState and carControl valids should be set by
+    #       sm.all_checks(), but this creates a circular dependency
+
+    # controlsState
+    dat = messaging.new_message('controlsState')
+    dat.valid = CS.canValid
+    cs = dat.controlsState
+
+    cs.curvature = self.curvature
+    cs.longitudinalPlanMonoTime = self.sm.logMonoTime['longitudinalPlan']
+    cs.lateralPlanMonoTime = self.sm.logMonoTime['modelV2']
+    cs.desiredCurvature = self.desired_curvature
+    cs.longControlState = self.LoC.long_control_state
+    cs.upAccelCmd = float(self.LoC.pid.p)
+    cs.uiAccelCmd = float(self.LoC.pid.i)
+    cs.ufAccelCmd = float(self.LoC.pid.f)
+    cs.forceDecel = bool((self.sm['driverMonitoringState'].awarenessStatus < 0.) or
+                         (self.sm['selfdriveState'].state == State.softDisabling))
+
+    lat_tuning = self.CP.lateralTuning.which()
+    if self.CP.steerControlType in (car.CarParams.SteerControlType.angle, car.CarParams.SteerControlType.curvatureDEPRECATED):
+      cs.lateralControlState.angleState = lac_log
+    elif lat_tuning == 'pid':
+      cs.lateralControlState.pidState = lac_log
+    elif lat_tuning == 'torque':
+      cs.lateralControlState.torqueState = lac_log
+
+    self.pm.send('controlsState', dat)
+
+    # carControl
+    cc_send = messaging.new_message('carControl')
+    cc_send.valid = CS.canValid
+    cc_send.carControl = CC
+    self.pm.send('carControl', cc_send)
+
+  def _tail_work(self) -> int:
+    started_ns = time.monotonic_ns()
+    self.refresh_iq_params(self.sm)
+    self.publish_iq_state(self.sm, self.pm)
+    return (time.monotonic_ns() - started_ns) // 1000
+
+  def _missing_services(self) -> list[str]:
+    missing = []
+    for service in CTRL_ESSENTIAL_SERVICES:
+      if not self.sm.alive[service] or not self.sm.freq_ok[service] or not self.sm.valid[service]:
+        missing.append(service)
+    return missing
+
+  def _emit_perf_trace(self, frame_id: int, loop_dt_us: int, update_us: int, state_control_us: int,
+                       publish_us: int, tail_work_us: int, rk_remaining_us: int) -> None:
+    missing_services = self._missing_services()
+    flags = 0
+    if missing_services:
+      flags |= CTRL_FLAG_MISSING_INPUTS
+    if rk_remaining_us < 0:
+      flags |= CTRL_FLAG_RK_OVERRUN
+
+    sample = PerfSample(
+      frame_id=frame_id,
+      loop_dt_us=loop_dt_us,
+      update_us=update_us,
+      state_control_us=state_control_us,
+      publish_us=publish_us,
+      tail_work_us=tail_work_us,
+      rk_remaining_us=rk_remaining_us,
+      flags=flags,
+    )
+    self._perf_ring.push(sample)
+
+    slow_phase = max(update_us, state_control_us, publish_us, tail_work_us)
+    if loop_dt_us < CTRL_LOOP_WARN_US and slow_phase < CTRL_PHASE_WARN_US and tail_work_us < CTRL_TAIL_WARN_US and flags == 0:
+      return
+
+    if loop_dt_us >= CTRL_LOOP_SEVERE_US:
+      severity = "critical"
+    elif loop_dt_us >= CTRL_LOOP_BAD_US or rk_remaining_us < -10_000:
+      severity = "error"
+    else:
+      severity = "warning"
+
+    detail = (
+      f"update_us={update_us} state_control_us={state_control_us} publish_us={publish_us} "
+      f"tail_work_us={tail_work_us} rk_remaining_us={rk_remaining_us}"
+    )
+    self._perf.emit(
+      "controlsd_slow_loop",
+      severity=severity,
+      frame_id=frame_id,
+      total_time_us=loop_dt_us,
+      rk_remaining_us=rk_remaining_us,
+      flags=flags,
+      samples=self._perf_ring.snapshot(),
+      missing_services=missing_services,
+      detail=detail,
+      min_interval_s=0.25,
+    )
+
+  def run(self):
+    rk = Ratekeeper(100, print_delay_threshold=None)
+    while True:
+      started_ns = time.monotonic_ns()
+      checkpoint_ns = started_ns
+      self.update()
+      update_us = (time.monotonic_ns() - checkpoint_ns) // 1000
+      checkpoint_ns = time.monotonic_ns()
+
+      CC, lac_log = self.state_control()
+      state_control_us = (time.monotonic_ns() - checkpoint_ns) // 1000
+      checkpoint_ns = time.monotonic_ns()
+
+      self.publish(CC, lac_log)
+      publish_us = (time.monotonic_ns() - checkpoint_ns) // 1000
+      checkpoint_ns = time.monotonic_ns()
+
+      tail_work_us = self._tail_work()
+      loop_dt_us = (time.monotonic_ns() - started_ns) // 1000
+      rk_remaining_us = int(rk.remaining * 1_000_000)
+      self._emit_perf_trace(self.sm.frame, int(loop_dt_us), int(update_us), int(state_control_us),
+                            int(publish_us), int(tail_work_us), rk_remaining_us)
+      rk.monitor_time()
+
+
+def main():
+  config_realtime_process(4, Priority.CTRL_HIGH)
+  lock_memory()
+  controls = Controls()
+  controls.run()
+
+
+if __name__ == "__main__":
+  main()

iqpilot/selfdrive/controls/iq_controls_layer.py

@@ -7,18 +7,17 @@ guidance continuity) and the lateral-engage gate, without touching stock control
 """
 import time
 
-import cereal.messaging as messaging
-from cereal import log, custom
+import iqpilot.cereal.messaging as messaging
+from iqpilot.cereal import log, custom
 
 from iqdbc.car import structs
-from openpilot.common.constants import CV
-from openpilot.common.params import Params
-from openpilot.common.swaglog import cloudlog
-from openpilot.iqpilot.common.steer_delay import resolve_steer_delay
-from openpilot.iqpilot.selfdrive.car.enhanced_stock_longitudinal_control import build_iq_control_params_from_plan
-from openpilot.iqpilot.selfdrive.iqmodeld.models.inference_state import InferenceStateBase
-from openpilot.iqpilot.selfdrive.controls.lib.helpers.blinker_pause import IQSignalPauseController
-from openpilot.iqpilot.selfdrive.controls.lib.iq_dynamic.radar_manager import RadarManager
+from iqpilot.common.constants import CV
+from iqpilot.common.params import Params
+from iqpilot.common.swaglog import cloudlog
+from iqpilot.selfdrive.car.enhanced_stock_longitudinal_control import build_iq_control_params_from_plan
+from iqpilot.selfdrive.iqmodeld.models.inference_state import InferenceStateBase
+from iqpilot.selfdrive.controls.lib.helpers.blinker_pause import IQSignalPauseController
+from iqpilot.selfdrive.controls.lib.iq_dynamic.radar_manager import RadarManager
 
 _PARAM_REFRESH_S = 3.0
 _LEAD_FIELDS = ("dRel", "yRel", "vRel", "aRel", "vLead", "dPath", "vLat", "vLeadK",
@@ -60,7 +59,7 @@ class IQControlsLayer(InferenceStateBase):
       return
     self.blinker_pause_lateral.get_params()
     if self.CP.lateralTuning.which() == 'torque':
-      self.lat_delay = resolve_steer_delay(self.params, sm["liveDelay"].lateralDelay)
+      self.lat_delay = sm["lateralDelay"].lateralDelay
     self._sync_set_speed = self._want_set_speed_to_limit()
     self.radar_manager.read_params()
     self._next_param_refresh = now
@@ -85,7 +84,7 @@ class IQControlsLayer(InferenceStateBase):
   # --- build + publish the IQ car-control message ------------------------------
   def _compose_iq_carcontrol(self, sm: messaging.SubMaster) -> custom.IQCarControl:
     CC_IQ = custom.IQCarControl.new_message()
-    lp = sm['liveParameters']
+    lp = sm['vehicleParameters']
     CC_IQ.angleOffsetDeg = float(getattr(lp, 'angleOffsetDeg', 0.0))
     CC_IQ.aol = sm['iqState'].aol
 

iqpilot/selfdrive/controls/lib/__init__.py

@@ -1,3 +0,0 @@
-"""
-Copyright © IQ.Lvbs, apart of Project Teal Lvbs, All Rights Reserved, licensed under https://konn3kt.com/tos
-"""

iqpilot/selfdrive/controls/lib/curvature_lookahead.py

@@ -0,0 +1,21 @@
+import math
+
+from iqpilot.selfdrive.controls.lib.drive_helpers import CONTROL_N, get_curvature_from_plan
+from iqpilot.selfdrive.iqmodeld.config import ModelConstants
+
+
+LOOKAHEAD_SECONDS = 0.20
+
+
+def get_lookahead_curvature(model_v2, v_ego: float, lat_delay: float) -> float | None:
+  try:
+    yaws = model_v2.orientation.z
+    yaw_rates = model_v2.orientationRate.z
+    if len(yaws) < CONTROL_N or len(yaw_rates) < CONTROL_N:
+      return None
+    if not all(math.isfinite(value) for value in yaws) or not all(math.isfinite(value) for value in yaw_rates):
+      return None
+    horizon = max(0.0, lat_delay) + LOOKAHEAD_SECONDS
+    return get_curvature_from_plan(yaws, yaw_rates, ModelConstants.T_IDXS, v_ego, horizon)
+  except (AttributeError, TypeError, ValueError):
+    return None

iqpilot/selfdrive/controls/lib/custom_stop_distance.py

@@ -25,9 +25,9 @@ Two mechanisms share the param:
 import numpy as np
 
 from iqdbc.car.interfaces import ACCEL_MIN
-from openpilot.common.params import Params
-from openpilot.common.realtime import DT_MDL
-from openpilot.selfdrive.modeld.constants import ModelConstants
+from iqpilot.common.params import Params
+from iqpilot.common.realtime import DT_MDL
+from iqpilot.selfdrive.iqmodeld.config import ModelConstants
 
 CUSTOM_STOP_DISTANCE_PARAM = "IQCustomStopDistance"
 MIN_DISTANCE_M = -2

iqpilot/selfdrive/controls/lib/desire_helper.py

@@ -0,0 +1,294 @@
+"""
+Copyright © IQ.Lvbs, apart of Project Teal Lvbs, All Rights Reserved, licensed under https://konn3kt.com/tos/
+"""
+
+from __future__ import annotations
+
+from iqpilot.cereal import car, custom, log
+
+from iqpilot.common.constants import CV
+from iqpilot.common.params import Params
+from iqpilot.common.realtime import DT_MDL
+from iqpilot.selfdrive.controls.lib.helpers.lane_change import (
+  IQLaneSwapController,
+  AutoLaneChangeMode,
+  NavExitLaneChangeController,
+)
+from iqpilot.selfdrive.controls.lib.helpers.lateral_edge_guard import LateralEdgeGuard
+from iqpilot.selfdrive.controls.lib.helpers.lane_turn import IQNavTurnController
+
+LaneChangeState = log.LaneChangeState
+LaneChangeDirection = log.LaneChangeDirection
+TurnDirection = custom.IQTurnSignalDirection
+LateralEdgeBlock = custom.IQLateralEdgeBlock
+NavManeuverPhase = custom.IQNavState.ManeuverPhase
+
+LANE_CHANGE_SPEED_MIN = 20 * CV.MPH_TO_MS
+LANE_CHANGE_TIME_MAX = 10.0
+TURN_DESIRE_STOP_HOLD_TIME = 3.4
+TURN_DESIRE_STOP_GAP_TIME = 0.2
+TURN_DESIRE_STOP_CYCLE_TIME = TURN_DESIRE_STOP_HOLD_TIME + TURN_DESIRE_STOP_GAP_TIME
+TURN_DESIRE_CYCLE_SPEED_MAX = 5 * CV.MPH_TO_MS
+TURN_DESIRE_COMMIT_YAW_RATE = 0.08
+
+_LANE_CHANGE_DESIRES = {
+  (LaneChangeDirection.none, LaneChangeState.off): log.Desire.none,
+  (LaneChangeDirection.none, LaneChangeState.preLaneChange): log.Desire.none,
+  (LaneChangeDirection.none, LaneChangeState.laneChangeStarting): log.Desire.none,
+  (LaneChangeDirection.none, LaneChangeState.laneChangeFinishing): log.Desire.none,
+  (LaneChangeDirection.left, LaneChangeState.off): log.Desire.none,
+  (LaneChangeDirection.left, LaneChangeState.preLaneChange): log.Desire.none,
+  (LaneChangeDirection.left, LaneChangeState.laneChangeStarting): log.Desire.laneChangeLeft,
+  (LaneChangeDirection.left, LaneChangeState.laneChangeFinishing): log.Desire.laneChangeLeft,
+  (LaneChangeDirection.right, LaneChangeState.off): log.Desire.none,
+  (LaneChangeDirection.right, LaneChangeState.preLaneChange): log.Desire.none,
+  (LaneChangeDirection.right, LaneChangeState.laneChangeStarting): log.Desire.laneChangeRight,
+  (LaneChangeDirection.right, LaneChangeState.laneChangeFinishing): log.Desire.laneChangeRight,
+}
+
+_TURN_DESIRES = {
+  TurnDirection.none: log.Desire.none,
+  TurnDirection.turnLeft: log.Desire.turnLeft,
+  TurnDirection.turnRight: log.Desire.turnRight,
+}
+
+_STOP_CYCLING_TURN_DESIRES = {
+  log.Desire.turnLeft,
+  log.Desire.turnRight,
+}
+
+
+def turn_desire(turn_direction) -> log.Desire:
+  return _TURN_DESIRES[getattr(turn_direction, "raw", turn_direction)]
+
+
+def _direction_from_blinkers(carstate) -> int:
+  if carstate.leftBlinker:
+    return LaneChangeDirection.left
+  if carstate.rightBlinker:
+    return LaneChangeDirection.right
+  return LaneChangeDirection.none
+
+
+def _steering_nudge_matches(carstate, direction: int) -> bool:
+  if not carstate.steeringPressed:
+    return False
+  return (
+    (direction == LaneChangeDirection.left and carstate.steeringTorque > 0) or
+    (direction == LaneChangeDirection.right and carstate.steeringTorque < 0)
+  )
+
+
+def _blindspot_matches(carstate, direction: int) -> bool:
+  return (
+    (direction == LaneChangeDirection.left and carstate.leftBlindspot) or
+    (direction == LaneChangeDirection.right and carstate.rightBlindspot)
+  )
+
+
+def _read_enable_bsm() -> bool:
+  try:
+    with car.CarParams.from_bytes(Params().get("CarParams")) as cp:
+      return bool(cp.enableBsm)
+  except Exception:
+    return False
+
+
+class DesireHelper:
+  def __init__(self):
+    self.lane_change_state = LaneChangeState.off
+    self.lane_change_direction = LaneChangeDirection.none
+    self.lane_change_timer = 0.0
+    self.lane_change_ll_prob = 1.0
+    self.prev_one_blinker = False
+    self.prev_nav_exit_active = False
+    self.desire = log.Desire.none
+
+    self.alc = IQLaneSwapController(self)
+    self.lane_turn_controller = IQNavTurnController(self)
+    self.nav_exit = NavExitLaneChangeController(_read_enable_bsm())
+    self.lateral_edge_guard = LateralEdgeGuard()
+    self.lateral_edge_block = LateralEdgeBlock.none
+    self.lane_turn_direction = TurnDirection.none
+    self.nav_turn_direction = TurnDirection.none
+    self.turn_desire_stop_timer = 0.0
+    self.turn_desire_stop_active = False
+    self.turn_desire_cycle_input = log.Desire.none
+    self.turn_desire_committed = False
+
+  @staticmethod
+  def get_lane_change_direction(carstate):
+    return _direction_from_blinkers(carstate)
+
+  @staticmethod
+  def _nav_turn_desire(nav_state):
+    if nav_state is None or not getattr(nav_state, "active", False):
+      return TurnDirection.none
+    if getattr(nav_state, "maneuverPhase", NavManeuverPhase.none) != NavManeuverPhase.turnActive:
+      return TurnDirection.none
+    if not getattr(nav_state, "shouldSendTurnDesire", False):
+      return TurnDirection.none
+    return getattr(nav_state, "turnDesireDirection", TurnDirection.none)
+
+  def _clear_lane_change(self) -> None:
+    self.lane_change_state = LaneChangeState.off
+    self.lane_change_direction = LaneChangeDirection.none
+
+  def _refresh_turn_overrides(self, carstate, nav_state) -> bool:
+    speed_mps = carstate.vEgo
+    self.lane_turn_controller.update_params()
+    self.lane_turn_controller.update_lane_turn(
+      blindspot_left=carstate.leftBlindspot,
+      blindspot_right=carstate.rightBlindspot,
+      left_blinker=carstate.leftBlinker,
+      right_blinker=carstate.rightBlinker,
+      v_ego=speed_mps,
+    )
+    self.lane_turn_direction = self.lane_turn_controller.get_turn_direction()
+    self.nav_turn_direction = self._nav_turn_desire(nav_state)
+
+    self.nav_exit.update_params()
+    self.nav_exit.update(nav_state, carstate)
+    return bool(self.nav_exit.active)
+
+  def _reset_required(self, lateral_active: bool, nav_exit_active: bool) -> bool:
+    timed_out = self.lane_change_timer > LANE_CHANGE_TIME_MAX
+    feature_disabled = self.alc.lane_change_set_timer == AutoLaneChangeMode.OFF and not nav_exit_active
+    return (not lateral_active) or timed_out or feature_disabled
+
+  def _begin_from_idle(self, one_blinker: bool, nav_exit_active: bool, below_speed: bool) -> None:
+    if below_speed:
+      return
+    if one_blinker and not self.prev_one_blinker:
+      self.lane_change_state = LaneChangeState.preLaneChange
+      self.lane_change_direction = _direction_from_blinkers(self._last_carstate)
+      self.lane_change_ll_prob = 1.0
+      return
+    if nav_exit_active and not self.prev_nav_exit_active:
+      self.lane_change_state = LaneChangeState.preLaneChange
+      self.lane_change_direction = self.nav_exit.direction
+      self.lane_change_ll_prob = 1.0
+
+  def _refresh_requested_direction(self, one_blinker: bool, nav_exit_active: bool) -> None:
+    if one_blinker:
+      self.lane_change_direction = _direction_from_blinkers(self._last_carstate)
+    elif nav_exit_active:
+      self.lane_change_direction = self.nav_exit.direction
+
+  def _step_pre_lane_change(self, one_blinker: bool, nav_exit_active: bool, below_speed: bool) -> None:
+    self._refresh_requested_direction(one_blinker, nav_exit_active)
+    blindspot_detected = _blindspot_matches(self._last_carstate, self.lane_change_direction)
+    self.lateral_edge_block = self.lateral_edge_guard.block_for_direction(self.lane_change_direction)
+    lateral_edge_blocked = self.lateral_edge_block != LateralEdgeBlock.none
+    steering_ready = _steering_nudge_matches(self._last_carstate, self.lane_change_direction)
+    nav_auto_start = nav_exit_active and self.nav_exit.auto_allowed
+
+    self.alc.update_lane_change(blindspot_detected=blindspot_detected, brake_pressed=self._last_carstate.brakePressed)
+    allowed_to_launch = steering_ready or self.alc.auto_lane_change_allowed or nav_auto_start
+
+    if (not (one_blinker or nav_exit_active)) or below_speed:
+      self._clear_lane_change()
+    elif allowed_to_launch and not blindspot_detected and not lateral_edge_blocked:
+      self.lane_change_state = LaneChangeState.laneChangeStarting
+
+  def _step_lane_change_starting(self, lane_change_prob: float) -> None:
+    self.lane_change_ll_prob = max(self.lane_change_ll_prob - (2.0 * DT_MDL), 0.0)
+    if lane_change_prob < 0.02 and self.lane_change_ll_prob < 0.01:
+      self.lane_change_state = LaneChangeState.laneChangeFinishing
+
+  def _step_lane_change_finishing(self, one_blinker: bool) -> None:
+    self.lane_change_ll_prob = min(self.lane_change_ll_prob + DT_MDL, 1.0)
+    if self.lane_change_ll_prob <= 0.99:
+      return
+    self.lane_change_direction = LaneChangeDirection.none
+    self.lane_change_state = LaneChangeState.preLaneChange if one_blinker else LaneChangeState.off
+
+  def _advance_lane_change_machine(self, one_blinker: bool, nav_exit_active: bool, below_speed: bool, lane_change_prob: float) -> None:
+    if self.lane_change_state == LaneChangeState.off:
+      self._begin_from_idle(one_blinker, nav_exit_active, below_speed)
+      return
+    if self.lane_change_state == LaneChangeState.preLaneChange:
+      self._step_pre_lane_change(one_blinker, nav_exit_active, below_speed)
+      return
+    if self.lane_change_state == LaneChangeState.laneChangeStarting:
+      self._step_lane_change_starting(lane_change_prob)
+      return
+    if self.lane_change_state == LaneChangeState.laneChangeFinishing:
+      self._step_lane_change_finishing(one_blinker)
+
+  def _update_timer(self) -> None:
+    if self.lane_change_state in (LaneChangeState.off, LaneChangeState.preLaneChange):
+      self.lane_change_timer = 0.0
+    else:
+      self.lane_change_timer += DT_MDL
+
+  def _clear_turn_desire_stop_cycle(self) -> None:
+    self.turn_desire_stop_timer = 0.0
+    self.turn_desire_stop_active = False
+    self.turn_desire_cycle_input = log.Desire.none
+    self.turn_desire_committed = False
+
+  def _cycle_turn_desire_when_stopped(self, desired_output: log.Desire) -> log.Desire:
+    if desired_output not in _STOP_CYCLING_TURN_DESIRES:
+      self._clear_turn_desire_stop_cycle()
+      return desired_output
+
+    if desired_output != self.turn_desire_cycle_input:
+      self.turn_desire_stop_timer = 0.0
+      self.turn_desire_stop_active = False
+      self.turn_desire_cycle_input = desired_output
+      self.turn_desire_committed = False
+
+    if abs(getattr(self._last_carstate, "yawRate", 0.0)) >= TURN_DESIRE_COMMIT_YAW_RATE:
+      self.turn_desire_committed = True
+
+    if self.turn_desire_committed:
+      self.turn_desire_stop_timer = 0.0
+      self.turn_desire_stop_active = False
+      return desired_output
+
+    if self._last_carstate.vEgo > TURN_DESIRE_CYCLE_SPEED_MAX:
+      self.turn_desire_stop_timer = 0.0
+      self.turn_desire_stop_active = False
+      return desired_output
+
+    if not self.turn_desire_stop_active:
+      self.turn_desire_stop_active = True
+      self.turn_desire_stop_timer = 0.0
+
+    cycle_phase = self.turn_desire_stop_timer % TURN_DESIRE_STOP_CYCLE_TIME
+    self.turn_desire_stop_timer += DT_MDL
+    if cycle_phase >= TURN_DESIRE_STOP_HOLD_TIME:
+      return log.Desire.none
+    return desired_output
+
+  def _pick_desire_output(self) -> None:
+    desired_output = log.Desire.none
+    if self.nav_turn_direction != TurnDirection.none:
+      desired_output = turn_desire(self.nav_turn_direction)
+    elif self.lane_turn_direction != TurnDirection.none:
+      desired_output = turn_desire(self.lane_turn_direction)
+    else:
+      desired_output = _LANE_CHANGE_DESIRES[(self.lane_change_direction, self.lane_change_state)]
+
+    self.desire = self._cycle_turn_desire_when_stopped(desired_output)
+
+  def update(self, carstate, lateral_active, lane_change_prob, nav_state=None, modeldata=None, radar_state=None):
+    self._last_carstate = carstate
+    self.lateral_edge_guard.update(modeldata, carstate.vEgo, DT_MDL)
+    self.lateral_edge_block = LateralEdgeBlock.none
+    one_blinker = carstate.leftBlinker != carstate.rightBlinker
+    below_speed = carstate.vEgo < LANE_CHANGE_SPEED_MIN
+    nav_exit_active = self._refresh_turn_overrides(carstate, nav_state)
+
+    self.alc.update_params()
+    if self._reset_required(lateral_active, nav_exit_active):
+      self._clear_lane_change()
+    else:
+      self._advance_lane_change_machine(one_blinker, nav_exit_active, below_speed, lane_change_prob)
+
+    self._update_timer()
+    self.prev_one_blinker = one_blinker and lateral_active
+    self.prev_nav_exit_active = nav_exit_active
+    self.alc.update_state()
+    self._pick_desire_output()

iqpilot/selfdrive/controls/lib/drive_helpers.py

@@ -0,0 +1,80 @@
+import numpy as np
+from iqpilot.common.constants import ACCELERATION_DUE_TO_GRAVITY
+from iqpilot.common.realtime import DT_CTRL, DT_MDL
+
+MIN_SPEED = 1.0
+CONTROL_N = 17
+CAR_ROTATION_RADIUS = 0.0
+# This is a turn radius smaller than most cars can achieve
+MAX_CURVATURE = 0.4
+MAX_VEL_ERR = 5.0  # m/s
+
+MAX_LATERAL_JERK = 5.0  # m/s^3
+MAX_LATERAL_ACCEL_NO_ROLL = 3.0  # m/s^2
+MAX_LATERAL_ACCEL_NO_ROLL_OVERRIDE = 5.0  # m/s^2
+DEFAULT_STOPPING_SPEED = 0.25  # m/s
+
+
+def should_stop(v_ego: float, a_target: float, stopping_speed: float = DEFAULT_STOPPING_SPEED) -> bool:
+  return bool(v_ego < stopping_speed and a_target < 0.1)
+
+
+def clamp(val, min_val, max_val):
+  clamped_val = float(np.clip(val, min_val, max_val))
+  return clamped_val, clamped_val != val
+
+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
+
+# "Model smoothing": when the policy's own predicted uncertainty (plan_stds) for the
+# 1s-ahead lateral position spikes, temporarily lengthen the desiredCurvature smoothing
+# time constant so a noisy/uncertain model output doesn't jerk the wheel.
+MODEL_SMOOTHING_STD_LOW = 0.15  # m, plan y_std at 1s below which no extra smoothing is added
+MODEL_SMOOTHING_STD_HIGH = 0.25  # m, plan y_std at 1s at/above which the full max_extra_seconds is added
+MODEL_SMOOTHING_MAX_TOTAL_SEC = 0.60  # hard ceiling on base + dynamic lat smoothing seconds
+
+def dynamic_lat_smooth_extra_seconds(y_std_1s: float, max_extra_seconds: float) -> float:
+  if max_extra_seconds <= 0.0:
+    return 0.0
+  return float(np.interp(y_std_1s, [MODEL_SMOOTHING_STD_LOW, MODEL_SMOOTHING_STD_HIGH], [0.0, max_extra_seconds]))
+
+def clip_curvature(v_ego, prev_curvature, new_curvature, roll, override=False) -> tuple[float, bool]:
+  # This function respects ISO lateral jerk and acceleration limits + a max curvature
+  v_ego = max(v_ego, MIN_SPEED)
+  max_curvature_rate = MAX_LATERAL_JERK / (v_ego ** 2)  # inexact calculation, check https://github.com/commaai/openpilot/pull/24755
+  new_curvature = np.clip(new_curvature,
+                          prev_curvature - max_curvature_rate * DT_CTRL,
+                          prev_curvature + max_curvature_rate * DT_CTRL)
+
+  max_lat_accel_no_roll = MAX_LATERAL_ACCEL_NO_ROLL_OVERRIDE if override else MAX_LATERAL_ACCEL_NO_ROLL
+  roll_compensation = roll * ACCELERATION_DUE_TO_GRAVITY
+  max_lat_accel = max_lat_accel_no_roll + roll_compensation
+  min_lat_accel = -max_lat_accel_no_roll + roll_compensation
+  new_curvature, limited_accel = clamp(new_curvature, min_lat_accel / v_ego ** 2, max_lat_accel / v_ego ** 2)
+
+  new_curvature, limited_max_curv = clamp(new_curvature, -MAX_CURVATURE, MAX_CURVATURE)
+  return float(new_curvature), limited_accel or limited_max_curv
+
+
+def get_accel_from_plan(speeds, accels, t_idxs, action_t=DT_MDL, stopping_speed=DEFAULT_STOPPING_SPEED):
+  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
+  else:
+    v_now = 0.0
+    v_target = 0.0
+    a_target = 0.0
+  return a_target, should_stop(v_now, a_target, stopping_speed)
+
+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)

iqpilot/selfdrive/controls/lib/helpers/blinker_pause.py

@@ -1,10 +1,10 @@
 """
 Copyright © IQ.Lvbs, apart of Project Teal Lvbs, All Rights Reserved, licensed under https://konn3kt.com/tos
 """
-from cereal import car
+from iqpilot.cereal import car
 
-from openpilot.common.constants import CV
-from openpilot.common.params import Params
+from iqpilot.common.constants import CV
+from iqpilot.common.params import Params
 
 
 class SignalPauseEngine:

iqpilot/selfdrive/controls/lib/helpers/curvature.py

@@ -1,48 +0,0 @@
-"""
-Copyright © IQ.Lvbs, apart of Project Teal Lvbs, All Rights Reserved, licensed under https://konn3kt.com/tos/
-"""
-
-from __future__ import annotations
-
-from numpy import clip, interp
-
-from openpilot.common.realtime import DT_MDL
-from openpilot.iqpilot.selfdrive.iqmodeld.config import ModelConstants
-from openpilot.selfdrive.controls.lib.drive_helpers import CONTROL_N, MAX_LATERAL_JERK, MIN_SPEED
-
-
-def _sanitize_plan(headings, curvatures):
-  valid_shape = len(headings) == CONTROL_N and len(curvatures) >= CONTROL_N
-  if valid_shape:
-    return headings, curvatures
-  placeholder = [0.0] * CONTROL_N
-  return placeholder, placeholder
-
-
-def _project_future_heading(delay_s: float, headings) -> float:
-  return float(interp(delay_s, ModelConstants.T_IDXS[:CONTROL_N], headings))
-
-
-def _convert_heading_to_curvature(projected_heading: float, speed_mps: float, current_curvature: float, delay_s: float) -> float:
-  turning_arc = projected_heading / (speed_mps * delay_s)
-  return (2.0 * turning_arc) - current_curvature
-
-
-def _limit_curvature_rate(target_curvature: float, current_curvature: float, speed_mps: float) -> float:
-  curvature_step = MAX_LATERAL_JERK / (speed_mps ** 2)
-  lower = current_curvature - (curvature_step * DT_MDL)
-  upper = current_curvature + (curvature_step * DT_MDL)
-  return float(clip(target_curvature, lower, upper))
-
-
-def solve_lag_curvature(steer_delay, v_ego, psis, curvatures):
-  headings, curvature_track = _sanitize_plan(psis, curvatures)
-  speed_mps = max(MIN_SPEED, v_ego)
-  delay_s = max(float(steer_delay), 1e-3)
-  current_curvature = float(curvature_track[0])
-  projected_heading = _project_future_heading(delay_s, headings)
-  target_curvature = _convert_heading_to_curvature(projected_heading, speed_mps, current_curvature, delay_s)
-  return _limit_curvature_rate(target_curvature, current_curvature, speed_mps)
-
-
-get_lag_adjusted_curvature = solve_lag_curvature

iqpilot/selfdrive/controls/lib/helpers/e2e_alerts.py

@@ -2,11 +2,11 @@
 Copyright © IQ.Lvbs, apart of Project Teal Lvbs, All Rights Reserved, licensed under https://konn3kt.com/tos
 """
 
-from cereal import messaging, custom
+from iqpilot.cereal import messaging, custom
 
-from openpilot.common.params import Params
-from openpilot.common.realtime import DT_MDL
-from openpilot.iqpilot.selfdrive.selfdrived.events import IQEvents
+from iqpilot.common.params import Params
+from iqpilot.common.realtime import DT_MDL
+from iqpilot.selfdrive.selfdrived.iq_events import IQEvents
 
 PARAM_PATH = "EndToEndAlert"
 PARAM_LEAD = "EndToEndLeadAlert"

iqpilot/selfdrive/controls/lib/helpers/lane_change.py

@@ -1,10 +1,10 @@
 """
 Copyright © IQ.Lvbs, apart of Project Teal Lvbs, All Rights Reserved, licensed under https://konn3kt.com/tos
 """
-from cereal import custom, log
+from iqpilot.cereal import custom, log
 
-from openpilot.common.params import Params
-from openpilot.common.realtime import DT_MDL
+from iqpilot.common.params import Params
+from iqpilot.common.realtime import DT_MDL
 
 NAV_EXIT_COMMIT_DISTANCE = 500.0  # m before a route exit to begin moving into the exit lane
 _ManeuverType = custom.IQNavState.ManeuverType

iqpilot/selfdrive/controls/lib/helpers/lane_turn.py

@@ -6,10 +6,10 @@ from __future__ import annotations
 
 from dataclasses import dataclass
 
-from cereal import custom
+from iqpilot.cereal import custom
 
-from openpilot.common.constants import CV
-from openpilot.common.params import Params
+from iqpilot.common.constants import CV
+from iqpilot.common.params import Params
 
 TurnDirection = custom.IQTurnSignalDirection
 

iqpilot/selfdrive/controls/lib/helpers/lateral_edge_guard.py

@@ -0,0 +1,182 @@
+"""
+Lateral Edge Guard uses the model's lateral road-edge geometry to withhold lane
+changes that lack room for a target lane. The model standard deviation remains
+in metres: measurements above the validity limit are rejected, while valid
+measurements use a two-sigma lower confidence bound for conservative clearance.
+Unavailable geometry briefly holds the last output, then fails open because a
+model dropout is not geometric evidence of a nearby edge.
+"""
+from __future__ import annotations
+
+import math
+from dataclasses import dataclass
+from enum import IntEnum
+from typing import Any
+
+from iqpilot.cereal import custom, log
+from iqpilot.common.constants import CV
+from iqpilot.common.swaglog import cloudlog
+
+
+MIN_ACTIVE_SPEED_MPS = 20.0 * CV.MPH_TO_MS  # Matches the lane-change speed gate and excludes parking manoeuvres.
+MAX_VALID_ROAD_EDGE_STD_M = 1.0  # A 2-sigma bound beyond 2 m cannot distinguish an adjacent 3.5 m lane reliably.
+EDGE_CONFIDENCE_SIGMA = 2.0  # 97.7% one-sided confidence under the model's Gaussian uncertainty assumption.
+ROAD_EDGE_LOOKAHEAD_MIN_M = 5.0  # Ignore near-field edge points dominated by vehicle-body perspective.
+ROAD_EDGE_LOOKAHEAD_MAX_M = 40.0  # Covers about 2 s at the 20 m/s model-training reference speed.
+LANE_CENTER_OFFSET_M = 3.5  # Typical freeway lane width and the target-centre lateral displacement.
+# CarParams exposes neither width nor track; 0.95 m is half of an assumed conservative 1.90 m body width.
+VEHICLE_LATERAL_HALF_WIDTH_M = 1.90 / 2.0
+EDGE_CLEARANCE_MARGIN_M = 0.25  # Additional lateral separation between the vehicle body and detected road edge.
+REQUIRED_ROAD_EDGE_DISTANCE_M = LANE_CENTER_OFFSET_M + VEHICLE_LATERAL_HALF_WIDTH_M + EDGE_CLEARANCE_MARGIN_M
+BLOCK_DEBOUNCE_S = 0.30  # Six model frames reject a transient close-edge prediction before blocking.
+CLEAR_DEBOUNCE_S = 0.50  # Ten model frames make release slower than assertion for conservative hysteresis.
+UNAVAILABLE_HOLD_S = 0.50  # Ten model frames bridge a short model-data dropout before failing open.
+TIMER_EPSILON_S = 1e-9  # Floating-point comparison tolerance, far below one model tick.
+
+LaneChangeDirection = log.LaneChangeDirection
+LateralEdgeBlock = custom.IQLateralEdgeBlock
+
+
+class RoadEdgeDataState(IntEnum):
+  VALID = 0
+  UNAVAILABLE = 1
+  INVALID = 2
+
+
+@dataclass(frozen=True, slots=True)
+class RoadEdgeMeasurement:
+  state: RoadEdgeDataState
+  lateral_distance_m: float | None = None
+  conservative_distance_m: float | None = None
+  should_block: bool | None = None
+
+
+@dataclass(frozen=True, slots=True)
+class _SideState:
+  blocked: bool = False
+  block_timer_s: float = 0.0
+  clear_timer_s: float = 0.0
+  unavailable_timer_s: float = 0.0
+  fallback_reported: bool = False
+
+
+def evaluate_road_edge(edge: Any, std_m: Any, direction: int) -> RoadEdgeMeasurement:
+  if edge is None or std_m is None:
+    return RoadEdgeMeasurement(RoadEdgeDataState.UNAVAILABLE)
+
+  try:
+    xs = edge.x
+    ys = edge.y
+    count = len(xs)
+    y_count = len(ys)
+  except (AttributeError, TypeError):
+    return RoadEdgeMeasurement(RoadEdgeDataState.UNAVAILABLE)
+
+  if count == 0 or y_count != count:
+    return RoadEdgeMeasurement(RoadEdgeDataState.UNAVAILABLE)
+
+  try:
+    std = float(std_m)
+  except (TypeError, ValueError):
+    return RoadEdgeMeasurement(RoadEdgeDataState.INVALID)
+  if not math.isfinite(std) or std < 0.0 or std > MAX_VALID_ROAD_EDGE_STD_M:
+    return RoadEdgeMeasurement(RoadEdgeDataState.INVALID)
+
+  lateral_distance_m: float | None = None
+  for idx in range(count):
+    try:
+      x_m = float(xs[idx])
+      y_m = float(ys[idx])
+    except (IndexError, TypeError, ValueError):
+      return RoadEdgeMeasurement(RoadEdgeDataState.UNAVAILABLE)
+    if not math.isfinite(x_m) or not math.isfinite(y_m):
+      return RoadEdgeMeasurement(RoadEdgeDataState.UNAVAILABLE)
+    if not ROAD_EDGE_LOOKAHEAD_MIN_M <= x_m <= ROAD_EDGE_LOOKAHEAD_MAX_M:
+      continue
+    if ((direction == LaneChangeDirection.left and y_m >= 0.0) or
+        (direction == LaneChangeDirection.right and y_m <= 0.0)):
+      return RoadEdgeMeasurement(RoadEdgeDataState.INVALID)
+    distance_m = abs(y_m)
+    lateral_distance_m = distance_m if lateral_distance_m is None else min(lateral_distance_m, distance_m)
+
+  if lateral_distance_m is None:
+    return RoadEdgeMeasurement(RoadEdgeDataState.UNAVAILABLE)
+
+  conservative_distance_m = lateral_distance_m - EDGE_CONFIDENCE_SIGMA * std
+  return RoadEdgeMeasurement(
+    RoadEdgeDataState.VALID,
+    lateral_distance_m,
+    conservative_distance_m,
+    conservative_distance_m < REQUIRED_ROAD_EDGE_DISTANCE_M,
+  )
+
+
+def step_side_guard(state: _SideState, measurement: RoadEdgeMeasurement, speed_active: bool,
+                    dt_s: float) -> tuple[_SideState, bool]:
+  if not speed_active:
+    return _SideState(), False
+
+  if measurement.state == RoadEdgeDataState.UNAVAILABLE:
+    unavailable_timer_s = state.unavailable_timer_s + dt_s
+    if unavailable_timer_s < UNAVAILABLE_HOLD_S - TIMER_EPSILON_S:
+      return _SideState(state.blocked, unavailable_timer_s=unavailable_timer_s,
+                        fallback_reported=state.fallback_reported), False
+    fallback_started = not state.fallback_reported
+    return _SideState(unavailable_timer_s=unavailable_timer_s, fallback_reported=True), fallback_started
+
+  should_block = bool(measurement.should_block) if measurement.state == RoadEdgeDataState.VALID else False
+  if should_block == state.blocked:
+    return _SideState(blocked=state.blocked), False
+
+  if should_block:
+    block_timer_s = state.block_timer_s + dt_s
+    if block_timer_s >= BLOCK_DEBOUNCE_S - TIMER_EPSILON_S:
+      return _SideState(blocked=True), False
+    return _SideState(block_timer_s=block_timer_s), False
+
+  clear_timer_s = state.clear_timer_s + dt_s
+  if clear_timer_s >= CLEAR_DEBOUNCE_S - TIMER_EPSILON_S:
+    return _SideState(), False
+  return _SideState(blocked=True, clear_timer_s=clear_timer_s), False
+
+
+class LateralEdgeGuard:
+  def __init__(self) -> None:
+    self._left = _SideState()
+    self._right = _SideState()
+    self.left_measurement = RoadEdgeMeasurement(RoadEdgeDataState.UNAVAILABLE)
+    self.right_measurement = RoadEdgeMeasurement(RoadEdgeDataState.UNAVAILABLE)
+
+  @staticmethod
+  def _model_side(modeldata: Any, side_index: int) -> tuple[Any | None, Any | None]:
+    if modeldata is None:
+      return None, None
+    try:
+      edges = modeldata.roadEdges
+      stds = modeldata.roadEdgeStds
+      if len(edges) <= side_index or len(stds) <= side_index:
+        return None, None
+      return edges[side_index], stds[side_index]
+    except (AttributeError, TypeError):
+      return None, None
+
+  def update(self, modeldata: Any, v_ego_mps: float, dt_s: float) -> None:
+    dt = max(float(dt_s), 0.0)
+    left_edge, left_std = self._model_side(modeldata, 0)
+    right_edge, right_std = self._model_side(modeldata, 1)
+    self.left_measurement = evaluate_road_edge(left_edge, left_std, LaneChangeDirection.left)
+    self.right_measurement = evaluate_road_edge(right_edge, right_std, LaneChangeDirection.right)
+    speed_active = math.isfinite(v_ego_mps) and v_ego_mps >= MIN_ACTIVE_SPEED_MPS
+    self._left, left_fallback = step_side_guard(self._left, self.left_measurement, speed_active, dt)
+    self._right, right_fallback = step_side_guard(self._right, self.right_measurement, speed_active, dt)
+    if left_fallback:
+      cloudlog.warning(f"lateral edge guard: left road edge unavailable for {UNAVAILABLE_HOLD_S:.2f} s; falling back to not blocking")
+    if right_fallback:
+      cloudlog.warning(f"lateral edge guard: right road edge unavailable for {UNAVAILABLE_HOLD_S:.2f} s; falling back to not blocking")
+
+  def block_for_direction(self, direction: int) -> custom.IQLateralEdgeBlock:
+    if direction == LaneChangeDirection.left and self._left.blocked:
+      return LateralEdgeBlock.left
+    if direction == LaneChangeDirection.right and self._right.blocked:
+      return LateralEdgeBlock.right
+    return LateralEdgeBlock.none

iqpilot/selfdrive/controls/lib/helpers/nav_torque_pulse.py

@@ -6,8 +6,8 @@ turns and highway exits. This is a lateral-control add-on driven by iqNavState;
 it is independent of the feed-forward model and is off by default.
 """
 import numpy as np
-import cereal.messaging as messaging
-from cereal import custom
+import iqpilot.cereal.messaging as messaging
+from iqpilot.cereal import custom
 
 TURN_NUDGE_TORQUE = 0.8
 EXIT_NUDGE_TORQUE = 0.6

iqpilot/selfdrive/controls/lib/helpers/tests/test_e2e_alerts.py

@@ -5,10 +5,10 @@ from types import SimpleNamespace
 
 import pytest
 
-import cereal.messaging as messaging
-from cereal import custom
-from openpilot.common.realtime import DT_MDL
-from openpilot.iqpilot.selfdrive.controls.lib.helpers.e2e_alerts import (
+import iqpilot.cereal.messaging as messaging
+from iqpilot.cereal import custom
+from iqpilot.common.realtime import DT_MDL
+from iqpilot.selfdrive.controls.lib.helpers.e2e_alerts import (
   EndToEndAlertEngine, CONFIRM_S, SETTLE_S, HORIZON_TAIL, PATH_SPEED_MPS, LEAD_SPEED_MPS, LEAD_GAP_M)
 
 E2E_CHIME = custom.IQOnroadEvent.EventName.e2eChime

iqpilot/selfdrive/controls/lib/helpers/tests/test_nav_torque_pulse.py

@@ -6,9 +6,9 @@ from types import SimpleNamespace
 import numpy as np
 import pytest
 
-from cereal import custom
-import openpilot.iqpilot.selfdrive.controls.lib.helpers.nav_torque_pulse as nav_pulse
-from openpilot.iqpilot.selfdrive.controls.lib.helpers.nav_torque_pulse import (
+from iqpilot.cereal import custom
+import iqpilot.selfdrive.controls.lib.helpers.nav_torque_pulse as nav_pulse
+from iqpilot.selfdrive.controls.lib.helpers.nav_torque_pulse import (
   NavTorquePulseBrain, TURN_PULSE_FRAMES, EXIT_PULSE_FRAMES)
 
 

iqpilot/selfdrive/controls/lib/iq_dynamic/engine.py

@@ -1,12 +1,12 @@
 """
 Copyright © IQ.Lvbs, apart of Project Teal Lvbs, All Rights Reserved, licensed under https://konn3kt.com/tos
 """
-from cereal import messaging
+from iqpilot.cereal import messaging
 from numpy import interp
 from iqdbc.car import structs
-from openpilot.common.params import Params
-from openpilot.common.realtime import DT_MDL
-from openpilot.iqpilot.selfdrive.controls.lib.iq_dynamic.imahelper import (
+from iqpilot.common.params import Params
+from iqpilot.common.realtime import DT_MDL
+from iqpilot.selfdrive.controls.lib.iq_dynamic.imahelper import (
   IQConstants,
   IQFilterEngine,
   IQModeEngine,

iqpilot/selfdrive/controls/lib/iq_dynamic/radar_manager.py

@@ -17,7 +17,7 @@ Intent produced:
   radarSetSpeedKph  OP set speed to sync the radar's ACA_V_Wunsch toward
   radarGapBars      OP follow-distance bars to mirror to the radar
 """
-from openpilot.common.constants import CV
+from iqpilot.common.constants import CV
 
 CANCEL_CEIL_MS = 1.0 * CV.KPH_TO_MS  # cancel the radar at/below 1 kph (it can still see speed -> would fault)
 

iqpilot/selfdrive/controls/lib/iq_longitudinal_planner.py

@@ -0,0 +1,256 @@
+"""
+Copyright © IQ.Lvbs, apart of Project Teal Lvbs, All Rights Reserved, licensed under https://konn3kt.com/tos
+"""
+from datetime import datetime
+
+import numpy as np
+
+from iqpilot.cereal import messaging, custom
+from iqdbc.car import structs
+from iqpilot.common.constants import CV
+from iqpilot.common.realtime import DT_MDL
+from iqpilot.selfdrive.car.cruise import V_CRUISE_MAX
+from iqpilot.selfdrive.controls.lib.custom_stop_distance import CustomStopDistance
+from iqpilot.selfdrive.controls.lib.iq_dynamic.engine import IQDynamicController
+from iqpilot.selfdrive.controls.lib.iq_dynamic.imahelper import IQConstants
+from iqpilot.selfdrive.controls.lib.helpers.e2e_alerts import EndToEndAlertEngine
+from iqpilot.selfdrive.controls.lib.slc_vcruise import SLCVCruise
+from iqpilot.selfdrive.controls.lib.speed_limit_controller import LIMIT_ADAPT_ACC
+from iqpilot.selfdrive.selfdrived.iq_events import IQEvents
+from iqpilot.selfdrive.iqmodeld.models.helpers import get_active_bundle
+
+IQDynamicState = custom.IQPlan.IQDynamicControl.IQDynamicControlState
+LongitudinalPlanSource = custom.IQPlan.LongitudinalPlanSource
+SpeedLimitAssistState = custom.IQPlan.SpeedLimit.AssistState
+SpeedLimitSource = custom.IQPlan.SpeedLimit.Source
+NavProvider = custom.IQNavState.LongitudinalProvider
+NavLongitudinalState = custom.IQNavState.LongitudinalState
+
+class LongitudinalPlannerIQ:
+  def __init__(self, CP: structs.CarParams, CP_IQ: structs.IQCarParams, mpc):
+    self.events_iq = IQEvents()
+    self.iq_dynamic = IQDynamicController(CP, mpc)
+    self.custom_stop_distance = CustomStopDistance()
+    self.slimit = SLCVCruise()
+    self.generation = int(model_bundle.generation) if (model_bundle := get_active_bundle()) else None
+    self.source = LongitudinalPlanSource.cruise
+    self.e2e_alerts = EndToEndAlertEngine()
+    self.output_v_target = 0.
+    self.output_a_target = 0.
+    self.speed_limit_last = 0.
+    self.speed_limit_final_last = 0.
+    self.speed_limit_source = SpeedLimitSource.none
+    self.nav_engaged = False
+    self.nav_provider = NavProvider.none
+    self.nav_state = NavLongitudinalState.disabled
+    self.nav_speed_target = 0.
+    self.nav_accel_target = 0.
+    self.nav_valid = False
+    self.force_stop_timer = 0.0
+    self.forcing_stop = False
+    self.override_force_stop = False
+    self.override_force_stop_timer = 0.0
+    self.tracked_model_length = 0.0
+
+  def is_e2e(self, sm: messaging.SubMaster) -> bool:
+    experimental_mode = sm['selfdriveState'].experimentalMode
+    if not self.iq_dynamic.active():
+      return experimental_mode
+
+    return experimental_mode and self.iq_dynamic.mode() == "blended"
+
+  def update_targets(self, sm: messaging.SubMaster, v_ego: float, v_cruise: float) -> float:
+    CS = sm['carState']
+    v_cruise_cluster_kph = min(CS.vCruiseCluster, V_CRUISE_MAX)
+    v_cruise_cluster = v_cruise_cluster_kph * CV.KPH_TO_MS
+    # SLC should apply whenever IQ.Pilot is engaged, even on stock-longitudinal cars
+    # where carControl.longActive stays false.
+    slc_apply_enabled = bool(getattr(sm['selfdriveState'], "enabled", False))
+
+    nav_state = sm['iqNavState']
+    self.nav_engaged = bool(getattr(nav_state, "longitudinalEngaged", False))
+    self.nav_provider = getattr(nav_state, "longitudinalProvider", NavProvider.none)
+    self.nav_state = getattr(nav_state, "longitudinalState", NavLongitudinalState.disabled)
+    self.nav_speed_target = float(getattr(nav_state, "speedTarget", 0.0))
+    self.nav_accel_target = float(getattr(nav_state, "accelTarget", 0.0))
+    self.nav_valid = bool(getattr(nav_state, "valid", False) and self.nav_engaged)
+
+    # IQ.Pilot custom Speed Limit Controller
+    now = datetime.now()
+    if hasattr(sm, "alive"):
+      time_validated = sm.alive.get('clocks', False) and getattr(sm['clocks'], 'timeValid', False)
+    else:
+      clocks = sm.get('clocks', None) if isinstance(sm, dict) else None
+      time_validated = bool(getattr(clocks, 'timeValid', False))
+    slc_v_cruise = self.slimit.update(slc_apply_enabled, now, time_validated, v_cruise, v_ego, sm)
+    self.iq_dynamic.set_slc_experimental_mode(self.slimit.slc_experimental_mode)
+    self.iq_dynamic.update(sm)
+    # Prefer confirmed controller output for UI/planner rendering.
+    # Fall back to active (policy-resolved) target/source when confirmed is unavailable.
+    display_speed_limit = self.slimit.slc_target if self.slimit.slc_target > 0 else self.slimit.slc_active_target
+    display_source = self.slimit.slc_source if self.slimit.slc_source != "None" else self.slimit.slc_active_source
+
+    if display_speed_limit > 0:
+      self.speed_limit_last = display_speed_limit
+      self.speed_limit_final_last = display_speed_limit + self.slimit.slc_offset
+    elif display_source == "None":
+      self.speed_limit_last = 0.0
+      self.speed_limit_final_last = 0.0
+    # Respect user-defined max cruise speed when applying SLC.
+    if v_cruise_cluster > 0 and self.speed_limit_final_last > 0:
+      self.speed_limit_final_last = min(self.speed_limit_final_last, v_cruise_cluster)
+    source_map = {
+      "Dashboard": SpeedLimitSource.car,
+      "Map Data": SpeedLimitSource.map,
+      "Mapbox": SpeedLimitSource.map,
+      "None": SpeedLimitSource.none,
+    }
+    self.speed_limit_source = source_map.get(display_source, SpeedLimitSource.none)
+
+    targets = {
+      LongitudinalPlanSource.cruise: v_cruise,
+      LongitudinalPlanSource.speedLimitAssist: slc_v_cruise,
+    }
+    if self.nav_valid:
+      targets[LongitudinalPlanSource.nav] = self.nav_speed_target
+
+    self.source = min(targets, key=lambda k: targets[k])
+    self.output_v_target = targets[self.source]
+    self.output_v_target = self._apply_force_stop(self.output_v_target, v_ego, sm, slc_apply_enabled)
+    # envelope shaping only in Assist mode: info/warn must never change the plan
+    self._envelope_enabled = (slc_apply_enabled and bool(getattr(self.slimit, "controller_enabled", False))
+                              and bool(getattr(self.slimit, "mode_assist", False)))
+    return self.output_v_target
+
+  def cruise_envelope(self, v_target: float, v_ego: float, t_idxs) -> np.ndarray:
+    """Per-timestep cruise speed over the MPC horizon: the scalar target, shaped down
+    ahead of an upcoming lower speed limit so the solver decelerates before the sign
+    instead of at it."""
+    env = np.full(len(t_idxs), max(float(v_target), 0.0))
+    if not getattr(self, "_envelope_enabled", False):
+      return env
+    slc = getattr(self.slimit, "slc", None)
+    next_limit = float(getattr(slc, "next_speed_limit", 0.0) or 0.0)
+    next_dist = float(getattr(slc, "next_speed_distance", 0.0) or 0.0)
+    if next_limit <= 0.0 or next_dist <= 0.0:
+      return env
+    next_target = max(next_limit + float(getattr(self.slimit, "slc_offset", 0.0) or 0.0), 0.0)
+    if next_target >= env[0]:
+      return env
+    travel = np.maximum(v_ego, 1.0) * np.asarray(t_idxs)
+    v_allowed = np.sqrt(np.maximum(next_target ** 2 + 2.0 * abs(LIMIT_ADAPT_ACC) * (next_dist - travel), next_target ** 2))
+    return np.minimum(env, v_allowed)
+
+  def update(self, sm: messaging.SubMaster) -> None:
+    self.events_iq.clear()
+    for event_name in getattr(self.slimit, 'pending_events', []):
+      self.events_iq.add(event_name)
+    self.custom_stop_distance.update()
+    self.e2e_alerts.update(sm, self.events_iq)
+    if bool(getattr(sm["iqCarState"], "alcOverrideAlert", False)):
+      self.events_iq.add(custom.IQOnroadEvent.EventName.steeringOverrideReengageAlc)
+
+  def apply_e2e_stop_distance(self, sm: messaging.SubMaster, v_ego: float, a_target: float, should_stop: bool) -> tuple[float, bool]:
+    if not self.is_e2e(sm):
+      return a_target, should_stop
+    return self.custom_stop_distance.adjust_e2e_stop(a_target, should_stop, v_ego, sm['modelV2'])
+
+  def _apply_force_stop(self, v_target: float, v_ego: float, sm: messaging.SubMaster, apply_enabled: bool) -> float:
+    force_stop = self.iq_dynamic.force_stop_requested() and apply_enabled and self.override_force_stop_timer <= 0.0
+    self.force_stop_timer = self.force_stop_timer + DT_MDL if force_stop else 0.0
+    force_stop_enabled = self.force_stop_timer >= 1.0
+    force_stop_ramp_time = max(float(getattr(self.iq_dynamic, "model_stop_time", IQConstants.FORCE_STOP_PLANNER_TIME)), DT_MDL)
+
+    accel_pressed = bool(getattr(sm["iqCarState"], "accelPressed", False))
+    self.override_force_stop |= sm["carState"].gasPressed or accel_pressed
+    self.override_force_stop &= force_stop_enabled
+
+    if self.override_force_stop:
+      self.override_force_stop_timer = 10.0
+    elif self.override_force_stop_timer > 0.0:
+      self.override_force_stop_timer = max(0.0, self.override_force_stop_timer - DT_MDL)
+    else:
+      self.override_force_stop = False
+
+    if force_stop_enabled and not self.override_force_stop:
+      self.forcing_stop = True
+      self.tracked_model_length = max(self.tracked_model_length - (v_ego * DT_MDL), 0.0)
+      if sm["carState"].standstill:
+        return 0.0
+      return min(self.tracked_model_length / force_stop_ramp_time, v_target)
+
+    self.forcing_stop = False
+    self.tracked_model_length = max(
+      float(getattr(self.iq_dynamic, "model_length", 0.0)),
+      float(getattr(self.iq_dynamic, "minimum_force_stop_length", 0.0)),
+      0.0,
+    )
+    return v_target
+
+  def publish_longitudinal_plan_iq(self, sm: messaging.SubMaster, pm: messaging.PubMaster) -> None:
+    def fill_plan(plan_msg) -> None:
+      plan_msg.longitudinalPlanSource = self.source
+      plan_msg.vTarget = float(self.output_v_target)
+      plan_msg.aTarget = float(self.output_a_target)
+      plan_msg.events = self.events_iq.to_msg()
+
+      # IQ.Dynamic control state
+      iq_dynamic = plan_msg.iqDynamic
+      iq_dynamic.state = IQDynamicState.blended if self.iq_dynamic.mode() == 'blended' else IQDynamicState.acc
+      iq_dynamic.enabled = self.iq_dynamic.enabled()
+      iq_dynamic.active = self.iq_dynamic.active()
+
+      nav_summary = plan_msg.iqNavState.nav
+      nav_summary.engaged = self.nav_engaged
+      nav_summary.provider = self.nav_provider
+      nav_summary.state = self.nav_state
+      nav_summary.speedTarget = float(self.nav_speed_target)
+      nav_summary.accelTarget = float(self.nav_accel_target)
+      nav_summary.valid = self.nav_valid
+
+      # Speed Limit
+      speedLimit = plan_msg.speedLimit
+      resolver = speedLimit.resolver
+      speed_limit = float(self.slimit.slc_target if self.slimit.slc_target > 0 else self.slimit.slc_active_target)
+      speed_limit_offset = float(self.slimit.slc_offset)
+      speed_limit_final = speed_limit + speed_limit_offset if speed_limit > 0 else 0.
+      speed_limit_valid = speed_limit > 0.
+      speed_limit_last_valid = self.speed_limit_last > 0.
+
+      resolver.speedLimit = speed_limit
+      resolver.speedLimitLast = float(self.speed_limit_last)
+      resolver.speedLimitFinal = float(speed_limit_final)
+      resolver.speedLimitFinalLast = float(self.speed_limit_final_last)
+      resolver.speedLimitValid = speed_limit_valid
+      resolver.speedLimitLastValid = speed_limit_last_valid
+      resolver.speedLimitOffset = speed_limit_offset
+      resolver.distToSpeedLimit = 0.
+      resolver.source = self.speed_limit_source
+
+      assist = speedLimit.assist
+      slc_assist_state = self.slimit.assist_state
+      assist.enabled = bool(self.slimit.slc_target > 0 or self.slimit.slc_unconfirmed > 0)
+      assist.active = self.source == LongitudinalPlanSource.speedLimitAssist and self.slimit.slc_target > 0
+      if slc_assist_state is not None:
+        assist.state = slc_assist_state
+      elif not assist.enabled:
+        assist.state = SpeedLimitAssistState.disabled
+      elif self.slimit.slc_unconfirmed > 0:
+        assist.state = SpeedLimitAssistState.preActive
+      elif assist.active:
+        assist.state = SpeedLimitAssistState.active
+      else:
+        assist.state = SpeedLimitAssistState.inactive
+      assist.vTarget = float(self.output_v_target if assist.active else 255.)
+      assist.aTarget = float(self.slimit.slc_a_target if assist.active else 0.)
+
+      e2eAlerts = plan_msg.e2eAlerts
+      e2eAlerts.pathOpen = self.e2e_alerts.path_alert
+      e2eAlerts.leadPullaway = self.e2e_alerts.lead_alert
+
+    valid = sm.all_checks(service_list=['carState', 'controlsState'])
+
+    plan_iq_send = messaging.new_message('iqPlan')
+    plan_iq_send.valid = valid
+    fill_plan(plan_iq_send.iqPlan)
+    pm.send('iqPlan', plan_iq_send)

iqpilot/selfdrive/controls/lib/latcontrol.py

@@ -0,0 +1,31 @@
+import numpy as np
+from abc import abstractmethod, ABC
+from iqpilot.selfdrive.locationd.helpers import Pose
+
+
+class LatControl(ABC):
+  def __init__(self, CP, CP_IQ, CI, dt):
+    self.dt = dt
+    self.sat_limit = CP.steerLimitTimer
+    self.sat_time = 0.
+    self.sat_check_min_speed = 10.
+
+    # we define the steer torque scale as [-1.0...1.0]
+    self.steer_max = 1.0
+
+  @abstractmethod
+  def update(self, active: bool, CS, VM, params, steer_limited_by_safety: bool, desired_curvature: float, calibrated_pose: Pose,
+             curvature_limited: bool, lat_delay: float):
+    pass
+
+  def reset(self):
+    self.sat_time = 0.
+
+  def _check_saturation(self, saturated, CS, steer_limited_by_safety, curvature_limited):
+    # Saturated only if control output is not being limited by car torque/angle rate limits
+    if (saturated or curvature_limited) and CS.vEgo > self.sat_check_min_speed and not steer_limited_by_safety and not CS.steeringPressed:
+      self.sat_time += self.dt
+    else:
+      self.sat_time -= self.dt
+    self.sat_time = np.clip(self.sat_time, 0.0, self.sat_limit)
+    return self.sat_time > (self.sat_limit - 1e-3)

iqpilot/selfdrive/controls/lib/latcontrol_angle.py

@@ -0,0 +1,52 @@
+import math
+
+from iqpilot.cereal import log
+from iqpilot.common.params import Params
+from iqpilot.selfdrive.controls.lib.latcontrol import LatControl
+from iqpilot.selfdrive.controls.lib.drive_helpers import clip_curvature
+
+# TODO This is speed dependent
+STEER_ANGLE_SATURATION_THRESHOLD = 2.5  # Degrees
+
+
+class LatControlAngle(LatControl):
+  def __init__(self, CP, CP_IQ, CI, dt):
+    super().__init__(CP, CP_IQ, CI, dt)
+    self.sat_check_min_speed = 5.
+    self.use_steer_limited_by_safety = CP.brand == "tesla"
+    self.curvature_lookahead_enabled = Params().get_bool("IQLateralCurvatureLookahead")
+    self.target_curvature_last = 0.0
+
+  def update(self, active, CS, VM, params, steer_limited_by_safety, desired_curvature, calibrated_pose, curvature_limited, lat_delay,
+             lookahead_curvature=None):
+    angle_log = log.ControlsState.LateralAngleState.new_message()
+
+    # the rack has ~70 ms of dead time before the wheel moves (measured on VW MQB), so track the
+    # curvature the path will need after lat_delay rather than the one it needs now. controlsd passes
+    # lookahead_curvature=None in maneuver mode, which keeps the maneuver report measuring raw response.
+    # controlsd only runs clip_curvature on desired_curvature, so the lookahead has to be bounded here
+    # or the ISO jerk/accel limits are bypassed on the way to the rack.
+    target_curvature = desired_curvature
+    if active and self.curvature_lookahead_enabled and lookahead_curvature is not None:
+      target_curvature, _ = clip_curvature(CS.vEgo, self.target_curvature_last, lookahead_curvature, params.roll)
+    self.target_curvature_last = target_curvature
+
+    if not active:
+      angle_log.active = False
+      angle_steers_des = float(CS.steeringAngleDeg)
+    else:
+      angle_log.active = True
+      angle_steers_des = math.degrees(VM.get_steer_from_curvature(-target_curvature, CS.vEgo, params.roll))
+      angle_steers_des += params.angleOffsetDeg
+
+    if self.use_steer_limited_by_safety:
+      # these cars' carcontrollers calculate max lateral accel and jerk, so we can rely on carOutput for saturation
+      angle_control_saturated = steer_limited_by_safety
+    else:
+      # for cars which use a method of limiting torque such as a torque signal (Nissan and Toyota)
+      # or relying on EPS (Ford Q3), carOutput does not capture maxing out torque  # TODO: this can be improved
+      angle_control_saturated = abs(angle_steers_des - CS.steeringAngleDeg) > STEER_ANGLE_SATURATION_THRESHOLD
+    angle_log.saturated = bool(self._check_saturation(angle_control_saturated, CS, False, curvature_limited))
+    angle_log.steeringAngleDeg = float(CS.steeringAngleDeg)
+    angle_log.steeringAngleDesiredDeg = angle_steers_des
+    return 0, float(angle_steers_des), angle_log

iqpilot/selfdrive/controls/lib/latcontrol_pid.py

@@ -0,0 +1,50 @@
+import math
+
+from iqpilot.cereal import log
+from iqpilot.selfdrive.controls.lib.latcontrol import LatControl
+from iqpilot.common.pid import PIDController
+
+
+class LatControlPID(LatControl):
+  def __init__(self, CP, CP_IQ, CI, dt):
+    super().__init__(CP, CP_IQ, CI, dt)
+    self.pid = PIDController((CP.lateralTuning.pid.kpBP, CP.lateralTuning.pid.kpV),
+                             (CP.lateralTuning.pid.kiBP, CP.lateralTuning.pid.kiV),
+                             pos_limit=self.steer_max, neg_limit=-self.steer_max)
+    self.ff_factor = CP.lateralTuning.pid.kf
+    self.get_steer_feedforward = CI.get_steer_feedforward_function()
+
+  def update(self, active, CS, VM, params, steer_limited_by_safety, desired_curvature, calibrated_pose, curvature_limited, lat_delay,
+             lookahead_curvature=None):
+    pid_log = log.ControlsState.LateralPIDState.new_message()
+    pid_log.steeringAngleDeg = float(CS.steeringAngleDeg)
+    pid_log.steeringRateDeg = float(CS.steeringRateDeg)
+
+    angle_steers_des_no_offset = math.degrees(VM.get_steer_from_curvature(-desired_curvature, CS.vEgo, params.roll))
+    angle_steers_des = angle_steers_des_no_offset + params.angleOffsetDeg
+    error = angle_steers_des - CS.steeringAngleDeg
+
+    pid_log.steeringAngleDesiredDeg = angle_steers_des
+    pid_log.angleError = error
+    if not active:
+      output_torque = 0.0
+      pid_log.active = False
+
+    else:
+      # offset does not contribute to resistive torque
+      ff = self.ff_factor * self.get_steer_feedforward(angle_steers_des_no_offset, CS.vEgo)
+      freeze_integrator = steer_limited_by_safety or CS.steeringPressed or CS.vEgo < 5
+
+      output_torque = self.pid.update(error,
+                                feedforward=ff,
+                                speed=CS.vEgo,
+                                freeze_integrator=freeze_integrator)
+
+      pid_log.active = True
+      pid_log.p = float(self.pid.p)
+      pid_log.i = float(self.pid.i)
+      pid_log.f = float(self.pid.f)
+      pid_log.output = float(output_torque)
+      pid_log.saturated = bool(self._check_saturation(self.steer_max - abs(output_torque) < 1e-3, CS, steer_limited_by_safety, curvature_limited))
+
+    return output_torque, angle_steers_des, pid_log

iqpilot/selfdrive/controls/lib/latcontrol_torque.py

@@ -0,0 +1,636 @@
+import json
+import math
+import os
+import tomllib
+from collections import deque
+from difflib import SequenceMatcher
+from importlib.resources import files
+
+import numpy as np
+
+from iqpilot.cereal import log, custom  # noqa: F401  (custom kept available for downstream imports)
+from iqdbc.car import structs
+from iqdbc.car.lateral import FRICTION_THRESHOLD, get_friction
+from iqdbc.lvbs.car.interfaces import LatControlInputs
+from iqdbc.lvbs.car.iq_lateral import get_friction as get_friction_in_torque_space
+from iqpilot.common.basedir import BASEDIR
+from iqpilot.common.constants import ACCELERATION_DUE_TO_GRAVITY
+from iqpilot.common.filter_simple import FirstOrderFilter
+from iqpilot.common.params import Params
+from iqpilot.common.pid import PIDController
+from iqpilot.selfdrive.controls.lib.drive_helpers import CONTROL_N
+from iqpilot.selfdrive.controls.lib.lateral_acceleration_slew_limiter import LateralAccelerationSlewLimiter
+from iqpilot.selfdrive.controls.lib.latcontrol import LatControl
+from iqpilot.selfdrive.iqmodeld.config import ModelConstants
+from iqpilot.selfdrive.iqmodeld.parser import safe_exp
+from iqpilot.selfdrive.controls.lib.helpers.nav_torque_pulse import NavTorquePulseBrain
+
+
+# ===== locator =====
+
+TORQUE_NN_MODEL_PATH = os.path.join(BASEDIR, "iqpilot", "iqpilot_iq_nnff_models", "neural_network_lateral_control")
+TORQUE_NN_MODEL_SUBSTITUTE_PATH = files("iqdbc").joinpath("car", "torque_data", "substitute.toml")
+MOCK_MODEL_PATH = os.path.join(TORQUE_NN_MODEL_PATH, "MOCK.json")
+
+# A candidate must reach this score for the fingerprint(+fw) match to count as exact.
+_EXACT_THRESHOLD = 0.99
+# Below this, we fall back to the next candidate ladder rung.
+_ACCEPT_THRESHOLD = 0.9
+
+
+def _score(a: str, b: str) -> float:
+  return SequenceMatcher(None, a, b).ratio()
+
+
+def _best_model_for(candidate: str) -> tuple[str | None, float]:
+  """Highest-scoring model file for a candidate string; (path, score)."""
+  best_path, best_score = None, -1.0
+  if not os.path.isdir(TORQUE_NN_MODEL_PATH):
+    return best_path, best_score
+  for entry in os.listdir(TORQUE_NN_MODEL_PATH):
+    if not entry.endswith(".json"):
+      continue
+    score = _score(os.path.splitext(entry)[0], candidate)
+    if score > best_score:
+      best_path, best_score = os.path.join(TORQUE_NN_MODEL_PATH, entry), score
+  return best_path, best_score
+
+
+def _substitute_for(fingerprint: str) -> str:
+  with open(TORQUE_NN_MODEL_SUBSTITUTE_PATH, 'rb') as f:
+    table = tomllib.load(f)
+  return table.get(fingerprint, fingerprint)
+
+
+def _eps_suffix(CP: structs.CarParams) -> str:
+  eps_fw = str(next((fw.fwVersion for fw in CP.carFw if fw.ecu == "eps"), ""))
+  return eps_fw.replace("\\", "") if len(eps_fw) > 3 else ""
+
+
+def get_nn_model_path(CP: structs.CarParams) -> tuple[str, str, bool]:
+  """Pick the closest NNFF model for this car.
+
+  Angle-steered cars always get MOCK. Otherwise walk a candidate ladder —
+  fingerprint+eps-fw, then fingerprint, then the substitute mapping — accepting
+  the first rung that clears the match threshold; the top rung landing at ~1.0
+  marks an exact (non-fuzzy) match.
+  """
+  if CP.steerControlType == structs.CarParams.SteerControlType.angle:
+    return MOCK_MODEL_PATH, "MOCK", False
+
+  fingerprint = CP.carFingerprint
+  suffix = _eps_suffix(CP)
+
+  # rung 1: fingerprint + eps fw (when we have a usable fw string)
+  if suffix:
+    path, score = _best_model_for(f"{fingerprint} {suffix}")
+    if path is not None and fingerprint in path and score >= _ACCEPT_THRESHOLD:
+      name = os.path.splitext(os.path.basename(path))[0]
+      return path, name, score >= _EXACT_THRESHOLD
+
+  # rung 2: fingerprint alone
+  path, score = _best_model_for(fingerprint)
+  if path is not None and fingerprint in path and score >= _ACCEPT_THRESHOLD:
+    name = os.path.splitext(os.path.basename(path))[0]
+    return path, name, score >= _EXACT_THRESHOLD
+
+  # rung 3: substitute mapping — never exact
+  path, _ = _best_model_for(_substitute_for(fingerprint))
+  name = os.path.splitext(os.path.basename(path))[0] if path else "MOCK"
+  return (path or MOCK_MODEL_PATH), name, False
+
+# ===== network =====
+
+# The JSON model format (Twilsonco NNFF) is external: unicode 'σ' names the
+# sigmoid activation, weights/biases live under keys suffixed _W/_b, and the
+# input normalisation is (x - mean) / std. We translate names through a registry
+# and keep the mean/std transposed once at load.
+_MIN_INPUT_LEN = 2
+_FRICTION_PROBE = (10.0, 0.0, 0.2)
+_FRICTION_THRESHOLD = 0.1
+
+_ACTIVATION_ALIASES = {"σ": "sigmoid"}
+
+
+def _sigmoid(x):
+  return 1.0 / (1.0 + safe_exp(-x))
+
+
+def _identity(x):
+  return x
+
+
+_ACTIVATIONS = {"sigmoid": _sigmoid, "identity": _identity}
+
+
+def _resolve_activation(name: str):
+  for symbol, canonical in _ACTIVATION_ALIASES.items():
+    name = name.replace(symbol, canonical)
+  fn = _ACTIVATIONS.get(name)
+  if fn is None:
+    raise ValueError(f"Unknown activation: {name}")
+  return fn
+
+
+def _pick(layer: dict, suffix: str):
+  key = next(k for k in layer if k.endswith(suffix))
+  return np.array(layer[key], dtype=np.float32).T
+
+
+class NNTorqueModel:
+  def __init__(self, params_file, zero_bias=False):
+    with open(params_file) as f:
+      params = json.load(f)
+
+    self.input_size = params["input_size"]
+    self.output_size = params["output_size"]
+    self.input_mean = np.array(params["input_mean"], dtype=np.float32).T
+    self.input_std = np.array(params["input_std"], dtype=np.float32).T
+
+    self._weights = []
+    self._biases = []
+    self._activations = []
+    for layer in params["layers"]:
+      weight = _pick(layer, "_W")
+      bias = np.zeros_like(_pick(layer, "_b")) if zero_bias else _pick(layer, "_b")
+      self._weights.append(weight)
+      self._biases.append(bias)
+      self._activations.append(_resolve_activation(layer["activation"]))
+
+    self.friction_override = self.evaluate(list(_FRICTION_PROBE)) < _FRICTION_THRESHOLD
+
+  def forward(self, x):
+    for weight, bias, activation in zip(self._weights, self._biases, self._activations, strict=True):
+      x = activation(x.dot(weight) + bias)
+    return x
+
+  def evaluate(self, input_array):
+    if len(input_array) != self.input_size:
+      if len(input_array) < _MIN_INPUT_LEN:
+        raise ValueError(f"Input array length {len(input_array)} must be length 2 or greater")
+      input_array = input_array + [0] * (self.input_size - len(input_array))
+    x = (np.array(input_array, dtype=np.float32) - self.input_mean) / self.input_std
+    return float(self.forward(x)[0, 0])
+
+  # names kept for callers/tests that introspected the old implementation
+  @staticmethod
+  def sigmoid(x):
+    return _sigmoid(x)
+
+  @staticmethod
+  def identity(x):
+    return _identity(x)
+
+# ===== brain =====
+
+PLAN_SAMPLE_START = 5
+LAG_EXTRA_S = 0.0
+
+BASE_P = 0.8
+BASE_I = 0.15
+PID_SPEED_BP = [1, 1.5, 2.0, 3.0, 5, 7.5, 10, 15, 30]
+PID_P_GAIN = [250, 120, 65, 30, 11.5, 5.5, 3.5, 2.0, BASE_P]
+
+_JERK_FALLBACK_IDX = 16   # T_IDXS index used when nothing exceeds the lookahead horizon
+
+
+def sign(value: float) -> float:
+  if value > 0.0:
+    return 1.0
+  if value < 0.0:
+    return -1.0
+  return 0.0
+
+
+polarity = sign
+
+
+def _pointwise_jerk(accel_trace, dt_trace) -> list:
+  """Finite-difference jerk from an acceleration trace over per-step dt."""
+  delta = np.diff(accel_trace)
+  span = min(len(delta), len(dt_trace))
+  if span <= 0:
+    return []
+  return (delta[:span] / np.array(dt_trace)[:span]).tolist()
+
+
+def sign_locked_min(future_vals, seed_val):
+  """Smallest-magnitude jerk over the horizon, but only if the whole horizon
+  agrees in sign with the seed; a sign disagreement collapses to 0."""
+  if not future_vals:
+    return seed_val
+  agreeing = [v for v in future_vals if sign(v) == sign(seed_val)]
+  if len(agreeing) < len(future_vals):
+    return 0.0
+  return min(agreeing + [seed_val], key=abs)
+
+
+class PilotLateralBrain:
+  """Shared lateral-control scaffolding: PID core, model snapshot, and the
+  forward-looking jerk/friction estimates the feed-forward controllers build on."""
+
+  def __init__(self, torque_ctrl, cp, cp_iq, car_if):
+    del cp_iq
+    self.lac_torque = torque_ctrl
+    self.torque_from_lateral_accel_in_torque_space = car_if.torque_from_lateral_accel_in_torque_space()
+
+    self.model_v2 = None
+    self.model_valid = False
+
+    self.jerk_now = 0.0
+    self.jerk_goal = 0.0
+    self.jerk_obs = 0.0
+    self.jerk_ahead = 0.0
+
+    # per-cycle control snapshot
+    self._ff = 0.0
+    self._pid = PIDController([PID_SPEED_BP, PID_P_GAIN], BASE_I)
+    self._pid_log = None
+    self._accel_goal = 0.0
+    self._accel_obs = 0.0
+    self._roll_g = 0.0
+    self._deadband = 0.0
+    self._want_la = 0.0
+    self._have_la = 0.0
+    self._want_cv = 0.0
+    self._have_cv = 0.0
+    self._grav_la = 0.0
+    self._capped = False
+    self._out_tq = 0.0
+
+    # friction-lookahead tuning
+    self.friction_look_ahead_v = [1.4, 2.0]
+    self.friction_look_ahead_bp = [9.0, 30.0]
+    self.lat_jerk_friction_factor = 0.4
+    self.lat_accel_friction_factor = 0.7
+
+    self.t_diffs = np.diff(ModelConstants.T_IDXS)
+    self.desired_lat_jerk_time = cp.steerActuatorDelay + LAG_EXTRA_S
+
+  def update_model_v2(self, model_packet):
+    self.model_v2 = model_packet
+    self.model_valid = model_packet is not None and len(model_packet.orientation.x) >= CONTROL_N
+
+  def update_lateral_lag(self, lag):
+    self.desired_lat_jerk_time = max(0.01, lag) + LAG_EXTRA_S
+
+  def update_friction_input(self, target_val, measured_val):
+    error = target_val - measured_val
+    return self.lat_accel_friction_factor * error + self.lat_jerk_friction_factor * self.jerk_ahead
+
+  def _measured_jerk(self, car_state, vehicle_model) -> float:
+    curvature_rate = -vehicle_model.calc_curvature(math.radians(car_state.steeringRateDeg), car_state.vEgo, 0.0)
+    return curvature_rate * car_state.vEgo ** 2
+
+  def _horizon_index(self, speed_mps: float) -> int:
+    lookahead = np.interp(speed_mps, self.friction_look_ahead_bp, self.friction_look_ahead_v)
+    return next((i for i, t in enumerate(ModelConstants.T_IDXS) if t > lookahead), _JERK_FALLBACK_IDX)
+
+  def _reset_jerk_estimates(self, car_state, vehicle_model):
+    self.jerk_now = self._measured_jerk(car_state, vehicle_model)
+    self.jerk_goal = 0.0
+    self.jerk_obs = 0.0
+    self.jerk_ahead = 0.0
+
+  def update_calculations(self, car_state, vehicle_model, desired_lat_accel):
+    self._reset_jerk_estimates(car_state, vehicle_model)
+    if not self.model_valid:
+      return
+
+    accel_y = self.model_v2.acceleration.y
+    horizon_accel = np.interp(self.desired_lat_jerk_time, ModelConstants.T_IDXS, accel_y)
+    desired_jerk = (horizon_accel - desired_lat_accel) / self.desired_lat_jerk_time
+
+    forecast = _pointwise_jerk(accel_y, self.t_diffs)
+    window = forecast[PLAN_SAMPLE_START:self._horizon_index(car_state.vEgo)]
+    self.jerk_ahead = sign_locked_min(window, desired_jerk)
+
+    if self.jerk_ahead == 0.0:
+      self.jerk_now = 0.0
+      self.lat_accel_friction_factor = 1.0
+
+    self.jerk_goal = self.lat_jerk_friction_factor * self.jerk_ahead
+    self.jerk_obs = self.lat_jerk_friction_factor * self.jerk_now
+
+
+TorqueBrainCore = PilotLateralBrain
+
+# ===== nnff =====
+
+LOW_SPEED_X = [0, 10, 20, 30]
+LOW_SPEED_Y = [12, 3, 1, 0]
+
+# NNFF input layout expected by the trained models (dictated by the model data):
+# 4 scalars (v_ego, target, jerk, roll) + past/future target repeats + past/future rolls.
+_ERROR_BLEND_BP = [1.0, 2.0]
+_ERROR_BLEND_V = [0.0, 1.0]
+
+
+def roll_pitch_adjust(roll, pitch):
+  return roll * math.cos(pitch)
+
+
+class _HistoryWindow:
+  """Rolling past/future sample windows the NNFF vector is assembled from."""
+
+  def __init__(self, past_times, future_times, jerk_time):
+    self.past_times = past_times
+    self.future_times = future_times
+    self.jerk_time = jerk_time
+    self.nn_future_times = [t + jerk_time for t in future_times]
+
+    check_frames = [int(abs(t) * 100) for t in past_times]
+    self.frame_offsets = [check_frames[0] - f for f in check_frames]
+    maxlen = check_frames[0]
+    self.roll = deque(maxlen=maxlen)
+    self.lat_accel_desired = deque(maxlen=maxlen)
+    self.past_future_len = len(past_times) + len(self.nn_future_times)
+
+  def refresh_lag(self, jerk_time):
+    self.jerk_time = jerk_time
+    self.nn_future_times = [t + jerk_time for t in self.future_times]
+
+  def push(self, roll, lat_accel_desired):
+    self.roll.append(roll)
+    self.lat_accel_desired.append(lat_accel_desired)
+
+  def _sample(self, buf):
+    return [buf[min(len(buf) - 1, i)] for i in self.frame_offsets]
+
+  def past_rolls(self):
+    return self._sample(self.roll)
+
+  def past_lat_accels(self):
+    return self._sample(self.lat_accel_desired)
+
+
+class NeuralNetworkFeedForward(PilotLateralBrain):
+  def __init__(self, lac_torque, CP, CP_IQ, CI):
+    super().__init__(lac_torque, CP, CP_IQ, CI)
+    self.params = Params()
+    self.enabled = self.params.get_bool("NeuralNetworkFeedForward")
+    # NNFF applies only when a real trained model for this car is present on disk.
+    # No models shipped (or no match / MOCK) -> skip NNFF entirely and fall back to
+    # the stock torque feed-forward. Models are re-added as they are retrained.
+    self.has_nn_model = (CP_IQ.iqLateralNet.model.path != MOCK_MODEL_PATH
+                         and os.path.isfile(CP_IQ.iqLateralNet.model.path))
+    self.model = NNTorqueModel(CP_IQ.iqLateralNet.model.path) if self.has_nn_model else None
+    self.pitch = FirstOrderFilter(0.0, 0.5, 0.01)
+    self.pitch_last = 0.0
+
+    self.future_times = [0.3, 0.6, 1.0, 1.5]
+    self._window = _HistoryWindow([-0.3, -0.2, -0.1], self.future_times, self.desired_lat_jerk_time)
+    self.nav_torque_pulse = NavTorquePulseBrain(lac_torque)
+
+  # -- back-compat views onto the history window -------------------------------
+  @property
+  def nn_future_times(self):
+    return self._window.nn_future_times
+
+  @property
+  def past_future_len(self):
+    return self._window.past_future_len
+
+  @property
+  def _nnff_enabled(self):
+    return self.enabled and self.model_valid and self.has_nn_model
+
+  def update_limits(self):
+    if not self._nnff_enabled:
+      return
+    self._pid.set_limits(self.lac_torque.steer_max, -self.lac_torque.steer_max)
+
+  def update_lateral_lag(self, lag):
+    super().update_lateral_lag(lag)
+    self._window.refresh_lag(self.desired_lat_jerk_time)
+
+  # -- torque-space feedforward (non-NN path used for error scaling) -----------
+  def _torque_space(self, lateral_accel, CS, gravity_adjusted):
+    return self.torque_from_lateral_accel_in_torque_space(
+      LatControlInputs(lateral_accel, self._roll_g, CS.vEgo, CS.aEgo),
+      self.lac_torque.torque_params, gravity_adjusted=gravity_adjusted)
+
+  def update_feedforward_torque_space(self, CS):
+    torque_from_setpoint = self._torque_space(self._accel_goal, CS, gravity_adjusted=False)
+    torque_from_measurement = self._torque_space(self._accel_obs, CS, gravity_adjusted=False)
+    self._pid_log.error = float(torque_from_setpoint - torque_from_measurement)
+    self._ff = self._torque_space(self._grav_la, CS, gravity_adjusted=True)
+    self._ff += get_friction_in_torque_space(self._want_la - self._have_la,
+                                             self._deadband, FRICTION_THRESHOLD,
+                                             self.lac_torque.torque_params)
+
+  def update_output_torque(self, CS):
+    freeze_integrator = self._capped or CS.steeringPressed or CS.vEgo < 5
+    self._out_tq = self._pid.update(self._pid_log.error, feedforward=self._ff,
+                                           speed=CS.vEgo, freeze_integrator=freeze_integrator)
+
+  # -- NN input assembly -------------------------------------------------------
+  def _effective_roll(self, params, calibrated_pose):
+    roll = params.roll
+    if calibrated_pose is not None:
+      pitch = self.pitch.update(calibrated_pose.orientation.pitch)
+      roll = roll_pitch_adjust(roll, pitch)
+      self.pitch_last = pitch
+    return roll
+
+  def _future_rolls(self, roll, adjusted_future_times):
+    return [roll_pitch_adjust(np.interp(t, ModelConstants.T_IDXS, self.model_v2.orientation.x) + roll,
+                              np.interp(t, ModelConstants.T_IDXS, self.model_v2.orientation.y) + self.pitch_last)
+            for t in adjusted_future_times]
+
+  def _future_lat_accels(self, adjusted_future_times):
+    return [np.interp(t, ModelConstants.T_IDXS, self.model_v2.acceleration.y) for t in adjusted_future_times]
+
+  def _query(self, lead_scalar, jerk_scalar, tail):
+    """Build one model input from its 4 leading scalars + the shared tail, then
+    run the interpreter. `tail` is (repeat_value_or_None, extra_pairs...)."""
+    head = [self._v, lead_scalar, jerk_scalar, self._roll]
+    return self.model.evaluate(head + tail)
+
+  def update_neural_network_feedforward(self, CS, params, calibrated_pose) -> None:
+    if not self._nnff_enabled:
+      return
+
+    self.update_feedforward_torque_space(CS)
+    creep = float(np.interp(CS.vEgo, LOW_SPEED_X, LOW_SPEED_Y)) ** 2
+    self._accel_goal = self._want_la + creep * self._want_cv
+    self._accel_obs = self._have_la + creep * self._have_cv
+
+    # cache per-cycle scalars the query builder reads
+    self._v = CS.vEgo
+    self._roll = self._effective_roll(params, calibrated_pose)
+    self._window.push(self._roll, self._want_la)
+
+    horizon = [t + 0.5 * CS.aEgo * (t / max(CS.vEgo, 1.0)) for t in self.nn_future_times]
+    roll_ctx = self._window.past_rolls() + self._future_rolls(self._roll, horizon)
+    accel_ctx = self._window.past_lat_accels() + self._future_lat_accels(horizon)
+
+    goal_torque = self._query(self._accel_goal, self.jerk_goal, [self._accel_goal] * self.past_future_len + roll_ctx)
+    obs_torque = self._query(self._accel_obs, self.jerk_obs, [self._accel_obs] * self.past_future_len + roll_ctx)
+    self._pid_log.error = goal_torque - obs_torque
+    self._apply_error_blend()
+
+    friction_input = self.update_friction_input(self._accel_goal, self._accel_obs)
+    self._ff = self._query(self._want_la, friction_input, accel_ctx + roll_ctx)
+    if self.model.friction_override:
+      self._pid_log.error += get_friction(friction_input, self._deadband,
+                                          FRICTION_THRESHOLD, self.lac_torque.torque_params)
+
+    self.update_output_torque(CS)
+
+  def _apply_error_blend(self):
+    blend = float(np.interp(abs(self._want_la), _ERROR_BLEND_BP, _ERROR_BLEND_V))
+    if blend <= 0.0:
+      return
+    # error query carries a 0.0 roll slot (not the live roll), so build it directly
+    from_error = self.model.evaluate([self._v, self._accel_goal - self._accel_obs,
+                                      self.jerk_goal - self.jerk_obs, 0.0])
+    live = self._pid_log.error
+    if sign(live) == sign(from_error) and abs(live) < abs(from_error):
+      self._pid_log.error = live * (1.0 - blend) + from_error * blend
+
+  # -- per-cycle snapshot + entry point ----------------------------------------
+  def _snapshot_cycle(self, feedforward_seed, pid_core, pid_trace, torque_goal, torque_actual, roll_bias,
+                      deadzone, lat_accel_goal, lat_accel_actual, curvature_goal, curvature_actual,
+                      gravity_lat_accel, safety_limited, torque_output) -> None:
+    self._ff = feedforward_seed
+    self._pid = pid_core
+    self._pid_log = pid_trace
+    self._accel_goal = torque_goal
+    self._accel_obs = torque_actual
+    self._roll_g = roll_bias
+    self._deadband = deadzone
+    self._want_la = lat_accel_goal
+    self._have_la = lat_accel_actual
+    self._want_cv = curvature_goal
+    self._have_cv = curvature_actual
+    self._grav_la = gravity_lat_accel
+    self._capped = safety_limited
+    self._out_tq = torque_output
+
+  def update(self, car_state, vehicle_model, pid_core, calibrator, feedforward_seed, pid_trace,
+             torque_goal, torque_actual, calibrated_pose, roll_bias, lat_accel_goal, lat_accel_actual,
+             deadzone, gravity_lat_accel, curvature_goal, curvature_actual, safety_limited, torque_output):
+    self._snapshot_cycle(feedforward_seed, pid_core, pid_trace, torque_goal, torque_actual, roll_bias,
+                         deadzone, lat_accel_goal, lat_accel_actual, curvature_goal, curvature_actual,
+                         gravity_lat_accel, safety_limited, torque_output)
+    self.update_calculations(car_state, vehicle_model, lat_accel_goal)
+    self.update_neural_network_feedforward(car_state, calibrator, calibrated_pose)
+    self._out_tq = self.nav_torque_pulse.nudge_output_torque(True, car_state, self._out_tq)
+    return self._pid_log, self._out_tq
+
+
+# At higher speeds (25+mph) we can assume:
+# Lateral acceleration achieved by a specific car correlates to
+# torque applied to the steering rack. It does not correlate to
+# wheel slip, or to speed.
+
+# This controller applies torque to achieve desired lateral
+# accelerations. To compensate for the low speed effects the
+# proportional gain is increased at low speeds by the PID controller.
+# Additionally, there is friction in the steering wheel that needs
+# to be overcome to move it at all, this is compensated for too.
+
+KP = 0.8
+KI = 0.15
+
+INTERP_SPEEDS = [1, 1.5, 2.0, 3.0, 5, 7.5, 10, 15, 30]
+KP_INTERP = [250, 120, 65, 30, 11.5, 5.5, 3.5, 2.0, KP]
+
+LP_FILTER_CUTOFF_HZ = 1.2
+JERK_LOOKAHEAD_SECONDS = 0.19
+JERK_GAIN = 0.3
+LAT_ACCEL_REQUEST_BUFFER_SECONDS = 1.0
+VERSION = 1
+
+class LatControlTorque(LatControl):
+  def __init__(self, CP, CP_IQ, CI, dt):
+    super().__init__(CP, CP_IQ, CI, dt)
+    self.torque_params = CP.lateralTuning.torque.as_builder()
+    self.torque_from_lateral_accel = CI.torque_from_lateral_accel()
+    self.lateral_accel_from_torque = CI.lateral_accel_from_torque()
+    self.pid = PIDController([INTERP_SPEEDS, KP_INTERP], KI, rate=1/self.dt)
+    self.update_limits()
+    self.steering_angle_deadzone_deg = self.torque_params.steeringAngleDeadzoneDeg
+    self.lat_accel_request_buffer_len = int(LAT_ACCEL_REQUEST_BUFFER_SECONDS / self.dt)
+    self.lat_accel_request_buffer = deque([0.] * self.lat_accel_request_buffer_len , maxlen=self.lat_accel_request_buffer_len)
+    self.lookahead_frames = int(JERK_LOOKAHEAD_SECONDS / self.dt)
+    self.jerk_filter = FirstOrderFilter(0.0, 1 / (2 * np.pi * LP_FILTER_CUTOFF_HZ), self.dt)
+    self.lateral_acceleration_slew_limiter = LateralAccelerationSlewLimiter(Params().get_bool("IQLateralAccelSlew"))
+    self.curvature_lookahead_enabled = Params().get_bool("IQLateralCurvatureLookahead")
+
+    self.nnff_assist = NeuralNetworkFeedForward(self, CP, CP_IQ, CI)
+
+  def update_live_torque_params(self, latAccelFactor, latAccelOffset, friction):
+    self.torque_params.latAccelFactor = latAccelFactor
+    self.torque_params.latAccelOffset = latAccelOffset
+    self.torque_params.friction = friction
+    self.update_limits()
+
+  def update_limits(self):
+    self.pid.set_limits(self.lateral_accel_from_torque(self.steer_max, self.torque_params),
+                        self.lateral_accel_from_torque(-self.steer_max, self.torque_params))
+
+  def update(self, active, CS, VM, params, steer_limited_by_safety, desired_curvature, calibrated_pose, curvature_limited, lat_delay,
+             lookahead_curvature=None):
+    pid_log = log.ControlsState.LateralTorqueState.new_message()
+    pid_log.version = VERSION
+    measured_curvature = -VM.calc_curvature(math.radians(CS.steeringAngleDeg - params.angleOffsetDeg), CS.vEgo, params.roll)
+    measurement = measured_curvature * CS.vEgo ** 2
+    target_curvature = desired_curvature
+    if self.curvature_lookahead_enabled and lookahead_curvature is not None:
+      target_curvature = lookahead_curvature
+    if not active and self.lateral_acceleration_slew_limiter.enabled:
+      self.lateral_acceleration_slew_limiter.reset(target_curvature * CS.vEgo ** 2)
+    limited_curvature = self.lateral_acceleration_slew_limiter.update(target_curvature, CS.vEgo, self.dt)
+    future_desired_lateral_accel = limited_curvature * CS.vEgo ** 2
+    self.lat_accel_request_buffer.append(future_desired_lateral_accel)
+
+    roll_compensation = params.roll * ACCELERATION_DUE_TO_GRAVITY
+    curvature_deadzone = abs(VM.calc_curvature(math.radians(self.steering_angle_deadzone_deg), CS.vEgo, 0.0))
+    lateral_accel_deadzone = curvature_deadzone * CS.vEgo ** 2
+
+    delay_frames = int(np.clip(lat_delay / self.dt + 1, 1, self.lat_accel_request_buffer_len))
+    expected_lateral_accel = self.lat_accel_request_buffer[-delay_frames]
+    setpoint = expected_lateral_accel
+    error = setpoint - measurement
+
+    lookahead_idx = int(np.clip(-delay_frames + self.lookahead_frames, -self.lat_accel_request_buffer_len+1, -2))
+    raw_lateral_jerk = (self.lat_accel_request_buffer[lookahead_idx+1] - self.lat_accel_request_buffer[lookahead_idx-1]) / (2 * self.dt)
+    desired_lateral_jerk = self.jerk_filter.update(raw_lateral_jerk)
+    gravity_adjusted_future_lateral_accel = future_desired_lateral_accel - roll_compensation
+    ff = gravity_adjusted_future_lateral_accel
+    # latAccelOffset corrects roll compensation bias from device roll misalignment relative to car roll
+    ff -= self.torque_params.latAccelOffset
+    ff += get_friction(error + JERK_GAIN * desired_lateral_jerk, lateral_accel_deadzone, FRICTION_THRESHOLD, self.torque_params)
+
+    if not active:
+      output_torque = 0.0
+      pid_log.active = False
+    else:
+      # do error correction in lateral acceleration space, convert at end to handle non-linear torque responses correctly
+      pid_log.error = float(error)
+
+      freeze_integrator = steer_limited_by_safety or CS.steeringPressed or CS.vEgo < 5
+      output_lataccel = self.pid.update(pid_log.error, speed=CS.vEgo, feedforward=ff, freeze_integrator=freeze_integrator)
+      output_torque = self.torque_from_lateral_accel(output_lataccel, self.torque_params)
+
+      # Lateral acceleration torque controller extension updates
+      # Overrides pid_log.error and output_torque
+      pid_log, output_torque = self.nnff_assist.update(CS, VM, self.pid, params, ff, pid_log, setpoint, measurement, calibrated_pose, roll_compensation,
+                                                       future_desired_lateral_accel, measurement, lateral_accel_deadzone, gravity_adjusted_future_lateral_accel,
+                                                       limited_curvature, measured_curvature, steer_limited_by_safety, output_torque)
+
+      pid_log.active = True
+      pid_log.p = float(self.pid.p)
+      pid_log.i = float(self.pid.i)
+      pid_log.d = float(self.pid.d)
+      pid_log.f = float(self.pid.f)
+      pid_log.output = float(-output_torque) # TODO: log lat accel?
+      pid_log.actualLateralAccel = float(measurement)
+      pid_log.desiredLateralAccel = float(setpoint)
+      pid_log.desiredLateralJerk = float(desired_lateral_jerk)
+      pid_log.saturated = bool(self._check_saturation(self.steer_max - abs(output_torque) < 1e-3, CS, steer_limited_by_safety, curvature_limited))
+
+    # TODO left is positive in this convention
+    return -output_torque, 0.0, pid_log

iqpilot/selfdrive/controls/lib/latcontrol_torque_pq.py

@@ -0,0 +1,131 @@
+import math
+import numpy as np
+from collections import deque
+
+from iqpilot.cereal import log
+from iqdbc.car.lateral import get_friction
+from iqpilot.common.constants import ACCELERATION_DUE_TO_GRAVITY
+from iqpilot.common.filter_simple import FirstOrderFilter
+from iqpilot.common.params import Params
+from iqpilot.selfdrive.controls.lib.latcontrol import LatControl
+from iqpilot.selfdrive.controls.lib.lateral_acceleration_slew_limiter import LateralAccelerationSlewLimiter
+from iqpilot.common.pid import PIDController
+
+FRICTION_THRESHOLD_PQ = 1.0
+KP = 0.8
+KI = 0.15
+
+INTERP_SPEEDS = [1, 1.5, 2.0, 3.0, 5, 7.5, 10, 15, 30]
+KP_INTERP = [250, 120, 65, 30, 11.5, 5.5, 3.5, 2.0, KP]
+
+LP_FILTER_CUTOFF_HZ = 1.5
+JERK_LOOKAHEAD_SECONDS = 0.34
+JERK_GAIN = 0.3
+LAT_ACCEL_REQUEST_BUFFER_SECONDS = 1.0
+VERSION = 1
+
+DEFAULT_LAT_ACCEL_FACTOR = 2.2
+DEFAULT_LAT_ACCEL_OFFSET = -0.13
+DEFAULT_FRICTION = 0.1
+FREEZE_LIVE_TORQUE_PARAMS = True
+
+ASSIST_COMPENSATION = True
+ASSIST_SPEEDS_KPH = [0.0, 50.0, 120.0]
+ASSIST_GAIN = [0.688, 0.883, 1.211]
+ASSIST_REF_KPH = 100.0
+
+
+def _assist_comp(v_ego_ms):
+  import numpy as _np
+  ref = _np.interp(ASSIST_REF_KPH, ASSIST_SPEEDS_KPH, ASSIST_GAIN)
+  g = _np.interp(v_ego_ms * 3.6, ASSIST_SPEEDS_KPH, ASSIST_GAIN)
+  return float(_np.clip(ref / g, 0.7, 1.6))
+
+
+class LatControlTorquePQ(LatControl):
+  def __init__(self, CP, CP_IQ, CI, dt):
+    super().__init__(CP, CP_IQ, CI, dt)
+    self.torque_params = CP.lateralTuning.torque.as_builder()
+    self.torque_params.latAccelFactor = DEFAULT_LAT_ACCEL_FACTOR
+    self.torque_params.latAccelOffset = DEFAULT_LAT_ACCEL_OFFSET
+    self.torque_params.friction = DEFAULT_FRICTION
+    self.torque_from_lateral_accel = CI.torque_from_lateral_accel()
+    self.lateral_accel_from_torque = CI.lateral_accel_from_torque()
+    self.pid = PIDController([INTERP_SPEEDS, KP_INTERP], KI, rate=1/self.dt)
+    self.update_limits()
+    self.steering_angle_deadzone_deg = self.torque_params.steeringAngleDeadzoneDeg
+    self.lat_accel_request_buffer_len = int(LAT_ACCEL_REQUEST_BUFFER_SECONDS / self.dt)
+    self.lat_accel_request_buffer = deque([0.] * self.lat_accel_request_buffer_len, maxlen=self.lat_accel_request_buffer_len)
+    self.lookahead_frames = int(JERK_LOOKAHEAD_SECONDS / self.dt)
+    self.jerk_filter = FirstOrderFilter(0.0, 1 / (2 * np.pi * LP_FILTER_CUTOFF_HZ), self.dt)
+    self.lateral_acceleration_slew_limiter = LateralAccelerationSlewLimiter(Params().get_bool("IQLateralAccelSlew"))
+    self.curvature_lookahead_enabled = Params().get_bool("IQLateralCurvatureLookahead")
+
+  def update_live_torque_params(self, latAccelFactor, latAccelOffset, friction):
+    if FREEZE_LIVE_TORQUE_PARAMS:
+      return
+    self.torque_params.latAccelFactor = latAccelFactor
+    self.torque_params.latAccelOffset = latAccelOffset
+    self.torque_params.friction = friction
+    self.update_limits()
+
+  def update_limits(self):
+    self.pid.set_limits(self.lateral_accel_from_torque(self.steer_max, self.torque_params),
+                        self.lateral_accel_from_torque(-self.steer_max, self.torque_params))
+
+  def update(self, active, CS, VM, params, steer_limited_by_safety, desired_curvature, calibrated_pose, curvature_limited, lat_delay,
+             lookahead_curvature=None):
+    pid_log = log.ControlsState.LateralTorqueState.new_message()
+    pid_log.version = VERSION
+    measured_curvature = -VM.calc_curvature(math.radians(CS.steeringAngleDeg - params.angleOffsetDeg), CS.vEgo, params.roll)
+    measurement = measured_curvature * CS.vEgo ** 2
+    target_curvature = desired_curvature
+    if self.curvature_lookahead_enabled and lookahead_curvature is not None:
+      target_curvature = lookahead_curvature
+    if not active and self.lateral_acceleration_slew_limiter.enabled:
+      self.lateral_acceleration_slew_limiter.reset(target_curvature * CS.vEgo ** 2)
+    limited_curvature = self.lateral_acceleration_slew_limiter.update(target_curvature, CS.vEgo, self.dt)
+    future_desired_lateral_accel = limited_curvature * CS.vEgo ** 2
+    self.lat_accel_request_buffer.append(future_desired_lateral_accel)
+
+    roll_compensation = params.roll * ACCELERATION_DUE_TO_GRAVITY
+    curvature_deadzone = abs(VM.calc_curvature(math.radians(self.steering_angle_deadzone_deg), CS.vEgo, 0.0))
+    lateral_accel_deadzone = curvature_deadzone * CS.vEgo ** 2
+
+    delay_frames = int(np.clip(lat_delay / self.dt + 1, 1, self.lat_accel_request_buffer_len))
+    expected_lateral_accel = self.lat_accel_request_buffer[-delay_frames]
+    setpoint = expected_lateral_accel
+    error = setpoint - measurement
+
+    lookahead_idx = int(np.clip(-delay_frames + self.lookahead_frames, -self.lat_accel_request_buffer_len + 1, -2))
+    raw_lateral_jerk = (self.lat_accel_request_buffer[lookahead_idx + 1] - self.lat_accel_request_buffer[lookahead_idx - 1]) / (2 * self.dt)
+    desired_lateral_jerk = self.jerk_filter.update(raw_lateral_jerk)
+    gravity_adjusted_future_lateral_accel = future_desired_lateral_accel - roll_compensation
+    ff = gravity_adjusted_future_lateral_accel
+    ff -= self.torque_params.latAccelOffset
+    ff += get_friction(error + JERK_GAIN * desired_lateral_jerk, lateral_accel_deadzone, FRICTION_THRESHOLD_PQ, self.torque_params)
+
+    if not active:
+      output_torque = 0.0
+      pid_log.active = False
+    else:
+      pid_log.error = float(error)
+      freeze_integrator = steer_limited_by_safety or CS.steeringPressed or CS.vEgo < 5
+      output_lataccel = self.pid.update(pid_log.error, speed=CS.vEgo, feedforward=ff, freeze_integrator=freeze_integrator)
+      output_torque = self.torque_from_lateral_accel(output_lataccel, self.torque_params)
+      if ASSIST_COMPENSATION:
+        output_torque = float(np.clip(output_torque * _assist_comp(CS.vEgo),
+                                      -self.steer_max, self.steer_max))
+
+      pid_log.active = True
+      pid_log.p = float(self.pid.p)
+      pid_log.i = float(self.pid.i)
+      pid_log.d = float(self.pid.d)
+      pid_log.f = float(self.pid.f)
+      pid_log.output = float(-output_torque)
+      pid_log.actualLateralAccel = float(measurement)
+      pid_log.desiredLateralAccel = float(setpoint)
+      pid_log.desiredLateralJerk = float(desired_lateral_jerk)
+      pid_log.saturated = bool(self._check_saturation(self.steer_max - abs(output_torque) < 1e-3, CS, steer_limited_by_safety, curvature_limited))
+
+    return -output_torque, 0.0, pid_log

iqpilot/selfdrive/controls/lib/lateral_acceleration_slew_limiter.py

@@ -0,0 +1,40 @@
+import numpy as np
+
+
+JERK_SPEED_BP = [0.0, 8.0, 20.0, 35.0]
+JERK_MAX_BP = [5.0, 4.0, 2.5, 2.0]
+A_LAT_MAX = 3.0
+MIN_LIMIT_SPEED = 5.0
+AVOIDANCE_BYPASS_ACCEL_DELTA = 2.0
+CURVATURE_SPEED_FLOOR = 0.1
+
+
+class LateralAccelerationSlewLimiter:
+  def __init__(self, enabled: bool):
+    self.enabled = enabled
+    self.a_lim = 0.0
+
+  def reset(self, a_lat: float) -> None:
+    self.a_lim = float(np.clip(a_lat, -A_LAT_MAX, A_LAT_MAX))
+
+  def jerk_max(self, v_ego: float) -> float:
+    return float(np.interp(v_ego, JERK_SPEED_BP, JERK_MAX_BP))
+
+  def update(self, desired_curvature: float, v_ego: float, dt: float) -> float:
+    if not self.enabled:
+      return desired_curvature
+
+    a_des = v_ego ** 2 * desired_curvature
+    if v_ego < MIN_LIMIT_SPEED:
+      self.reset(a_des)
+      return desired_curvature
+
+    if abs(a_des - self.a_lim) > AVOIDANCE_BYPASS_ACCEL_DELTA:
+      self.reset(a_des)
+    else:
+      da_max = self.jerk_max(v_ego) * dt
+      self.a_lim += float(np.clip(a_des - self.a_lim, -da_max, da_max))
+      self.a_lim = float(np.clip(self.a_lim, -A_LAT_MAX, A_LAT_MAX))
+
+    speed = max(abs(v_ego), CURVATURE_SPEED_FLOOR)
+    return self.a_lim / speed ** 2

iqpilot/selfdrive/controls/lib/lateral_mpc_lib/.gitignore

@@ -0,0 +1,2 @@
+acados_ocp_lat.json
+c_generated_code/

iqpilot/selfdrive/controls/lib/lateral_mpc_lib/SConscript

@@ -0,0 +1,100 @@
+Import('env', 'envCython', 'arch', 'msgq_python', 'common_python', 'np_version')
+
+gen = "c_generated_code"
+
+casadi_model = [
+  f'{gen}/lat_model/lat_expl_ode_fun.c',
+  f'{gen}/lat_model/lat_expl_vde_forw.c',
+]
+
+casadi_cost_y = [
+  f'{gen}/lat_cost/lat_cost_y_fun.c',
+  f'{gen}/lat_cost/lat_cost_y_fun_jac_ut_xt.c',
+  f'{gen}/lat_cost/lat_cost_y_hess.c',
+]
+
+casadi_cost_e = [
+  f'{gen}/lat_cost/lat_cost_y_e_fun.c',
+  f'{gen}/lat_cost/lat_cost_y_e_fun_jac_ut_xt.c',
+  f'{gen}/lat_cost/lat_cost_y_e_hess.c',
+]
+
+casadi_cost_0 = [
+  f'{gen}/lat_cost/lat_cost_y_0_fun.c',
+  f'{gen}/lat_cost/lat_cost_y_0_fun_jac_ut_xt.c',
+  f'{gen}/lat_cost/lat_cost_y_0_hess.c',
+]
+
+build_files = [f'{gen}/acados_solver_lat.c'] + casadi_model + casadi_cost_y + casadi_cost_e + casadi_cost_0
+
+# extra generated files used to trigger a rebuild
+generated_files = [
+  f'{gen}/Makefile',
+
+  f'{gen}/main_lat.c',
+  f'{gen}/main_sim_lat.c',
+  f'{gen}/acados_solver_lat.h',
+  f'{gen}/acados_sim_solver_lat.h',
+  f'{gen}/acados_sim_solver_lat.c',
+  f'{gen}/acados_solver.pxd',
+
+  f'{gen}/lat_model/lat_expl_vde_adj.c',
+
+  f'{gen}/lat_model/lat_model.h',
+  f'{gen}/lat_constraints/lat_constraints.h',
+  f'{gen}/lat_cost/lat_cost.h',
+] + build_files
+
+acados_dir = '#iqpilot/third_party/acados'
+acados_templates_dir = '#iqpilot/third_party/acados/acados_template/c_templates_tera'
+
+source_list = ['lat_mpc.py',
+  '#iqpilot/selfdrive/iqmodeld/config.py',
+  f'{acados_dir}/include/acados_c/ocp_nlp_interface.h',
+  f'{acados_templates_dir}/acados_solver.in.c',
+]
+
+lenv = env.Clone()
+acados_rel_path = Dir(gen).rel_path(Dir(f"#iqpilot/third_party/acados/{arch}/lib"))
+lenv["RPATH"] += [lenv.Literal(f'\\$$ORIGIN/{acados_rel_path}')]
+lenv.Clean(generated_files, Dir(gen))
+
+_mpc_dir = Dir('.').abspath
+generated_lat = lenv.Command(generated_files,
+                             source_list,
+                             lenv.PrettyAction(f"cd {_mpc_dir} && python3 lat_mpc.py", 'GEN',
+                                               logfile=f"{_mpc_dir}/gen.log", capture_stderr=True))
+lenv.Depends(generated_lat, [msgq_python, common_python])
+
+lenv["CFLAGS"].append("-DACADOS_WITH_QPOASES")
+lenv["CXXFLAGS"].append("-DACADOS_WITH_QPOASES")
+lenv["CCFLAGS"].append("-Wno-unused")
+if arch != "Darwin":
+  lenv["LINKFLAGS"].append("-Wl,--disable-new-dtags")
+else:
+  lenv["LINKFLAGS"].append("-Wl,-install_name,@loader_path/libacados_ocp_solver_lat.dylib")
+  lenv["LINKFLAGS"].append(f"-Wl,-rpath,@loader_path/{acados_rel_path}")
+lib_solver = lenv.SharedLibrary(f"{gen}/acados_ocp_solver_lat",
+                                build_files,
+                                LIBS=['m', 'acados', 'hpipm', 'blasfeo', 'qpOASES_e'])
+
+# generate cython stuff
+acados_ocp_solver_pyx = File("#iqpilot/third_party/acados/acados_template/acados_ocp_solver_pyx.pyx")
+acados_ocp_solver_common = File("#iqpilot/third_party/acados/acados_template/acados_solver_common.pxd")
+libacados_ocp_solver_pxd = File(f'{gen}/acados_solver.pxd')
+libacados_ocp_solver_c = File(f'{gen}/acados_ocp_solver_pyx.c')
+
+lenv2 = envCython.Clone()
+lenv2["LIBPATH"] += [lib_solver[0].dir.abspath]
+lenv2["RPATH"] += [lenv2.Literal('\\$$ORIGIN')]
+lenv2.Command(libacados_ocp_solver_c,
+  [acados_ocp_solver_pyx, acados_ocp_solver_common, libacados_ocp_solver_pxd],
+  lenv2.PrettyAction(
+    f'cython' + \
+    f' -o {libacados_ocp_solver_c.get_labspath()}' + \
+    f' -I {libacados_ocp_solver_pxd.get_dir().get_labspath()}' + \
+    f' -I {acados_ocp_solver_common.get_dir().get_labspath()}' + \
+    f' {acados_ocp_solver_pyx.get_labspath()}', 'CYTHON'))
+lib_cython = lenv2.Program(f'{gen}/acados_ocp_solver_pyx.so', [libacados_ocp_solver_c], LIBS=['acados_ocp_solver_lat'])
+lenv2.Depends(lib_cython, lib_solver)
+lenv2.Depends(libacados_ocp_solver_c, np_version)

iqpilot/selfdrive/controls/lib/lateral_mpc_lib/lat_mpc.py

@@ -0,0 +1,199 @@
+#!/usr/bin/env python3
+import os
+import time
+import numpy as np
+
+from casadi import SX, vertcat, sin, cos
+# WARNING: imports outside of constants will not trigger a rebuild
+from iqpilot.selfdrive.iqmodeld.config import ModelConstants
+
+if __name__ == '__main__':  # generating code
+  from iqpilot.third_party.acados.acados_template import AcadosModel, AcadosOcp, AcadosOcpSolver
+else:
+  from iqpilot.selfdrive.controls.lib.lateral_mpc_lib.c_generated_code.acados_ocp_solver_pyx import AcadosOcpSolverCython
+
+LAT_MPC_DIR = os.path.dirname(os.path.abspath(__file__))
+EXPORT_DIR = os.path.join(LAT_MPC_DIR, "c_generated_code")
+JSON_FILE = os.path.join(LAT_MPC_DIR, "acados_ocp_lat.json")
+X_DIM = 4
+P_DIM = 2
+COST_E_DIM = 3
+COST_DIM = COST_E_DIM + 2
+SPEED_OFFSET = 10.0
+MODEL_NAME = 'lat'
+ACADOS_SOLVER_TYPE = 'SQP_RTI'
+N = 32
+
+def gen_lat_model():
+  model = AcadosModel()
+  model.name = MODEL_NAME
+
+  # set up states & controls
+  x_ego = SX.sym('x_ego')
+  y_ego = SX.sym('y_ego')
+  psi_ego = SX.sym('psi_ego')
+  psi_rate_ego = SX.sym('psi_rate_ego')
+  model.x = vertcat(x_ego, y_ego, psi_ego, psi_rate_ego)
+
+  # parameters
+  v_ego = SX.sym('v_ego')
+  rotation_radius = SX.sym('rotation_radius')
+  model.p = vertcat(v_ego, rotation_radius)
+
+  # controls
+  psi_accel_ego = SX.sym('psi_accel_ego')
+  model.u = vertcat(psi_accel_ego)
+
+  # xdot
+  x_ego_dot = SX.sym('x_ego_dot')
+  y_ego_dot = SX.sym('y_ego_dot')
+  psi_ego_dot = SX.sym('psi_ego_dot')
+  psi_rate_ego_dot = SX.sym('psi_rate_ego_dot')
+
+  model.xdot = vertcat(x_ego_dot, y_ego_dot, psi_ego_dot, psi_rate_ego_dot)
+
+  # dynamics model
+  f_expl = vertcat(v_ego * cos(psi_ego) - rotation_radius * sin(psi_ego) * psi_rate_ego,
+                   v_ego * sin(psi_ego) + rotation_radius * cos(psi_ego) * psi_rate_ego,
+                   psi_rate_ego,
+                   psi_accel_ego)
+  model.f_impl_expr = model.xdot - f_expl
+  model.f_expl_expr = f_expl
+  return model
+
+
+def gen_lat_ocp():
+  ocp = AcadosOcp()
+  ocp.model = gen_lat_model()
+
+  Tf = np.array(ModelConstants.T_IDXS)[N]
+
+  # set dimensions
+  ocp.dims.N = N
+
+  # set cost module
+  ocp.cost.cost_type = 'NONLINEAR_LS'
+  ocp.cost.cost_type_e = 'NONLINEAR_LS'
+
+  Q = np.diag(np.zeros(COST_E_DIM))
+  QR = np.diag(np.zeros(COST_DIM))
+
+  ocp.cost.W = QR
+  ocp.cost.W_e = Q
+
+  y_ego, psi_ego, psi_rate_ego = ocp.model.x[1], ocp.model.x[2], ocp.model.x[3]
+  psi_rate_ego_dot = ocp.model.u[0]
+  v_ego = ocp.model.p[0]
+
+  ocp.parameter_values = np.zeros((P_DIM, ))
+
+  ocp.cost.yref = np.zeros((COST_DIM, ))
+  ocp.cost.yref_e = np.zeros((COST_E_DIM, ))
+  # Add offset to smooth out low speed control
+  # TODO unclear if this right solution long term
+  v_ego_offset = v_ego + SPEED_OFFSET
+  # TODO there are two costs on psi_rate_ego_dot, one
+  # is correlated to jerk the other to steering wheel movement
+  # the steering wheel movement cost is added to prevent excessive
+  # wheel movements
+  ocp.model.cost_y_expr = vertcat(y_ego,
+                                  v_ego_offset * psi_ego,
+                                  v_ego_offset * psi_rate_ego,
+                                  v_ego_offset * psi_rate_ego_dot,
+                                  psi_rate_ego_dot / (v_ego + 0.1))
+  ocp.model.cost_y_expr_e = vertcat(y_ego,
+                                   v_ego_offset * psi_ego,
+                                   v_ego_offset * psi_rate_ego)
+
+  # set constraints
+  ocp.constraints.constr_type = 'BGH'
+  ocp.constraints.idxbx = np.array([2,3])
+  ocp.constraints.ubx = np.array([np.radians(90), np.radians(50)])
+  ocp.constraints.lbx = np.array([-np.radians(90), -np.radians(50)])
+  x0 = np.zeros((X_DIM,))
+  ocp.constraints.x0 = x0
+
+  ocp.solver_options.qp_solver = 'PARTIAL_CONDENSING_HPIPM'
+  ocp.solver_options.hessian_approx = 'GAUSS_NEWTON'
+  ocp.solver_options.integrator_type = 'ERK'
+  ocp.solver_options.nlp_solver_type = ACADOS_SOLVER_TYPE
+  ocp.solver_options.qp_solver_iter_max = 1
+  ocp.solver_options.qp_solver_cond_N = 1
+
+  # set prediction horizon
+  ocp.solver_options.tf = Tf
+  ocp.solver_options.shooting_nodes = np.array(ModelConstants.T_IDXS)[:N+1]
+
+  ocp.code_export_directory = EXPORT_DIR
+  return ocp
+
+
+class LateralMpc:
+  def __init__(self, x0=None):
+    if x0 is None:
+      x0 = np.zeros(X_DIM)
+    self.solver = AcadosOcpSolverCython(MODEL_NAME, ACADOS_SOLVER_TYPE, N)
+    self.reset(x0)
+
+  def reset(self, x0=None):
+    if x0 is None:
+      x0 = np.zeros(X_DIM)
+    self.x_sol = np.zeros((N+1, X_DIM))
+    self.u_sol = np.zeros((N, 1))
+    self.yref = np.zeros((N+1, COST_DIM))
+    for i in range(N):
+      self.solver.cost_set(i, "yref", self.yref[i])
+    self.solver.cost_set(N, "yref", self.yref[N][:COST_E_DIM])
+
+    # Somehow needed for stable init
+    for i in range(N+1):
+      self.solver.set(i, 'x', np.zeros(X_DIM))
+      self.solver.set(i, 'p', np.zeros(P_DIM))
+    self.solver.constraints_set(0, "lbx", x0)
+    self.solver.constraints_set(0, "ubx", x0)
+    self.solver.solve()
+    self.solution_status = 0
+    self.solve_time = 0.0
+    self.cost = 0
+
+  def set_weights(self, path_weight, heading_weight,
+                  lat_accel_weight, lat_jerk_weight,
+                  steering_rate_weight):
+    W = np.asfortranarray(np.diag([path_weight, heading_weight,
+                                   lat_accel_weight, lat_jerk_weight,
+                                   steering_rate_weight]))
+    for i in range(N):
+      self.solver.cost_set(i, 'W', W)
+    self.solver.cost_set(N, 'W', W[:COST_E_DIM,:COST_E_DIM])
+
+  def run(self, x0, p, y_pts, heading_pts, yaw_rate_pts):
+    x0_cp = np.copy(x0)
+    p_cp = np.copy(p)
+    self.solver.constraints_set(0, "lbx", x0_cp)
+    self.solver.constraints_set(0, "ubx", x0_cp)
+    self.yref[:,0] = y_pts
+    v_ego = p_cp[0, 0]
+    # rotation_radius = p_cp[1]
+    self.yref[:,1] = heading_pts * (v_ego + SPEED_OFFSET)
+    self.yref[:,2] = yaw_rate_pts * (v_ego + SPEED_OFFSET)
+    for i in range(N):
+      self.solver.cost_set(i, "yref", self.yref[i])
+      self.solver.set(i, "p", p_cp[i])
+    self.solver.set(N, "p", p_cp[N])
+    self.solver.cost_set(N, "yref", self.yref[N][:COST_E_DIM])
+
+    t = time.monotonic()
+    self.solution_status = self.solver.solve()
+    self.solve_time = time.monotonic() - t
+
+    for i in range(N+1):
+      self.x_sol[i] = self.solver.get(i, 'x')
+    for i in range(N):
+      self.u_sol[i] = self.solver.get(i, 'u')
+    self.cost = self.solver.get_cost()
+
+
+if __name__ == "__main__":
+  ocp = gen_lat_ocp()
+  AcadosOcpSolver.generate(ocp, json_file=JSON_FILE)
+  # AcadosOcpSolver.build(ocp.code_export_directory, with_cython=True)

iqpilot/selfdrive/controls/lib/ldw.py

@@ -0,0 +1,41 @@
+from iqpilot.cereal import log
+from iqpilot.common.realtime import DT_CTRL
+from iqpilot.common.constants import CV
+
+
+CAMERA_OFFSET = 0.04
+LDW_MIN_SPEED = 31 * CV.MPH_TO_MS
+LANE_DEPARTURE_THRESHOLD = 0.1
+
+class LaneDepartureWarning:
+  def __init__(self):
+    self.left = False
+    self.right = False
+    self.last_blinker_frame = 0
+
+  def update(self, frame, modelV2, CS, CC):
+    if CS.leftBlinker or CS.rightBlinker:
+      self.last_blinker_frame = frame
+
+    recent_blinker = (frame - self.last_blinker_frame) * DT_CTRL < 5.0  # 5s blinker cooldown
+    ldw_allowed = CS.vEgo > LDW_MIN_SPEED and not recent_blinker and not CC.latActive
+
+    desire_prediction = modelV2.meta.desirePrediction
+    if len(desire_prediction) and ldw_allowed:
+      right_lane_visible = modelV2.laneLineProbs[2] > 0.5
+      left_lane_visible = modelV2.laneLineProbs[1] > 0.5
+      l_lane_change_prob = desire_prediction[log.Desire.laneChangeLeft]
+      r_lane_change_prob = desire_prediction[log.Desire.laneChangeRight]
+
+      lane_lines = modelV2.laneLines
+      l_lane_close = left_lane_visible and (lane_lines[1].y[0] > -(1.08 + CAMERA_OFFSET))
+      r_lane_close = right_lane_visible and (lane_lines[2].y[0] < (1.08 - CAMERA_OFFSET))
+
+      self.left = bool(l_lane_change_prob > LANE_DEPARTURE_THRESHOLD and l_lane_close)
+      self.right = bool(r_lane_change_prob > LANE_DEPARTURE_THRESHOLD and r_lane_close)
+    else:
+      self.left, self.right = False, False
+
+  @property
+  def warning(self) -> bool:
+    return bool(self.left or self.right)

iqpilot/selfdrive/controls/lib/longcontrol.py

@@ -0,0 +1,95 @@
+import numpy as np
+from iqpilot.cereal import car
+from iqpilot.common.realtime import DT_CTRL
+from iqpilot.selfdrive.controls.lib.drive_helpers import CONTROL_N
+from iqpilot.common.pid import PIDController
+from iqpilot.selfdrive.iqmodeld.config import ModelConstants
+from iqpilot.selfdrive.controls.lib.smooth_stops import SmoothStopController
+
+CONTROL_N_T_IDX = ModelConstants.T_IDXS[:CONTROL_N]
+
+LongCtrlState = car.CarControl.Actuators.LongControlState
+
+
+def long_control_state_trans(CP_IQ, active, long_control_state, should_stop, brake_pressed, cruise_standstill):
+  # Gas Interceptor
+  cruise_standstill = cruise_standstill and not CP_IQ.enableGasInterceptor
+
+  starting_condition = (not should_stop and
+                        not cruise_standstill and
+                        not brake_pressed)
+
+  if not active:
+    long_control_state = LongCtrlState.off
+
+  else:
+    if long_control_state == LongCtrlState.off:
+      if not starting_condition:
+        long_control_state = LongCtrlState.stopping
+      else:
+        long_control_state = LongCtrlState.pid
+
+    elif long_control_state == LongCtrlState.stopping:
+      if starting_condition:
+        long_control_state = LongCtrlState.pid
+
+    elif long_control_state == LongCtrlState.pid:
+      if should_stop:
+        long_control_state = LongCtrlState.stopping
+  return long_control_state
+
+class LongControl:
+  def __init__(self, CP, CP_IQ):
+    self.CP = CP
+    self.CP_IQ = CP_IQ
+    self.long_control_state = LongCtrlState.off
+    self.pid = PIDController((CP.longitudinalTuning.kpBP, CP.longitudinalTuning.kpV),
+                             (CP.longitudinalTuning.kiBP, CP.longitudinalTuning.kiV),
+                             rate=1 / DT_CTRL)
+    self.last_output_accel = 0.0
+    self.stopping_decel_rate = CP_IQ.stoppingDecelRateOverride or 1.0
+    self.smooth = SmoothStopController()
+
+  def reset(self):
+    self.pid.reset()
+
+  def update(self, active, CS, a_target, should_stop, accel_limits, lead_distance=0.0, has_lead=False, gas_override=False):
+    """Update longitudinal control. This updates the state machine and runs a PID loop"""
+    self.pid.neg_limit = accel_limits[0]
+    self.pid.pos_limit = accel_limits[1]
+    self.smooth.update()
+
+    if self.smooth.enabled and active and self.long_control_state != LongCtrlState.stopping:
+      stop_now = self.smooth.want_hold(should_stop, CS.vEgo, CS.standstill)
+    else:
+      stop_now = should_stop
+
+    self.long_control_state = long_control_state_trans(self.CP_IQ, active, self.long_control_state, stop_now, CS.brakePressed,
+                                                       CS.cruiseState.standstill)
+    if self.long_control_state == LongCtrlState.off:
+      self.reset()
+      self.smooth.reset()
+      output_accel = 0.
+
+    elif self.long_control_state == LongCtrlState.stopping:
+      output_accel = self.last_output_accel
+      if output_accel > self.CP.stopAccel:
+        output_accel = min(output_accel, 0.0)
+        # TODO: can we just go straight to stopAccel?
+        output_accel -= self.stopping_decel_rate * DT_CTRL  # m/s^2/s while trying to stop
+      self.reset()
+      self.smooth.reset()
+
+    else:  # LongCtrlState.pid
+      if self.smooth.enabled and active and should_stop:
+        output_accel = self.smooth.settle(a_target, CS.vEgo, lead_distance, has_lead, self.last_output_accel)
+        self.reset()
+      else:
+        error = a_target - CS.aEgo
+        output_accel = self.pid.update(error, speed=CS.vEgo,
+                                       feedforward=a_target,
+                                       freeze_integrator=gas_override)
+        self.smooth.reset()
+
+    self.last_output_accel = np.clip(output_accel, accel_limits[0], accel_limits[1])
+    return self.last_output_accel

iqpilot/selfdrive/controls/lib/longitudinal_mpc_lib/.gitignore

@@ -0,0 +1,2 @@
+acados_ocp_long.json
+c_generated_code/

iqpilot/selfdrive/controls/lib/longitudinal_mpc_lib/SConscript

@@ -0,0 +1,105 @@
+Import('env', 'envCython', 'arch', 'msgq_python', 'common_python', 'np_version')
+
+gen = "c_generated_code"
+
+casadi_model = [
+  f'{gen}/long_model/long_expl_ode_fun.c',
+  f'{gen}/long_model/long_expl_vde_forw.c',
+]
+
+casadi_cost_y = [
+  f'{gen}/long_cost/long_cost_y_fun.c',
+  f'{gen}/long_cost/long_cost_y_fun_jac_ut_xt.c',
+  f'{gen}/long_cost/long_cost_y_hess.c',
+]
+
+casadi_cost_e = [
+  f'{gen}/long_cost/long_cost_y_e_fun.c',
+  f'{gen}/long_cost/long_cost_y_e_fun_jac_ut_xt.c',
+  f'{gen}/long_cost/long_cost_y_e_hess.c',
+]
+
+casadi_cost_0 = [
+  f'{gen}/long_cost/long_cost_y_0_fun.c',
+  f'{gen}/long_cost/long_cost_y_0_fun_jac_ut_xt.c',
+  f'{gen}/long_cost/long_cost_y_0_hess.c',
+]
+
+casadi_constraints = [
+  f'{gen}/long_constraints/long_constr_h_fun.c',
+  f'{gen}/long_constraints/long_constr_h_fun_jac_uxt_zt.c',
+]
+
+build_files = [f'{gen}/acados_solver_long.c'] + casadi_model + casadi_cost_y + casadi_cost_e +  \
+              casadi_cost_0 + casadi_constraints
+
+# extra generated files used to trigger a rebuild
+generated_files = [
+  f'{gen}/Makefile',
+
+  f'{gen}/main_long.c',
+  f'{gen}/main_sim_long.c',
+  f'{gen}/acados_solver_long.h',
+  f'{gen}/acados_sim_solver_long.h',
+  f'{gen}/acados_sim_solver_long.c',
+  f'{gen}/acados_solver.pxd',
+
+  f'{gen}/long_model/long_expl_vde_adj.c',
+
+  f'{gen}/long_model/long_model.h',
+  f'{gen}/long_constraints/long_constraints.h',
+  f'{gen}/long_cost/long_cost.h',
+] + build_files
+
+acados_dir = '#iqpilot/third_party/acados'
+acados_templates_dir = '#iqpilot/third_party/acados/acados_template/c_templates_tera'
+
+source_list = ['long_mpc.py',
+  '#iqpilot/selfdrive/iqmodeld/config.py',
+  f'{acados_dir}/include/acados_c/ocp_nlp_interface.h',
+  f'{acados_templates_dir}/acados_solver.in.c',
+]
+
+lenv = env.Clone()
+acados_rel_path = Dir(gen).rel_path(Dir(f"#iqpilot/third_party/acados/{arch}/lib"))
+lenv["RPATH"] += [lenv.Literal(f'\\$$ORIGIN/{acados_rel_path}')]
+lenv.Clean(generated_files, Dir(gen))
+_mpc_dir = Dir('.').abspath
+generated_long = lenv.Command(generated_files,
+                              source_list,
+                              lenv.PrettyAction(f"cd {_mpc_dir} && python3 long_mpc.py", 'GEN',
+                                                logfile=f"{_mpc_dir}/gen.log", capture_stderr=True))
+lenv.Depends(generated_long, [msgq_python, common_python])
+
+lenv["CFLAGS"].append("-DACADOS_WITH_QPOASES")
+lenv["CXXFLAGS"].append("-DACADOS_WITH_QPOASES")
+lenv["CCFLAGS"].append("-Wno-unused")
+if arch != "Darwin":
+  lenv["LINKFLAGS"].append("-Wl,--disable-new-dtags")
+else:
+  lenv["LINKFLAGS"].append("-Wl,-install_name,@loader_path/libacados_ocp_solver_long.dylib")
+  lenv["LINKFLAGS"].append(f"-Wl,-rpath,@loader_path/{acados_rel_path}")
+lib_solver = lenv.SharedLibrary(f"{gen}/acados_ocp_solver_long",
+                                build_files,
+                                LIBS=['m', 'acados', 'hpipm', 'blasfeo', 'qpOASES_e'])
+
+# generate cython stuff
+acados_ocp_solver_pyx = File("#iqpilot/third_party/acados/acados_template/acados_ocp_solver_pyx.pyx")
+acados_ocp_solver_common = File("#iqpilot/third_party/acados/acados_template/acados_solver_common.pxd")
+libacados_ocp_solver_pxd = File(f'{gen}/acados_solver.pxd')
+libacados_ocp_solver_c = File(f'{gen}/acados_ocp_solver_pyx.c')
+
+lenv2 = envCython.Clone()
+lenv2["LIBPATH"] += [lib_solver[0].dir.abspath]
+lenv2["RPATH"] += [lenv2.Literal('\\$$ORIGIN')]
+lenv2.Command(libacados_ocp_solver_c,
+  [acados_ocp_solver_pyx, acados_ocp_solver_common, libacados_ocp_solver_pxd],
+  lenv2.PrettyAction(
+    f'cython' + \
+    f' -o {libacados_ocp_solver_c.get_labspath()}' + \
+    f' -I {libacados_ocp_solver_pxd.get_dir().get_labspath()}' + \
+    f' -I {acados_ocp_solver_common.get_dir().get_labspath()}' + \
+    f' {acados_ocp_solver_pyx.get_labspath()}', 'CYTHON'))
+lib_cython = lenv2.Program(f'{gen}/acados_ocp_solver_pyx.so', [libacados_ocp_solver_c], LIBS=['acados_ocp_solver_long'])
+lenv2.Depends(lib_cython, lib_solver)
+lenv2.Depends(libacados_ocp_solver_c, np_version)

iqpilot/selfdrive/controls/lib/longitudinal_mpc_lib/long_mpc.py

@@ -0,0 +1,433 @@
+#!/usr/bin/env python3
+import os
+import time
+import numpy as np
+from iqpilot.cereal import log
+from iqdbc.car.interfaces import ACCEL_MIN, ACCEL_MAX
+from iqpilot.common.realtime import DT_MDL
+from iqpilot.common.swaglog import cloudlog
+# WARNING: imports outside of constants will not trigger a rebuild
+from iqpilot.selfdrive.iqmodeld.config import index_function, ModelConstants
+from iqpilot.selfdrive.controls.radard import _LEAD_ACCEL_TAU  # legacy lead extrapolation (newLeadMpc=False)
+from iqpilot.common.params import Params, UnknownKeyName
+
+LEAD_T_IDXS_MODEL = np.array(ModelConstants.LEAD_T_IDXS)  # [0, 2, 4, 6, 8, 10]s
+
+if __name__ == '__main__':  # generating code
+  from iqpilot.third_party.acados.acados_template import AcadosModel, AcadosOcp, AcadosOcpSolver
+else:
+  from iqpilot.selfdrive.controls.lib.longitudinal_mpc_lib.c_generated_code.acados_ocp_solver_pyx import AcadosOcpSolverCython
+
+from casadi import SX, vertcat
+
+MODEL_NAME = 'long'
+LONG_MPC_DIR = os.path.dirname(os.path.abspath(__file__))
+EXPORT_DIR = os.path.join(LONG_MPC_DIR, "c_generated_code")
+JSON_FILE = os.path.join(LONG_MPC_DIR, "acados_ocp_long.json")
+
+LongitudinalPlanSource = log.LongitudinalPlan.LongitudinalPlanSource
+MPC_SOURCES = (LongitudinalPlanSource.lead0, LongitudinalPlanSource.lead1)
+
+X_DIM = 3
+U_DIM = 1
+PARAM_DIM = 5
+COST_E_DIM = 4
+COST_DIM = COST_E_DIM + 1
+CONSTR_DIM = 4
+
+X_EGO_OBSTACLE_COST = 3.
+X_EGO_COST = 0.
+V_EGO_COST = 0.
+A_EGO_COST = 0.
+J_EGO_COST = 20.
+DANGER_ZONE_COST = 100.
+CRASH_DISTANCE = .25
+LEAD_DANGER_FACTOR = 0.75
+LIMIT_COST = 1e6
+ACADOS_SOLVER_TYPE = 'SQP_RTI'
+
+# Fewer timestamps don't hurt performance and lead to
+# much better convergence of the MPC with low iterations
+N = 12
+MAX_T = 10.0
+T_IDXS_LST = [index_function(idx, max_val=MAX_T, max_idx=N) for idx in range(N+1)]
+
+T_IDXS = np.array(T_IDXS_LST)
+FCW_IDXS = T_IDXS < 5.0
+T_DIFFS = np.diff(T_IDXS, prepend=[0.])
+COMFORT_BRAKE = 2.5
+STOP_DISTANCE = 3.0
+MIN_X_LEAD_FACTOR = 0.5
+LEAD_PULLAWAY_VREL = 0.5
+LEAD_PULLAWAY_ABRAKE = -0.5
+
+def get_jerk_factor(personality=log.LongitudinalPersonality.standard):
+  if personality==log.LongitudinalPersonality.relaxed:
+    return 1.0
+  elif personality==log.LongitudinalPersonality.standard:
+    return 1.0
+  elif personality==log.LongitudinalPersonality.aggressive:
+    return 0.5
+  else:
+    raise NotImplementedError("Longitudinal personality not supported")
+
+
+def get_T_FOLLOW(personality=log.LongitudinalPersonality.standard):
+  if personality==log.LongitudinalPersonality.relaxed:
+    return 1.75
+  elif personality==log.LongitudinalPersonality.standard:
+    return 1.45
+  elif personality==log.LongitudinalPersonality.aggressive:
+    return 1.25
+  else:
+    raise NotImplementedError("Longitudinal personality not supported")
+
+def get_stopped_equivalence_factor(v_lead):
+  return (v_lead**2) / (2 * COMFORT_BRAKE)
+
+def get_safe_obstacle_distance(v_ego, t_follow):
+  return (v_ego**2) / (2 * COMFORT_BRAKE) + t_follow * v_ego + STOP_DISTANCE
+
+def gen_long_model():
+  model = AcadosModel()
+  model.name = MODEL_NAME
+
+  # states
+  x_ego, v_ego, a_ego = SX.sym('x_ego'), SX.sym('v_ego'), SX.sym('a_ego')
+  model.x = vertcat(x_ego, v_ego, a_ego)
+
+  # controls
+  j_ego = SX.sym('j_ego')
+  model.u = vertcat(j_ego)
+
+  # xdot
+  x_ego_dot = SX.sym('x_ego_dot')
+  v_ego_dot = SX.sym('v_ego_dot')
+  a_ego_dot = SX.sym('a_ego_dot')
+  model.xdot = vertcat(x_ego_dot, v_ego_dot, a_ego_dot)
+
+  # live parameters
+  a_min = SX.sym('a_min')
+  a_max = SX.sym('a_max')
+  x_obstacle = SX.sym('x_obstacle')
+  lead_t_follow = SX.sym('lead_t_follow')
+  lead_danger_factor = SX.sym('lead_danger_factor')
+  model.p = vertcat(a_min, a_max, x_obstacle, lead_t_follow, lead_danger_factor)
+
+  # dynamics model
+  f_expl = vertcat(v_ego, a_ego, j_ego)
+  model.f_impl_expr = model.xdot - f_expl
+  model.f_expl_expr = f_expl
+  return model
+
+def gen_long_ocp():
+  ocp = AcadosOcp()
+  ocp.model = gen_long_model()
+
+  Tf = T_IDXS[-1]
+
+  # set dimensions
+  ocp.dims.N = N
+
+  # set cost module
+  ocp.cost.cost_type = 'NONLINEAR_LS'
+  ocp.cost.cost_type_e = 'NONLINEAR_LS'
+
+  QR = np.zeros((COST_DIM, COST_DIM))
+  Q = np.zeros((COST_E_DIM, COST_E_DIM))
+
+  ocp.cost.W = QR
+  ocp.cost.W_e = Q
+
+  x_ego, v_ego, a_ego = ocp.model.x[0], ocp.model.x[1], ocp.model.x[2]
+  j_ego = ocp.model.u[0]
+
+  a_min, a_max = ocp.model.p[0], ocp.model.p[1]
+  x_obstacle = ocp.model.p[2]
+  lead_t_follow = ocp.model.p[3]
+  lead_danger_factor = ocp.model.p[4]
+
+  ocp.cost.yref = np.zeros((COST_DIM, ))
+  ocp.cost.yref_e = np.zeros((COST_E_DIM, ))
+
+  desired_dist_comfort = get_safe_obstacle_distance(v_ego, lead_t_follow)
+
+  # The main cost in normal operation is how close you are to the "desired" distance
+  # from an obstacle at every timestep. This obstacle can be a lead car
+  # or other object. In e2e mode we can use x_position targets as a cost
+  # instead.
+  costs = [((x_obstacle - x_ego) - (desired_dist_comfort)) / (v_ego + 10.),
+           x_ego,
+           v_ego,
+           a_ego,
+           j_ego]
+  ocp.model.cost_y_expr = vertcat(*costs)
+  ocp.model.cost_y_expr_e = vertcat(*costs[:-1])
+
+  # Constraints on speed, acceleration and desired distance to
+  # the obstacle, which is treated as a slack constraint so it
+  # behaves like an asymmetrical cost.
+  constraints = vertcat(v_ego,
+                        (a_ego - a_min),
+                        (a_max - a_ego),
+                        ((x_obstacle - x_ego) - lead_danger_factor * (desired_dist_comfort)) / (v_ego + 10.))
+  ocp.model.con_h_expr = constraints
+
+  x0 = np.zeros(X_DIM)
+  ocp.constraints.x0 = x0
+  ocp.parameter_values = np.array([-1.2, 1.2, 0.0, get_T_FOLLOW(), LEAD_DANGER_FACTOR])
+
+
+  # We put all constraint cost weights to 0 and only set them at runtime
+  cost_weights = np.zeros(CONSTR_DIM)
+  ocp.cost.zl = cost_weights
+  ocp.cost.Zl = cost_weights
+  ocp.cost.Zu = cost_weights
+  ocp.cost.zu = cost_weights
+
+  ocp.constraints.lh = np.zeros(CONSTR_DIM)
+  ocp.constraints.uh = 1e4*np.ones(CONSTR_DIM)
+  ocp.constraints.idxsh = np.arange(CONSTR_DIM)
+
+  # The HPIPM solver can give decent solutions even when it is stopped early
+  # Which is critical for our purpose where compute time is strictly bounded
+  # We use HPIPM in the SPEED_ABS mode, which ensures fastest runtime. This
+  # does not cause issues since the problem is well bounded.
+  ocp.solver_options.qp_solver = 'PARTIAL_CONDENSING_HPIPM'
+  ocp.solver_options.hessian_approx = 'GAUSS_NEWTON'
+  ocp.solver_options.integrator_type = 'ERK'
+  ocp.solver_options.nlp_solver_type = ACADOS_SOLVER_TYPE
+  ocp.solver_options.qp_solver_cond_N = 1
+
+  # More iterations take too much time and less lead to inaccurate convergence in
+  # some situations. Ideally we would run just 1 iteration to ensure fixed runtime.
+  ocp.solver_options.qp_solver_iter_max = 10
+  ocp.solver_options.qp_tol = 1e-3
+
+  # set prediction horizon
+  ocp.solver_options.tf = Tf
+  ocp.solver_options.shooting_nodes = T_IDXS
+
+  ocp.code_export_directory = EXPORT_DIR
+  return ocp
+
+
+class LongitudinalMpc:
+  def __init__(self, dt=DT_MDL):
+    self.dt = dt
+    self._params = Params()
+    self.new_lead_mpc = self._read_new_lead_mpc()
+    self.solver = AcadosOcpSolverCython(MODEL_NAME, ACADOS_SOLVER_TYPE, N)
+    self.reset()
+    self.source = LongitudinalPlanSource.cruise
+
+  def _read_new_lead_mpc(self) -> bool:
+    try:
+      return self._params.get_bool("newLeadMpc")
+    except UnknownKeyName:
+      return True
+
+  def reset(self):
+    self.solver.reset()
+
+    self.x_sol = np.zeros((N+1, X_DIM))
+    self.u_sol = np.zeros((N, 1))
+    self.v_solution = np.zeros(N+1)
+    self.a_solution = np.zeros(N+1)
+    self.j_solution = np.zeros(N)
+    self.yref = np.zeros((N+1, COST_DIM))
+
+    for i in range(N):
+      self.solver.cost_set(i, "yref", self.yref[i])
+    self.solver.cost_set(N, "yref", self.yref[N][:COST_E_DIM])
+
+    self.params = np.zeros((N+1, PARAM_DIM))
+    for i in range(N+1):
+      self.solver.set(i, 'x', np.zeros(X_DIM))
+
+    self.last_cloudlog_t = 0
+    self.status = False
+    self.crash_cnt = 0.0
+    self.solution_status = 0
+    # timers
+    self.solve_time = 0.0
+    self.time_qp_solution = 0.0
+    self.time_linearization = 0.0
+    self.time_integrator = 0.0
+    self.x0 = np.zeros(X_DIM)
+    self.lead_xv_0 = np.zeros((N+1, 2))
+    self.lead_xv_1 = np.zeros((N+1, 2))
+    self.set_weights()
+
+  def set_cost_weights(self, cost_weights, constraint_cost_weights):
+    W = np.asfortranarray(np.diag(cost_weights))
+    for i in range(N):
+      self.solver.cost_set(i, 'W', W)
+    # Setting the slice without the copy make the array not contiguous,
+    # causing issues with the C interface.
+    self.solver.cost_set(N, 'W', np.copy(W[:COST_E_DIM, :COST_E_DIM]))
+
+    # Set L2 slack cost on lower bound constraints
+    Zl = np.array(constraint_cost_weights)
+    for i in range(N):
+      self.solver.cost_set(i, 'Zl', Zl)
+
+  def set_weights(self, personality=log.LongitudinalPersonality.standard):
+    jerk_factor = get_jerk_factor(personality)
+    cost_weights = [X_EGO_OBSTACLE_COST, X_EGO_COST, V_EGO_COST, A_EGO_COST, jerk_factor * J_EGO_COST]
+    constraint_cost_weights = [LIMIT_COST, LIMIT_COST, LIMIT_COST, DANGER_ZONE_COST]
+    self.set_cost_weights(cost_weights, constraint_cost_weights)
+
+  def set_cur_state(self, v, a):
+    v_prev = self.x0[1]
+    self.x0[1] = v
+    self.x0[2] = a
+    if abs(v_prev - v) > 2.:  # probably only helps if v < v_prev
+      for i in range(N+1):
+        self.solver.set(i, 'x', self.x0)
+
+  @staticmethod
+  def extrapolate_lead(x_lead, v_lead, a_lead, a_lead_tau):
+    a_lead_traj = a_lead * np.exp(-a_lead_tau * (T_IDXS**2)/2.)
+    v_lead_traj = np.clip(v_lead + np.cumsum(T_DIFFS * a_lead_traj), 0.0, 1e8)
+    x_lead_traj = x_lead + np.cumsum(T_DIFFS * v_lead_traj)
+    lead_xv = np.column_stack((x_lead_traj, v_lead_traj))
+    return lead_xv
+
+  def process_lead_legacy(self, lead):
+    # behavior before PR #37824 (newLeadMpc=False): one immediate radar lead prediction
+    # extrapolated forward with acceleration decaying to 0
+    v_ego = self.x0[1]
+    if lead is not None and lead.status:
+      x_lead = lead.dRel
+      v_lead = lead.vLead
+      a_lead = lead.aLeadK
+      a_lead_tau = lead.aLeadTau
+    else:
+      # Fake a fast lead car, so mpc can keep running in the same mode
+      x_lead = 50.0
+      v_lead = v_ego + 10.0
+      a_lead = 0.0
+      a_lead_tau = _LEAD_ACCEL_TAU
+
+    # MPC will not converge if immediate crash is expected
+    # Clip lead distance to what is still possible to brake for
+    min_x_lead = MIN_X_LEAD_FACTOR * (v_ego + v_lead) * (v_ego - v_lead) / (-ACCEL_MIN * 2)
+    x_lead = np.clip(x_lead, min_x_lead, 1e8)
+    v_lead = np.clip(v_lead, 0.0, 1e8)
+    a_lead = np.clip(a_lead, -10., 5.)
+    lead_xv = self.extrapolate_lead(x_lead, v_lead, a_lead, a_lead_tau)
+    return lead_xv
+
+  def process_lead(self, model_lead, radar_lead):
+    v_ego = self.x0[1]
+    try:
+      x_model = np.asarray(model_lead.x, dtype=np.float64)
+      v_model = np.asarray(model_lead.v, dtype=np.float64)
+      valid_model_lead = (float(model_lead.prob) > 0.5 and radar_lead.status and float(radar_lead.modelProb) > 0.5 and
+                          x_model.shape == LEAD_T_IDXS_MODEL.shape and v_model.shape == LEAD_T_IDXS_MODEL.shape and
+                          np.all(np.isfinite(x_model)) and np.all(np.isfinite(v_model)))
+    except (AttributeError, TypeError, ValueError):
+      valid_model_lead = False
+
+    if not valid_model_lead:
+      return self.process_lead_legacy(radar_lead)
+
+    x_lead_traj = float(radar_lead.dRel) + (x_model - x_model[0])
+    v_lead_traj = float(radar_lead.vLead) + (v_model - v_model[0])
+
+    # MPC won't converge on immediate crashes; lift h=0 to the minimum braking distance.
+    v_lead_0 = v_lead_traj[0]
+    min_x_lead = MIN_X_LEAD_FACTOR * (v_ego + v_lead_0) * (v_ego - v_lead_0) / (-ACCEL_MIN * 2)
+    x_lead_traj[0] = max(x_lead_traj[0], min_x_lead)
+    v_lead_traj = np.clip(v_lead_traj, 0.0, 1e8)
+
+    x_lead_mpc = np.maximum.accumulate(np.interp(T_IDXS, LEAD_T_IDXS_MODEL, x_lead_traj))
+    v_lead_mpc = np.interp(T_IDXS, LEAD_T_IDXS_MODEL, v_lead_traj)
+    if radar_lead.status and radar_lead.vRel > LEAD_PULLAWAY_VREL and radar_lead.aLeadK > LEAD_PULLAWAY_ABRAKE:
+      # ty spysyweeb for lead pull away fix phantom launch braking so you don't ram the lead in edge cases.
+      radar_velocity_floor = np.full_like(T_IDXS, float(radar_lead.vLead))
+      radar_distance_floor = float(radar_lead.dRel) + float(radar_lead.vLead) * T_IDXS
+      v_lead_mpc = np.maximum(v_lead_mpc, radar_velocity_floor)
+      x_lead_mpc = np.maximum(x_lead_mpc, radar_distance_floor)
+    return np.column_stack((x_lead_mpc, v_lead_mpc))
+
+  def update(self, modelV2, radarstate, personality=log.LongitudinalPersonality.standard):
+    self.new_lead_mpc = self._read_new_lead_mpc()
+    t_follow = get_T_FOLLOW(personality)
+    model_leads = modelV2.leadsV3
+
+    if self.new_lead_mpc:
+      self.status = radarstate.leadOne.status or radarstate.leadTwo.status
+      model_lead_0 = model_leads[0] if len(model_leads) > 0 else None
+      model_lead_1 = model_leads[1] if len(model_leads) > 1 else None
+      lead_xv_0 = self.process_lead(model_lead_0, radarstate.leadOne)
+      lead_xv_1 = self.process_lead(model_lead_1, radarstate.leadTwo)
+    else:
+      # pre-PR behavior: radar lead extrapolated with accel decay
+      self.status = radarstate.leadOne.status or radarstate.leadTwo.status
+      lead_xv_0 = self.process_lead_legacy(radarstate.leadOne)
+      lead_xv_1 = self.process_lead_legacy(radarstate.leadTwo)
+    self.lead_xv_0 = lead_xv_0
+    self.lead_xv_1 = lead_xv_1
+
+    # To estimate a safe distance from a moving lead, we calculate how much stopping
+    # distance that lead needs as a minimum. We can add that to the current distance
+    # and then treat that as a stopped car/obstacle at this new distance.
+    lead_0_obstacle = lead_xv_0[:,0] + get_stopped_equivalence_factor(lead_xv_0[:,1])
+    lead_1_obstacle = lead_xv_1[:,0] + get_stopped_equivalence_factor(lead_xv_1[:,1])
+
+    x_obstacles = np.column_stack([lead_0_obstacle, lead_1_obstacle])
+    self.source = MPC_SOURCES[np.argmin(x_obstacles[0])]
+
+    self.yref[:,:] = 0.0
+    for i in range(N):
+      self.solver.set(i, "yref", self.yref[i])
+    self.solver.set(N, "yref", self.yref[N][:COST_E_DIM])
+
+    self.params[:,0] = ACCEL_MIN
+    self.params[:,1] = ACCEL_MAX
+    self.params[:,2] = np.min(x_obstacles, axis=1)
+    self.params[:,3] = t_follow
+    self.params[:,4] = LEAD_DANGER_FACTOR
+
+    self.run()
+    lead_crash_prob = radarstate.leadOne.modelProb
+    if (np.any(lead_xv_0[FCW_IDXS,0] - self.x_sol[FCW_IDXS,0] < CRASH_DISTANCE) and
+            lead_crash_prob > 0.9):
+      self.crash_cnt += 1
+    else:
+      self.crash_cnt = 0
+
+  def run(self):
+    for i in range(N+1):
+      self.solver.set(i, 'p', self.params[i])
+    self.solver.constraints_set(0, "lbx", self.x0)
+    self.solver.constraints_set(0, "ubx", self.x0)
+
+    self.solution_status = self.solver.solve()
+    self.solve_time = float(self.solver.get_stats('time_tot')[0])
+    self.time_qp_solution = float(self.solver.get_stats('time_qp')[0])
+    self.time_linearization = float(self.solver.get_stats('time_lin')[0])
+    self.time_integrator = float(self.solver.get_stats('time_sim')[0])
+
+    for i in range(N+1):
+      self.x_sol[i] = self.solver.get(i, 'x')
+    for i in range(N):
+      self.u_sol[i] = self.solver.get(i, 'u')
+
+    self.v_solution = self.x_sol[:,1]
+    self.a_solution = self.x_sol[:,2]
+    self.j_solution = self.u_sol[:,0]
+
+    t = time.monotonic()
+    if self.solution_status != 0:
+      if t > self.last_cloudlog_t + 5.0:
+        self.last_cloudlog_t = t
+        cloudlog.warning(f"Long mpc reset, solution_status: {self.solution_status}")
+      self.reset()
+
+
+if __name__ == "__main__":
+  ocp = gen_long_ocp()
+  AcadosOcpSolver.generate(ocp, json_file=JSON_FILE)

iqpilot/selfdrive/controls/lib/longitudinal_planner.py

@@ -1,256 +1,297 @@
-"""
-Copyright © IQ.Lvbs, apart of Project Teal Lvbs, All Rights Reserved, licensed under https://konn3kt.com/tos
-"""
-from datetime import datetime
-
+#!/usr/bin/env python3
+import math
 import numpy as np
 
-from cereal import messaging, custom
-from iqdbc.car import structs
-from openpilot.common.constants import CV
-from openpilot.common.realtime import DT_MDL
-from openpilot.selfdrive.car.cruise import V_CRUISE_MAX
-from openpilot.iqpilot.selfdrive.controls.lib.custom_stop_distance import CustomStopDistance
-from openpilot.iqpilot.selfdrive.controls.lib.iq_dynamic.engine import IQDynamicController
-from openpilot.iqpilot.selfdrive.controls.lib.iq_dynamic.imahelper import IQConstants
-from openpilot.iqpilot.selfdrive.controls.lib.helpers.e2e_alerts import EndToEndAlertEngine
-from openpilot.iqpilot.selfdrive.controls.lib.slc_vcruise import SLCVCruise
-from openpilot.iqpilot.selfdrive.controls.lib.speed_limit_controller import LIMIT_ADAPT_ACC
-from openpilot.iqpilot.selfdrive.selfdrived.events import IQEvents
-from openpilot.iqpilot.selfdrive.iqmodeld.models.helpers import get_active_bundle
+import iqpilot.cereal.messaging as messaging
+from iqdbc.car.interfaces import ACCEL_MIN, ACCEL_MAX
+from iqpilot.common.constants import CV
+from iqpilot.common.filter_simple import FirstOrderFilter
+from iqpilot.common.params import Params, UnknownKeyName
+from iqpilot.common.realtime import DT_MDL
+from iqpilot.selfdrive.iqmodeld.config import ModelConstants
+from iqpilot.selfdrive.controls.lib.longcontrol import LongCtrlState
+from iqpilot.selfdrive.controls.lib.longitudinal_mpc_lib.long_mpc import LongitudinalMpc, LongitudinalPlanSource
+from iqpilot.selfdrive.controls.lib.longitudinal_mpc_lib.long_mpc import T_IDXS as T_IDXS_MPC
+from iqpilot.selfdrive.controls.lib.drive_helpers import CONTROL_N, DEFAULT_STOPPING_SPEED, get_accel_from_plan
+from iqpilot.selfdrive.car.cruise import V_CRUISE_MAX, V_CRUISE_UNSET
+from iqpilot.common.swaglog import cloudlog
+from iqpilot.common.issue_debug import log_issue_limited
 
-IQDynamicState = custom.IQPlan.IQDynamicControl.IQDynamicControlState
-LongitudinalPlanSource = custom.IQPlan.LongitudinalPlanSource
-SpeedLimitAssistState = custom.IQPlan.SpeedLimit.AssistState
-SpeedLimitSource = custom.IQPlan.SpeedLimit.Source
-NavProvider = custom.IQNavState.LongitudinalProvider
-NavLongitudinalState = custom.IQNavState.LongitudinalState
+from iqpilot.selfdrive.controls.lib.iq_longitudinal_planner import LongitudinalPlannerIQ
 
-class LongitudinalPlannerIQ:
-  def __init__(self, CP: structs.CarParams, CP_IQ: structs.IQCarParams, mpc):
-    self.events_iq = IQEvents()
-    self.iq_dynamic = IQDynamicController(CP, mpc)
-    self.custom_stop_distance = CustomStopDistance()
-    self.slimit = SLCVCruise()
-    self.generation = int(model_bundle.generation) if (model_bundle := get_active_bundle()) else None
-    self.source = LongitudinalPlanSource.cruise
-    self.e2e_alerts = EndToEndAlertEngine()
-    self.output_v_target = 0.
-    self.output_a_target = 0.
-    self.speed_limit_last = 0.
-    self.speed_limit_final_last = 0.
-    self.speed_limit_source = SpeedLimitSource.none
-    self.nav_engaged = False
-    self.nav_provider = NavProvider.none
-    self.nav_state = NavLongitudinalState.disabled
-    self.nav_speed_target = 0.
-    self.nav_accel_target = 0.
-    self.nav_valid = False
-    self.force_stop_timer = 0.0
-    self.forcing_stop = False
-    self.override_force_stop = False
-    self.override_force_stop_timer = 0.0
-    self.tracked_model_length = 0.0
+A_CRUISE_MAX_VALS = [2.0, 1.6, 0.8, 0.6]
+A_CRUISE_MAX_BP = [0., 10.0, 25., 40.]
+A_CRUISE_MIN = -1.2
+J_CRUISE = 1.0
+CONTROL_N_T_IDX = ModelConstants.T_IDXS[:CONTROL_N]
+ALLOW_THROTTLE_THRESHOLD = 0.4
+MIN_ALLOW_THROTTLE_SPEED = 2.5
 
-  def is_e2e(self, sm: messaging.SubMaster) -> bool:
-    experimental_mode = sm['selfdriveState'].experimentalMode
-    if not self.iq_dynamic.active():
-      return experimental_mode
+LAUNCH_DISARM_SPEED = 2.0
+LAUNCH_COMMIT_T = 3.5
+LAUNCH_MOVING_SPEED = 1.2
+LAUNCH_MAX_ACCEL = 1.5
 
-    return experimental_mode and self.iq_dynamic.mode() == "blended"
+E2E_CRUISE_CONVERGENCE_TAU = 15.0
+E2E_CRUISE_ACCEL_MAX = 0.5
+E2E_MODEL_SPEED_HORIZON = 5.0
+E2E_ACCEL_INTENT_BP = [-0.05, 0.05]
+E2E_MODEL_SPEED_INTENT_BP = [-0.5, 0.0]
 
-  def update_targets(self, sm: messaging.SubMaster, v_ego: float, a_ego: float, v_cruise: float) -> tuple[float, float]:
-    CS = sm['carState']
-    v_cruise_cluster_kph = min(CS.vCruiseCluster, V_CRUISE_MAX)
-    v_cruise_cluster = v_cruise_cluster_kph * CV.KPH_TO_MS
-    # SLC should apply whenever IQ.Pilot is engaged, even on stock-longitudinal cars
-    # where carControl.longActive stays false.
-    slc_apply_enabled = bool(getattr(sm['selfdriveState'], "enabled", False))
+# Lookup table for turns
+_A_TOTAL_MAX_V = [1.7, 3.2]
+_A_TOTAL_MAX_BP = [20., 40.]
 
-    nav_state = sm['iqNavState']
-    self.nav_engaged = bool(getattr(nav_state, "longitudinalEngaged", False))
-    self.nav_provider = getattr(nav_state, "longitudinalProvider", NavProvider.none)
-    self.nav_state = getattr(nav_state, "longitudinalState", NavLongitudinalState.disabled)
-    self.nav_speed_target = float(getattr(nav_state, "speedTarget", 0.0))
-    self.nav_accel_target = float(getattr(nav_state, "accelTarget", 0.0))
-    self.nav_valid = bool(getattr(nav_state, "valid", False) and self.nav_engaged)
+def get_max_accel(v_ego):
+  return np.interp(v_ego, A_CRUISE_MAX_BP, A_CRUISE_MAX_VALS)
 
-    # IQ.Pilot custom Speed Limit Controller
-    now = datetime.now()
-    if hasattr(sm, "alive"):
-      time_validated = sm.alive.get('clocks', False) and getattr(sm['clocks'], 'timeValid', False)
+def get_coast_accel(pitch):
+  return np.sin(pitch) * -5.65 - 0.3  # fitted from data using xx/projects/allow_throttle/compute_coast_accel.py
+
+def get_lead_distance(radarState):
+  if radarState.leadOne.status and (not radarState.leadTwo.status or radarState.leadOne.dRel < radarState.leadTwo.dRel):
+    return radarState.leadOne.dRel
+  if radarState.leadTwo.status:
+    return radarState.leadTwo.dRel
+  return 0
+
+def get_cruise_accel(e2e, v_cruise, v_ego, a_cruise_prev, angle_steers, CP, dt, accel_coast, allow_throttle):
+  max_accel = ACCEL_MAX if e2e else get_max_accel(v_ego)
+
+  if not e2e:
+    a_total_max = np.interp(v_ego, _A_TOTAL_MAX_BP, _A_TOTAL_MAX_V)
+    a_y = v_ego ** 2 * angle_steers * CV.DEG_TO_RAD / (CP.steerRatio * CP.wheelbase)
+    a_x_allowed = math.sqrt(max(a_total_max ** 2 - a_y ** 2, 0.))
+    max_accel = min(max_accel, a_x_allowed)
+  if not allow_throttle:
+    clipped_accel_coast = max(accel_coast, ACCEL_MIN)
+    coast_limit = np.interp(v_ego, [MIN_ALLOW_THROTTLE_SPEED, MIN_ALLOW_THROTTLE_SPEED*2], [max_accel, clipped_accel_coast])
+    max_accel = min(max_accel, coast_limit)
+
+  target_accel = np.clip(v_cruise - v_ego, A_CRUISE_MIN, max_accel)
+  target_accel = float(np.clip(target_accel, a_cruise_prev - J_CRUISE * dt, a_cruise_prev + J_CRUISE * dt))
+
+  cruise_should_stop = v_cruise == 0.0
+  return target_accel, cruise_should_stop
+
+
+def get_e2e_accel(v_ego, v_cruise, model_v, a_target, should_stop):
+  if should_stop or v_cruise <= v_ego or len(model_v) != len(T_IDXS_MPC):
+    return a_target
+
+  convergence_accel = min((v_cruise - v_ego) / E2E_CRUISE_CONVERGENCE_TAU, E2E_CRUISE_ACCEL_MAX)
+  if convergence_accel <= a_target:
+    return a_target
+
+  # Only help the model converge to cruise when both its immediate action and
+  # velocity trajectory show no active deceleration intent. The lead MPC and
+  # cruise candidates remain hard upper bounds on the final acceleration.
+  accel_intent = np.interp(a_target, E2E_ACCEL_INTENT_BP, [0.0, 1.0])
+  model_speed = np.interp(E2E_MODEL_SPEED_HORIZON, T_IDXS_MPC, model_v)
+  speed_intent = np.interp(model_speed - v_ego, E2E_MODEL_SPEED_INTENT_BP, [0.0, 1.0])
+  return float(np.interp(min(accel_intent, speed_intent), [0.0, 1.0], [a_target, convergence_accel]))
+
+
+def get_accel_candidates(e2e, has_lead, mpc_candidate, cruise_candidate, e2e_candidate):
+  candidates = []
+  # With no lead, the MPC follows a synthetic fast lead. It remains the ACC
+  # policy, but must not limit the model policy in full E2E.
+  if not e2e or has_lead:
+    candidates.append(mpc_candidate)
+  candidates.append(cruise_candidate)
+  if e2e:
+    candidates.append(e2e_candidate)
+  return candidates
+
+
+class LongitudinalPlanner(LongitudinalPlannerIQ):
+  def __init__(self, CP, CP_IQ, init_v=0.0, init_a=0.0, dt=DT_MDL):
+    self.CP = CP
+    self.stopping_speed = CP_IQ.longitudinalStoppingSpeedOverride or DEFAULT_STOPPING_SPEED
+    self.mpc = LongitudinalMpc(dt=dt)
+    LongitudinalPlannerIQ.__init__(self, self.CP, CP_IQ, self.mpc)
+    self.fcw = False
+    self.dt = dt
+    self.allow_throttle = True
+
+    self.a_desired = init_a
+    self.v_desired_filter = FirstOrderFilter(init_v, 2.0, self.dt)
+    self.a_cruise = init_a
+    self.output_a_target = 0.0
+    self.output_should_stop = False
+    self.launch_armed = False
+    try:
+      self.exp_speed_conv = Params().get_bool("expSpeedConv")
+    except UnknownKeyName:
+      self.exp_speed_conv = False
+
+    self.v_desired_trajectory = np.zeros(CONTROL_N)
+    self.a_desired_trajectory = np.zeros(CONTROL_N)
+    self.j_desired_trajectory = np.zeros(CONTROL_N)
+
+  @staticmethod
+  def parse_model(model_msg):
+    if (len(model_msg.position.x) == ModelConstants.IDX_N and
+      len(model_msg.velocity.x) == ModelConstants.IDX_N and
+      len(model_msg.acceleration.x) == ModelConstants.IDX_N):
+      x = np.interp(T_IDXS_MPC, ModelConstants.T_IDXS, model_msg.position.x)
+      v = np.interp(T_IDXS_MPC, ModelConstants.T_IDXS, model_msg.velocity.x)
+      a = np.interp(T_IDXS_MPC, ModelConstants.T_IDXS, model_msg.acceleration.x)
+      j = np.zeros(len(T_IDXS_MPC))
     else:
-      clocks = sm.get('clocks', None) if isinstance(sm, dict) else None
-      time_validated = bool(getattr(clocks, 'timeValid', False))
-    slc_v_cruise = self.slimit.update(slc_apply_enabled, now, time_validated, v_cruise, v_ego, sm)
-    self.iq_dynamic.set_slc_experimental_mode(self.slimit.slc_experimental_mode)
-    self.iq_dynamic.update(sm)
-    # Prefer confirmed controller output for UI/planner rendering.
-    # Fall back to active (policy-resolved) target/source when confirmed is unavailable.
-    display_speed_limit = self.slimit.slc_target if self.slimit.slc_target > 0 else self.slimit.slc_active_target
-    display_source = self.slimit.slc_source if self.slimit.slc_source != "None" else self.slimit.slc_active_source
-
-    if display_speed_limit > 0:
-      self.speed_limit_last = display_speed_limit
-      self.speed_limit_final_last = display_speed_limit + self.slimit.slc_offset
-    elif display_source == "None":
-      self.speed_limit_last = 0.0
-      self.speed_limit_final_last = 0.0
-    # Respect user-defined max cruise speed when applying SLC.
-    if v_cruise_cluster > 0 and self.speed_limit_final_last > 0:
-      self.speed_limit_final_last = min(self.speed_limit_final_last, v_cruise_cluster)
-    source_map = {
-      "Dashboard": SpeedLimitSource.car,
-      "Map Data": SpeedLimitSource.map,
-      "Mapbox": SpeedLimitSource.map,
-      "None": SpeedLimitSource.none,
-    }
-    self.speed_limit_source = source_map.get(display_source, SpeedLimitSource.none)
-
-    targets = {
-      LongitudinalPlanSource.cruise: (v_cruise, a_ego),
-      LongitudinalPlanSource.speedLimitAssist: (slc_v_cruise, a_ego),
-    }
-    if self.nav_valid:
-      targets[LongitudinalPlanSource.nav] = (self.nav_speed_target, self.nav_accel_target)
-
-    self.source = min(targets, key=lambda k: targets[k][0])
-    self.output_v_target, self.output_a_target = targets[self.source]
-    self.output_v_target = self._apply_force_stop(self.output_v_target, v_ego, sm, slc_apply_enabled)
-    # envelope shaping only in Assist mode: info/warn must never change the plan
-    self._envelope_enabled = (slc_apply_enabled and bool(getattr(self.slimit, "controller_enabled", False))
-                              and bool(getattr(self.slimit, "mode_assist", False)))
-    return self.output_v_target, self.output_a_target
-
-  def cruise_envelope(self, v_target: float, v_ego: float, t_idxs) -> np.ndarray:
-    """Per-timestep cruise speed over the MPC horizon: the scalar target, shaped down
-    ahead of an upcoming lower speed limit so the solver decelerates before the sign
-    instead of at it."""
-    env = np.full(len(t_idxs), max(float(v_target), 0.0))
-    if not getattr(self, "_envelope_enabled", False):
-      return env
-    slc = getattr(self.slimit, "slc", None)
-    next_limit = float(getattr(slc, "next_speed_limit", 0.0) or 0.0)
-    next_dist = float(getattr(slc, "next_speed_distance", 0.0) or 0.0)
-    if next_limit <= 0.0 or next_dist <= 0.0:
-      return env
-    next_target = max(next_limit + float(getattr(self.slimit, "slc_offset", 0.0) or 0.0), 0.0)
-    if next_target >= env[0]:
-      return env
-    travel = np.maximum(v_ego, 1.0) * np.asarray(t_idxs)
-    v_allowed = np.sqrt(np.maximum(next_target ** 2 + 2.0 * abs(LIMIT_ADAPT_ACC) * (next_dist - travel), next_target ** 2))
-    return np.minimum(env, v_allowed)
-
-  def update(self, sm: messaging.SubMaster) -> None:
-    self.events_iq.clear()
-    for event_name in getattr(self.slimit, 'pending_events', []):
-      self.events_iq.add(event_name)
-    self.custom_stop_distance.update()
-    self.e2e_alerts.update(sm, self.events_iq)
-    if bool(getattr(sm["iqCarState"], "alcOverrideAlert", False)):
-      self.events_iq.add(custom.IQOnroadEvent.EventName.steeringOverrideReengageAlc)
-
-  def apply_e2e_stop_distance(self, sm: messaging.SubMaster, v_ego: float, a_target: float, should_stop: bool) -> tuple[float, bool]:
-    if not self.is_e2e(sm):
-      return a_target, should_stop
-    return self.custom_stop_distance.adjust_e2e_stop(a_target, should_stop, v_ego, sm['modelV2'])
-
-  def _apply_force_stop(self, v_target: float, v_ego: float, sm: messaging.SubMaster, apply_enabled: bool) -> float:
-    force_stop = self.iq_dynamic.force_stop_requested() and apply_enabled and self.override_force_stop_timer <= 0.0
-    self.force_stop_timer = self.force_stop_timer + DT_MDL if force_stop else 0.0
-    force_stop_enabled = self.force_stop_timer >= 1.0
-    force_stop_ramp_time = max(float(getattr(self.iq_dynamic, "model_stop_time", IQConstants.FORCE_STOP_PLANNER_TIME)), DT_MDL)
-
-    accel_pressed = bool(getattr(sm["iqCarState"], "accelPressed", False))
-    self.override_force_stop |= sm["carState"].gasPressed or accel_pressed
-    self.override_force_stop &= force_stop_enabled
-
-    if self.override_force_stop:
-      self.override_force_stop_timer = 10.0
-    elif self.override_force_stop_timer > 0.0:
-      self.override_force_stop_timer = max(0.0, self.override_force_stop_timer - DT_MDL)
+      x = np.zeros(len(T_IDXS_MPC))
+      v = np.zeros(len(T_IDXS_MPC))
+      a = np.zeros(len(T_IDXS_MPC))
+      j = np.zeros(len(T_IDXS_MPC))
+    if len(model_msg.meta.disengagePredictions.gasPressProbs) > 1:
+      throttle_prob = model_msg.meta.disengagePredictions.gasPressProbs[1]
     else:
-      self.override_force_stop = False
+      throttle_prob = 1.0
+    return x, v, a, j, throttle_prob
 
-    if force_stop_enabled and not self.override_force_stop:
-      self.forcing_stop = True
-      self.tracked_model_length = max(self.tracked_model_length - (v_ego * DT_MDL), 0.0)
-      if sm["carState"].standstill:
-        return 0.0
-      return min(self.tracked_model_length / force_stop_ramp_time, v_target)
+  def update(self, sm):
+    LongitudinalPlannerIQ.update(self, sm)
 
-    self.forcing_stop = False
-    self.tracked_model_length = max(
-      float(getattr(self.iq_dynamic, "model_length", 0.0)),
-      float(getattr(self.iq_dynamic, "minimum_force_stop_length", 0.0)),
-      0.0,
+    if len(sm['carControl'].orientationNED) == 3:
+      accel_coast = get_coast_accel(sm['carControl'].orientationNED[1])
+    else:
+      accel_coast = ACCEL_MAX
+
+    v_ego = sm['carState'].vEgo
+    v_cruise_kph = min(sm['carState'].vCruise, V_CRUISE_MAX)
+    v_cruise = v_cruise_kph * CV.KPH_TO_MS
+    if sm['controlsState'].forceDecel:
+      v_cruise = 0.0
+
+    long_control_off = sm['controlsState'].longControlState == LongCtrlState.off
+
+    # Reset current state when not engaged, or user is controlling the speed
+    reset_state = long_control_off if self.CP.openpilotLongitudinalControl else not sm['selfdriveState'].enabled
+    # PCM cruise speed may be updated a few cycles later, check if initialized
+    v_cruise_initialized = sm['carState'].vCruise != V_CRUISE_UNSET
+    reset_state = reset_state or not v_cruise_initialized
+    steer_angle_without_offset = sm['carState'].steeringAngleDeg - sm['vehicleParameters'].angleOffsetDeg
+
+    if reset_state:
+      self.v_desired_filter.x = v_ego
+      self.a_desired = np.clip(sm['carState'].aEgo, ACCEL_MIN, ACCEL_MAX)
+      self.a_cruise = self.a_desired
+
+    # Prevent divergence, smooth in current v_ego
+    self.v_desired_filter.x = max(0.0, self.v_desired_filter.update(v_ego))
+    _, model_v, model_a, _, throttle_prob = self.parse_model(sm['modelV2'])
+    # Don't clip at low speeds since throttle_prob doesn't account for creep
+    self.allow_throttle = throttle_prob > ALLOW_THROTTLE_THRESHOLD or v_ego <= MIN_ALLOW_THROTTLE_SPEED
+
+    # Get new v_cruise from Smart Cruise Control and Speed Limit Assist
+    v_cruise = LongitudinalPlannerIQ.update_targets(self, sm, self.v_desired_filter.x, v_cruise)
+
+    if sm['controlsState'].forceDecel:
+      v_cruise = 0.0
+
+    personality = sm['selfdriveState'].personality
+    self.mpc.set_weights(personality=personality)
+    self.mpc.set_cur_state(self.v_desired_filter.x, self.a_desired)
+    self.mpc.update(sm['modelV2'], sm['radarState'], personality=personality)
+
+    self.v_desired_trajectory = np.interp(CONTROL_N_T_IDX, T_IDXS_MPC, self.mpc.v_solution)
+    self.a_desired_trajectory = np.interp(CONTROL_N_T_IDX, T_IDXS_MPC, self.mpc.a_solution)
+    self.j_desired_trajectory = np.interp(CONTROL_N_T_IDX, T_IDXS_MPC[:-1], self.mpc.j_solution)
+
+    # TODO counter is only needed because radar is glitchy, remove once radar is gone
+    self.fcw = self.mpc.crash_cnt > 2 and not sm['carState'].standstill
+    if self.fcw:
+      cloudlog.info("FCW triggered")
+
+    # Save starting point for next iteration
+    a_prev = self.a_desired
+
+    action_t =  self.CP.longitudinalActuatorDelay + DT_MDL
+    output_a_target_mpc, output_should_stop_mpc = get_accel_from_plan(self.v_desired_trajectory, self.a_desired_trajectory, CONTROL_N_T_IDX,
+                                                                        action_t=action_t, stopping_speed=self.stopping_speed)
+
+    output_a_target_e2e = sm['modelV2'].action.desiredAcceleration
+    output_should_stop_e2e = sm['modelV2'].action.shouldStop
+    output_a_target_e2e, output_should_stop_e2e = self.apply_e2e_stop_distance(sm, v_ego, output_a_target_e2e, output_should_stop_e2e)
+    if self.is_e2e(sm) and self.exp_speed_conv and not self.mpc.status:
+      output_a_target_e2e = get_e2e_accel(v_ego, v_cruise, model_v, output_a_target_e2e, output_should_stop_e2e)
+
+    if sm['carState'].standstill:
+      self.launch_armed = True
+    elif v_ego > LAUNCH_DISARM_SPEED:
+      self.launch_armed = False
+    if (self.launch_armed and self.is_e2e(sm) and not output_should_stop_e2e and
+        np.interp(LAUNCH_COMMIT_T, T_IDXS_MPC, model_v) > LAUNCH_DISARM_SPEED):
+      t_cut = min(float(T_IDXS_MPC[np.argmax(model_v > LAUNCH_MOVING_SPEED)]), LAUNCH_COMMIT_T)
+      t_shifted = T_IDXS_MPC + t_cut
+      v_shifted = np.interp(t_shifted, T_IDXS_MPC, model_v)
+      a_shifted = np.interp(t_shifted, T_IDXS_MPC, model_a)
+      a_launch = get_accel_from_plan(v_shifted, a_shifted, T_IDXS_MPC, action_t=action_t)[0]
+      a_launch_max = np.interp(v_ego, [LAUNCH_MOVING_SPEED, LAUNCH_DISARM_SPEED], [LAUNCH_MAX_ACCEL, 0.])
+      output_a_target_e2e = max(output_a_target_e2e, min(a_launch, a_launch_max))
+
+    e2e = self.is_e2e(sm)
+    self.a_cruise, cruise_should_stop = get_cruise_accel(e2e, v_cruise, v_ego, self.a_cruise,
+                                                          steer_angle_without_offset, self.CP, self.dt,
+                                                          accel_coast, self.allow_throttle)
+
+    candidates = get_accel_candidates(
+      e2e,
+      self.mpc.status,
+      (output_a_target_mpc, self.mpc.source, output_should_stop_mpc),
+      (self.a_cruise, LongitudinalPlanSource.cruise, cruise_should_stop),
+      (output_a_target_e2e, LongitudinalPlanSource.e2e, output_should_stop_e2e),
     )
-    return v_target
 
-  def publish_longitudinal_plan_iq(self, sm: messaging.SubMaster, pm: messaging.PubMaster) -> None:
-    def fill_plan(plan_msg) -> None:
-      plan_msg.longitudinalPlanSource = self.source
-      plan_msg.vTarget = float(self.output_v_target)
-      plan_msg.aTarget = float(self.output_a_target)
-      plan_msg.events = self.events_iq.to_msg()
+    output_a_target, self.mpc.source, _ = min(candidates, key=lambda c: c[0])
+    self.output_should_stop = any(should_stop for _, _, should_stop in candidates)
 
-      # IQ.Dynamic control state
-      iq_dynamic = plan_msg.iqDynamic
-      iq_dynamic.state = IQDynamicState.blended if self.iq_dynamic.mode() == 'blended' else IQDynamicState.acc
-      iq_dynamic.enabled = self.iq_dynamic.enabled()
-      iq_dynamic.active = self.iq_dynamic.active()
+    self.output_should_stop = self.output_should_stop or self.forcing_stop
+    self.output_a_target = np.clip(output_a_target, ACCEL_MIN, ACCEL_MAX)
 
-      nav_summary = plan_msg.iqNavState.nav
-      nav_summary.engaged = self.nav_engaged
-      nav_summary.provider = self.nav_provider
-      nav_summary.state = self.nav_state
-      nav_summary.speedTarget = float(self.nav_speed_target)
-      nav_summary.accelTarget = float(self.nav_accel_target)
-      nav_summary.valid = self.nav_valid
+    self.a_desired = float(self.output_a_target)
+    self.v_desired_filter.x = self.v_desired_filter.x + self.dt * (self.output_a_target + a_prev) / 2.0
 
-      # Speed Limit
-      speedLimit = plan_msg.speedLimit
-      resolver = speedLimit.resolver
-      speed_limit = float(self.slimit.slc_target if self.slimit.slc_target > 0 else self.slimit.slc_active_target)
-      speed_limit_offset = float(self.slimit.slc_offset)
-      speed_limit_final = speed_limit + speed_limit_offset if speed_limit > 0 else 0.
-      speed_limit_valid = speed_limit > 0.
-      speed_limit_last_valid = self.speed_limit_last > 0.
+  def publish(self, sm, pm):
+    plan_send = messaging.new_message('longitudinalPlan')
 
-      resolver.speedLimit = speed_limit
-      resolver.speedLimitLast = float(self.speed_limit_last)
-      resolver.speedLimitFinal = float(speed_limit_final)
-      resolver.speedLimitFinalLast = float(self.speed_limit_final_last)
-      resolver.speedLimitValid = speed_limit_valid
-      resolver.speedLimitLastValid = speed_limit_last_valid
-      resolver.speedLimitOffset = speed_limit_offset
-      resolver.distToSpeedLimit = 0.
-      resolver.source = self.speed_limit_source
+    gate_services = ['carState', 'controlsState', 'selfdriveState', 'radarState']
+    plan_send.valid = sm.all_checks(service_list=gate_services)
+    if not plan_send.valid:
+      log_issue_limited(
+        "longitudinal_plan_invalid",
+        "planner",
+        f"longitudinalPlan invalid alive={ {s: sm.alive[s] for s in gate_services} } "
+        f"freq_ok={ {s: sm.freq_ok[s] for s in gate_services} } valid={ {s: sm.valid[s] for s in gate_services} } "
+        f"subchecks=({sm.all_alive(gate_services)},{sm.all_freq_ok(gate_services)},{sm.all_valid(gate_services)}) "
+        f"recheck={sm.all_checks(service_list=gate_services)}",
+        interval_sec=5.0,
+      )
 
-      assist = speedLimit.assist
-      slc_assist_state = self.slimit.assist_state
-      assist.enabled = bool(self.slimit.slc_target > 0 or self.slimit.slc_unconfirmed > 0)
-      assist.active = self.source == LongitudinalPlanSource.speedLimitAssist and self.slimit.slc_target > 0
-      if slc_assist_state is not None:
-        assist.state = slc_assist_state
-      elif not assist.enabled:
-        assist.state = SpeedLimitAssistState.disabled
-      elif self.slimit.slc_unconfirmed > 0:
-        assist.state = SpeedLimitAssistState.preActive
-      elif assist.active:
-        assist.state = SpeedLimitAssistState.active
-      else:
-        assist.state = SpeedLimitAssistState.inactive
-      assist.vTarget = float(self.output_v_target if assist.active else 255.)
-      assist.aTarget = float(self.slimit.slc_a_target if assist.active else 0.)
+    longitudinalPlan = plan_send.longitudinalPlan
+    longitudinalPlan.modelMonoTime = sm.logMonoTime['modelV2']
+    longitudinalPlan.processingDelay = (plan_send.logMonoTime / 1e9) - sm.logMonoTime['modelV2']
+    longitudinalPlan.solverExecutionTime = self.mpc.solve_time
 
-      e2eAlerts = plan_msg.e2eAlerts
-      e2eAlerts.pathOpen = self.e2e_alerts.path_alert
-      e2eAlerts.leadPullaway = self.e2e_alerts.lead_alert
+    longitudinalPlan.speeds = self.v_desired_trajectory.tolist()
+    longitudinalPlan.accels = self.a_desired_trajectory.tolist()
+    longitudinalPlan.jerks = self.j_desired_trajectory.tolist()
 
-    valid = sm.all_checks(service_list=['carState', 'controlsState'])
+    longitudinalPlan.hasLead = sm['radarState'].leadOne.status
+    longitudinalPlan.leadDistance = get_lead_distance(sm['radarState'])
+    longitudinalPlan.longitudinalPlanSource = self.mpc.source
+    longitudinalPlan.fcw = self.fcw
 
-    plan_iq_send = messaging.new_message('iqPlan')
-    plan_iq_send.valid = valid
-    fill_plan(plan_iq_send.iqPlan)
-    pm.send('iqPlan', plan_iq_send)
+    longitudinalPlan.leadTrajectoryX0 = self.mpc.lead_xv_0[:, 0].tolist()
+    longitudinalPlan.leadTrajectoryV0 = self.mpc.lead_xv_0[:, 1].tolist()
+    longitudinalPlan.leadTrajectoryX1 = self.mpc.lead_xv_1[:, 0].tolist()
+    longitudinalPlan.leadTrajectoryV1 = self.mpc.lead_xv_1[:, 1].tolist()
+
+    longitudinalPlan.aTarget = float(self.output_a_target)
+    longitudinalPlan.shouldStop = bool(self.output_should_stop)
+    longitudinalPlan.allowBrake = True
+    longitudinalPlan.allowThrottle = bool(self.allow_throttle)
+
+    pm.send('longitudinalPlan', plan_send)
+
+    self.publish_longitudinal_plan_iq(sm, pm)

iqpilot/selfdrive/controls/lib/neural_network_feed_forward/tests/test_locator.py

@@ -10,7 +10,13 @@ import os
 import pytest
 
 from iqdbc.car import structs
-import openpilot.selfdrive.controls.lib.latcontrol_torque as locator
+import iqpilot.selfdrive.controls.lib.latcontrol_torque as locator
+
+
+def test_packaged_substitute_table():
+  assert locator.TORQUE_NN_MODEL_SUBSTITUTE_PATH.is_file()
+  assert locator._substitute_for("MAZDA_3") == "MAZDA_CX9_2021"
+  assert locator._substitute_for("UNKNOWN") == "UNKNOWN"
 
 
 @pytest.fixture
@@ -83,3 +89,13 @@ def test_short_eps_fw_ignored(model_dir):
   # a 3-char-or-less fw string is not used to build the candidate
   path, name, _ = locator.get_nn_model_path(make_cp("HONDA_CIVIC", eps_fw=b"ab"))
   assert name == "HONDA_CIVIC"
+
+
+def test_missing_model_directory_falls_back(model_dir, tmp_path, monkeypatch):
+  missing = tmp_path / "missing"
+  monkeypatch.setattr(locator, "TORQUE_NN_MODEL_PATH", str(missing))
+  monkeypatch.setattr(locator, "MOCK_MODEL_PATH", str(missing / "MOCK.json"))
+  path, name, exact = locator.get_nn_model_path(make_cp("HONDA_CIVIC"))
+  assert path == locator.MOCK_MODEL_PATH
+  assert name == "MOCK"
+  assert exact is False

iqpilot/selfdrive/controls/lib/neural_network_feed_forward/tests/test_network.py

@@ -9,24 +9,24 @@ import os
 import numpy as np
 import pytest
 
-from openpilot.selfdrive.controls.lib.latcontrol_torque import NNTorqueModel
-from openpilot.selfdrive.controls.lib.latcontrol_torque import TORQUE_NN_MODEL_PATH
+from iqpilot.selfdrive.controls.lib.latcontrol_torque import NNTorqueModel
+from iqpilot.selfdrive.controls.lib.latcontrol_torque import TORQUE_NN_MODEL_PATH
 
 # A minimal valid NNFF model (Twilsonco format: column-vector mean/std, dense_N_W/b
 # layers). Used as a fallback so the loader logic is still exercised when no trained
 # models are shipped (they are removed pending retraining and re-added over time).
 _SYNTHETIC_MODEL = {
-  "input_size": 4,
+  "input_size": 18,
   "output_size": 1,
-  "input_mean": [[0.0], [0.0], [0.0], [0.0]],
-  "input_std": [[1.0], [1.0], [1.0], [1.0]],
+  "input_mean": [[0.0]] * 18,
+  "input_std": [[1.0]] * 18,
   "layers": [
-    {"dense_1_W": [[0.5, 0.5, 0.5, 0.5], [0.5, 0.5, 0.5, 0.5]], "dense_1_b": [[0.0], [0.0]], "activation": "sigmoid"},
+    {"dense_1_W": [[0.5] * 18, [0.5] * 18], "dense_1_b": [[0.0], [0.0]], "activation": "sigmoid"},
     {"dense_2_W": [[2.0, 2.0]], "dense_2_b": [[-1.0]], "activation": "identity"},
   ],
 }
 
-MODEL_FILES = sorted(f for f in os.listdir(TORQUE_NN_MODEL_PATH) if f.endswith(".json"))
+MODEL_FILES = sorted(f for f in os.listdir(TORQUE_NN_MODEL_PATH) if f.endswith(".json")) if os.path.isdir(TORQUE_NN_MODEL_PATH) else []
 if MODEL_FILES:
   _MODEL_DIR = TORQUE_NN_MODEL_PATH
   _NAMES = MODEL_FILES
@@ -83,7 +83,8 @@ class TestModelBehavior:
 
 
 def test_activation_registry_rejects_unknown(tmp_path):
-  base = json.load(open(_path(SAMPLE[0])))
+  with open(_path(SAMPLE[0])) as model_file:
+    base = json.load(model_file)
   base["layers"][-1]["activation"] = "not_a_real_activation"
   bad = tmp_path / "bad.json"
   bad.write_text(json.dumps(base))

iqpilot/selfdrive/controls/lib/neural_network_feed_forward/tests/test_nnff_assembly.py

@@ -10,20 +10,14 @@ from types import SimpleNamespace
 import numpy as np
 import pytest
 
-from cereal import log
-from openpilot.common.params import Params
-from openpilot.common.pid import PIDController
-from openpilot.selfdrive.modeld.constants import ModelConstants
-from openpilot.selfdrive.controls.lib.latcontrol_torque import NeuralNetworkFeedForward
-from openpilot.selfdrive.controls.lib.latcontrol_torque import TORQUE_NN_MODEL_PATH
+from iqpilot.cereal import log
+from iqpilot.common.params import Params
+from iqpilot.common.pid import PIDController
+from iqpilot.selfdrive.iqmodeld.config import ModelConstants
+from iqpilot.selfdrive.controls.lib.latcontrol_torque import NeuralNetworkFeedForward
+from iqpilot.selfdrive.controls.lib.neural_network_feed_forward.tests.test_network import _MODEL_DIR, _NAMES
 
-_REAL_MODEL = next((f for f in sorted(os.listdir(TORQUE_NN_MODEL_PATH))
-                    if f.endswith(".json") and f != "MOCK.json"), None)
-
-# Models are shipped separately and re-added as retrained; with none present,
-# NNFF is a no-op (falls back to stock torque FF), so the assembly tests skip.
-pytestmark = pytest.mark.skipif(_REAL_MODEL is None,
-                                reason="no NNFF models present (nuked pending retraining)")
+_REAL_MODEL = next((f for f in _NAMES if f != "MOCK.json"), _NAMES[0])
 
 
 def _torque_fn():
@@ -53,7 +47,7 @@ def _model_v2():
 
 def _make_controller(model_file):
   Params().put_bool("NeuralNetworkFeedForward", True)
-  path = os.path.join(TORQUE_NN_MODEL_PATH, model_file)
+  path = os.path.join(_MODEL_DIR, model_file)
   cp = SimpleNamespace(steerActuatorDelay=0.15)
   cp_iq = SimpleNamespace(iqLateralNet=SimpleNamespace(
     model=SimpleNamespace(path=path, name=os.path.splitext(model_file)[0])))
@@ -84,7 +78,10 @@ class TestControllerWiring:
   def test_mock_model_reports_absent(self):
     nnff = _make_controller("MOCK.json")
     assert nnff.has_nn_model is False
-    assert nnff.model.input_size >= 2  # MOCK still loads as a valid net
+    if "MOCK.json" in _NAMES:
+      assert nnff.model.input_size >= 2
+    else:
+      assert nnff.model is None
 
   def test_update_returns_finite_torque(self):
     nnff = _make_controller(_REAL_MODEL)

iqpilot/selfdrive/controls/lib/slc_vcruise.py

@@ -1,11 +1,11 @@
 #!/usr/bin/env python3
 import time
 
-from openpilot.common.constants import CV
-from openpilot.common.params import Params
-from openpilot.common.swaglog import cloudlog
-from openpilot.iqpilot.common.k3_slc_log import k3_slc_log
-from openpilot.iqpilot.selfdrive.controls.lib.speed_limit_controller import SpeedLimitController
+from iqpilot.common.constants import CV
+from iqpilot.common.params import Params
+from iqpilot.common.swaglog import cloudlog
+from iqpilot.common.k3_slc_log import k3_slc_log
+from iqpilot.selfdrive.controls.lib.speed_limit_controller import SpeedLimitController
 
 CRUISING_SPEED = 7
 

iqpilot/selfdrive/controls/lib/smooth_stops.py

@@ -4,8 +4,8 @@ Copyright © IQ.Lvbs, apart of Project Teal Lvbs, All Rights Reserved, licensed
 Original concept and implementation by SpysyWeeb (github.com/SpysyWeeb)
 """
 from iqdbc.car.interfaces import ACCEL_MIN
-from openpilot.common.params import Params
-from openpilot.common.realtime import DT_CTRL
+from iqpilot.common.params import Params
+from iqpilot.common.realtime import DT_CTRL
 
 STANDSTILL_SPEED = 0.05
 STANDSTILL_HOLD_SPEED = 0.15

iqpilot/selfdrive/controls/lib/speed_limit_controller.py

@@ -11,13 +11,13 @@ from concurrent.futures import ThreadPoolExecutor
 
 import numpy as np
 
-from cereal import car, custom
-from openpilot.common.constants import CV
-from openpilot.common.realtime import DT_MDL
-from openpilot.common.swaglog import cloudlog
-from openpilot.iqpilot.common.k3_slc_log import k3_slc_log
-from openpilot.iqpilot.common.slc_utilities import calculate_bearing_offset, is_url_pingable
-from openpilot.iqpilot.common.slc_variables import FREE_MAPBOX_REQUESTS, OFFSET_MAP_IMPERIAL, OFFSET_MAP_METRIC, OFFSET_PERCENT_MAX
+from iqpilot.cereal import car, custom
+from iqpilot.common.constants import CV
+from iqpilot.common.realtime import DT_MDL
+from iqpilot.common.swaglog import cloudlog
+from iqpilot.common.k3_slc_log import k3_slc_log
+from iqpilot.common.slc_utilities import calculate_bearing_offset, is_url_pingable
+from iqpilot.common.slc_variables import FREE_MAPBOX_REQUESTS, OFFSET_MAP_IMPERIAL, OFFSET_MAP_METRIC, OFFSET_PERCENT_MAX
 
 try:
   import requests
@@ -375,8 +375,9 @@ class SpeedLimitController:
 
   def _resolve_tomtom_token(self) -> str:
     try:
-      from openpilot.iqpilot.navd.runtime_common import resolve_tomtom_token
-      return resolve_tomtom_token(self.params) or ""
+      from iqpilot.system.proprietary_runtime._verified_import import import_verified_module
+      runtime_common = import_verified_module("iqpilot_navd_private", "iqpilot_private.navd.runtime_common")
+      return runtime_common.resolve_tomtom_token(self.params) or ""
     except Exception:
       tok = self.params.get("TomTomToken")
       return (tok.decode("utf-8") if isinstance(tok, bytes) else (tok or "")).strip()
@@ -505,7 +506,7 @@ class SpeedLimitController:
       self.segment_distance = 0.0
       return
 
-    steer_angle = sm["carState"].steeringAngleDeg - sm["liveParameters"].angleOffsetDeg
+    steer_angle = sm["carState"].steeringAngleDeg - sm["vehicleParameters"].angleOffsetDeg
     if not self.gps_valid or not self.mapbox_token or steer_angle >= 45:
       self._log_mapbox_diag(f"SLC Mapbox skipped: gps_valid={self.gps_valid} token={bool(self.mapbox_token)} steer_angle={round(float(steer_angle), 2)}")
       self.mapbox_limit = 0.0
@@ -642,7 +643,7 @@ class SpeedLimitController:
       self.tomtom_limit = 0.0
       return
 
-    steer_angle = sm["carState"].steeringAngleDeg - sm["liveParameters"].angleOffsetDeg
+    steer_angle = sm["carState"].steeringAngleDeg - sm["vehicleParameters"].angleOffsetDeg
     if not self.gps_valid or steer_angle >= 45 or v_ego < 1:
       self.tomtom_limit = 0.0
       return

iqpilot/selfdrive/controls/lib/tests/test_curvature_lookahead.py

@@ -0,0 +1,27 @@
+from types import SimpleNamespace
+
+import numpy as np
+
+from iqpilot.selfdrive.controls.lib.curvature_lookahead import LOOKAHEAD_SECONDS, get_lookahead_curvature
+from iqpilot.selfdrive.controls.lib.drive_helpers import get_curvature_from_plan
+from iqpilot.selfdrive.iqmodeld.config import ModelConstants
+
+
+def test_lookahead_samples_total_delay_horizon():
+  yaws = np.square(np.asarray(ModelConstants.T_IDXS)) * 0.02
+  yaw_rates = np.asarray(ModelConstants.T_IDXS) * 0.04
+  model_v2 = SimpleNamespace(
+    orientation=SimpleNamespace(z=yaws.tolist()),
+    orientationRate=SimpleNamespace(z=yaw_rates.tolist()),
+  )
+  lat_delay = 0.3
+  expected = get_curvature_from_plan(yaws, yaw_rates, ModelConstants.T_IDXS, 20.0, lat_delay + LOOKAHEAD_SECONDS)
+  assert get_lookahead_curvature(model_v2, 20.0, lat_delay) == expected
+
+
+def test_invalid_trajectory_falls_back_to_none():
+  model_v2 = SimpleNamespace(
+    orientation=SimpleNamespace(z=[0.0]),
+    orientationRate=SimpleNamespace(z=[0.0]),
+  )
+  assert get_lookahead_curvature(model_v2, 20.0, 0.3) is None

iqpilot/selfdrive/controls/lib/tests/test_custom_stop_distance.py

@@ -6,8 +6,8 @@ Original concept ("Increased Stop Distance") by SpysyWeeb (github.com/SpysyWeeb)
 from types import SimpleNamespace
 
 from iqdbc.car.interfaces import ACCEL_MIN
-from openpilot.selfdrive.modeld.constants import ModelConstants
-from openpilot.iqpilot.selfdrive.controls.lib.custom_stop_distance import (
+from iqpilot.selfdrive.iqmodeld.config import ModelConstants
+from iqpilot.selfdrive.controls.lib.custom_stop_distance import (
   CustomStopDistance,
   MIN_ADJUSTED_D_REL,
 )

iqpilot/selfdrive/controls/lib/tests/test_iq_dynamic_force_stop.py

@@ -4,8 +4,8 @@ Copyright © IQ.Lvbs, apart of Project Teal Lvbs, All Rights Reserved, license
 
 from types import SimpleNamespace
 
-from openpilot.common.realtime import DT_MDL
-from openpilot.iqpilot.selfdrive.controls.lib.longitudinal_planner import LongitudinalPlannerIQ
+from iqpilot.common.realtime import DT_MDL
+from iqpilot.selfdrive.controls.lib.iq_longitudinal_planner import LongitudinalPlannerIQ
 
 
 class _FakeIQDynamic:

iqpilot/selfdrive/controls/lib/tests/test_lateral_acceleration_slew_limiter.py

@@ -0,0 +1,87 @@
+import numpy as np
+
+from iqpilot.selfdrive.controls.lib.lateral_acceleration_slew_limiter import (
+  A_LAT_MAX,
+  AVOIDANCE_BYPASS_ACCEL_DELTA,
+  LateralAccelerationSlewLimiter,
+)
+
+
+def test_disabled_is_exact_passthrough_without_state_change():
+  limiter = LateralAccelerationSlewLimiter(False)
+  limiter.reset(1.25)
+  rng = np.random.default_rng(0)
+  for curvature in rng.standard_normal(100):
+    assert limiter.update(curvature, 25.0, 0.01) is curvature
+    assert limiter.a_lim == 1.25
+
+
+def test_step_is_limited_by_speed_scheduled_jerk():
+  limiter = LateralAccelerationSlewLimiter(True)
+  limiter.reset(0.0)
+  v_ego = 20.0
+  dt = 0.01
+  target = 1.5 / v_ego ** 2
+  previous = limiter.a_lim
+  for _ in range(100):
+    limiter.update(target, v_ego, dt)
+    assert abs(limiter.a_lim - previous) <= limiter.jerk_max(v_ego) * dt + 1e-12
+    previous = limiter.a_lim
+
+
+def test_converges_to_held_target():
+  limiter = LateralAccelerationSlewLimiter(True)
+  v_ego = 20.0
+  target_accel = 1.0
+  limiter.reset(0.0)
+  for _ in range(100):
+    limiter.update(target_accel / v_ego ** 2, v_ego, 0.01)
+  assert limiter.a_lim == target_accel
+
+
+def test_reset_prevents_reengagement_jump():
+  limiter = LateralAccelerationSlewLimiter(True)
+  v_ego = 20.0
+  target_accel = 1.0
+  limiter.reset(target_accel)
+  curvature = limiter.update(target_accel / v_ego ** 2, v_ego, 0.01)
+  assert curvature == target_accel / v_ego ** 2
+  assert limiter.a_lim == target_accel
+
+
+def test_low_speed_passes_through_and_resets():
+  limiter = LateralAccelerationSlewLimiter(True)
+  limiter.reset(-1.0)
+  curvature = 0.2
+  assert limiter.update(curvature, 4.0, 0.01) == curvature
+  assert limiter.a_lim == A_LAT_MAX
+
+
+def test_speed_schedule_changes_slew_rate():
+  limiter = LateralAccelerationSlewLimiter(True)
+  limiter.reset(0.0)
+  limiter.update(1.0 / 8.0 ** 2, 8.0, 0.01)
+  low_speed_step = limiter.a_lim
+  limiter.reset(0.0)
+  limiter.update(1.0 / 35.0 ** 2, 35.0, 0.01)
+  high_speed_step = limiter.a_lim
+  assert low_speed_step > high_speed_step
+
+
+def test_sharp_avoidance_bypasses_limiter():
+  limiter = LateralAccelerationSlewLimiter(True)
+  v_ego = 20.0
+  limiter.reset(0.0)
+  target_accel = AVOIDANCE_BYPASS_ACCEL_DELTA + 0.1
+  curvature = limiter.update(target_accel / v_ego ** 2, v_ego, 0.01)
+  assert limiter.a_lim == target_accel
+  assert curvature == target_accel / v_ego ** 2
+
+
+def test_acceleration_space_couples_speed_and_curvature_changes():
+  limiter = LateralAccelerationSlewLimiter(True)
+  limiter.reset(0.5)
+  limiter.update(0.005, 10.0, 0.01)
+  previous = limiter.a_lim
+  limiter.update(0.003, 20.0, 0.01)
+  assert limiter.a_lim - previous <= limiter.jerk_max(20.0) * 0.01 + 1e-12

iqpilot/selfdrive/controls/lib/tests/test_lateral_edge_guard.py

@@ -0,0 +1,195 @@
+from __future__ import annotations
+
+from dataclasses import dataclass
+import math
+
+from iqpilot.cereal import custom, log
+import iqpilot.cereal.messaging as messaging
+from iqpilot.common.realtime import DT_MDL
+from iqpilot.selfdrive.controls.lib.desire_helper import DesireHelper
+from iqpilot.selfdrive.controls.lib.helpers.lane_change import AutoLaneChangeMode
+from iqpilot.selfdrive.controls.lib.helpers.lateral_edge_guard import (
+  BLOCK_DEBOUNCE_S,
+  CLEAR_DEBOUNCE_S,
+  MAX_VALID_ROAD_EDGE_STD_M,
+  MIN_ACTIVE_SPEED_MPS,
+  REQUIRED_ROAD_EDGE_DISTANCE_M,
+  UNAVAILABLE_HOLD_S,
+  LateralEdgeGuard,
+  RoadEdgeDataState,
+  evaluate_road_edge,
+)
+from iqpilot.selfdrive.selfdrived.iq_events import EVENTS_IQ, ET
+from iqpilot.selfdrive.selfdrived.selfdrived import SelfdriveD
+
+
+@dataclass
+class Edge:
+  x: list[float]
+  y: list[float]
+
+
+@dataclass
+class ModelData:
+  roadEdges: list[Edge]
+  roadEdgeStds: list[float]
+
+
+class CarState:
+  def __init__(self, left_blindspot: bool = False) -> None:
+    self.vEgo = MIN_ACTIVE_SPEED_MPS + 1.0
+    self.leftBlinker = True
+    self.rightBlinker = False
+    self.leftBlindspot = left_blindspot
+    self.rightBlindspot = False
+    self.steeringPressed = True
+    self.steeringTorque = 1.0
+    self.brakePressed = False
+    self.standstill = False
+
+
+def edge_model(left_distance_m: float = 6.0, right_distance_m: float = 6.0,
+               left_std_m: float = 0.0, right_std_m: float = 0.0) -> ModelData:
+  xs = [5.0, 20.0, 40.0]
+  return ModelData(
+    [Edge(xs, [-left_distance_m] * len(xs)), Edge(xs, [right_distance_m] * len(xs))],
+    [left_std_m, right_std_m],
+  )
+
+
+def cycles(duration_s: float) -> int:
+  return math.ceil(duration_s / DT_MDL)
+
+
+def update_for(guard: LateralEdgeGuard, modeldata: ModelData | None, duration_s: float,
+               speed_mps: float = MIN_ACTIVE_SPEED_MPS) -> None:
+  for _ in range(cycles(duration_s)):
+    guard.update(modeldata, speed_mps, DT_MDL)
+
+
+def test_valid_geometry_blocks_and_clear_geometry_does_not_block() -> None:
+  blocked = evaluate_road_edge(edge_model(4.0).roadEdges[0], 0.2, log.LaneChangeDirection.left)
+  clear = evaluate_road_edge(edge_model(6.0).roadEdges[0], 0.2, log.LaneChangeDirection.left)
+  assert blocked.state == RoadEdgeDataState.VALID
+  assert blocked.should_block is True
+  assert clear.state == RoadEdgeDataState.VALID
+  assert clear.should_block is False
+
+
+def test_unavailable_and_invalid_are_distinct() -> None:
+  unavailable = evaluate_road_edge(Edge([5.0], []), 0.2, log.LaneChangeDirection.left)
+  invalid = evaluate_road_edge(edge_model().roadEdges[0], MAX_VALID_ROAD_EDGE_STD_M + 0.01,
+                               log.LaneChangeDirection.left)
+  assert unavailable.state == RoadEdgeDataState.UNAVAILABLE
+  assert unavailable.lateral_distance_m is None
+  assert invalid.state == RoadEdgeDataState.INVALID
+  assert invalid.should_block is None
+
+
+def test_two_sigma_bound_uses_std_in_metres() -> None:
+  measurement = evaluate_road_edge(edge_model(5.0).roadEdges[0], 0.2, log.LaneChangeDirection.left)
+  assert measurement.lateral_distance_m == 5.0
+  assert measurement.conservative_distance_m == 4.6
+  assert measurement.should_block is True
+
+
+def test_distance_threshold_on_either_side() -> None:
+  epsilon_m = 0.001
+  for direction, edge_index in ((log.LaneChangeDirection.left, 0), (log.LaneChangeDirection.right, 1)):
+    below = edge_model(REQUIRED_ROAD_EDGE_DISTANCE_M - epsilon_m, REQUIRED_ROAD_EDGE_DISTANCE_M - epsilon_m)
+    above = edge_model(REQUIRED_ROAD_EDGE_DISTANCE_M + epsilon_m, REQUIRED_ROAD_EDGE_DISTANCE_M + epsilon_m)
+    assert evaluate_road_edge(below.roadEdges[edge_index], 0.0, direction).should_block is True
+    assert evaluate_road_edge(above.roadEdges[edge_index], 0.0, direction).should_block is False
+
+
+def test_block_debounce_rejects_a_single_clear_frame() -> None:
+  guard = LateralEdgeGuard()
+  blocking = edge_model(4.0)
+  clear = edge_model(6.0)
+  update_for(guard, blocking, BLOCK_DEBOUNCE_S - DT_MDL)
+  assert guard.block_for_direction(log.LaneChangeDirection.left) == custom.IQLateralEdgeBlock.none
+  guard.update(clear, MIN_ACTIVE_SPEED_MPS, DT_MDL)
+  update_for(guard, blocking, BLOCK_DEBOUNCE_S)
+  assert guard.block_for_direction(log.LaneChangeDirection.left) == custom.IQLateralEdgeBlock.left
+
+
+def test_clear_debounce_rejects_a_single_blocking_frame() -> None:
+  guard = LateralEdgeGuard()
+  update_for(guard, edge_model(4.0), BLOCK_DEBOUNCE_S)
+  update_for(guard, edge_model(6.0), CLEAR_DEBOUNCE_S - DT_MDL)
+  assert guard.block_for_direction(log.LaneChangeDirection.left) == custom.IQLateralEdgeBlock.left
+  guard.update(edge_model(4.0), MIN_ACTIVE_SPEED_MPS, DT_MDL)
+  update_for(guard, edge_model(6.0), CLEAR_DEBOUNCE_S)
+  assert guard.block_for_direction(log.LaneChangeDirection.left) == custom.IQLateralEdgeBlock.none
+
+
+def test_unavailable_holds_then_falls_back_to_not_blocking() -> None:
+  guard = LateralEdgeGuard()
+  update_for(guard, edge_model(4.0), BLOCK_DEBOUNCE_S)
+  update_for(guard, None, UNAVAILABLE_HOLD_S - DT_MDL)
+  assert guard.left_measurement.state == RoadEdgeDataState.UNAVAILABLE
+  assert guard.block_for_direction(log.LaneChangeDirection.left) == custom.IQLateralEdgeBlock.left
+  guard.update(None, MIN_ACTIVE_SPEED_MPS, DT_MDL)
+  assert guard.block_for_direction(log.LaneChangeDirection.left) == custom.IQLateralEdgeBlock.none
+
+
+def test_invalid_measurement_clears_through_release_debounce() -> None:
+  guard = LateralEdgeGuard()
+  update_for(guard, edge_model(4.0), BLOCK_DEBOUNCE_S)
+  invalid = edge_model(4.0, left_std_m=MAX_VALID_ROAD_EDGE_STD_M + 0.01)
+  update_for(guard, invalid, CLEAR_DEBOUNCE_S - DT_MDL)
+  assert guard.block_for_direction(log.LaneChangeDirection.left) == custom.IQLateralEdgeBlock.left
+  guard.update(invalid, MIN_ACTIVE_SPEED_MPS, DT_MDL)
+  assert guard.block_for_direction(log.LaneChangeDirection.left) == custom.IQLateralEdgeBlock.none
+
+
+def test_speed_gate_is_inactive_below_threshold() -> None:
+  guard = LateralEdgeGuard()
+  update_for(guard, edge_model(4.0), BLOCK_DEBOUNCE_S, MIN_ACTIVE_SPEED_MPS - 0.01)
+  assert guard.block_for_direction(log.LaneChangeDirection.left) == custom.IQLateralEdgeBlock.none
+  update_for(guard, edge_model(4.0), BLOCK_DEBOUNCE_S, MIN_ACTIVE_SPEED_MPS)
+  assert guard.block_for_direction(log.LaneChangeDirection.left) == custom.IQLateralEdgeBlock.left
+
+
+def test_desire_helper_keeps_edge_block_out_of_blindspot_path() -> None:
+  helper = DesireHelper()
+  helper.alc.lane_change_set_timer = AutoLaneChangeMode.NUDGE
+  helper.lane_change_state = log.LaneChangeState.preLaneChange
+  helper.lane_change_direction = log.LaneChangeDirection.left
+  update_for(helper.lateral_edge_guard, edge_model(4.0), BLOCK_DEBOUNCE_S)
+  blindspot_arguments: list[bool] = []
+
+  def record_blindspot(blindspot_detected: bool, brake_pressed: bool) -> None:
+    blindspot_arguments.append(blindspot_detected)
+
+  helper.alc.update_lane_change = record_blindspot
+  helper.update(CarState(left_blindspot=False), True, 1.0, modeldata=edge_model(4.0))
+  assert blindspot_arguments == [False]
+  assert helper.lateral_edge_block == custom.IQLateralEdgeBlock.left
+  assert helper.lane_change_state == log.LaneChangeState.preLaneChange
+
+  helper.update(CarState(left_blindspot=True), True, 1.0, modeldata=edge_model(4.0))
+  assert blindspot_arguments[-1] is True
+
+
+def test_published_edge_block_maps_to_distinct_event_and_alert() -> None:
+  message = messaging.new_message("iqDriveModelData")
+  message.iqDriveModelData.lateralEdgeBlock = custom.IQLateralEdgeBlock.right
+
+  class SubMaster:
+    updated = {"iqDriveModelData": True}
+
+    def __getitem__(self, service: str):
+      assert service == "iqDriveModelData"
+      return message.iqDriveModelData
+
+  selfdrived = SelfdriveD.__new__(SelfdriveD)
+  selfdrived.sm = SubMaster()
+  selfdrived._cached_model_event_names = ()
+  selfdrived._refresh_cached_model_events()
+
+  event_name = custom.IQOnroadEvent.EventName.lateralEdgeBlocked
+  assert selfdrived._cached_model_event_names == (event_name,)
+  alert = EVENTS_IQ[event_name][ET.WARNING]
+  assert alert.alert_text_1 == "Lane Change Blocked"
+  assert alert.alert_text_2 == "Road edge detected"

iqpilot/selfdrive/controls/lib/tests/test_longitudinal_planner_accel_state.py

@@ -0,0 +1,190 @@
+"""
+Copyright © IQ.Lvbs, apart of Project Teal Lvbs, All Rights Reserved, licensed under https://konn3kt.com/tos
+"""
+import numpy as np
+import pytest
+
+from iqdbc.car.honda.interface import CarInterface
+from iqdbc.car.honda.values import CAR
+from iqpilot.cereal import custom, log
+import iqpilot.cereal.messaging as messaging
+from iqpilot.selfdrive.controls.lib.longcontrol import LongCtrlState
+from iqpilot.selfdrive.controls.lib.longitudinal_planner import LongitudinalPlanner
+from iqpilot.selfdrive.iqmodeld.config import ModelConstants
+
+CRUISE = "cruise"
+SPEED_LIMIT_ASSIST = "speedLimitAssist"
+NAV = "nav"
+SOURCES = [CRUISE, SPEED_LIMIT_ASSIST, NAV]
+PLAN_SOURCE = custom.IQPlan.LongitudinalPlanSource
+
+V_CRUISE_MS = 25.0
+NAV_SPEED_TARGET = 11.0
+SLC_SPEED_TARGET = 12.0
+
+APPROACH_V_EGO = 11.2
+APPROACH_D_REL = 100.0
+APPROACH_STEPS = 250
+MIN_SAFE_GAP = 2.0
+COAST_THROTTLE_PROB = 0.1
+
+
+def build_planner(init_v=V_CRUISE_MS, init_a=0.0):
+  CP = CarInterface.get_non_essential_params(CAR.HONDA_CIVIC)
+  CP_IQ = CarInterface.get_non_essential_params_iq(CP, CAR.HONDA_CIVIC)
+  return LongitudinalPlanner(CP, CP_IQ, init_v=init_v, init_a=init_a)
+
+
+def build_sm(v_ego, d_rel, v_lead, source, enabled=True, throttle_prob=1.0, a_ego=0.0, v_cruise=V_CRUISE_MS):
+  radar = messaging.new_message('radarState')
+  control = messaging.new_message('controlsState')
+  ss = messaging.new_message('selfdriveState')
+  car_state = messaging.new_message('carState')
+  car_control = messaging.new_message('carControl')
+  vehicle_params = messaging.new_message('vehicleParameters')
+  model = messaging.new_message('modelV2')
+  iq_car_state = messaging.new_message('iqCarState')
+  iq_nav_state = messaging.new_message('iqNavState')
+  iq_live_data = messaging.new_message('iqLiveData')
+  gps = messaging.new_message('gpsLocation')
+
+  lead = log.RadarState.LeadData.new_message()
+  lead.dRel = float(d_rel)
+  lead.vRel = float(v_lead - v_ego)
+  lead.vLead = float(v_lead)
+  lead.vLeadK = float(v_lead)
+  lead.status = True
+  lead.modelProb = 1.0
+  radar.radarState.leadOne = lead
+
+  t_idxs = np.array(ModelConstants.T_IDXS)
+  position = log.XYZTData.new_message()
+  position.x = [float(x) for x in v_ego * t_idxs]
+  model.modelV2.position = position
+  velocity = log.XYZTData.new_message()
+  velocity.x = [float(v_ego) for _ in t_idxs]
+  model.modelV2.velocity = velocity
+  acceleration = log.XYZTData.new_message()
+  acceleration.x = [0.0 for _ in t_idxs]
+  model.modelV2.acceleration = acceleration
+  model.modelV2.action.desiredAcceleration = 0.0
+  model.modelV2.meta.disengagePredictions.gasPressProbs = [float(throttle_prob) for _ in range(6)]
+
+  lead_times = np.array(ModelConstants.LEAD_T_IDXS)
+  for lead_prediction in model.modelV2.leadsV3:
+    lead_prediction.prob = 1.0
+    lead_prediction.x = [float(d_rel + v_lead * t) for t in lead_times]
+    lead_prediction.v = [float(v_lead) for _ in lead_times]
+
+  control.controlsState.longControlState = LongCtrlState.pid if enabled else LongCtrlState.off
+  ss.selfdriveState.enabled = enabled
+  car_state.carState.vEgo = float(v_ego)
+  car_state.carState.aEgo = float(a_ego)
+  car_state.carState.standstill = bool(v_ego < 0.01)
+  car_state.carState.vCruise = float(v_cruise * 3.6)
+  car_control.carControl.orientationNED = [0.0, 0.0, 0.0]
+
+  if source == NAV:
+    iq_nav_state.iqNavState.longitudinalEngaged = True
+    iq_nav_state.iqNavState.valid = True
+    iq_nav_state.iqNavState.speedTarget = NAV_SPEED_TARGET
+    iq_nav_state.iqNavState.accelTarget = 0.0
+
+  return {
+    'radarState': radar.radarState,
+    'carState': car_state.carState,
+    'carControl': car_control.carControl,
+    'controlsState': control.controlsState,
+    'selfdriveState': ss.selfdriveState,
+    'vehicleParameters': vehicle_params.vehicleParameters,
+    'modelV2': model.modelV2,
+    'iqCarState': iq_car_state.iqCarState,
+    'iqNavState': iq_nav_state.iqNavState,
+    'iqLiveData': iq_live_data.iqLiveData,
+    'gpsLocation': gps.gpsLocation,
+  }
+
+
+def stub_speed_limit_assist(planner):
+  planner.slimit.update = lambda *args, **kwargs: SLC_SPEED_TARGET
+
+
+def run_approach(planner, source, v_ego_0=APPROACH_V_EGO, d_rel_0=APPROACH_D_REL,
+                 steps=APPROACH_STEPS, throttle_prob=COAST_THROTTLE_PROB):
+  if source == SPEED_LIMIT_ASSIST:
+    stub_speed_limit_assist(planner)
+
+  v_ego = v_ego_0
+  d_rel = d_rel_0
+  prev_output_a_target = None
+  trace = []
+  for _ in range(steps):
+    planner.update(build_sm(v_ego, d_rel, 0.0, source, throttle_prob=throttle_prob))
+    trace.append({
+      'v_ego': v_ego,
+      'd_rel': d_rel,
+      'accels_0': float(planner.a_desired_trajectory[0]),
+      'prev_output_a_target': prev_output_a_target,
+      'output_a_target': float(planner.output_a_target),
+    })
+    prev_output_a_target = float(planner.output_a_target)
+    v_ego = max(0.0, v_ego + prev_output_a_target * planner.dt)
+    d_rel = max(0.0, d_rel - v_ego * planner.dt)
+  return trace
+
+
+@pytest.mark.parametrize("source", SOURCES)
+def test_mpc_initial_accel_state_carries_previous_command(source):
+  planner = build_planner(init_v=APPROACH_V_EGO)
+  trace = run_approach(planner, source)
+
+  for i, step in enumerate(trace):
+    if step['prev_output_a_target'] is None:
+      continue
+    assert step['accels_0'] == pytest.approx(step['prev_output_a_target'], abs=1e-6), (
+      f"step {i} source={source}: MPC initial accel state was {step['accels_0']:.4f} "
+      f"but the previous commanded accel was {step['prev_output_a_target']:.4f}"
+    )
+
+
+@pytest.mark.parametrize("source", SOURCES)
+def test_brakes_for_stopped_lead(source):
+  planner = build_planner(init_v=APPROACH_V_EGO)
+  trace = run_approach(planner, source)
+
+  min_gap = min(step['d_rel'] for step in trace)
+  assert min_gap > MIN_SAFE_GAP, (
+    f"source={source}: closed to {min_gap:.2f} m of a stopped lead first seen at "
+    f"{APPROACH_D_REL:.0f} m while coasting from {APPROACH_V_EGO:.1f} m/s"
+  )
+
+
+@pytest.mark.parametrize("source,expected", [
+  (CRUISE, V_CRUISE_MS),
+  (SPEED_LIMIT_ASSIST, SLC_SPEED_TARGET),
+  (NAV, NAV_SPEED_TARGET),
+])
+def test_speed_source_arbitration_unchanged(source, expected):
+  planner = build_planner(init_v=APPROACH_V_EGO)
+  if source == SPEED_LIMIT_ASSIST:
+    stub_speed_limit_assist(planner)
+  planner.update(build_sm(APPROACH_V_EGO, APPROACH_D_REL, 0.0, source))
+
+  assert planner.output_v_target == pytest.approx(expected, abs=1e-6)
+  assert planner.source == getattr(PLAN_SOURCE, source)
+
+
+def test_cruise_accel_initializes_from_planner_accel():
+  planner = build_planner(init_a=-0.35)
+
+  assert planner.a_cruise == pytest.approx(-0.35)
+
+
+def test_cruise_accel_resets_from_measured_accel():
+  a_ego = -0.45
+  v_ego = 20.0
+  planner = build_planner(init_v=v_ego)
+  planner.a_cruise = 0.5
+  planner.update(build_sm(v_ego, APPROACH_D_REL, v_ego, CRUISE, enabled=False, a_ego=a_ego, v_cruise=v_ego + a_ego))
+
+  assert planner.a_cruise == pytest.approx(a_ego, abs=1e-6)

iqpilot/selfdrive/controls/lib/tests/test_nav_exit_lane_change.py

@@ -1,9 +1,9 @@
 """
 Copyright © IQ.Lvbs, apart of Project Teal Lvbs, All Rights Reserved, licensed under https://konn3kt.com/tos
 """
-from cereal import custom, log
-from openpilot.selfdrive.controls.lib.desire_helper import DesireHelper, LaneChangeState
-from openpilot.iqpilot.selfdrive.controls.lib.helpers.lane_change import AutoLaneChangeMode
+from iqpilot.cereal import custom, log
+from iqpilot.selfdrive.controls.lib.desire_helper import DesireHelper, LaneChangeState
+from iqpilot.selfdrive.controls.lib.helpers.lane_change import AutoLaneChangeMode
 
 ManeuverType = custom.IQNavState.ManeuverType
 NavDirection = custom.NavDirection

iqpilot/selfdrive/controls/lib/tests/test_slc_speed_limit_control.py

@@ -5,10 +5,10 @@ Copyright © IQ.Lvbs, apart of Project Teal Lvbs, All Rights Reserved, licensed
 from datetime import datetime
 from types import SimpleNamespace
 
-from openpilot.common.constants import CV
-from openpilot.iqpilot.common.slc_variables import OFFSET_MAP_IMPERIAL
-from openpilot.iqpilot.selfdrive.controls.lib.slc_vcruise import SLCVCruise, CRUISING_SPEED
-from openpilot.iqpilot.selfdrive.controls.lib.speed_limit_controller import SpeedLimitController, POLICY_MAP_DATA_PRIORITY, POLICY_COMBINED
+from iqpilot.common.constants import CV
+from iqpilot.common.slc_variables import OFFSET_MAP_IMPERIAL
+from iqpilot.selfdrive.controls.lib.slc_vcruise import SLCVCruise, CRUISING_SPEED
+from iqpilot.selfdrive.controls.lib.speed_limit_controller import SpeedLimitController, POLICY_MAP_DATA_PRIORITY, POLICY_COMBINED
 
 
 class FakeParams:
@@ -36,7 +36,7 @@ def _build_sm(v_cruise_cluster=100.0, v_ego_cluster=27.8, gas=False, enabled=Tru
                                 steeringAngleDeg=0.0, buttonEvents=[]),
     "iqCarState": SimpleNamespace(speedLimit=iq_limit, accelPressed=False, decelPressed=False),
     "selfdriveState": SimpleNamespace(enabled=enabled),
-    "liveParameters": SimpleNamespace(angleOffsetDeg=0.0),
+    "vehicleParameters": SimpleNamespace(angleOffsetDeg=0.0),
   }
 
 
@@ -443,7 +443,7 @@ def test_get_offset_percent_clamped():
 
 
 def test_construction_zone_fires_event_once_per_zone_entry():
-  from cereal import custom
+  from iqpilot.cereal import custom
   event = custom.IQOnroadEvent.EventName.constructionZoneDetected
 
   controller = _construction_controller()

iqpilot/selfdrive/controls/lib/tests/test_smooth_stops.py

@@ -3,8 +3,8 @@ Copyright © IQ.Lvbs, apart of Project Teal Lvbs, All Rights Reserved, licensed
 
 Original concept and implementation by SpysyWeeb (github.com/SpysyWeeb)
 """
-from openpilot.common.realtime import DT_CTRL
-from openpilot.iqpilot.selfdrive.controls.lib.smooth_stops import (
+from iqpilot.common.realtime import DT_CTRL
+from iqpilot.selfdrive.controls.lib.smooth_stops import (
   SmoothStopController,
   read_smooth_stops_enabled,
   STANDSTILL_SPEED,

iqpilot/selfdrive/controls/lib/tests/test_turn_desire_cycling.py

@@ -0,0 +1,78 @@
+from types import SimpleNamespace
+
+import pytest
+
+from iqpilot.cereal import custom, log
+from iqpilot.common.realtime import DT_MDL
+from iqpilot.selfdrive.controls.lib.desire_helper import (
+  DesireHelper,
+  TURN_DESIRE_STOP_CYCLE_TIME,
+  TURN_DESIRE_STOP_HOLD_TIME,
+)
+
+
+TurnDirection = custom.IQTurnSignalDirection
+
+
+def helper(v_ego=0.0, yaw_rate=0.0):
+  result = DesireHelper.__new__(DesireHelper)
+  result._last_carstate = SimpleNamespace(vEgo=v_ego, yawRate=yaw_rate)
+  result.turn_desire_stop_timer = 0.0
+  result.turn_desire_stop_active = False
+  result.turn_desire_cycle_input = log.Desire.none
+  result.turn_desire_committed = False
+  result.nav_turn_direction = TurnDirection.none
+  result.lane_turn_direction = TurnDirection.none
+  result.lane_change_direction = log.LaneChangeDirection.none
+  result.lane_change_state = log.LaneChangeState.off
+  result.desire = log.Desire.none
+  return result
+
+
+@pytest.mark.parametrize("source", ["manual", "nav"])
+def test_manual_and_nav_turn_desires_receive_rising_edges(source):
+  h = helper()
+  if source == "manual":
+    h.lane_turn_direction = TurnDirection.turnLeft
+  else:
+    h.nav_turn_direction = TurnDirection.turnLeft
+
+  outputs = []
+  for _ in range(round((TURN_DESIRE_STOP_CYCLE_TIME + 2 * DT_MDL) / DT_MDL)):
+    h._pick_desire_output()
+    outputs.append(h.desire)
+
+  gap_index = next(i for i, output in enumerate(outputs) if output == log.Desire.none)
+  assert gap_index * DT_MDL == pytest.approx(TURN_DESIRE_STOP_HOLD_TIME, abs=DT_MDL * 1.1)
+  assert log.Desire.turnLeft in outputs[gap_index + 1:]
+
+
+def test_creeping_restarts_stopped_turn_cycle():
+  h = helper()
+  for _ in range(round(TURN_DESIRE_STOP_HOLD_TIME / DT_MDL)):
+    h._cycle_turn_desire_when_stopped(log.Desire.turnRight)
+
+  h._last_carstate.vEgo = 3.0
+  assert h._cycle_turn_desire_when_stopped(log.Desire.turnRight) == log.Desire.turnRight
+  h._last_carstate.vEgo = 0.0
+  assert h._cycle_turn_desire_when_stopped(log.Desire.turnRight) == log.Desire.turnRight
+  assert h.turn_desire_stop_timer == pytest.approx(DT_MDL)
+
+
+def test_measured_turn_commitment_stops_cycling():
+  h = helper()
+  h._last_carstate.yawRate = -0.1
+  assert h._cycle_turn_desire_when_stopped(log.Desire.turnLeft) == log.Desire.turnLeft
+  h._last_carstate.yawRate = 0.0
+
+  outputs = [h._cycle_turn_desire_when_stopped(log.Desire.turnLeft) for _ in range(300)]
+  assert set(outputs) == {log.Desire.turnLeft}
+
+
+def test_new_turn_direction_rearms_cycle_after_commitment():
+  h = helper(yaw_rate=0.1)
+  h._cycle_turn_desire_when_stopped(log.Desire.turnLeft)
+  h._last_carstate.yawRate = 0.0
+  h._cycle_turn_desire_when_stopped(log.Desire.turnRight)
+  assert h.turn_desire_committed is False
+  assert h.turn_desire_cycle_input == log.Desire.turnRight

iqpilot/selfdrive/controls/plannerd.py

@@ -0,0 +1,52 @@
+#!/usr/bin/env python3
+from iqpilot.cereal import car, custom
+from iqpilot.common.gps import get_gps_location_service
+from iqpilot.common.params import Params
+from iqpilot.common.realtime import Priority, config_realtime_process
+from iqpilot.common.swaglog import cloudlog
+from iqpilot.selfdrive.controls.lib.ldw import LaneDepartureWarning
+from iqpilot.selfdrive.controls.lib.longitudinal_planner import LongitudinalPlanner
+import iqpilot.cereal.messaging as messaging
+
+
+def main():
+  config_realtime_process(5, Priority.CTRL_LOW)
+
+  cloudlog.info("plannerd is waiting for CarParams")
+  params = Params()
+  CP = messaging.log_from_bytes(params.get("CarParams", block=True), car.CarParams)
+  cloudlog.info("plannerd got CarParams: %s", CP.brand)
+
+  cloudlog.info("plannerd is waiting for IQCarParams")
+  CP_IQ = messaging.log_from_bytes(params.get("IQCarParams", block=True), custom.IQCarParams)
+  cloudlog.info("plannerd got IQCarParams")
+
+  gps_location_service = get_gps_location_service(params)
+
+  ldw = LaneDepartureWarning()
+  longitudinal_planner = LongitudinalPlanner(CP, CP_IQ)
+  pm = messaging.PubMaster(['longitudinalPlan', 'driverAssistance', 'iqPlan'])
+  # poll modelV2 (stock): the whole loop body is gated on sm.updated['modelV2'], so a
+  # carState poll only makes carState the polled service with strict 100Hz receive bounds —
+  # each 20Hz planner iteration (>10ms) then swallows conflated carState messages, freq_ok
+  # drops below 80Hz, and every published plan goes event-invalid (commIssue, long degraded).
+  sm = messaging.SubMaster(['carControl', 'carState', 'controlsState', 'vehicleParameters', 'radarState', 'modelV2', 'selfdriveState',
+                            'iqLiveLocation', 'iqLiveData', 'iqNavState', 'iqCarState', 'iqConstructionZone', gps_location_service],
+                           poll='modelV2')
+
+  while True:
+    sm.update()
+    if sm.updated['modelV2']:
+      longitudinal_planner.update(sm)
+      longitudinal_planner.publish(sm, pm)
+
+      ldw.update(sm.frame, sm['modelV2'], sm['carState'], sm['carControl'])
+      msg = messaging.new_message('driverAssistance')
+      msg.valid = sm.all_checks(['carState', 'carControl', 'modelV2', 'vehicleParameters'])
+      msg.driverAssistance.leftLaneDeparture = ldw.left
+      msg.driverAssistance.rightLaneDeparture = ldw.right
+      pm.send('driverAssistance', msg)
+
+
+if __name__ == "__main__":
+  main()

iqpilot/selfdrive/controls/radard.py

@@ -0,0 +1,303 @@
+#!/usr/bin/env python3
+import math
+import numpy as np
+from collections import deque
+from typing import Any
+
+import capnp
+from iqpilot.cereal import messaging, log, car, custom
+from iqpilot.common.filter_simple import FirstOrderFilter
+from iqpilot.common.params import Params
+from iqpilot.common.realtime import DT_MDL, Priority, config_realtime_process
+from iqpilot.common.swaglog import cloudlog
+from iqpilot.common.simple_kalman import KF1D
+
+from iqdbc.car import structs
+from iqdbc.car.hyundai.values import HyundaiFlags, HyundaiFlagsIQ
+from iqpilot.selfdrive.controls.lib.custom_stop_distance import CustomStopDistance
+
+
+# Default lead acceleration decay set to 50% at 1s
+_LEAD_ACCEL_TAU = 1.5
+
+# radar tracks
+SPEED, ACCEL = 0, 1     # Kalman filter states enum
+
+# stationary qualification parameters
+V_EGO_STATIONARY = 4.   # no stationary object flag below this speed
+
+RADAR_TO_CENTER = 2.7   # (deprecated) RADAR is ~ 2.7m ahead from center of car
+RADAR_TO_CAMERA = 1.52  # RADAR is ~ 1.5m ahead from center of mesh frame
+
+
+class KalmanParams:
+  def __init__(self, dt: float):
+    # Lead Kalman Filter params, calculating K from A, C, Q, R requires the control library.
+    # hardcoding a lookup table to compute K for values of radar_ts between 0.01s and 0.2s
+    assert dt > .01 and dt < .2, "Radar time step must be between .01s and 0.2s"
+    self.A = [[1.0, dt], [0.0, 1.0]]
+    self.C = [1.0, 0.0]
+    #Q = np.matrix([[10., 0.0], [0.0, 100.]])
+    #R = 1e3
+    #K = np.matrix([[ 0.05705578], [ 0.03073241]])
+    dts = [i * 0.01 for i in range(1, 21)]
+    K0 = [0.12287673, 0.14556536, 0.16522756, 0.18281627, 0.1988689,  0.21372394,
+          0.22761098, 0.24069424, 0.253096,   0.26491023, 0.27621103, 0.28705801,
+          0.29750003, 0.30757767, 0.31732515, 0.32677158, 0.33594201, 0.34485814,
+          0.35353899, 0.36200124]
+    K1 = [0.29666309, 0.29330885, 0.29042818, 0.28787125, 0.28555364, 0.28342219,
+          0.28144091, 0.27958406, 0.27783249, 0.27617149, 0.27458948, 0.27307714,
+          0.27162685, 0.27023228, 0.26888809, 0.26758976, 0.26633338, 0.26511557,
+          0.26393339, 0.26278425]
+    self.K = [[np.interp(dt, dts, K0)], [np.interp(dt, dts, K1)]]
+
+
+class Track:
+  def __init__(self, identifier: int, v_lead: float, kalman_params: KalmanParams):
+    self.identifier = identifier
+    self.cnt = 0
+    self.aLeadTau = FirstOrderFilter(_LEAD_ACCEL_TAU, 0.45, DT_MDL)
+    self.K_A = kalman_params.A
+    self.K_C = kalman_params.C
+    self.K_K = kalman_params.K
+    self.kf = KF1D([[v_lead], [0.0]], self.K_A, self.K_C, self.K_K)
+
+  def update(self, d_rel: float, y_rel: float, v_rel: float, v_lead: float, measured: float):
+    # relative values, copy
+    self.dRel = d_rel   # LONG_DIST
+    self.yRel = y_rel   # -LAT_DIST
+    self.vRel = v_rel   # REL_SPEED
+    self.vLead = v_lead
+    self.measured = measured   # measured or estimate
+
+    # computed velocity and accelerations
+    if self.cnt > 0:
+      self.kf.update(self.vLead)
+
+    self.vLeadK = float(self.kf.x[SPEED][0])
+    self.aLeadK = float(self.kf.x[ACCEL][0])
+
+    # Learn if constant acceleration
+    if abs(self.aLeadK) < 0.5:
+      self.aLeadTau.x = _LEAD_ACCEL_TAU
+    else:
+      self.aLeadTau.update(0.0)
+
+    self.cnt += 1
+
+  def get_RadarState(self, model_prob: float = 0.0):
+    return {
+      "dRel": float(self.dRel),
+      "yRel": float(self.yRel),
+      "vRel": float(self.vRel),
+      "vLead": float(self.vLead),
+      "vLeadK": float(self.vLeadK),
+      "aLeadK": float(self.aLeadK),
+      "aLeadTau": float(self.aLeadTau.x),
+      "status": True,
+      "fcw": self.is_potential_fcw(model_prob),
+      "modelProb": model_prob,
+      "radar": True,
+      "radarTrackId": self.identifier,
+    }
+
+  def potential_low_speed_lead(self, v_ego: float):
+    # stop for stuff in front of you and low speed, even without model confirmation
+    # Radar points closer than 0.75, are almost always glitches on toyota radars
+    return abs(self.yRel) < 1.0 and (v_ego < V_EGO_STATIONARY) and (0.75 < self.dRel < 25)
+
+  def is_potential_fcw(self, model_prob: float):
+    return model_prob > .9
+
+  def __str__(self):
+    ret = f"x: {self.dRel:4.1f}  y: {self.yRel:4.1f}  v: {self.vRel:4.1f}  a: {self.aLeadK:4.1f}"
+    return ret
+
+
+def laplacian_pdf(x: float, mu: float, b: float):
+  b = max(b, 1e-4)
+  return math.exp(-abs(x-mu)/b)
+
+
+def match_vision_to_track(v_ego: float, lead: capnp._DynamicStructReader, tracks: dict[int, Track]):
+  offset_vision_dist = lead.x[0] - RADAR_TO_CAMERA
+
+  def prob(c):
+    prob_d = laplacian_pdf(c.dRel, offset_vision_dist, lead.xStd[0])
+    prob_y = laplacian_pdf(c.yRel, -lead.y[0], lead.yStd[0])
+    prob_v = laplacian_pdf(c.vRel + v_ego, lead.v[0], lead.vStd[0])
+
+    # This isn't exactly right, but it's a good heuristic
+    return prob_d * prob_y * prob_v
+
+  track = max(tracks.values(), key=prob)
+
+  # if no 'sane' match is found return -1
+  # stationary radar points can be false positives
+  dist_sane = abs(track.dRel - offset_vision_dist) < max([(offset_vision_dist)*.25, 5.0])
+  vel_sane = (abs(track.vRel + v_ego - lead.v[0]) < 10) or (v_ego + track.vRel > 3)
+  if dist_sane and vel_sane:
+    return track
+  else:
+    return None
+
+
+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
+  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]),
+    "aLeadTau": 0.3,
+    "fcw": False,
+    "modelProb": float(lead_msg.prob),
+    "status": True,
+    "radar": False,
+    "radarTrackId": -1,
+  }
+
+
+def get_lead(v_ego: float, ready: bool, tracks: dict[int, Track], lead_msg: capnp._DynamicStructReader,
+             model_v_ego: float, CP: structs.CarParams, CP_IQ: structs.IQCarParams, low_speed_override: bool = True) -> dict[str, Any]:
+  # Determine leads, this is where the essential logic happens
+  if len(tracks) > 0 and ready and lead_msg.prob > .5:
+    track = match_vision_to_track(v_ego, lead_msg, tracks)
+  else:
+    track = None
+
+  lead_dict = {'status': False}
+  if track is not None:
+    lead_dict = track.get_RadarState(lead_msg.prob)
+    lead_dict = get_custom_yrel(CP, CP_IQ, lead_dict, lead_msg)
+  elif (track is None) and ready and (lead_msg.prob > .5):
+    lead_dict = get_RadarState_from_vision(lead_msg, v_ego, model_v_ego)
+
+  if low_speed_override:
+    low_speed_tracks = [c for c in tracks.values() if c.potential_low_speed_lead(v_ego)]
+    if len(low_speed_tracks) > 0:
+      closest_track = min(low_speed_tracks, key=lambda c: c.dRel)
+
+      # Only choose new track if it is actually closer than the previous one
+      if (not lead_dict['status']) or (closest_track.dRel < lead_dict['dRel']):
+        lead_dict = closest_track.get_RadarState()
+
+  return lead_dict
+
+
+def get_custom_yrel(CP: structs.CarParams, CP_IQ: structs.IQCarParams, lead_dict: dict[str, Any],
+                    lead_msg: capnp._DynamicStructReader) -> dict[str, Any]:
+  if CP.brand == "hyundai" and (CP_IQ.flags & HyundaiFlagsIQ.ENHANCED_SCC or
+                                CP.flags & (HyundaiFlags.CANFD_CAMERA_SCC | HyundaiFlags.CAMERA_SCC)):
+    lead_dict['yRel'] = float(-lead_msg.y[0])
+
+  return lead_dict
+
+
+class RadarD:
+  def __init__(self, CP: structs.CarParams, CP_IQ: structs.CarParams, delay: float = 0.0):
+    self.CP = CP
+    self.CP_IQ = CP_IQ
+
+    self.current_time = 0.0
+
+    self.tracks: dict[int, Track] = {}
+    self.kalman_params = KalmanParams(DT_MDL)
+
+    self.v_ego = 0.0
+    self.v_ego_hist = deque([0.0], maxlen=int(round(delay / DT_MDL))+1)
+    self.last_v_ego_frame = -1
+
+    self.radar_state: capnp._DynamicStructBuilder | None = None
+    self.radar_state_valid = False
+
+    self.ready = False
+
+    self.custom_stop_distance = CustomStopDistance()
+
+  def update(self, sm: messaging.SubMaster, rr: car.RadarData):
+    self.ready = sm.seen['modelV2']
+    self.current_time = 1e-9*max(sm.logMonoTime.values())
+    self.custom_stop_distance.update()
+
+    if sm.recv_frame['carState'] != self.last_v_ego_frame:
+      self.v_ego = sm['carState'].vEgo
+      self.v_ego_hist.append(self.v_ego)
+      self.last_v_ego_frame = sm.recv_frame['carState']
+
+    ar_pts = {pt.trackId: [pt.dRel, pt.yRel, pt.vRel, pt.measured] for pt in rr.points}
+
+    # *** remove missing points from meta data ***
+    for ids in list(self.tracks.keys()):
+      if ids not in ar_pts:
+        self.tracks.pop(ids, None)
+
+    # *** compute the tracks ***
+    for ids in ar_pts:
+      rpt = ar_pts[ids]
+
+      # align v_ego by a fixed time to align it with the radar measurement
+      v_lead = rpt[2] + self.v_ego_hist[0]
+
+      # create the track if it doesn't exist or it's a new track
+      if ids not in self.tracks:
+        self.tracks[ids] = Track(ids, v_lead, self.kalman_params)
+      self.tracks[ids].update(rpt[0], rpt[1], rpt[2], v_lead, rpt[3])
+
+    # *** publish radarState ***
+    self.radar_state_valid = sm.all_checks()
+    self.radar_state = log.RadarState.new_message()
+    self.radar_state.mdMonoTime = sm.logMonoTime['modelV2']
+    self.radar_state.radarErrors = rr.errors
+    self.radar_state.carStateMonoTime = sm.logMonoTime['carState']
+
+    if len(sm['modelV2'].velocity.x):
+      model_v_ego = sm['modelV2'].velocity.x[0]
+    else:
+      model_v_ego = self.v_ego
+    leads_v3 = sm['modelV2'].leadsV3
+    if len(leads_v3) > 1:
+      lead_one = get_lead(self.v_ego, self.ready, self.tracks, leads_v3[0], model_v_ego, self.CP, self.CP_IQ, low_speed_override=True)
+      lead_two = get_lead(self.v_ego, self.ready, self.tracks, leads_v3[1], model_v_ego, self.CP, self.CP_IQ, low_speed_override=False)
+      self.radar_state.leadOne = self.custom_stop_distance.apply_lead(lead_one)
+      self.radar_state.leadTwo = self.custom_stop_distance.apply_lead(lead_two)
+
+  def publish(self, pm: messaging.PubMaster):
+    assert self.radar_state is not None
+
+    radar_msg = messaging.new_message("radarState")
+    radar_msg.valid = self.radar_state_valid
+    radar_msg.radarState = self.radar_state
+    pm.send("radarState", radar_msg)
+
+
+# fuses camera and radar data for best lead detection
+def main() -> None:
+  config_realtime_process(5, Priority.CTRL_LOW)
+
+  # wait for stats about the car to come in from controls
+  cloudlog.info("radard is waiting for CarParams")
+  CP = messaging.log_from_bytes(Params().get("CarParams", block=True), car.CarParams)
+  cloudlog.info("radard got CarParams")
+
+  cloudlog.info("radard is waiting for IQCarParams")
+  CP_IQ = messaging.log_from_bytes(Params().get("IQCarParams", block=True), custom.IQCarParams)
+  cloudlog.info("radard got IQCarParams")
+
+  # *** setup messaging
+  sm = messaging.SubMaster(['modelV2', 'carState', 'radarTracks'], poll='modelV2')
+  pm = messaging.PubMaster(['radarState'])
+
+  RD = RadarD(CP, CP_IQ, CP.radarDelay)
+
+  while 1:
+    sm.update()
+
+    RD.update(sm, sm['radarTracks'])
+    RD.publish(pm)
+
+
+if __name__ == "__main__":
+  main()

iqpilot/selfdrive/controls/tests/test_drive_helpers.py

@@ -0,0 +1,20 @@
+import pytest
+
+from iqpilot.selfdrive.controls.lib.drive_helpers import DEFAULT_STOPPING_SPEED, should_stop
+
+
+class TestShouldStop:
+  @pytest.mark.parametrize("v_ego, expected", [
+    (DEFAULT_STOPPING_SPEED - 0.01, True),
+    (DEFAULT_STOPPING_SPEED, False),
+  ])
+  def test_upstream_default(self, v_ego, expected):
+    assert should_stop(v_ego, -0.1) == expected
+
+  @pytest.mark.parametrize("stopping_speed", [0.55 / 3.6, 1.5 / 3.6])
+  def test_car_override(self, stopping_speed):
+    assert should_stop(stopping_speed - 0.01, -0.1, stopping_speed)
+    assert not should_stop(stopping_speed, -0.1, stopping_speed)
+
+  def test_requires_deceleration(self):
+    assert not should_stop(0.0, 0.1, 1.0)

iqpilot/selfdrive/controls/tests/test_following_distance.py

@@ -0,0 +1,46 @@
+import pytest
+import itertools
+from parameterized import parameterized_class
+
+from iqpilot.cereal import log
+
+from iqpilot.selfdrive.controls.lib.longitudinal_mpc_lib.long_mpc import get_safe_obstacle_distance, get_stopped_equivalence_factor, get_T_FOLLOW
+from iqpilot.selfdrive.test.longitudinal_maneuvers.maneuver import Maneuver
+
+
+def desired_follow_distance(v_ego, v_lead, t_follow=None):
+  if t_follow is None:
+    t_follow = get_T_FOLLOW()
+  return get_safe_obstacle_distance(v_ego, t_follow) - get_stopped_equivalence_factor(v_lead)
+
+def run_following_distance_simulation(v_lead, t_end=100.0, e2e=False, personality=0):
+  man = Maneuver(
+    '',
+    duration=t_end,
+    initial_speed=float(v_lead),
+    lead_relevancy=True,
+    initial_distance_lead=100,
+    speed_lead_values=[v_lead],
+    breakpoints=[0.],
+    e2e=e2e,
+    personality=personality,
+  )
+  valid, output = man.evaluate()
+  assert valid
+  return output[-1,2] - output[-1,1]
+
+
+@parameterized_class(("e2e", "personality", "speed"), itertools.product(
+                      [True, False], # e2e
+                      [log.LongitudinalPersonality.relaxed, # personality
+                       log.LongitudinalPersonality.standard,
+                       log.LongitudinalPersonality.aggressive],
+                      [0,10,35])) # speed
+class TestFollowingDistance:
+  def test_following_distance(self):
+    v_lead = float(self.speed)
+    simulation_steady_state = run_following_distance_simulation(v_lead, e2e=self.e2e, personality=self.personality)
+    correct_steady_state = desired_follow_distance(v_lead, v_lead, get_T_FOLLOW(self.personality))
+    err_ratio = 0.2 if self.e2e else 0.1
+    abs_err_margin = 0.5 if v_lead > 0.0 else 1.15
+    assert simulation_steady_state == pytest.approx(correct_steady_state, abs=err_ratio * correct_steady_state + abs_err_margin)

iqpilot/selfdrive/controls/tests/test_latcontrol.py

@@ -0,0 +1,106 @@
+from parameterized import parameterized
+
+from iqpilot.cereal import car, log
+from iqdbc.car.car_helpers import interfaces
+from iqdbc.car.honda.values import CAR as HONDA
+from iqdbc.car.toyota.values import CAR as TOYOTA
+from iqdbc.car.nissan.values import CAR as NISSAN
+from iqdbc.car.gm.values import CAR as GM
+from iqdbc.car.vehicle_model import VehicleModel
+from iqpilot.common.realtime import DT_CTRL
+from iqpilot.selfdrive.car.helpers import convert_to_capnp
+from iqpilot.selfdrive.controls.lib.latcontrol_pid import LatControlPID
+from iqpilot.selfdrive.controls.lib.latcontrol_torque import LatControlTorque
+from iqpilot.selfdrive.controls.lib.latcontrol_torque_pq import LatControlTorquePQ
+import iqpilot.selfdrive.controls.lib.latcontrol_torque_pq as latcontrol_torque_pq
+from iqpilot.selfdrive.controls.lib.latcontrol_angle import LatControlAngle
+from iqpilot.selfdrive.locationd.helpers import Pose
+from iqpilot.common.mock.generators import generate_deviceMotion
+from iqpilot.selfdrive.car import interfaces as iqpilot_interfaces
+
+
+class TestLatControl:
+
+  @staticmethod
+  def build_pq_controller():
+    car_name = TOYOTA.TOYOTA_RAV4
+    CarInterface = interfaces[car_name]
+    CP = CarInterface.get_non_essential_params(car_name)
+    CP_IQ = CarInterface.get_non_essential_params_iq(CP, car_name)
+    CI = CarInterface(CP, CP_IQ)
+    iqpilot_interfaces.apply_iq_car_config(CI)
+    return CP, LatControlTorquePQ(CP.as_reader(), convert_to_capnp(CP_IQ).as_reader(), CI, DT_CTRL)
+
+  @parameterized.expand([(HONDA.HONDA_CIVIC, LatControlPID), (TOYOTA.TOYOTA_RAV4, LatControlTorque),
+                         (NISSAN.NISSAN_LEAF, LatControlAngle), (GM.CHEVROLET_BOLT_EUV, LatControlTorque)])
+  def test_saturation(self, car_name, controller):
+    CarInterface = interfaces[car_name]
+    CP = CarInterface.get_non_essential_params(car_name)
+    CP_IQ = CarInterface.get_non_essential_params_iq(CP, car_name)
+    CI = CarInterface(CP, CP_IQ)
+    iqpilot_interfaces.apply_iq_car_config(CI)
+    CP_IQ = convert_to_capnp(CP_IQ)
+    VM = VehicleModel(CP)
+
+    controller = controller(CP.as_reader(), CP_IQ.as_reader(), CI, DT_CTRL)
+
+    CS = car.CarState.new_message()
+    CS.vEgo = 30
+    CS.steeringPressed = False
+
+    params = log.VehicleParameters.new_message()
+
+    lp = generate_deviceMotion()
+    pose = Pose.from_live_pose(lp.deviceMotion)
+
+    # Saturate for curvature limited and controller limited
+    for _ in range(1000):
+      _, _, lac_log = controller.update(True, CS, VM, params, False, 0, pose, True, 0.2)
+    assert lac_log.saturated
+
+    for _ in range(1000):
+      _, _, lac_log = controller.update(True, CS, VM, params, False, 0, pose, False, 0.2)
+    assert not lac_log.saturated
+
+    for _ in range(1000):
+      _, _, lac_log = controller.update(True, CS, VM, params, False, 1, pose, False, 0.2)
+    assert lac_log.saturated
+
+  def test_pq_controller_update(self):
+    CP, controller = self.build_pq_controller()
+    VM = VehicleModel(CP)
+
+    CS = car.CarState.new_message()
+    CS.vEgo = 30
+    params = log.VehicleParameters.new_message()
+    pose = Pose.from_live_pose(generate_deviceMotion().deviceMotion)
+
+    _, _, lac_log = controller.update(True, CS, VM, params, False, 0.001, pose, False, 0.2)
+    assert lac_log.active
+
+  def test_pq_controller_inactive_lookahead_and_slew_reset(self):
+    CP, controller = self.build_pq_controller()
+    controller.curvature_lookahead_enabled = True
+    controller.lateral_acceleration_slew_limiter.enabled = True
+    VM = VehicleModel(CP)
+    CS = car.CarState.new_message(vEgo=30)
+    params = log.VehicleParameters.new_message()
+    pose = Pose.from_live_pose(generate_deviceMotion().deviceMotion)
+
+    torque, angle, lac_log = controller.update(False, CS, VM, params, False, 0.001, pose, False, 0.2, lookahead_curvature=0.002)
+    assert torque == 0.0
+    assert angle == 0.0
+    assert not lac_log.active
+    assert controller.lateral_acceleration_slew_limiter.a_lim == 1.8
+
+  def test_pq_live_torque_update_freeze_and_unfreeze(self, monkeypatch):
+    _, controller = self.build_pq_controller()
+    initial = (controller.torque_params.latAccelFactor, controller.torque_params.latAccelOffset, controller.torque_params.friction)
+    controller.update_live_torque_params(3.0, 0.2, 0.4)
+    assert (controller.torque_params.latAccelFactor, controller.torque_params.latAccelOffset, controller.torque_params.friction) == initial
+
+    monkeypatch.setattr(latcontrol_torque_pq, "FREEZE_LIVE_TORQUE_PARAMS", False)
+    controller.update_live_torque_params(3.0, 0.2, 0.4)
+    assert controller.torque_params.latAccelFactor == 3.0
+    assert abs(controller.torque_params.latAccelOffset - 0.2) < 1e-6
+    assert abs(controller.torque_params.friction - 0.4) < 1e-6

iqpilot/selfdrive/controls/tests/test_latcontrol_torque_buffer.py

@@ -0,0 +1,47 @@
+from parameterized import parameterized
+
+from iqpilot.cereal import car, log
+from iqdbc.car.car_helpers import interfaces
+from iqdbc.car.toyota.values import CAR as TOYOTA
+from iqdbc.car.vehicle_model import VehicleModel
+from iqpilot.common.realtime import DT_CTRL
+from iqpilot.selfdrive.controls.lib.latcontrol_torque import LatControlTorque, LAT_ACCEL_REQUEST_BUFFER_SECONDS
+
+from iqpilot.selfdrive.car.helpers import convert_to_capnp
+from iqpilot.selfdrive.locationd.helpers import Pose
+from iqpilot.common.mock.generators import generate_deviceMotion
+from iqpilot.selfdrive.car import interfaces as iqpilot_interfaces
+
+def get_controller(car_name):
+  CarInterface = interfaces[car_name]
+  CP = CarInterface.get_non_essential_params(car_name)
+  CP_IQ = CarInterface.get_non_essential_params_iq(CP, car_name)
+  CI = CarInterface(CP, CP_IQ)
+  iqpilot_interfaces.apply_iq_car_config(CI)
+  CP_IQ = convert_to_capnp(CP_IQ)
+  VM = VehicleModel(CP)
+  controller = LatControlTorque(CP.as_reader(), CP_IQ.as_reader(), CI, DT_CTRL)
+  return controller, VM
+
+class TestLatControlTorqueBuffer:
+
+  @parameterized.expand([(TOYOTA.TOYOTA_COROLLA_TSS2,)])
+  def test_request_buffer_consistency(self, car_name):
+    buffer_steps = int(LAT_ACCEL_REQUEST_BUFFER_SECONDS / DT_CTRL)
+    controller, VM = get_controller(car_name)
+
+    CS = car.CarState.new_message()
+    CS.vEgo = 30
+    CS.steeringPressed = False
+    params = log.VehicleParameters.new_message()
+
+    lp = generate_deviceMotion()
+    pose = Pose.from_live_pose(lp.deviceMotion)
+
+    for _ in range(buffer_steps):
+      controller.update(True, CS, VM, params, False, 0.001, pose, False, 0.2)
+    assert all(val != 0 for val in controller.lat_accel_request_buffer)
+
+    for _ in range(buffer_steps):
+      controller.update(False, CS, VM, params, False, 0.0, pose, False, 0.2)
+    assert all(val == 0 for val in controller.lat_accel_request_buffer)

iqpilot/selfdrive/controls/tests/test_lateral_mpc.py

@@ -0,0 +1,85 @@
+import pytest
+import numpy as np
+from iqpilot.selfdrive.controls.lib.lateral_mpc_lib.lat_mpc import LateralMpc
+from iqpilot.selfdrive.controls.lib.drive_helpers import CAR_ROTATION_RADIUS
+from iqpilot.selfdrive.controls.lib.lateral_mpc_lib.lat_mpc import N as LAT_MPC_N
+
+
+def run_mpc(lat_mpc=None, v_ref=30., x_init=0., y_init=0., psi_init=0., curvature_init=0.,
+            lane_width=3.6, poly_shift=0.):
+
+  if lat_mpc is None:
+    lat_mpc = LateralMpc()
+  lat_mpc.set_weights(1., .1, 0.0, .05, 800)
+
+  y_pts = poly_shift * np.ones(LAT_MPC_N + 1)
+  heading_pts = np.zeros(LAT_MPC_N + 1)
+  curv_rate_pts = np.zeros(LAT_MPC_N + 1)
+
+  x0 = np.array([x_init, y_init, psi_init, curvature_init])
+  p = np.column_stack([v_ref * np.ones(LAT_MPC_N + 1),
+                      CAR_ROTATION_RADIUS * np.ones(LAT_MPC_N + 1)])
+
+  # converge in no more than 10 iterations
+  for _ in range(10):
+    lat_mpc.run(x0, p,
+                y_pts, heading_pts, curv_rate_pts)
+  return lat_mpc.x_sol
+
+
+class TestLateralMpc:
+
+  def _assert_null(self, sol, curvature=1e-6):
+    for i in range(len(sol)):
+      assert sol[0,i,1] == pytest.approx(0, abs=curvature)
+      assert sol[0,i,2] == pytest.approx(0, abs=curvature)
+      assert sol[0,i,3] == pytest.approx(0, abs=curvature)
+
+  def _assert_simmetry(self, sol, curvature=1e-6):
+    for i in range(len(sol)):
+      assert sol[0,i,1] == pytest.approx(-sol[1,i,1], abs=curvature)
+      assert sol[0,i,2] == pytest.approx(-sol[1,i,2], abs=curvature)
+      assert sol[0,i,3] == pytest.approx(-sol[1,i,3], abs=curvature)
+      assert sol[0,i,0] == pytest.approx(sol[1,i,0], abs=curvature)
+
+  def test_straight(self):
+    sol = run_mpc()
+    self._assert_null(np.array([sol]))
+
+  def test_y_symmetry(self):
+    sol = []
+    for y_init in [-0.5, 0.5]:
+      sol.append(run_mpc(y_init=y_init))
+    self._assert_simmetry(np.array(sol))
+
+  def test_poly_symmetry(self):
+    sol = []
+    for poly_shift in [-1., 1.]:
+      sol.append(run_mpc(poly_shift=poly_shift))
+    self._assert_simmetry(np.array(sol))
+
+  def test_curvature_symmetry(self):
+    sol = []
+    for curvature_init in [-0.1, 0.1]:
+      sol.append(run_mpc(curvature_init=curvature_init))
+    self._assert_simmetry(np.array(sol))
+
+  def test_psi_symmetry(self):
+    sol = []
+    for psi_init in [-0.1, 0.1]:
+      sol.append(run_mpc(psi_init=psi_init))
+    self._assert_simmetry(np.array(sol))
+
+  def test_no_overshoot(self):
+    y_init = 1.
+    sol = run_mpc(y_init=y_init)
+    for y in list(sol[:,1]):
+      assert y_init >= abs(y)
+
+  def test_switch_convergence(self):
+    lat_mpc = LateralMpc()
+    sol = run_mpc(lat_mpc=lat_mpc, poly_shift=3.0, v_ref=7.0)
+    right_psi_deg = np.degrees(sol[:,2])
+    sol = run_mpc(lat_mpc=lat_mpc, poly_shift=-3.0, v_ref=7.0)
+    left_psi_deg = np.degrees(sol[:,2])
+    np.testing.assert_almost_equal(right_psi_deg, -left_psi_deg, decimal=3)

iqpilot/selfdrive/controls/tests/test_leads.py

@@ -0,0 +1,33 @@
+import iqpilot.cereal.messaging as messaging
+import pytest
+
+from iqdbc.car.toyota.values import CAR as TOYOTA
+from iqpilot.selfdrive.test.process_replay import replay_process_with_name
+
+
+class TestLeads:
+  @pytest.mark.linux
+  def test_radar_fault(self):
+    # if there's no radar-related can traffic, radard should either not respond or respond with an error
+    # this is tightly coupled with underlying car radar_interface implementation, but it's a good sanity check
+    def single_iter_pkg():
+      # single iter package, with meaningless cans and empty carState/modelV2
+      msgs = []
+      for _ in range(500):
+        can = messaging.new_message("can", 1)
+        cs = messaging.new_message("carState")
+        cp = messaging.new_message("carParams")
+        msgs.append(can.as_reader())
+        msgs.append(cs.as_reader())
+        msgs.append(cp.as_reader())
+      model = messaging.new_message("modelV2")
+      msgs.append(model.as_reader())
+
+      return msgs
+
+    msgs = [m for _ in range(3) for m in single_iter_pkg()]
+    out = replay_process_with_name("card", msgs, fingerprint=TOYOTA.TOYOTA_COROLLA_TSS2)
+    states = [m for m in out if m.which() == "radarTracks"]
+    failures = [not state.valid for state in states]
+
+    assert len(states) == 0 or all(failures)

iqpilot/selfdrive/controls/tests/test_longcontrol.py

@@ -0,0 +1,72 @@
+from types import SimpleNamespace
+
+from iqpilot.cereal import custom
+from iqpilot.selfdrive.controls.lib.longcontrol import LongControl, LongCtrlState, long_control_state_trans
+
+
+class TestLongControlStateTransition:
+
+  def test_stay_stopped(self):
+    CP_IQ = custom.IQCarParams.new_message()
+    active = True
+    current_state = LongCtrlState.stopping
+    next_state = long_control_state_trans(CP_IQ, active, current_state,
+                             should_stop=True, brake_pressed=False, cruise_standstill=False)
+    assert next_state == LongCtrlState.stopping
+    next_state = long_control_state_trans(CP_IQ, active, current_state,
+                             should_stop=False, brake_pressed=True, cruise_standstill=False)
+    assert next_state == LongCtrlState.stopping
+    next_state = long_control_state_trans(CP_IQ, active, current_state,
+                             should_stop=False, brake_pressed=False, cruise_standstill=True)
+    assert next_state == LongCtrlState.stopping
+    next_state = long_control_state_trans(CP_IQ, active, current_state,
+                             should_stop=False, brake_pressed=False, cruise_standstill=False)
+    assert next_state == LongCtrlState.pid
+    active = False
+    next_state = long_control_state_trans(CP_IQ, active, current_state,
+                             should_stop=False, brake_pressed=False, cruise_standstill=False)
+    assert next_state == LongCtrlState.off
+
+def test_engage():
+  CP_IQ = custom.IQCarParams.new_message()
+  active = True
+  current_state = LongCtrlState.off
+  next_state = long_control_state_trans(CP_IQ, active, current_state,
+                             should_stop=True, brake_pressed=False, cruise_standstill=False)
+  assert next_state == LongCtrlState.stopping
+  next_state = long_control_state_trans(CP_IQ, active, current_state,
+                             should_stop=False, brake_pressed=True, cruise_standstill=False)
+  assert next_state == LongCtrlState.stopping
+  next_state = long_control_state_trans(CP_IQ, active, current_state,
+                             should_stop=False, brake_pressed=False, cruise_standstill=True)
+  assert next_state == LongCtrlState.stopping
+  next_state = long_control_state_trans(CP_IQ, active, current_state,
+                             should_stop=False, brake_pressed=False, cruise_standstill=False)
+  assert next_state == LongCtrlState.pid
+
+
+def test_gas_override_preserves_negative_accel_command():
+  pid_calls = []
+  control = object.__new__(LongControl)
+  control.CP = SimpleNamespace(stopAccel=-0.55)
+  control.CP_IQ = SimpleNamespace(enableGasInterceptor=False)
+  control.long_control_state = LongCtrlState.pid
+  control.pid = SimpleNamespace(
+    update=lambda error, **kwargs: pid_calls.append((error, kwargs)) or -0.5,
+    reset=lambda: None,
+  )
+  control.last_output_accel = -0.4
+  control.stopping_decel_rate = 1.0
+  control.smooth = SimpleNamespace(enabled=False, update=lambda: None, reset=lambda: None)
+  car_state = SimpleNamespace(
+    vEgo=15.0,
+    aEgo=0.0,
+    brakePressed=False,
+    standstill=False,
+    cruiseState=SimpleNamespace(standstill=False),
+  )
+
+  output = control.update(True, car_state, -0.5, False, (-3.5, 2.0), gas_override=True)
+
+  assert output == -0.5
+  assert pid_calls == [(-0.5, {"speed": 15.0, "feedforward": -0.5, "freeze_integrator": True})]

iqpilot/selfdrive/controls/tests/test_longitudinal_planner.py

@@ -0,0 +1,136 @@
+import numpy as np
+import pytest
+from types import SimpleNamespace
+
+from iqpilot.selfdrive.controls.lib.longitudinal_mpc_lib.long_mpc import LEAD_T_IDXS_MODEL, T_IDXS
+from iqpilot.selfdrive.controls.lib.longitudinal_mpc_lib.long_mpc import LongitudinalMpc
+from iqpilot.selfdrive.controls.lib.longitudinal_mpc_lib.long_mpc import LongitudinalPlanSource
+from iqpilot.selfdrive.controls.lib.longitudinal_planner import J_CRUISE, get_accel_candidates, get_cruise_accel, get_e2e_accel
+
+
+def model_velocity(v_ego, v_future):
+  return np.interp(T_IDXS, [T_IDXS[0], T_IDXS[-1]], [v_ego, v_future])
+
+
+class TestCruiseAccel:
+  @pytest.mark.parametrize("v_cruise,v_ego,a_cruise_prev,direction", [
+    (10.0, 30.0, 0.4, -1.0),
+    (40.0, 30.0, -0.4, 1.0),
+  ])
+  def test_e2e_rate_limit(self, v_cruise, v_ego, a_cruise_prev, direction):
+    dt = 0.05
+    accel, _ = get_cruise_accel(True, v_cruise, v_ego, a_cruise_prev, 0.0, SimpleNamespace(), dt, 0.0, True)
+
+    assert accel == pytest.approx(a_cruise_prev + direction * J_CRUISE * dt)
+
+
+class TestE2eCruiseConvergence:
+  def test_converges_when_model_wants_to_accelerate(self):
+    assert get_e2e_accel(20.0, 30.0, model_velocity(20.0, 25.0), 0.1, False) == pytest.approx(0.5)
+
+  def test_scales_down_near_cruise_speed(self):
+    assert get_e2e_accel(28.5, 30.0, model_velocity(28.5, 30.0), 0.0, False) == pytest.approx(0.05)
+
+  def test_preserves_active_model_deceleration(self):
+    assert get_e2e_accel(20.0, 30.0, model_velocity(20.0, 25.0), -0.05, False) == pytest.approx(-0.05)
+
+  def test_preserves_future_model_slowdown(self):
+    assert get_e2e_accel(20.0, 30.0, model_velocity(20.0, 18.0), 0.1, False) == pytest.approx(0.1)
+
+  @pytest.mark.parametrize("v_ego, v_cruise, should_stop", [
+    (30.0, 30.0, False),
+    (31.0, 30.0, False),
+    (20.0, 30.0, True),
+  ])
+  def test_never_overrides_cruise_or_stop(self, v_ego, v_cruise, should_stop):
+    assert get_e2e_accel(v_ego, v_cruise, model_velocity(v_ego, v_ego + 5.0), -0.2, should_stop) == pytest.approx(-0.2)
+
+
+class TestAccelCandidates:
+  MPC = (-0.2, LongitudinalPlanSource.lead0, True)
+  CRUISE = (0.5, LongitudinalPlanSource.cruise, False)
+  E2E = (0.1, LongitudinalPlanSource.e2e, False)
+
+  def test_e2e_without_lead_frees_model_from_mpc(self):
+    candidates = get_accel_candidates(True, False, self.MPC, self.CRUISE, self.E2E)
+    assert candidates == [self.CRUISE, self.E2E]
+    assert min(candidates, key=lambda c: c[0])[1] == LongitudinalPlanSource.e2e
+    assert not any(should_stop for _, _, should_stop in candidates)
+
+  def test_e2e_with_lead_keeps_mpc_safety_constraint(self):
+    candidates = get_accel_candidates(True, True, self.MPC, self.CRUISE, self.E2E)
+    assert candidates == [self.MPC, self.CRUISE, self.E2E]
+    assert min(candidates, key=lambda c: c[0])[1] == LongitudinalPlanSource.lead0
+    assert any(should_stop for _, _, should_stop in candidates)
+
+  def test_acc_without_lead_keeps_mpc_policy(self):
+    candidates = get_accel_candidates(False, False, self.MPC, self.CRUISE, self.E2E)
+    assert candidates == [self.MPC, self.CRUISE]
+
+
+class TestExperimentalLeadMpc:
+  @staticmethod
+  def mpc(v_ego=20.0):
+    mpc = object.__new__(LongitudinalMpc)
+    mpc.x0 = np.array([0.0, v_ego, 0.0])
+    return mpc
+
+  @staticmethod
+  def model_lead(prob=0.9, x=None, v=None):
+    return SimpleNamespace(
+      prob=prob,
+      x=np.asarray(x if x is not None else [30.0, 66.0, 98.0, 126.0, 150.0, 170.0]),
+      v=np.asarray(v if v is not None else [20.0, 18.0, 16.0, 14.0, 12.0, 10.0]),
+    )
+
+  @staticmethod
+  def radar_lead(status=True, model_prob=0.9, radar=True):
+    return SimpleNamespace(
+      status=status,
+      dRel=28.0,
+      vLead=19.0,
+      aLeadK=-0.5,
+      aLeadTau=1.5,
+      vRel=-1.0,
+      modelProb=model_prob,
+      radar=radar,
+    )
+
+  def test_uses_valid_trajectory_with_radar_anchor(self):
+    lead_xv = self.mpc().process_lead(self.model_lead(), self.radar_lead())
+
+    assert lead_xv[0, 0] == pytest.approx(28.0)
+    assert lead_xv[0, 1] == pytest.approx(19.0)
+    assert lead_xv[-1, 0] == pytest.approx(168.0)
+    assert lead_xv[-1, 1] == pytest.approx(9.0)
+    assert np.all(np.diff(lead_xv[:, 0]) >= 0.0)
+
+  def test_uses_valid_vision_only_trajectory(self):
+    radar_lead = self.radar_lead(radar=False)
+    lead_xv = self.mpc().process_lead(self.model_lead(), radar_lead)
+
+    assert lead_xv[-1, 0] == pytest.approx(168.0)
+    assert lead_xv[-1, 1] == pytest.approx(9.0)
+
+  @pytest.mark.parametrize("model_lead, radar_lead", [
+    (model_lead.__func__(prob=0.5), radar_lead.__func__()),
+    (model_lead.__func__(x=[30.0, 66.0]), radar_lead.__func__()),
+    (model_lead.__func__(v=[20.0, 18.0]), radar_lead.__func__()),
+    (model_lead.__func__(x=[30.0, 66.0, 98.0, np.nan, 150.0, 170.0]), radar_lead.__func__()),
+    (model_lead.__func__(v=[20.0, 18.0, 16.0, np.inf, 12.0, 10.0]), radar_lead.__func__()),
+    (model_lead.__func__(), radar_lead.__func__(model_prob=0.0)),
+    (None, radar_lead.__func__()),
+  ])
+  def test_falls_back_to_radar_extrapolation(self, model_lead, radar_lead):
+    mpc = self.mpc()
+
+    assert np.array_equal(mpc.process_lead(model_lead, radar_lead), mpc.process_lead_legacy(radar_lead))
+
+  def test_prevents_backward_position_trajectory(self):
+    model_lead = self.model_lead(x=[30.0, 40.0, 38.0, 60.0, 80.0, 100.0])
+    lead_xv = self.mpc().process_lead(model_lead, self.radar_lead())
+
+    assert np.all(np.diff(lead_xv[:, 0]) >= 0.0)
+
+  def test_model_time_shape_matches_expected_horizon(self):
+    assert LEAD_T_IDXS_MODEL.shape == (6,)

iqpilot/selfdrive/controls/tests/test_torqued_lat_accel_offset.py

@@ -0,0 +1,70 @@
+import numpy as np
+from iqpilot.cereal import car, messaging
+from iqdbc.car import ACCELERATION_DUE_TO_GRAVITY
+from iqdbc.car import structs
+from iqdbc.car.lateral import get_friction, FRICTION_THRESHOLD
+from iqpilot.common.realtime import DT_MDL
+from iqpilot.selfdrive.locationd.torqued import TorqueEstimator, MIN_BUCKET_POINTS, POINTS_PER_BUCKET, STEER_BUCKET_BOUNDS
+
+np.random.seed(0)
+
+LA_ERR_STD = 1.0
+INPUT_NOISE_STD = 0.08
+V_EGO = 30.0
+
+WARMUP_BUCKET_POINTS = (1.5*MIN_BUCKET_POINTS).astype(int)
+STRAIGHT_ROAD_LA_BOUNDS = (0.02, 0.03)
+
+ROLL_BIAS_DEG = 2.0
+ROLL_COMPENSATION_BIAS = ACCELERATION_DUE_TO_GRAVITY*float(np.sin(np.deg2rad(ROLL_BIAS_DEG)))
+TORQUE_TUNE = structs.CarParams.LateralTorqueTuning(latAccelFactor=2.0, latAccelOffset=0.0, friction=0.2)
+TORQUE_TUNE_BIASED = structs.CarParams.LateralTorqueTuning(latAccelFactor=2.0, latAccelOffset=-ROLL_COMPENSATION_BIAS, friction=0.2)
+
+def generate_inputs(torque_tune, la_err_std, input_noise_std=None):
+  rng = np.random.default_rng(0)
+  steer_torques = np.concat([rng.uniform(bnd[0], bnd[1], pts) for bnd, pts in zip(STEER_BUCKET_BOUNDS, WARMUP_BUCKET_POINTS, strict=True)])
+  la_errs = rng.normal(scale=la_err_std, size=steer_torques.size)
+  frictions = np.array([get_friction(la_err, 0.0, FRICTION_THRESHOLD, torque_tune) for la_err in la_errs])
+  lat_accels = torque_tune.latAccelFactor*steer_torques + torque_tune.latAccelOffset + frictions
+  if input_noise_std is not None:
+    steer_torques += rng.normal(scale=input_noise_std, size=steer_torques.size)
+    lat_accels += rng.normal(scale=input_noise_std, size=steer_torques.size)
+  return steer_torques, lat_accels
+
+def get_warmed_up_estimator(steer_torques, lat_accels):
+  est = TorqueEstimator(car.CarParams())
+  for steer_torque, lat_accel in zip(steer_torques, lat_accels, strict=True):
+    est.filtered_points.add_point(steer_torque, lat_accel)
+  return est
+
+def simulate_straight_road_msgs(est):
+  carControl = messaging.new_message('carControl').carControl
+  carOutput = messaging.new_message('carOutput').carOutput
+  carState = messaging.new_message('carState').carState
+  deviceMotion = messaging.new_message('deviceMotion').deviceMotion
+  carControl.latActive = True
+  carState.vEgo = V_EGO
+  carState.steeringPressed = False
+  ts = DT_MDL*np.arange(2*POINTS_PER_BUCKET)
+  steer_torques = np.concat((np.linspace(-0.03, -0.02, POINTS_PER_BUCKET), np.linspace(0.02, 0.03, POINTS_PER_BUCKET)))
+  lat_accels = TORQUE_TUNE.latAccelFactor * steer_torques
+  for t, steer_torque, lat_accel in zip(ts, steer_torques, lat_accels, strict=True):
+    carOutput.actuatorsOutput.torque = float(-steer_torque)
+    deviceMotion.orientationNED.x = float(np.deg2rad(ROLL_BIAS_DEG))
+    deviceMotion.angularVelocityDevice.z = float(lat_accel / V_EGO)
+    for which, msg in (('carControl', carControl), ('carOutput', carOutput), ('carState', carState), ('deviceMotion', deviceMotion)):
+      est.handle_log(t, which, msg)
+
+def test_estimated_offset():
+  steer_torques, lat_accels = generate_inputs(TORQUE_TUNE_BIASED, la_err_std=LA_ERR_STD, input_noise_std=INPUT_NOISE_STD)
+  est = get_warmed_up_estimator(steer_torques, lat_accels)
+  msg = est.get_msg()
+  # TODO add lataccelfactor and friction check when we have more accurate estimates
+  assert abs(msg.lateralTorqueParameters.latAccelOffsetRaw - TORQUE_TUNE_BIASED.latAccelOffset) < 0.1
+
+def test_straight_road_roll_bias():
+  steer_torques, lat_accels = generate_inputs(TORQUE_TUNE, la_err_std=LA_ERR_STD, input_noise_std=INPUT_NOISE_STD)
+  est = get_warmed_up_estimator(steer_torques, lat_accels)
+  simulate_straight_road_msgs(est)
+  msg = est.get_msg()
+  assert (msg.lateralTorqueParameters.latAccelOffsetRaw < -0.05) and np.isfinite(msg.lateralTorqueParameters.latAccelOffsetRaw)