Race Maneuvers & MAVLink Commands

Inputs & outputs

Capture pipeline · GStreamer + appsink

The sim emits H.264 or raw JPEG over RTP. We use GStreamer with the nvv4l2decoder element on Orin NX to hand off decode to the GPU, then push frames into a Python appsink bound to a queue the perception thread reads from.

import gi, numpy as np, queue, time
gi.require_version("Gst", "1.0")
from gi.repository import Gst, GLib

Gst.init(None)

PIPELINE = (
    "udpsrc port=5600 caps=\"application/x-rtp,encoding-name=JPEG,payload=26\" "
    "! rtpjitterbuffer latency=10 "          # 10 ms buffer → drops late pkts
    "! rtpjpegdepay "
    "! nvv4l2decoder mjpeg=1 "                 # NVJPEG · GPU-side decode
    "! nvvidconv ! video/x-raw,format=BGRx "    # NV12 → BGRx (CUDA copy)
    "! videoconvert ! video/x-raw,format=BGR "  # BGRx → BGR (one CPU copy)
    "! appsink name=sink emit-signals=true sync=false drop=true max-buffers=2"
)

class FrameSource:
    def __init__(self, queue_size=2):
        self.q = queue.Queue(maxsize=queue_size)
        self.pipe = Gst.parse_launch(PIPELINE)
        sink = self.pipe.get_by_name("sink")
        sink.connect("new-sample", self._on_sample)
        self.pipe.set_state(Gst.State.PLAYING)

    def _on_sample(self, sink):
        sample = sink.emit("pull-sample")
        buf, caps = sample.get_buffer(), sample.get_caps()
        ok, mi = buf.map(Gst.MapFlags.READ)
        if not ok:
            return Gst.FlowReturn.ERROR
        try:
            arr = np.frombuffer(mi.data, dtype=np.uint8).reshape(360, 640, 3)
            ts_ns = buf.pts                              # RTP-derived presentation ts
            try:
                self.q.put_nowait((arr.copy(), ts_ns))
            except queue.Full:
                _ = self.q.get_nowait()                # drop oldest, keep latest
                self.q.put_nowait((arr.copy(), ts_ns))
        finally:
            buf.unmap(mi)
        return Gst.FlowReturn.OK

Inputs & outputs

Inference loop · zero-copy TRT bindings

We bypass the high-level Ultralytics Python loop in production — too much overhead in predict() for the per-frame call. Direct ICudaEngine + execute_async_v3 shaves ~2 ms vs. the wrapped path.

import tensorrt as trt, pycuda.driver as cuda, numpy as np, cv2

class YoloPose:
    INPUT_SIZE = 640
    CONF_THR   = 0.18            # Neyman-Pearson: FN cost ≫ FP cost
    IOU_THR    = 0.45
    MAX_DET    = 8

    def __init__(self, engine_path):
        logger = trt.Logger(trt.Logger.WARNING)
        with open(engine_path, "rb") as f:
            self.engine = trt.Runtime(logger).deserialize_cuda_engine(f.read())
        self.ctx    = self.engine.create_execution_context()
        self.stream = cuda.Stream()

        # Pinned host buffers + device bindings — allocated ONCE at startup
        in_shape  = (1, 3, self.INPUT_SIZE, self.INPUT_SIZE)
        out_shape = (1, 21, 8400)        # 5 classes + 4 kpts × (x,y,v) + bbox
        self.h_in   = cuda.pagelocked_empty(int(np.prod(in_shape)),  np.float16)
        self.h_out  = cuda.pagelocked_empty(int(np.prod(out_shape)), np.float16)
        self.d_in   = cuda.mem_alloc(self.h_in.nbytes)
        self.d_out  = cuda.mem_alloc(self.h_out.nbytes)
        self.ctx.set_tensor_address("images", int(self.d_in))
        self.ctx.set_tensor_address("output0", int(self.d_out))

    def infer(self, bgr):
        # 1. Letterbox to 640×640, normalize, NCHW, FP16
        img, scale, pad = self._letterbox(bgr, self.INPUT_SIZE)
        rgb = cv2.cvtColor(img, cv2.COLOR_BGR2RGB).astype(np.float16) / 255.0
        np.copyto(self.h_in, rgb.transpose(2, 0, 1).reshape(-1))

        # 2. Forward (async, single stream)
        cuda.memcpy_htod_async(self.d_in, self.h_in, self.stream)
        self.ctx.execute_async_v3(self.stream.handle)
        cuda.memcpy_dtoh_async(self.h_out, self.d_out, self.stream)
        self.stream.synchronize()                # ~4.2 ms on Orin NX

        # 3. Decode, NMS, scale-back to original image coords
        out = self.h_out.reshape(21, 8400).T
        boxes, scores, classes, kpts = self._decode(out, self.CONF_THR)
        keep = self._nms(boxes, scores, self.IOU_THR, self.MAX_DET)
        return self._rescale(boxes[keep], scores[keep],
                              classes[keep], kpts[keep], scale, pad)

