ArduPilot Precision Landing with Raspberry Pi and ArUco

ArduPilot Precision Landing with Raspberry Pi and ArUco

Here is a complete, engineering-grade guide to implementing precision landing with a Raspberry Pi, OpenCV ArUco markers, and ArduPilot. It covers hardware, wiring, camera calibration, marker design, OpenCV pose/angle extraction, MAVLink integration, ArduPilot parameters, and test/tuning.

1) System overview

Goal: During LAND/RTL, the autopilot steers to a fiducial marker on the pad and descends onto it using vision corrections from a companion computer.
Data path: Pi camera → OpenCV/ArUco → compute bearing (and optionally range) to pad → send LANDING_TARGET at 10–50 Hz over MAVLink → ArduPilot fuses target with state and controls approach. (MAVLink)

2) Hardware

  • Companion: Raspberry Pi (Zero 2 W or better).

  • Camera: Fixed downward-facing module with known intrinsics. Global shutter is ideal but not required.

  • Autopilot: Any ArduPilot-compatible flight controller with a free telemetry UART.

  • Power: Provide stable 5 V to the Pi (2 A headroom recommended). Many carrier boards expose 5 V on TELEM ports; verify your specific board’s pinout and current capability before powering the Pi directly from the FC. Example carrier documentation confirms TELEM ports use JST-GH and 3.3 V-TTL UART signaling; 5 V rail availability depends on the board. (CubePilot)

  • Optional but recommended: Downward lidar/rangefinder for accurate AGL during the flare. Configure orientation as Down. (ArduPilot.org)

3) Wiring

Use a JST-GH 6-pin TELEM cable to the flight controller:

  • Pi TX (GPIO 14) → FC TELEM RX

  • Pi RX (GPIO 15) → FC TELEM TX

  • GND ↔ GND

  • 5 V only if your carrier board supports powering peripherals on TELEM and you’ve verified current limits; otherwise power the Pi separately.

UART levels are 3.3 V on both sides. Avoid cross-powering from USB and FC simultaneously.

On the Pi: enable UART, disable the serial login console.

sudo raspi-config   # Interface Options -> Serial: login shell “No”, serial hardware “Yes”

4) Software setup on the Pi

Install Python packages:

sudo apt update
sudo apt install -y python3-pip python3-opencv
pip install numpy pymavlink

Check your camera device (/dev/video0 for V4L2 or libcamera pipeline as applicable).

5) Camera calibration

You need the intrinsic matrix K and distortion coefficients D. Calibrate once with a chessboard or ChArUco and save to camera.yaml:

camera_matrix:
  data: [fx, 0, cx, 0, fy, cy, 0, 0, 1]
dist_coeffs:
  data: [k1, k2, p1, p2, k3]

6) Marker design and printing

  • Use a robust dictionary, e.g., DICT_6X6_100 or DICT_5X5_100.

  • Print at known side length (e.g., 0.20 m). Mount on a rigid, matte surface.

  • For higher altitude capture, consider an ArUco “board” (grid of tags) to increase pixel coverage and robustness.

7) Pose and bearing estimation

Two options:

A. Angles-only (recommended for simplicity):
Compute optical-axis angles from the pixel location of the marker center using calibrated intrinsics. This integrates cleanly with ArduPilot’s LANDING_TARGET when you also provide an accurate AGL from a rangefinder.

B. Full pose:
Use aruco.estimatePoseSingleMarkers with the known tag size to get tvec and rvec. Derive angles as:

  • angle_x = atan2(tvec.x, tvec.z)

  • angle_y = atan2(tvec.y, tvec.z)

When using angles-only, ArduPilot expects position_valid=0. If you provide body-frame x,y,z to the target you must set position_valid=1 (MAVLink v2 extensions). (ArduPilot.org)

8) MAVLink: LANDING_TARGET

  • Send at 10–50 Hz while the target is visible. Start at 20 Hz and adjust. (MAVLink)

  • Frames: use MAV_FRAME_BODY_NED for angles/position in the vehicle body frame.

  • Minimal fields that ArduPilot uses reliably: angle_x, angle_y, and either distance or a downward rangefinder. If you publish x,y,z, set position_valid=1 and use MAVLink2 on that serial port. (ArduPilot.org)

Serial setup: On the FC telemetry port you connect to the Pi, set SERIALx_PROTOCOL=2 (MAVLink2) and match baud. (ArduPilot.org)

9) ArduPilot parameters

  1. Enable precision landing and select source:

  • PLND_ENABLED = 1

  • PLND_TYPE = 1 (MAVLink LANDING_TARGET) (ArduPilot.org)

  1. Orientation and alignment:

  • If your camera is not straight down or x-axis not forward, set PLND_ORIENT and PLND_YAW_ALIGN to match the physical mount. (ArduPilot.org)

  1. Rangefinder (if used, recommended):

  • Configure RNGFND1_TYPE to your device, and set RNGFND1_ORIENT = Down. (ArduPilot.org)

  1. Telemetry port:

  • SERIALx_PROTOCOL = 2 (MAVLink2), baud to match the Pi link. (ArduPilot.org)

  1. Landing options:

  • Precision landing engages in LAND and at the end of RTL if target is detected. You can tune strictness and behavior with the documented precision-land options. (ArduPilot.org)

