อ่าน telemetry ด้วย pymavlink

ทำไมต้องรู้

โดรนที่ใช้ PX4 หรือ ArduPilot ส่งสถานะของตัวเองให้สถานีภาคพื้นผ่านโพรโทคอล MAVLink และสถานีภาคพื้นส่วนใหญ่บันทึกทุกข้อความที่ได้รับลงไฟล์ .tlog เมื่อเกิดเหตุผิดปกติ ไฟล์นี้คือหลักฐานว่าโดรนรายงานอะไร เมื่อไร และลิงก์ขาดตอนไหน (UAT 312 โมดูล 4) ผู้ที่อ่านไฟล์นี้ด้วยโปรแกรมได้จะวิเคราะห์เหตุได้เร็วและแม่นกว่าการเลื่อนดูกราฟในโปรแกรมสำเร็จรูป

โมดูลนี้ใช้ไฟล์ telemetry_sample.tlog ซึ่งเป็น ข้อมูลสังเคราะห์ จากแบบจำลองเที่ยวบินเดียวกับโมดูล 3 มีลิงก์ขาดที่ตั้งใจใส่ไว้ 8 วินาที

โครงสร้างเฟรม MAVLink 2

ข้อความ MAVLink 2 ทุกข้อความเริ่มด้วยไบต์ 0xFD (MAVLink 1 ใช้ 0xFE) ตามด้วยส่วนหัวที่บอกความยาว เลขลำดับ ผู้ส่ง และรหัสข้อความ จากนั้นเป็นข้อมูล (payload) และ checksum

ไฟล์ .tlog เก็บแต่ละข้อความต่อท้ายเวลาที่รับ 8 ไบต์ (ไมโครวินาทีตั้งแต่ปี 1970 เรียงแบบ big-endian) การอ่านไบต์ดิบหนึ่งครั้งช่วยให้เห็นว่าไลบรารีทำอะไรให้เรา

import struct
from datetime import datetime, timezone

with open("telemetry_sample.tlog", "rb") as f:
    raw = f.read(20)

ts_us = struct.unpack(">Q", raw[:8])[0]
print(datetime.fromtimestamp(ts_us / 1e6, tz=timezone.utc).isoformat())
print(" ".join(f"{b:02X}" for b in raw[8:20]))

stx, length, incompat, compat, seq, sysid, compid = raw[8:15]
msgid = int.from_bytes(raw[15:18], "little")
print(f"stx=0x{stx:02X} len={length} seq={seq} sysid={sysid} compid={compid} msgid={msgid}")

2026-03-14T02:30:00+00:00
FD 09 00 00 00 01 01 00 00 00 04 00
stx=0xFD len=9 seq=0 sysid=1 compid=1 msgid=0

รหัสข้อความ 0 คือ HEARTBEAT และ payload ยาว 9 ไบต์ เพราะ MAVLink 2 ตัดไบต์ศูนย์ท้าย payload ออกก่อนส่ง

อ่านไฟล์ด้วย pymavlink

pymavlink แยกไบต์เป็นข้อความให้ ใช้ mavutil.mavlink_connection() เปิดได้ทั้งไฟล์และการเชื่อมต่อสด

from collections import Counter
from pymavlink import mavutil


def read_all(path, msg_type=None):
    """Yield every message (optionally of one type) from a .tlog file."""
    link = mavutil.mavlink_connection(path)
    while True:
        msg = link.recv_match(type=msg_type, blocking=False)
        if msg is None:
            return
        yield msg


counts = Counter(msg.get_type() for msg in read_all("telemetry_sample.tlog"))
print(counts.most_common())

[('ATTITUDE', 3955), ('GLOBAL_POSITION_INT', 1582), ('VFR_HUD', 1582), ('HEARTBEAT', 396), ('SYS_STATUS', 396)]

ฟังก์ชัน read_all() ใช้ yield ทำให้เป็น generator ซึ่งส่งข้อความออกมาทีละข้อความโดยไม่ต้องเก็บทั้งไฟล์ในหน่วยความจำ เหมาะกับ log ขนาดใหญ่

หน่วยของข้อความสำคัญ

ข้อความ MAVLink เก็บตัวเลขเป็นจำนวนเต็มในหน่วยที่ต่างจากที่คนใช้ ต้องแปลงหน่วยทุกครั้ง ตามเอกสาร common message set

hb = next(read_all("telemetry_sample.tlog", "HEARTBEAT"))
print("quadrotor:", hb.type == mavutil.mavlink.MAV_TYPE_QUADROTOR)
print("ArduPilot:", hb.autopilot == mavutil.mavlink.MAV_AUTOPILOT_ARDUPILOTMEGA)
print("mode:", mavutil.mode_string_v10(hb))
print("armed:", bool(hb.base_mode & mavutil.mavlink.MAV_MODE_FLAG_SAFETY_ARMED))

pos = next(m for m in read_all("telemetry_sample.tlog", "GLOBAL_POSITION_INT") if m.relative_alt > 29_000)
print(f"lat {pos.lat / 1e7:.6f}, lon {pos.lon / 1e7:.6f}")
print(f"height above home {pos.relative_alt / 1000:.1f} m, heading {pos.hdg / 100:.0f} deg")
print(f"climb rate {-pos.vz / 100:.1f} m/s")

quadrotor: True
ArduPilot: True
mode: GUIDED
armed: False
lat 14.099999, lon 100.599999
height above home 29.5 m, heading 90 deg
climb rate 2.5 m/s