def _nms(boxes, scores, kpts, iou_thr=0.45, max_det=8):
    order = np.argsort(scores)[::-1]
    keep = []
    while len(order) > 0 and len(keep) < max_det:
        i = order[0]
        keep.append(i)
        ious = box_iou(boxes[i], boxes[order[1:]])
        # Tie-break: if IoU is borderline, keep the one with higher kpt visibility
        vis = kpts[order[1:], :, 2].mean(axis=1)
        suppress = (ious > iou_thr) & (vis < kpts[i, :, 2].mean())
        order = order[1:][~suppress]
    return np.array(keep)

Failure modes & mitigations

Step 1 · PnP per gate

Gate corners in the gate's own frame are known to mm precision: a flat square of half-width w/2 (1.5 m std / 2.7 m dgate / split). With 4 coplanar points + intrinsics, OpenCV's SOLVEPNP_IPPE_SQUARE gives us a closed-form solution in ~80 μs · no iteration · single ambiguity (front/back) we resolve with the sign of the projected normal.

import cv2, numpy as np

# VADR-TS-002 §3.7 — cam intrinsics, fixed and known
K = np.array([[320.0, 0.0, 320.0],
              [0.0, 320.0, 180.0],
              [0.0, 0.0,   1.0]], dtype=np.float32)
DIST = np.zeros(5, dtype=np.float32)            # sim camera is rectilinear

GATE_HALF_W = {                                  # meters · per VADR-TS-002 §2.4
    "std":    0.75,
    "dgate":  1.35,
    "splits": 1.35,
    "dive":   0.75,
}

def gate_pose(kpts_px, gate_class):
    """kpts_px: ndarray(4,2) image pixels · [TL, TR, BR, BL] clockwise.
       Returns (R_cw, t_cw) — gate-corner frame in camera frame."""
    half = GATE_HALF_W[gate_class]
    obj = np.array([
        [-half,  half, 0],   # TL
        [ half,  half, 0],   # TR
        [ half, -half, 0],   # BR
        [-half, -half, 0],   # BL
    ], dtype=np.float32)

    ok, rvec, tvec = cv2.solvePnP(
        obj, kpts_px.astype(np.float32), K, DIST,
        flags=cv2.SOLVEPNP_IPPE_SQUARE,
    )
    if not ok:
        return None

    R, _ = cv2.Rodrigues(rvec)
    # Reprojection error — drop if > 4 px RMS (degenerate keypoints)
    proj, _ = cv2.projectPoints(obj, rvec, tvec, K, DIST)
    err = np.linalg.norm(proj.reshape(-1, 2) - kpts_px, axis=1).mean()
    if err > 4.0:
        return None
    return R, tvec.flatten()

x = [p_n, p_e, p_d,        # NED position (m)
     v_n, v_e, v_d,        # NED velocity (m/s)
     φ,    θ,    ψ,         # roll, pitch, yaw (rad)
     b_gx, b_gy, b_gz]     # gyro bias (rad/s)

def measurement_update(self, R_cw, t_cw, gate_world_pose):
    """R_cw, t_cw: gate in cam frame (from PnP)
       gate_world_pose: gate in world NED (from course map)"""
    R_gw, t_gw = gate_world_pose

    # Camera ⊕ gate ⊕ world → drone pose in world
    # T_wd = T_wg @ T_gc @ T_cd  (cd = inverse of body→cam = fixed extrinsics)
    R_cd, t_cd = self.cam_extrinsics                 # cam→body
    R_wd = R_gw @ R_cw.T @ R_cd
    t_wd = R_gw @ (-R_cw.T @ t_cw) + R_gw @ (-R_cw.T @ R_cd @ -t_cd) + t_gw

    # Innovation in pos + att
    z      = np.concatenate([t_wd, rot2eul(R_wd)])    # 6-vec measurement
    H      = self.measurement_jacobian()             # 6×12
    y      = z - H @ self.x
    S      = H @ self.P @ H.T + self.R_vision
    K_gain = self.P @ H.T @ np.linalg.inv(S)
    self.x += K_gain @ y
    self.P  = (np.eye(12) - K_gain @ H) @ self.P

Why IPPE_SQUARE (not P3P or iterative)

• Closed-form · ~80 μs vs. iterative SOLVEPNP's 800 μs · saves us 700 μs per gate, multiplied by up to 8 gates per frame.
• Designed for planar 4-point sets · exactly our case · disambiguates the two PnP solutions using the planar prior.
• Lower jitter at near-perpendicular viewing angles (where iterative methods get unstable).

