How controlsd Implements Lateral and Longitudinal Control in openpilot

The controlsd process executes a 100 Hz real-time loop that ingests sensor and model data, computes steering and acceleration commands via specialized lateral and longitudinal controllers, and publishes CarControl actuation messages for the vehicle CAN bridge.

The controlsd daemon serves as the primary control node in the commaai/openpilot repository, bridging perception outputs with vehicle hardware. Located in selfdrive/controls/controlsd.py, it orchestrates the transformation of high-level driving intentions into low-level actuator commands for steering, throttle, and brakes. By managing both lateral path tracking and longitudinal speed control, it generates the precise CarControl and ControlsState messages consumed by the downstream vehicle interface.

Architecture and Main Loop

The control logic runs inside Controls.run() at a fixed 100 Hz using a Ratekeeper to maintain timing:

def run(self):
    rk = Ratekeeper(100, print_delay_threshold=None)
    while True:
        self.update()
        CC, lac_log = self.state_control()
        self.publish(CC, lac_log)
        rk.monitor_time()

Each iteration follows a strict three-stage pipeline: update (ingest sensor data), state_control (compute lateral and longitudinal actuation), and publish (send commands to the vehicle bus).

Data Acquisition and Calibration

During the update phase, controlsd pulls the latest messages from the SubMaster (self.sm) with a 15 ms timeout:

self.sm.update(15)
if self.sm.updated["liveCalibration"]:
    self.pose_calibrator.feed_live_calib(...)
if self.sm.updated["livePose"]:
    self.calibrated_pose = self.pose_calibrator.build_calibrated_pose(...)

This ingests carState, modelV2, longitudinalPlan, liveParameters, and liveDelay while maintaining calibrated pose estimates for subsequent control calculations.

Lateral Control Implementation

Vehicle Model and Curvature Calculation

Before computing steering commands, controlsd updates the VehicleModel (self.VM) from selfdrive/vehicle_model.py to translate wheel angles into curvature:

lp = self.sm['liveParameters']
self.VM.update_params(lp.stiffnessFactor, lp.steerRatio)
steer_angle_without_offset = math.radians(CS.steeringAngleDeg - lp.angleOffsetDeg)
self.curvature = -self.VM.calc_curvature(steer_angle_without_offset,
                                         CS.vEgo, lp.roll)

This calculation accounts for live-calibrated stiffness factors and steering ratios to produce an accurate curvature estimate of the current vehicle path.

LatControl Strategy Selection

controlsd instantiates one of three lateral controller implementations during initialization based on the car's steerControlType or lateralTuning configuration:

• LatControlAngle (selfdrive/controls/lib/latcontrol_angle.py): Direct curvature-to-angle mapping via lookup tables for vehicles requiring absolute steering angle commands.
• LatControlPID (selfdrive/controls/lib/latcontrol_pid.py): Classic PID control on curvature error for fine-tuned path tracking.
• LatControlTorque (selfdrive/controls/lib/latcontrol_torque.py): Torque-based control using live-calibrated feedforward parameters and friction compensation.

Selection logic examines the CarParams interface definition to determine the appropriate control law for the specific vehicle platform.

Curvature Clipping and Delay Compensation

The desired curvature from the driving model undergoes rate limiting and safety clipping:

new_desired_curvature = model_v2.action.desiredCurvature if CC.latActive else self.curvature
self.desired_curvature, curvature_limited = clip_curvature(
        CS.vEgo, self.desired_curvature, new_desired_curvature, lp.roll)
lat_delay = self.sm["liveDelay"].lateralDelay + LAT_SMOOTH_SECONDS

The clip_curvature function in selfdrive/controls/lib/drive_helpers.py enforces vehicle-specific kinematic limits based on current speed (CS.vEgo) and road roll conditions to prevent commanding physically impossible turn rates.

Generating Steering Actuation

The selected LatControl subclass computes final steering outputs through its update method:

steer, steeringAngleDeg, lac_log = self.LaC.update(
        CC.latActive, CS, self.VM, lp,
        self.steer_limited_by_safety, self.desired_curvature,
        curvature_limited, lat_delay)