หาช่วงลิงก์ขาดและแพ็กเก็ตหาย

ข้อความแต่ละข้อความมี เลขลำดับ (sequence number) 0–255 ที่วนกลับ ถ้าเลขกระโดด แปลว่ามีแพ็กเก็ตที่ผู้ส่งส่งแต่เราไม่ได้รับ ส่วน heartbeat นิยมส่งราว 1 ครั้งต่อวินาทีบนลิงก์วิทยุ (MAVLink ระบุว่าอัตรานี้ขึ้นกับช่องทาง ไม่ได้กำหนดตายตัว) ช่วงที่ไม่มี heartbeat นานผิดปกติจึงบอกว่าลิงก์ขาด

t0 = prev_t = prev_seq = None
gaps, modes = [], []
received = lost = 0

for msg in read_all("telemetry_sample.tlog"):
    received += 1
    seq = msg.get_seq()
    if prev_seq is not None:
        lost += (seq - prev_seq - 1) % 256
    prev_seq = seq

    if msg.get_type() == "HEARTBEAT":
        t = msg._timestamp
        t0 = t if t0 is None else t0
        if prev_t is not None and t - prev_t > 3:
            gaps.append((round(prev_t - t0, 1), round(t - prev_t, 1)))
        prev_t = t
        mode = mavutil.mode_string_v10(msg)
        if not modes or modes[-1][1] != mode:
            modes.append((round(t - t0, 1), mode))

print("heartbeat gaps (start s, length s):", gaps)
print(f"lost {lost} of {received + lost} packets ({100 * lost / (received + lost):.2f}%)")
print("mode changes:", modes)

heartbeat gaps (start s, length s): [(149.0, 9.0)]
lost 160 of 8071 packets (1.98%)
mode changes: [(0.0, 'GUIDED'), (22.0, 'AUTO'), (352.0, 'RTL'), (387.0, 'LAND')]

ส่งออกเป็น CSV

import pandas as pd

rows = [
    {
        "time_s": m.time_boot_ms / 1000,
        "lat_deg": m.lat / 1e7,
        "lon_deg": m.lon / 1e7,
        "rel_alt_m": m.relative_alt / 1000,
    }
    for m in read_all("telemetry_sample.tlog", "GLOBAL_POSITION_INT")
]
position = pd.DataFrame(rows)
position.to_csv("position_from_tlog.csv", index=False)
print(position.shape, "max height", position["rel_alt_m"].max(), "m")

(1582, 4) max height 30.0 m

ไฟล์ที่ได้ใช้ต่อด้วยเครื่องมือจากโมดูล 3 ได้ทันที time_boot_ms เป็นเวลานับจากที่ออโตไพลอตเปิดเครื่อง ไม่ใช่เวลาจริง ถ้าต้องการเวลาจริงให้ใช้ _timestamp ที่สถานีภาคพื้นบันทึก

เชื่อมต่อ SITL แบบสด

เมื่อพร้อมแล้วจึงต่อกับ SITL (UAT 312 โมดูล 2) โค้ดเดียวกันใช้ได้ เปลี่ยนแค่ต้นทาง ArduPilot SITL ผ่าน MAVProxy ส่งออกทาง UDP พอร์ต 14550 และ PX4 SITL ส่งให้สถานีภาคพื้นทาง UDP 14550 และให้โปรแกรมควบคุมทาง UDP 14540

from pymavlink import mavutil

link = mavutil.mavlink_connection("udp:127.0.0.1:14550")
link.wait_heartbeat(timeout=10)
print("connected to system", link.target_system)

for _ in range(20):
    att = link.recv_match(type="ATTITUDE", blocking=True, timeout=2)
    if att is not None:
        print(f"roll {att.roll:.3f} rad, pitch {att.pitch:.3f} rad")

ปฏิบัติการประจำโมดูล

ข้อผิดพลาดที่พบบ่อย

สรุป

• เฟรม MAVLink 2 เริ่มด้วย 0xFD มีส่วนหัวบอกความยาว ลำดับ ผู้ส่ง และรหัสข้อความ
• mavutil.mavlink_connection() เปิดได้ทั้งไฟล์ .tlog และการเชื่อมต่อสด ถ้าอ่านไฟล์ต้องใช้ blocking=False
• ต้องแปลงหน่วยทุกครั้ง: degE7 → องศา, mm → m, cm/s → m/s, cdeg → องศา
• ช่วงไม่มี heartbeat บอกลิงก์ขาด และเลขลำดับที่กระโดดบอกจำนวนแพ็กเก็ตที่หาย
• ส่งออกเป็น CSV แล้ววิเคราะห์ต่อด้วย pandas และทดสอบกับ SITL ก่อนทำงานกับอากาศยานจริง

แบบฝึกตรวจความเข้าใจ

1. ไบต์แรกของเฟรม MAVLink 2 คืออะไร และ MAVLink 1 คืออะไร
2. lat = 140999986 ในข้อความ GLOBAL_POSITION_INT คือละติจูดกี่องศา
3. voltage_battery = 24310 ใน SYS_STATUS คือกี่โวลต์
4. เลขลำดับข้อความก่อนหน้าคือ 250 ข้อความถัดไปที่ได้รับคือ 3 มีแพ็กเก็ตหายกี่แพ็กเก็ต
5. ทำไมอ่านไฟล์ .tlog ด้วย recv_match(blocking=True) แล้วโปรแกรมค้าง