Failure modes

Why minimum-jerk

Quadrotors are differentially flat in (x, y, z, ψ) — meaning any smooth-enough trajectory in those four variables uniquely determines all body rates and thrust. Mellinger & Kumar showed that minimizing the integral of jerk (derivative of acceleration) over the trajectory produces flyable, energy-efficient paths. The closed-form solution is a piecewise degree-5 polynomial per segment.

import numpy as np

def min_jerk_segment(p0, v0, a0, pf, vf, af, T):
    """Closed-form min-jerk polynomial p(t), t∈[0,T].
       Boundary conditions on p, v, a at both endpoints (6 constraints → degree 5)."""
    # Solve A @ coeffs = b  where coeffs = [c0..c5]
    A = np.array([
        [1, 0, 0,        0,            0,            0          ],
        [0, 1, 0,        0,            0,            0          ],
        [0, 0, 2,        0,            0,            0          ],
        [1, T, T**2,      T**3,          T**4,          T**5        ],
        [0, 1, 2*T,      3*T**2,        4*T**3,        5*T**4      ],
        [0, 0, 2,        6*T,          12*T**2,       20*T**3     ],
    ])
    b = np.array([p0, v0, a0, pf, vf, af])
    return np.linalg.solve(A, b)              # coeffs · 6-vec per axis

class RacePlanner:
    LOOKAHEAD = 3                            # gates ahead
    V_CRUISE  = 12.0                         # m/s — COMPLETION mode (VQ1)
    V_ATTACK  = 22.0                         # m/s — ATTACK mode (when ranked)

    def replan(self, x_drone, gate_stack, mode="completion"):
        """Produce next-3-gate reference trajectory."""
        v_target = self.V_ATTACK if mode == "attack" else self.V_CRUISE
        knots = [x_drone.pos] + [g.center for g in gate_stack[:self.LOOKAHEAD]]

        # Per-segment time from gate spacing / cruise speed (re-time after fitting)
        segs = []
        v_prev, a_prev = x_drone.vel, np.zeros(3)
        for i in range(len(knots) - 1):
            d = np.linalg.norm(knots[i+1] - knots[i])
            T = d / v_target                          # first-order time estimate
            v_dir = (knots[i+1] - knots[i]) / d
            v_next = v_dir * v_target if i < len(knots) - 2 else np.zeros(3)
            coeffs = np.array([
                min_jerk_segment(knots[i][k], v_prev[k], a_prev[k],
                                  knots[i+1][k], v_next[k], 0, T)
                for k in range(3)
            ])                                        # 3 axes × 6 coeffs
            segs.append(Seg(coeffs, T, t0=sum(s.T for s in segs)))
            v_prev, a_prev = v_next, np.zeros(3)
        return Trajectory(segs, knots)

def yaw_setpoint(self, drone_pos, g0, g1):
    proj_frac = self.projected_area_fraction(g0, drone_pos)
    look_at = g1.center if proj_frac < 0.08 else g0.center
    dx, dy = look_at[1] - drone_pos[1], look_at[0] - drone_pos[0]
    return np.arctan2(dx, dy)              # NED yaw

Outer loop · position → velocity

The position loop produces a velocity setpoint that the FC's inner loop tracks. Single P gain per axis (no I, no D — the FC handles steady-state, we just inject the slope).

from pymavlink import mavutil
import numpy as np, time

# SET_POSITION_TARGET_LOCAL_NED bitmask · use position + velocity + yaw
TYPEMASK_POSVELYAW = (
    0           # 0–2: use x,y,z
  | 0           # 3–5: use vx,vy,vz
  | (0b111 << 6)   # 6–8 : ignore afx,afy,afz
  | 0           # 9   : use yaw
  | (1 << 10)    # 10  : ignore yaw_rate
)

class RaceController:
    KP_POS = np.array([1.4, 1.4, 2.0])     # N, E, D — heavier on altitude
    V_MAX  = np.array([30.0, 30.0, 8.0])

    def __init__(self, fc_uri="udp:127.0.0.1:14540"):
        self.fc = mavutil.mavlink_connection(fc_uri)
        self.fc.wait_heartbeat()
        self.boot_t = time.monotonic()

    def tick(self, drone_state, traj, t):
        ref_p, ref_v, ref_yaw = traj.sample(t)

        # Outer P · velocity ff from trajectory
        err_p = ref_p - drone_state.pos
        cmd_v = self.KP_POS * err_p + ref_v
        cmd_v = np.clip(cmd_v, -self.V_MAX, self.V_MAX)

        self.fc.mav.set_position_target_local_ned_send(
            int((time.monotonic() - self.boot_t) * 1e3),
            self.fc.target_system, self.fc.target_component,
            mavutil.mavlink.MAV_FRAME_LOCAL_NED,
            TYPEMASK_POSVELYAW,
            ref_p[0], ref_p[1], ref_p[2],            # x, y, z
            cmd_v[0], cmd_v[1], cmd_v[2],            # vx, vy, vz
            0, 0, 0,                                  # afx, afy, afz (ignored)
            ref_yaw, 0,                              # yaw, yaw_rate (ignored)
        )

