Control

Cubs2 provides attitude stabilization and manual control for fixed-wing aircraft.

Controller Architecture

Autolevel Controller

The primary controller stabilizes roll and pitch angles while tracking airspeed.

Control Loops:

Roll (φ) - PID control with anti-windup
Pitch (θ) - PID control with anti-windup
Airspeed (V) - Throttle control

Mode Switching:

Manual Mode - Direct pass-through of pilot commands
Stabilized Mode - Attitude stabilization with pilot rate commands
Auto Mode - Full attitude hold

Usage Example

from cubs2_control.autolevel_controller import autolevel_controller
from cubs2_dynamics.sportcub import sportcub
from cyecca.dynamics import ModelSX

# Create controller
ctrl = autolevel_controller()

# Compose with aircraft
aircraft = sportcub()
system = ModelSX.compose({
    "plant": aircraft,
    "controller": ctrl
})

# Connect signals
system.connect("controller.u.q", "plant.x.r")
system.connect("plant.u.ail", "controller.y.ail")
system.build_composed(integrator="rk4")

Tuning

PID Gains

Edit cubs2_data/config/control.yaml:

autolevel:
  roll:
    kp: 1.0
    ki: 0.1
    kd: 0.05
  pitch:
    kp: 0.8
    ki: 0.1
    kd: 0.04

Anti-Windup

Integral limits prevent windup:

pid = PIDController(kp=1.0, ki=0.1, kd=0.05, dt=0.01)
pid.set_output_limits(-1.0, 1.0)  # Clamp output
pid.set_integral_limits(-0.5, 0.5)  # Limit integral term

Manual Control

Gamepad

Connect a gamepad and launch:

ros2 launch cubs2_bringup gamepad_control.xml

Default Mapping:

Left stick: Throttle (Y), Rudder (X)
Right stick: Pitch (Y), Roll (X)

Virtual Joystick

Use the RViz Joy Panel:

Launch RViz: ros2 launch cubs2_bringup viz.xml
Use virtual sticks in Joy Panel
Enable/disable to switch between manual and auto

Control Topics

Published

/control (cubs2_msgs/AircraftControl) - Control commands

Subscribed

/sportcub/pose - Aircraft state (for feedback)
/sportcub/velocity - Velocity (for airspeed control)

Advanced Usage

Custom Controllers

Create new controllers inheriting from ModelSX:

from cyecca.dynamics import ModelSX
from dataclasses import dataclass

@dataclass
class MyControllerState:
    integral_error: float = 0.0

@dataclass
class MyControllerInput:
    setpoint: float = 0.0
    measurement: float = 0.0

@dataclass
class MyControllerOutput:
    command: float = 0.0

def my_controller() -> ModelSX:
    # Define controller dynamics
    ...
    return model

Closed-Loop Analysis

Linearize the closed-loop system:

from cyecca.dynamics.linearize import linearize_dynamics
from cubs2_control.closed_loop import closed_loop_sportcub

# Get closed-loop model
model = closed_loop_sportcub()

# Linearize around trim
A, B = linearize_dynamics(model, trim_state, trim_input)

# Analyze eigenvalues
import numpy as np
eigenvalues = np.linalg.eigvals(A)