Skip to content

Controllers

The controller stack turns high-level targets into backend-independent actuator commands. Concrete simulators and hardware integrations are responsible for applying those commands to their actuator models.

At the package level, concrete controllers are organized around the common interface ControllerBase, which provides the shared lifecycle and target-management pattern used by PID, geometric, and adaptive controller variants.

flowchart LR
  Target[Target or agent output] --> Mapping[Mapping]
  Mapping --> Controller[Controller]
  Controller --> Mixer[Mixer]
  Mixer --> Rotor[Rotor dynamics]
  Rotor --> FlightAdapter[Flight actuator adapter]

  Mapping --> Differential[Differential-drive controller]
  Differential --> GroundAdapter[Platform drive adapter]
  GroundAdapter --> GroundActuators[Ground actuators]

Control Flow

The flight controller path follows this order:

  1. Read pose, velocity, attitude, and body rates from the active state provider.
  2. Build the controller state dict expected by FlightController. All state entries are batched with a leading environment dimension, e.g. pos(N, 3), vel(N, 3), att_euler(N, 3), ang_vel(N, 3).
  3. Convert target and state into total thrust plus body moments.
  4. Pass that wrench through Mixer to get rotor RPM commands.
  5. Pass rotor RPM commands through RotorDynamics.
  6. Pass the resulting thrust and torque values to the backend actuator adapter.

The ground loop uses DifferentialDriveController to generate left/right drive-shaft speed targets. A platform adapter maps those two targets to the platform's physical transmission and actuator model.

PID Controllers

lav2.controllers.pid contains the reusable PID block and the composed FlightController. Differential ground control lives in its own module:

Flight Controller

The flight controller is organized as a cascaded PID stack:

  • position loop produces velocity targets
  • velocity loop produces acceleration targets
  • acceleration loop produces roll/pitch commands and thrust
  • attitude loop produces angular-rate targets
  • angular-rate loop produces body moments
flowchart LR
  TPos[Position target]
  TVel[Velocity target]
  TAcc[Acceleration target]
  TAtt[Attitude target]
  TRate[Body-rate target]
  TThr[Collective thrust target]

  subgraph Cascade[Flight control cascade]
    direction LR
    Pos[Position loop] --> Vel[Velocity loop] --> Acc[Acceleration loop] --> Att[Attitude loop] --> Rate[Body-rate loop] --> Mixer[Mixer] --> Rotor[Rotor dynamics] --> Act[Actuator adapter]
  end

  TPos --> Pos
  TVel --> Vel
  TAcc --> Acc
  TAtt --> Att
  TRate --> Rate
  TThr --> Mixer

The control_mask determines which target channels are externally supplied and which ones are synthesized inside the cascade. That is the key hook that lets the same controller support position-style or lower-level command modes.

For cmd_ctatt and cmd_ctbr, the thrust-related target channel passes through Mixer via apply_thrust_curve(...) and is then injected directly into the mixer input. The outer position and velocity loops do not generate this term. As a result, these modes behave as nested attitude or body-rate control with throttle pass-through, not as a full outer-loop position controller.

Differential-Drive Controller

The differential controller follows the same batched state, lifecycle, and control_mask conventions as the flight controller. It consumes pos(N, 3), att_euler(N, 3), vel_b(N, 3), and ang_vel(N, 3) and supports three cascade entry points:

  • cmd_pos: position error generates forward speed, while bearing error generates yaw rate.
  • cmd_ctatt: external body-forward speed plus an external heading target.
  • cmd_ctbr: external body-forward speed plus an external yaw-rate target.

These names mirror the flight controller's attitude and body-rate entry points. cmd_ctbr corresponds to the yaw-rate-controlled part of PX4 Rover Acro, while cmd_ctatt exposes the heading-controlled layer associated with Stabilized. The forward channel remains an output-shaft speed target in this simulator, not the direct motor command used by PX4 Acro and Stabilized, and cmd_ctatt accepts an explicit heading rather than PX4's yaw-rate stick with heading hold at zero input.

The LAV2 limits reflect the recorded PX4 manual-control path: 2.5 m/s maximum chassis speed, 7.0 rad/s Acro yaw-rate input, and a 0.5 m waypoint acceptance radius. The available ULogs are manual-mode runs, so they validate cmd_ctbr, differential allocation, and drivetrain response, but not the upper cmd_ctatt or cmd_pos PID loops. Outside the acceptance radius, the position loop generates forward speed; inside it, forward speed is zero and a requested final heading can still produce a point turn.

The single default gain set mirrors the recorded PX4 parameter snapshot: position Kp/Ki/Kd = 5.0/0/0 and yaw Kp/Ki/Kd = 5.0/0.15/0.001. These are alignment defaults, not a claim that the inactive upper loops were identified from the manual-mode logs.

The allocator applies

\[ v_L = v - \frac{B}{2}\dot\psi, \qquad v_R = v + \frac{B}{2}\dot\psi \]

and returns [omega_left, omega_right] in output-shaft rad/s. LAV2 uses an effective allocation width fitted to the recorded yaw-rate response; it is not the physical track center spacing. If a side exceeds its limit, the allocator shifts both side targets together so the feasible yaw-speed difference is retained before sacrificing forward speed. This matches the behavior of PX4's diff_wheel_ctrl without carrying hardware motor-reversal or reduction-ratio conventions into simulation coordinates.

The controller is platform-independent. LAV2 maps each side target to one drive sprocket actuator. ATMO maps each side target to one motor output whose front and rear wheels are synchronized by a belt transmission. Geometry, limits, and gains belong in robot parameters; motor, transmission, and actuator dynamics belong in the platform adapter.

Other Controller Families

The package also includes other controller layouts built on top of ControllerBase:

They follow the same package-level organization even when their internal control laws differ from the PID cascade.

Mixer And Command Allocation

Mixer is specific to the rotor vehicle path. It allocates total thrust and roll/pitch/yaw moments into four rotor commands by inverting a geometry-aware allocation matrix derived from VehicleParams.

The mixer supports two allocation strategies, selected via the allocation_mode constructor parameter:

  • pinv (default) performs direct pseudo-inverse allocation followed by clipping. This is a simpler path suitable for maximum-throughput batched workloads.
  • saturation preserves roll and pitch authority before allocating yaw. When individual rotor commands would exceed their physical limits, the mixer trims roll and pitch first around the average rotor speed, then uses any remaining margin for yaw.

Both modes compute rotor-speed-squared targets from the same inverted allocation matrix, clip to [0, max_rpm^2], and return the square-rooted rotor speed magnitudes.

The mixer also handles normalized thrust conversion for PX4-aligned command paths. PX4 fundamentally works with normalized thrust and torque style inputs, so the mixer performs the translation between normalized thrust conventions and total thrust in Newtons.

Helper Modules

Two supporting modules are worth knowing when work extends above the raw control law itself:

  • lav2.controllers.utils provides logging and visualization helpers for controller response, actuator behavior, and data export.
  • lav2.controllers.mapping aligns agent outputs with the expected control-loop inputs, which is useful for layered RL control where different agents may drive different parts of the cascade.

API Cross-References