Split-S override · ATTITUDE_TARGET path

For the wingover at the top of the split-S we leave SET_POSITION_TARGET and switch to ATTITUDE_TARGET (#82) — we need to command an inverted attitude and a specific roll rate that the position controller's solver wouldn't pick on its own. The transition is a single mode-flag change in tick():

def tick_splits_wingover(self, drone_state):
    # Target: 180° roll over 0.4 s, pitch -10°, full throttle held
    target_q  = quat_from_eul(np.pi, np.deg2rad(-10), drone_state.yaw)
    roll_rate = np.pi / 0.4                  # rad/s · constant during the flip
    TYPEMASK  = 0b00000000                    # use q + body rates + thrust

    self.fc.mav.attitude_target_send(
        int((time.monotonic() - self.boot_t) * 1e3),
        self.fc.target_system, self.fc.target_component,
        TYPEMASK,
        [target_q[0], target_q[1], target_q[2], target_q[3]],
        roll_rate, 0.0, 0.0,
        thrust=0.92,                          # preserve altitude during inversion
    )

Enabling OFFBOARD · the arming sequence

OFFBOARD must be enabled by us, not by the GCS — and PX4 will only accept the mode switch if we're already streaming setpoints. Order matters:

1. Open MAVLink, wait for HEARTBEAT.
2. Start streaming SET_POSITION_TARGET_LOCAL_NED at ≥ 10 Hz (we use 100).
3. Wait until 1 s of stream is buffered (PX4's safety check).
4. Send MAV_CMD_DO_SET_MODE → custom_mode = OFFBOARD.
5. Send MAV_CMD_COMPONENT_ARM_DISARM with param1 = 1.

def arm_offboard(fc):
    """Stream-then-arm sequence per PX4 v1.15 safety check."""
    boot_t = time.monotonic()

    # 1. Pre-stream 1 second of zero setpoints (drone is on the ground)
    for _ in range(100):
        fc.mav.set_position_target_local_ned_send(
            int((time.monotonic() - boot_t) * 1e3),
            fc.target_system, fc.target_component,
            mavutil.mavlink.MAV_FRAME_LOCAL_NED,
            0b110111111000,        # pos only, hold ground
            0, 0, 0,                # x,y,z
            0, 0, 0,                # vx,vy,vz
            0, 0, 0,                # afx,afy,afz
            0, 0,                   # yaw, yaw_rate
        )
        time.sleep(0.01)

    # 2. Switch to OFFBOARD (custom_mode magic for PX4)
    fc.mav.command_long_send(
        fc.target_system, fc.target_component,
        mavutil.mavlink.MAV_CMD_DO_SET_MODE, 0,
        mavutil.mavlink.MAV_MODE_FLAG_CUSTOM_MODE_ENABLED,
        6,    # PX4 main_mode OFFBOARD
        0, 0, 0, 0, 0,
    )

    # 3. Arm
    fc.mav.command_long_send(
        fc.target_system, fc.target_component,
        mavutil.mavlink.MAV_CMD_COMPONENT_ARM_DISARM, 0,
        1, 0, 0, 0, 0, 0, 0,
    )
    # 4. Wait for HEARTBEAT.base_mode & MAV_MODE_FLAG_SAFETY_ARMED
    while not (fc.wait_heartbeat().base_mode & 0x80):
        time.sleep(0.01)

# Rate controller — tighter than stock, this is a racing drone
MC_ROLLRATE_P   = 0.18       # default 0.15 — sharper roll for split-S
MC_PITCHRATE_P  = 0.18       # default 0.15
MC_YAWRATE_P    = 0.22       # default 0.20 — pop yaw faster for s-turns

# Position controller
MPC_XY_P        = 1.4        # default 0.95 — sharper position tracking
MPC_Z_P         = 2.0        # default 1.0  — altitude matters in dive/splits
MPC_XY_VEL_MAX  = 30.0       # default 12.0 — match V_ATTACK
MPC_TILTMAX_AIR = 75         # default 45 — needed for high-G turns

# Safety
COM_OBL_RC_ACT  = 3          # OFFBOARD loss → LAND (not RETURN, no GPS)
COM_OF_LOSS_T   = 0.5        # default 0.5 s — re-fail if our 100 Hz stream stalls

Failure modes & mitigations · what PX4 does without us