actuators.torque = float(steer)
actuators.steeringAngleDeg = float(steeringAngleDeg)
actuators.curvature = self.desired_curvature

The controller returns normalized torque (for torque-controlled vehicles), absolute steering angle in degrees (for angle-controlled vehicles), and a logging structure (lac_log) containing debug data for the UI and telemetry.

Longitudinal Control Implementation

State Machine Logic

The LongControl class in selfdrive/controls/lib/longcontrol.py manages a finite state machine with four distinct states: off, stopping, starting, and pid. State transitions occur in long_control_state_trans based on engagement status (CC.longActive), vehicle standstill conditions, and the shouldStop flag from the longitudinal plan.

PID Acceleration Control

When operating in the pid state, the controller executes a PIDController from common/pid.py on the acceleration error:

actuators.longControlState = self.LoC.long_control_state
pid_accel_limits = self.CI.get_pid_accel_limits(self.CP, 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))

The PID loop compares the planned acceleration target (long_plan.aTarget) against the measured vehicle acceleration (CS.aEgo). Output limits are dynamically determined by get_pid_accel_limits in the car interface (self.CI) to respect powertrain-specific constraints such as maximum regen braking or engine braking limits.

Publishing Vehicle Commands

The publish method finalizes control by populating the CarControl and ControlsState protobuf messages:

1. Safety verification: Compares commanded torque and angle against actual outputs reported in carOutput to detect safety-limit violations and set self.steer_limited_by_safety.
2. Message construction: Packages curvature, torque or angle requests, and longitudinal acceleration into the CarControl message.
3. State logging: Creates ControlsState containing controller internals, desired curvature, and the lateral control log (lac_log) for visualization.
4. IPC transmission: Sends via self.pm.send('controlsState', ...) and self.pm.send('carControl', ...) to the messaging queue for consumption by the CAN bridge and UI processes.

Summary

• controlsd runs at 100 Hz in selfdrive/controls/controlsd.py, executing a three-stage pipeline of update, state_control, and publish to maintain real-time vehicle control.
• Lateral control uses the VehicleModel for curvature calculations, selects between Angle, PID, or Torque controller implementations, and clips curvature via clip_curvature before generating final steering commands.
• Longitudinal control employs a state machine (off/stopping/starting/pid) and a PIDController on acceleration error to produce throttle and brake commands constrained by vehicle-specific limits.
• All controllers output to CarControl protobuf messages consumed by the vehicle CAN bridge, while ControlsState provides comprehensive telemetry for monitoring, debugging, and UI visualization.

Frequently Asked Questions

What frequency does the controlsd process operate at?

The process operates at a fixed 100 Hz using a Ratekeeper(100, print_delay_threshold=None) in the main run() loop. This frequency ensures low-latency response to vehicle dynamics while maintaining synchronization with the 20 Hz model inference and 100 Hz sensor streams.

How does controlsd select between Angle, PID, and Torque lateral controllers?

Selection occurs during initialization in controlsd.py based on the CarParams configuration. Vehicles with SteerControlType.angle use LatControlAngle; those with lateralTuning == 'pid' use LatControlPID; and those with lateralTuning == 'torque' use LatControlTorque. This scheme allows vehicle-specific tuning without modifying the core control logic.

What limits the commanded curvature in the lateral control loop?

The clip_curvature function in selfdrive/controls/lib/drive_helpers.py enforces limits based on current vehicle speed (CS.vEgo) and the road roll angle from liveParameters. These constraints prevent commanding turn rates that would exceed tire grip limits or steering system mechanical capabilities.

How does the longitudinal controller handle vehicle standstill conditions?

The LongControl state machine transitions between stopping, starting, and pid states via long_control_state_trans. When long_plan.shouldStop is true and vehicle speed approaches zero, the controller enters the stopping state to command maximum braking. Upon resumption, it briefly enters starting to apply initial torque before transitioning back to pid, preventing integral windup and ensuring smooth launches.

{
  "mcpServers": {
    "instagit": {
      "command": "npx",
      "args": ["-y", "instagit@latest"]
    }
  }
}