Active Dynamics Control System

High-Performance Active Suspension Control for Autonomous Vehicles (Tegra SoC)

Overview

This project implements a real-time active suspension control system designed for high-speed autonomous operation. It targets NVIDIA Tegra platforms (e.g., Jetson AGX Orin) running Linux/RTOS. The system maintains a level chassis by ingesting high-frequency IMU data and adjusting active dampers in real-time.

Compatibility Note: This project is optimized for Bazel 9.0+. An included .bazelrc automatically handles the legacy WORKSPACE support by disabling Bzlmod (build --noenable_bzlmod).

Key Performance constraints:

• Loop Rate: 1000 Hz (1ms period)
• Jitter Tolerance: < 50us
• Latency: Zero-copy architecture

Key Technologies

• C++20: Utilizing modern features like std::span and concepts.
• Lock-Free Concurrency: SPSC (Single Producer Single Consumer) Ring Buffer for thread-safe, non-blocking sensor ingestion.
• Eigen: High-performance linear algebra for Kalman Filtering and geometric transformations.
• Real-Time Optimization: POSIX thread pinning (pthread_setaffinity_np) to isolate Sensor and Control loops on dedicated CPU cores.
• Build System: Google Bazel.

Architecture

graph TD
    subgraph Hardware
        IMU["IMU Sensor<br/>(Accel/Gyro)"]
        Dampers["Active Dampers<br/>(FL, FR, RL, RR)"]
    end

    subgraph Tegra_SoC [NVIDIA Tegra SoC]
        subgraph Core_2 [Core 2: Sensor Thread]
            Ingest["Data Ingestion<br/>@ 1kHz"]
        end

        subgraph Shared_Memory [Shared Memory]
            RingBuffer["Lock-Free SPSC<br/>Ring Buffer"]
        end

        subgraph Core_3 [Core 3: Control Thread]
            Kalman["Kalman Filter<br/>(Pitch/Roll Est)"]
            PID["PID Controller<br/>(Gravity Comp)"]
            Mixer["Force Mixer"]
        end
    end

    IMU -- "Raw Data" --> Ingest
    Ingest -- "Push(IMUData)" --> RingBuffer
    RingBuffer -- "Pop()" --> Kalman
    Kalman -- "Theta, Phi" --> PID
    PID -- "Counter-Force" --> Mixer
    Mixer -- "Target Newtons" --> Dampers

    style RingBuffer fill:#f9f,stroke:#333,stroke-width:2px
    style Core_2 fill:#e1f5fe,stroke:#01579b
    style Core_3 fill:#e1f5fe,stroke:#01579b

Loading

The system consists of two primary real-time threads:

1. Sensor Thread (Core 2):

   • Ingests Accelerometer and Gyroscope data at 1kHz.
   • Pushes data into a lock-free circular buffer.

2. Control Thread (Core 3):

   • Polls the ring buffer for new measurements.
   • Step 1 (State Estimation): Updates a Kalman Filter to estimate Pitch and Roll.
   • Step 2 (Control): Calculates counter-forces using a PID loop.
   • Step 3 (Actuation): Outputs force commands for FL, FR, RL, RR dampers.

Control Theory & Mathematics

1. State Estimation (Kalman Filter)

To minimize latency and noise, we fuse the high-frequency Gyroscope data (low latency, drifting) with Accelerometer data (stable, noisy).

State Vector $x_k$: $$ x_k = \begin{bmatrix} \theta \ b \end{bmatrix} $$ Where $\theta$ is the angle (Pitch/Roll) and $b$ is the gyro bias.

Prediction (Time Update): $$ \begin{aligned} \hat{x}{k}^- &= \begin{bmatrix} 1 & -\Delta t \ 0 & 1 \end{bmatrix} \hat{x}{k-1} + \begin{bmatrix} \Delta t \ 0 \end{bmatrix} \omega_{gyro} \ P_{k}^- &= A P_{k-1} A^T + Q \end{aligned} $$

Correction (Measurement Update): Using the angle derived from the accelerometer $\theta_{accel}$: $$ \begin{aligned} K_k &= P_k^- H^T (H P_k^- H^T + R)^{-1} \ \hat{x}_k &= \hat{x}k^- + K_k (\theta{accel} - H \hat{x}_k^-) \ P_k &= (I - K_k H) P_k^- \end{aligned} $$

2. Force Control

The system calculates the required counter-torque to level the chassis using a PID controller for both Pitch and Roll axes.

$$ u(t) = K_p e(t) + K_i \int_0^t e(\tau)d\tau + K_d \frac{de(t)}{dt} $$

The scalar outputs $u_{pitch}$ and $u_{roll}$ are then mapped to the four active dampers:

$$ \begin{bmatrix} F_{FL} \ F_{FR} \ F_{RL} \ F_{RR} \end{bmatrix} = \begin{bmatrix} 1 & 1 \ 1 & -1 \ -1 & 1 \ -1 & -1 \end{bmatrix} \begin{bmatrix} u_{pitch} \ u_{roll} \end{bmatrix} $$

Build & Run

Prerequisites

• Bazel
• C++20 compatible compiler (GCC 10+ / Clang 11+)
• Linux Environment (for thread pinning APIs)

Compiling

bazel build -c opt //:active_dynamics_main

Running

Note: Root privileges may be required to set thread affinity.

sudo ./bazel-bin/active_dynamics_main

System Modules

• active_dynamics::ActiveSuspensionController: Main interface.
• active_dynamics::LockFreeRingBuffer: Atomic SPSC buffer implementation.
• active_dynamics::KalmanFilter: 2D orientation estimator (Gyro/Accel fusion).
• active_dynamics::PIDController: Standard PID implementation with clamping.