10) Reference implementation (Python, OpenCV + pymavlink)

The following script detects a single ArUco tag, computes angles, and sends LANDING_TARGET at ~20 Hz. It assumes you use a downward rangefinder for AGL; we therefore set position_valid=0 and leave x,y,z at zero. If you want to provide full body-frame position from vision, compute x,y,z from tvec and set position_valid=1 (MAVLink2 required). (ArduPilot.org)

#!/usr/bin/env python3
import cv2 as cv
import numpy as np
import time, math
from pymavlink import mavutil

# --- User config ---
CAMERA_ID = 0
FRAME_W, FRAME_H = 640, 480
ARUCO_DICT = cv.aruco.DICT_6X6_100
TAG_ID = 0
TAG_SIZE_M = 0.20  # marker side length in meters (used only if you want pose/xyz)
SERIAL_DEV = '/dev/ttyAMA0'  # Pixhawk TELEM port wired to Pi UART
BAUD = 921600                # or 57600 depending on your setup
USE_FULL_POSE = False        # True: send xyz + position_valid=1; False: angles-only

# --- Load calibration ---
fs = cv.FileStorage('camera.yaml', cv.FILE_STORAGE_READ)
K = fs.getNode('camera_matrix').mat()
D = fs.getNode('dist_coeffs').mat()
fs.release()

cam = cv.VideoCapture(CAMERA_ID)
cam.set(cv.CAP_PROP_FRAME_WIDTH, FRAME_W)
cam.set(cv.CAP_PROP_FRAME_HEIGHT, FRAME_H)
cam.set(cv.CAP_PROP_FPS, 60)

aruco = cv.aruco
dict_ = aruco.getPredefinedDictionary(ARUCO_DICT)
params = aruco.DetectorParameters()
detector = aruco.ArucoDetector(dict_, params)

# MAVLink connection
m = mavutil.mavlink_connection(SERIAL_DEV, baud=BAUD)
# Wait for heartbeat so target system/component IDs are known (optional but helpful)
try:
    m.wait_heartbeat(timeout=5)
except Exception:
    pass  # continue anyway

def center_from_corners(corners):
    # corners: (4,1,2) or similar; flatten:
    pts = corners.reshape(-1, 2)
    c = pts.mean(axis=0)
    return float(c[0]), float(c[1])

fx, fy = K[0,0], K[1,1]
cx, cy = K[0,2], K[1,2]

rate_hz = 20.0
period = 1.0 / rate_hz
next_t = time.time()

rvecs = None
tvecs = None

while True:
    ok, frame = cam.read()
    if not ok: 
        time.sleep(0.01)
        continue

    # Detect ArUco markers
    corners, ids, _ = detector.detectMarkers(frame)
    if ids is not None and TAG_ID in ids.flatten():
        idx = np.where(ids.flatten() == TAG_ID)[0][0]
        marker_corners = corners[idx]

        # --- Compute angles relative to optical axis ---
        u, v = center_from_corners(marker_corners)
        # Small-angle exact form:
        angle_x = math.atan2((u - cx) / fx, 1.0)
        angle_y = math.atan2((v - cy) / fy, 1.0)

        # Optional: full pose to get body-frame xyz from camera frame
        x_b = y_b = z_b = 0.0
        position_valid = 0
        if USE_FULL_POSE:
            obj_pts = np.array([
                [-TAG_SIZE_M/2,  TAG_SIZE_M/2, 0],
                [ TAG_SIZE_M/2,  TAG_SIZE_M/2, 0],
                [ TAG_SIZE_M/2, -TAG_SIZE_M/2, 0],
                [-TAG_SIZE_M/2, -TAG_SIZE_M/2, 0],
            ], dtype=np.float32)
            img_pts = marker_corners.reshape(-1,2).astype(np.float32)
            okp, rvec, tvec = cv.solvePnP(obj_pts, img_pts, K, D, flags=cv.SOLVEPNP_IPPE_SQUARE)
            if okp:
                # Camera frame: x right, y down, z forward
                # Convert to BODY(NED): x forward, y right, z down
                # Approximate mapping: x_b =  tvec[2]; y_b =  tvec[0]; z_b =  tvec[1]
                x_b = float(tvec[2])
                y_b = float(tvec[0])
                z_b = float(tvec[1])
                position_valid = 1
                # Re-derive angles from tvec for consistency
                angle_x = math.atan2(y_b, x_b)  # body: forward=x_b, right=y_b
                angle_y = math.atan2(z_b, x_b)  # body: down=z_b, forward=x_b

        # Throttle to target rate
        tnow = time.time()
        if tnow >= next_t:
            next_t += period

            # LANDING_TARGET send (MAVLink2 fields via keyword args are supported by pymavlink)
            m.mav.landing_target_send(
                int(tnow * 1e6),        # time_usec
                0,                      # target_num
                mavutil.mavlink.MAV_FRAME_BODY_NED,
                float(angle_x), float(angle_y),
                0.0,                    # distance (set 0 if using rangefinder)
                TAG_SIZE_M, TAG_SIZE_M, # size_x, size_y
                x_b, y_b, z_b,          # position in body frame (if available)
                [1.0, 0.0, 0.0, 0.0],   # orientation (unused here)
                mavutil.mavlink.LANDING_TARGET_TYPE_VISION_FIDUCIAL,
                position_valid
            )

    # Optional visualization for bench testing:
    # aruco.drawDetectedMarkers(frame, corners, ids)
    # cv.imshow("landing", frame)
    # if cv.waitKey(1) == 27:
    #     break

Notes:

  • If you use angles-only, set distance to zero and rely on the rangefinder AGL. If you provide x,y,z, set position_valid=1 and ensure the telem port is MAVLink2. (ArduPilot.org)

  • Send at a stable rate during approach; 10–50 Hz is typical. (MAVLink)

11) Flight-controller serial link

Pick a free TELEM port. Set:

  • SERIALx_PROTOCOL = 2 (MAVLink2)

  • SERIALx_BAUD to match your Pi (e.g., 921600 or 57600)

  • Flow control off unless wired. (ArduPilot.org)

12) Ground truth altitude

For best repeatability, configure a downward rangefinder and set orientation to Down. The autopilot will use it for low-altitude height-above-ground and to scale the final descent. (ArduPilot.org)

13) ArduPilot operation

  • Precision Landing engages in LAND mode when the target is detected and can also be used at the end of RTL. Behavior is tunable via documented options and strictness. (ArduPilot.org)

  • Precision Loiter is available for testing alignment without committing to a landing. (ArduPilot.org)

14) Bench test checklist

  1. With props off, power the FC and Pi.

  2. Confirm LANDING_TARGET traffic on the link at the expected rate.

  3. In a GCS, verify lateral offsets change sensibly as you move the marker under the camera.

  4. Confirm rangefinder readout is sane.

  5. Enter Loiter with the vehicle held in a rig and observe that position corrections trend toward the marker.

15) Flight test procedure

  1. Hover at ~3–5 m above the pad.

  2. Activate Precision Loiter (if configured) and confirm steady convergence.

  3. Switch to LAND while the pad is detected. Precision landing should engage and steer to the pad. If the pad is lost, behavior depends on your strictness setting.

16) Tuning guidelines

  • Marker size vs altitude: Increase marker area or use a board for earlier acquisition.

  • Frame rate vs exposure: Favor short exposure to reduce motion blur during descent.

  • Detection thresholds: Adjust ArUco detector parameters if you get false positives or dropouts.

  • Latency: Set PLND_* lag parameters to match your pipeline latency if exposed; otherwise reduce camera and processing latency.

  • Rate: Start at 20 Hz LANDING_TARGET. Increase if the vehicle is fast on descent. (MAVLink)

17) Common failure modes

  • No engagement in LAND/RTL: PLND_ENABLED not set, wrong PLND_TYPE, or no LANDING_TARGET data arriving. (ArduPilot.org)

  • Erratic last meter: Noisy/incorrect AGL. Add or fix the downward rangefinder and set orientation Down. (ArduPilot.org)

  • Angles inverted/rotated: Camera frame mis-aligned. Correct PLND_ORIENT/PLND_YAW_ALIGN and confirm BODY frame mapping. (ArduPilot.org)

  • Serial link silent: Wrong baud or protocol; set SERIALx_PROTOCOL=2, match baud. (ArduPilot.org)

18) Variations and improvements

  • ArUco board for robustness at higher altitudes.

  • Full pose fusion: Provide x,y,z with position_valid=1 if your pipeline yields stable body-frame position; keep MAVLink2 enabled on that port. (ArduPilot.org)

  • Gimbal stabilization: Keep the tag centered in FOV at altitude for earlier handoff.

  • Thermal/IR tags: Enables night operations with appropriate cameras; see current research for range/robustness ideas. (arXiv)

19) Security and reliability notes

  • Use MAVLink2 on the companion link and enable signing if the companion and FC both support it. (ArduPilot.org)

  • Add watchdogs: if detection stalls, stop sending or flag loss so the FC can fall back per your strictness settings. (ArduPilot.org)

  • Log both camera detections and outgoing MAVLink for post-flight diagnosis.

20) Parameter quick-sheet

  • PLND_ENABLED = 1

  • PLND_TYPE = 1 (MAVLink LANDING_TARGET) (ArduPilot.org)

  • SERIALx_PROTOCOL = 2 (MAVLink2) and matching SERIALx_BAUD (ArduPilot.org)

  • RNGFND1_* configured and RNGFND1_ORIENT = Down if using lidar (ArduPilot.org)

  • PLND_ORIENT, PLND_YAW_ALIGN to match camera mounting (ArduPilot.org)


This implementation favors a simple, robust “angles + rangefinder” pipeline first, then optional full-pose fusion if you want tighter control in the last meter. The cited ArduPilot and MAVLink references above are the ground truth for message semantics, serial protocol, and parameter meanings.